static void
applyImpulse(cpPivotJoint *joint)
{
	cpBody *a = joint->constraint.a;
	cpBody *b = joint->constraint.b;
	
	cpVect r1 = joint->r1;
	cpVect r2 = joint->r2;
		
	// compute relative velocity
	cpVect vr = relative_velocity(a, b, r1, r2);
	
	// compute normal impulse
	cpVect j = mult_k(cpvsub(joint->bias, vr), joint->k1, joint->k2);
	cpVect jOld = joint->jAcc;
	joint->jAcc = cpvclamp(cpvadd(joint->jAcc, j), joint->jMaxLen);
	j = cpvsub(joint->jAcc, jOld);
	
	// apply impulse
	apply_impulses(a, b, joint->r1, joint->r2, j);
}

void
cpArbiterApplyCachedImpulse(cpArbiter *arb)
{
	cpShape *shapea = arb->a;
	cpShape *shapeb = arb->b;
		
	arb->u = shapea->u * shapeb->u;
	arb->target_v = cpvsub(shapeb->surface_v, shapea->surface_v);

	cpBody *a = shapea->body;
	cpBody *b = shapeb->body;
	
	for(int i=0; i<arb->numContacts; i++){
		cpContact *con = &arb->contacts[i];
		
		cpVect t = cpvperp(con->n);
		cpVect j = cpvadd(cpvmult(con->n, con->jnAcc), cpvmult(t, con->jtAcc));
		cpBodyApplyImpulse(a, cpvneg(j), con->r1);
		cpBodyApplyImpulse(b, j, con->r2);
	}
}

static void
applyImpulse(cpPivotJoint *joint, cpFloat dt)
{
	cpBody *a = joint->constraint.a;
	cpBody *b = joint->constraint.b;
	
	cpVect r1 = joint->r1;
	cpVect r2 = joint->r2;
		
	// compute relative velocity
	cpVect vr = relative_velocity(a, b, r1, r2);
	
	// compute normal impulse
	cpVect j = cpMat2x2Transform(joint->k, cpvsub(joint->bias, vr));
	cpVect jOld = joint->jAcc;
	joint->jAcc = cpvclamp(cpvadd(joint->jAcc, j), joint->constraint.maxForce*dt);
	j = cpvsub(joint->jAcc, jOld);
	
	// apply impulse
	apply_impulses(a, b, joint->r1, joint->r2, j);
}

// Add contact points for circle to circle collisions.
// Used by several collision tests.
static int
circle2circleQuery(const cpVect p1, const cpVect p2, const cpFloat r1, const cpFloat r2, cpContact *con)
{
	cpFloat mindist = r1 + r2;
	cpVect delta = cpvsub(p2, p1);
	cpFloat distsq = cpvlengthsq(delta);
	if(distsq >= mindist*mindist) return 0;
	
	cpFloat dist = cpfsqrt(distsq);

	// Allocate and initialize the contact.
	cpContactInit(
		con,
		cpvadd(p1, cpvmult(delta, 0.5f + (r1 - 0.5f*mindist)/(dist ? dist : INFINITY))),
		(dist ? cpvmult(delta, 1.0f/dist) : cpv(1.0f, 0.0f)),
		dist - mindist,
		0
	);
	
	return 1;
}

static void
cpPolyShapeNearestPointQuery(cpPolyShape *poly, cpVect p, cpNearestPointQueryInfo *info){
	int count = poly->numVerts;
	cpSplittingPlane *planes = poly->tPlanes;
	cpVect *verts = poly->tVerts;
	cpFloat r = poly->r;
	
	cpVect v0 = verts[count - 1];
	cpFloat minDist = INFINITY;
	cpVect closestPoint = cpvzero;
	cpVect closestNormal = cpvzero;
	cpBool outside = cpFalse;
	
	for(int i=0; i<count; i++){
		if(cpSplittingPlaneCompare(planes[i], p) > 0.0f) outside = cpTrue;
		
		cpVect v1 = verts[i];
		cpVect closest = cpClosetPointOnSegment(p, v0, v1);
		
		cpFloat dist = cpvdist(p, closest);
		if(dist < minDist){
			minDist = dist;
			closestPoint = closest;
			closestNormal = planes[i].n;
		}
		
		v0 = v1;
	}
	
	cpFloat dist = (outside ? minDist : -minDist);
	cpVect g = cpvmult(cpvsub(p, closestPoint), 1.0f/dist);
	
	info->shape = (cpShape *)poly;
	info->p = cpvadd(closestPoint, cpvmult(g, r));
	info->d = dist - r;
	
	// Use the normal of the closest segment if the distance is small.
	info->g = (minDist > MAGIC_EPSILON ? g : closestNormal);
}

static void DrawShape(cpShape *shape, DrawNode *renderer)
{
    cpBody *body = cpShapeGetBody(shape);
    Color4F color = ColorForBody(body);
    
    switch (shape->CP_PRIVATE(klass)->type)
    {
        case CP_CIRCLE_SHAPE:
        {
            cpCircleShape *circle = (cpCircleShape *)shape;
            cpVect center = circle->tc;
            cpFloat radius = circle->r;
            renderer->drawDot(cpVert2Point(center), cpfmax(radius, 1.0), color);
            renderer->drawSegment(cpVert2Point(center), cpVert2Point(cpvadd(center, cpvmult(cpBodyGetRotation(body), radius))), 1.0, color);
        }
             break;
        case CP_SEGMENT_SHAPE:
        {
            cpSegmentShape *seg = (cpSegmentShape *)shape;
            renderer->drawSegment(cpVert2Point(seg->ta), cpVert2Point(seg->tb), cpfmax(seg->r, 2.0), color);
        }
            break;
        case CP_POLY_SHAPE:
        {
            cpPolyShape* poly = (cpPolyShape*)shape;
            Color4F line = color;
            line.a = cpflerp(color.a, 1.0, 0.5);
            int num = poly->count;
            Vec2* pPoints = new (std::nothrow) Vec2[num];
            for(int i=0;i<num;++i)
                pPoints[i] = cpVert2Point(poly->planes[i].v0);
            renderer->drawPolygon(pPoints, num, color, 1.0, line);
            CC_SAFE_DELETE_ARRAY(pPoints);
        }
            break;
        default:
            cpAssertHard(false, "Bad assertion in DrawShape()");
    }
}

static cpBody *
addChassis(cpVect pos, cpVect boxOffset)
{
	int num = 4;
	cpVect verts[] = {
		cpv(-40,-15),
		cpv(-40, 15),
		cpv( 40, 15),
		cpv( 40,-15),
	};

	
	cpFloat mass = 5.0f;
	cpBody *body = cpSpaceAddBody(space, cpBodyNew(mass, cpMomentForPoly(mass, num, verts, cpvzero)));
	body->p = cpvadd(pos, boxOffset);
	
	cpShape *shape = cpSpaceAddShape(space, cpPolyShapeNew(body, num, verts, cpvzero));
	shape->e = 0.0f; shape->u = 0.7f;
	shape->group = 1; // use a group to keep the car parts from colliding
	
	return body;
}

static cpBB
cpPolyShapeTransformVerts(cpPolyShape *poly, cpVect p, cpVect rot)
{
	cpVect *src = poly->verts;
	cpVect *dst = poly->tVerts;
	
	cpFloat l = (cpFloat)INFINITY, r = -(cpFloat)INFINITY;
	cpFloat b = (cpFloat)INFINITY, t = -(cpFloat)INFINITY;
	
	for(int i=0; i<poly->numVerts; i++){
		cpVect v = cpvadd(p, cpvrotate(src[i], rot));
		
		dst[i] = v;
		l = cpfmin(l, v.x);
		r = cpfmax(r, v.x);
		b = cpfmin(b, v.y);
		t = cpfmax(t, v.y);
	}
	
	cpFloat radius = poly->r;
	return cpBBNew(l - radius, b - radius, r + radius, t + radius);
}

bool Player::update(float time, int direction) {
	this->direction = direction;
	loops++;

	if(direction & UP){
		jumpState = 1;
	}else{
		jumpState = 0;
	}

	if(jumpState && !lastJumpState && grounded){
		cpFloat jump_v = cpfsqrt(2.0*JUMP_HEIGHT*GRAVITY);
		playerBody->v = cpvadd(playerBody->v, cpv(0.0, -jump_v));
		
		remainingBoost = JUMP_BOOST_HEIGHT/jump_v;
	}
	
	remainingBoost -= 1.0f/60.0f;
	lastJumpState = jumpState;

	return true;
}

static void
update(int ticks)
{
	messageString[0] = '\0';
	
	cpVect start = cpvzero;
	cpVect end = /*cpv(0, 85);//*/mousePoint;
	cpVect lineEnd = end;
	
	{
		char infoString[1024];
		sprintf(infoString, "Query: Dist(%f) Point%s, ", cpvdist(start, end), cpvstr(end));
		strcat(messageString, infoString);
	}
	
	cpSegmentQueryInfo info = {};
	if(cpSpaceSegmentQueryFirst(space, start, end, CP_ALL_LAYERS, CP_NO_GROUP, &info)){
		cpVect point = cpSegmentQueryHitPoint(start, end, info);
		lineEnd = cpvadd(point, cpvzero);//cpvmult(info.n, 4.0f));
		
		char infoString[1024];
		sprintf(infoString, "Segment Query: Dist(%f) Normal%s", cpSegmentQueryHitDist(start, end, info), cpvstr(info.n));
		strcat(messageString, infoString);
	} else {
		strcat(messageString, "Segment Query (None)");
	}
	
	cpSegmentShapeSetEndpoints(querySeg, start, lineEnd);
	cpShapeCacheBB(querySeg); // force it to update it's collision detection data so it will draw
	
	// normal other stuff.
	int steps = 1;
	cpFloat dt = 1.0f/60.0f/(cpFloat)steps;
	
	for(int i=0; i<steps; i++){
		cpSpaceStep(space, dt);
	}
}

static void
cpPolyShapePointQuery(cpPolyShape *poly, cpVect p, cpPointQueryInfo *info){
	int count = poly->count;
	struct cpSplittingPlane *planes = poly->planes;
	cpFloat r = poly->r;
	
	cpVect v0 = planes[count - 1].v0;
	cpFloat minDist = INFINITY;
	cpVect closestPoint = cpvzero;
	cpVect closestNormal = cpvzero;
	cpBool outside = cpFalse;
	
	for(int i=0; i<count; i++){
		cpVect v1 = planes[i].v0;
		outside = outside || (cpvdot(planes[i].n, cpvsub(p,v1)) > 0.0f);
		
		cpVect closest = cpClosetPointOnSegment(p, v0, v1);
		
		cpFloat dist = cpvdist(p, closest);
		if(dist < minDist){
			minDist = dist;
			closestPoint = closest;
			closestNormal = planes[i].n;
		}
		
		v0 = v1;
	}
	
	cpFloat dist = (outside ? minDist : -minDist);
	cpVect g = cpvmult(cpvsub(p, closestPoint), 1.0f/dist);
	
	info->shape = (cpShape *)poly;
	info->point = cpvadd(closestPoint, cpvmult(g, r));
	info->distance = dist - r;
	
	// Use the normal of the closest segment if the distance is small.
	info->gradient = (minDist > MAGIC_EPSILON ? g : closestNormal);
}

// Add contact points for circle to circle collisions.
// Used by several collision tests.
static int
circle2circleQuery(cpVect p1, cpVect p2, cpFloat r1, cpFloat r2, cpContact *con)
{
	cpFloat mindist = r1 + r2;
	cpVect delta = cpvsub(p2, p1);
	cpFloat distsq = cpvlengthsq(delta);
	if(distsq >= mindist*mindist) return 0;
	
	cpFloat dist = cpfsqrt(distsq);
	// To avoid singularities, do nothing in the case of dist = 0.
	cpFloat non_zero_dist = (dist ? dist : INFINITY);

	// Allocate and initialize the contact.
	cpContactInit(
		con,
		cpvadd(p1, cpvmult(delta, 0.5f + (r1 - 0.5f*mindist)/non_zero_dist)),
		cpvmult(delta, 1.0f/non_zero_dist),
		dist - mindist,
		0
	);
	
	return 1;
}

static void
update(int ticks)
{
	static int lastJumpState = 0;
	int jumpState = (arrowDirection.y > 0.0f);
	
	cpVect groundNormal = playerInstance.groundNormal;
	if(groundNormal.y > 0.0f){
		playerInstance.shape->surface_v = cpvmult(cpvperp(groundNormal), 400.0f*arrowDirection.x);
	} else {
		playerInstance.shape->surface_v = cpvzero;
	}
	
	cpBody *body = playerInstance.shape->body;
	
	// apply jump
	if(jumpState && !lastJumpState && cpvlengthsq(groundNormal)){
//		body->v = cpvmult(cpvslerp(groundNormal, cpv(0.0f, 1.0f), 0.5f), 500.0f);
		body->v = cpvadd(body->v, cpvmult(cpvslerp(groundNormal, cpv(0.0f, 1.0f), 0.75f), 500.0f));
	}
	
	if(playerInstance.groundShapes->num == 0){
		cpFloat air_accel = body->v.x + arrowDirection.x*(2000.0f);
		body->f.x = body->m*air_accel;
//		body->v.x = cpflerpconst(body->v.x, 400.0f*arrowDirection.x, 2000.0f/60.0f);
	}
	
	int steps = 3;
	cpFloat dt = 1.0f/60.0f/(cpFloat)steps;
	
	playerInstance.groundNormal = cpvzero;
	for(int i=0; i<steps; i++){
		cpSpaceStep(space, dt);
	}
	
	lastJumpState = jumpState;
}

static void
update(cpSpace *space)
{
	int jumpState = (ChipmunkDemoKeyboard.y > 0.0f);
	
	// If the jump key was just pressed this frame, jump!
	if(jumpState && !lastJumpState && grounded){
		cpFloat jump_v = cpfsqrt(2.0*JUMP_HEIGHT*GRAVITY);
		playerBody->v = cpvadd(playerBody->v, cpv(0.0, jump_v));
		
		remainingBoost = JUMP_BOOST_HEIGHT/jump_v;
	}
	
	// Step the space
	int steps = 3;
	cpFloat dt = 1.0f/60.0f/(cpFloat)steps;
	
	for(int i=0; i<steps; i++){
		cpSpaceStep(space, dt);
		
		remainingBoost -= dt;
		lastJumpState = jumpState;
	}
}

static void DrawShape(cpShape *shape, DrawNode *renderer)
{
    cpBody *body = shape->body;
    Color4F color = ColorForBody(body);

    switch (shape->CP_PRIVATE(klass)->type)
    {
    case CP_CIRCLE_SHAPE:
    {
        cpCircleShape *circle = (cpCircleShape *)shape;
        cpVect center = circle->tc;
        cpFloat radius = circle->r;
        renderer->drawDot(cpVert2Point(center), cpfmax(radius, 1.0), color);
        renderer->drawSegment(cpVert2Point(center), cpVert2Point(cpvadd(center, cpvmult(body->rot, radius))), 1.0, color);
    }
    break;
    case CP_SEGMENT_SHAPE:
    {
        cpSegmentShape *seg = (cpSegmentShape *)shape;
        renderer->drawSegment(cpVert2Point(seg->ta), cpVert2Point(seg->tb), cpfmax(seg->r, 2.0), color);
    }
    break;
    case CP_POLY_SHAPE:
    {
        cpPolyShape *poly = (cpPolyShape *)shape;
        Color4F line = color;
        line.a = cpflerp(color.a, 1.0, 0.5);
        Vec2* pPoints = cpVertArray2ccpArrayN(poly->tVerts, poly->numVerts);
        renderer->drawPolygon(pPoints, poly->numVerts, color, 1.0, line);
        CC_SAFE_DELETE_ARRAY(pPoints);
    }
    break;
    default:
        cpAssertHard(false, "Bad assertion in DrawShape()");
    }
}

static void
cpSegmentShapePointQuery(cpSegmentShape *seg, cpVect p, cpPointQueryExtendedInfo *info){
	if(!cpBBContainsVect(seg->shape.bb, p)) return;
	
	cpVect a = seg->ta;
	cpVect b = seg->tb;
	
	cpVect seg_delta = cpvsub(b, a);
	cpFloat closest_t = cpfclamp01(cpvdot(seg_delta, cpvsub(p, a))/cpvlengthsq(seg_delta));
	cpVect closest = cpvadd(a, cpvmult(seg_delta, closest_t));
	
	cpVect delta = cpvsub(p, closest);
	cpFloat distsq = cpvlengthsq(delta);
	cpFloat r = seg->r;
	
	if(distsq < r*r){
		info->shape = (cpShape *)seg;
		
		cpFloat dist = cpfsqrt(distsq);
		info->d = r - dist;
		info->n = cpvmult(delta, 1.0/dist);
	}

}

void
cpArbiterPreStep(cpArbiter *arb, cpFloat dt, cpFloat slop, cpFloat bias)
{
	cpBody *a = arb->body_a;
	cpBody *b = arb->body_b;
	cpVect n = arb->n;
	cpVect body_delta = cpvsub(b->p, a->p);
	
	for(int i=0; i<arb->count; i++){
		struct cpContact *con = &arb->contacts[i];
		
		// Calculate the mass normal and mass tangent.
		con->nMass = 1.0f/k_scalar(a, b, con->r1, con->r2, n);
		con->tMass = 1.0f/k_scalar(a, b, con->r1, con->r2, cpvperp(n));
				
		// Calculate the target bias velocity.
		cpFloat dist = cpvdot(cpvadd(cpvsub(con->r2, con->r1), body_delta), n);
		con->bias = -bias*cpfmin(0.0f, dist + slop)/dt;
		con->jBias = 0.0f;
		
		// Calculate the target bounce velocity.
		con->bounce = normal_relative_velocity(a, b, con->r1, con->r2, n)*arb->e;
	}
}

cpVect * bmx_cpvect_add(cpVect * vec, cpVect * vec2) {
	return bmx_cpvect_new(cpvadd(*vec, *vec2));
}

// Basically the same as cpvlerp(), except t = [-1, 1]
static inline cpVect
LerpT(const cpVect a, const cpVect b, const cpFloat t)
{
    cpFloat ht = 0.5f*t;
    return cpvadd(cpvmult(a, 0.5f - ht), cpvmult(b, 0.5f + ht));
}

// Find the closest p(t) to (0, 0) where p(t) = a*(1-t)/2 + b*(1+t)/2
// The range for t is [-1, 1] to avoid floating point issues if the parameters are swapped.
static inline cpFloat
ClosestT(const cpVect a, const cpVect b)
{
    cpVect delta = cpvsub(b, a);
    return -cpfclamp(cpvdot(delta, cpvadd(a, b))/cpvlengthsq(delta), -1.0f, 1.0f);
}