Example #1
0
void
cpDampedSpring(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat rlen, cpFloat k, cpFloat dmp, cpFloat dt)
{
	// Calculate the world space anchor coordinates.
	cpVect r1 = cpvrotate(anchr1, a->rot);
	cpVect r2 = cpvrotate(anchr2, b->rot);
	
	cpVect delta = cpvsub(cpvadd(b->p, r2), cpvadd(a->p, r1));
	cpFloat dist = cpvlength(delta);
	cpVect n = dist ? cpvmult(delta, 1.0f/dist) : cpvzero;
	
	cpFloat f_spring = (dist - rlen)*k;

	// Calculate the world relative velocities of the anchor points.
	cpVect v1 = cpvadd(a->v, cpvmult(cpvperp(r1), a->w));
	cpVect v2 = cpvadd(b->v, cpvmult(cpvperp(r2), b->w));
	
	// Calculate the damping force.
	// This really should be in the impulse solver and can produce problems when using large damping values.
	cpFloat vrn = cpvdot(cpvsub(v2, v1), n);
	cpFloat f_damp = vrn*cpfmin(dmp, 1.0f/(dt*(a->m_inv + b->m_inv)));
	
	// Apply!
	cpVect f = cpvmult(n, f_spring + f_damp);
	cpBodyApplyForce(a, f, r1);
	cpBodyApplyForce(b, cpvneg(f), r2);
}
void
cpArbiterApplyImpulse(cpArbiter *arb)
{
	cpBody *a = arb->a->body;
	cpBody *b = arb->b->body;

	for(int i=0; i<arb->numContacts; i++){
		cpContact *con = &arb->contacts[i];
		cpVect n = con->n;
		cpVect r1 = con->r1;
		cpVect r2 = con->r2;
		
		// Calculate the relative bias velocities.
		cpVect vb1 = cpvadd(a->v_bias, cpvmult(cpvperp(r1), a->w_bias));
		cpVect vb2 = cpvadd(b->v_bias, cpvmult(cpvperp(r2), b->w_bias));
		cpFloat vbn = cpvdot(cpvsub(vb2, vb1), n);
		
		// Calculate and clamp the bias impulse.
		cpFloat jbn = (con->bias - vbn)*con->nMass;
		cpFloat jbnOld = con->jBias;
		con->jBias = cpfmax(jbnOld + jbn, 0.0f);
		jbn = con->jBias - jbnOld;
		
		// Apply the bias impulse.
		cpVect jb = cpvmult(n, jbn);
		cpBodyApplyBiasImpulse(a, cpvneg(jb), r1);
		cpBodyApplyBiasImpulse(b, jb, r2);

		// Calculate the relative velocity.
		cpVect v1 = cpvadd(a->v, cpvmult(cpvperp(r1), a->w));
		cpVect v2 = cpvadd(b->v, cpvmult(cpvperp(r2), b->w));
		cpVect vr = cpvsub(v2, v1);
		cpFloat vrn = cpvdot(vr, n);
		
		// Calculate and clamp the normal impulse.
		cpFloat jn = -(con->bounce + vrn)*con->nMass;
		cpFloat jnOld = con->jnAcc;
		con->jnAcc = cpfmax(jnOld + jn, 0.0f);
		jn = con->jnAcc - jnOld;
		
		// Calculate the relative tangent velocity.
		cpVect t = cpvperp(n);
		cpFloat vrt = cpvdot(cpvadd(vr, arb->target_v), t);
		
		// Calculate and clamp the friction impulse.
		cpFloat jtMax = arb->u*con->jnAcc;
		cpFloat jt = -vrt*con->tMass;
		cpFloat jtOld = con->jtAcc;
		con->jtAcc = cpfmin(cpfmax(jtOld + jt, -jtMax), jtMax);
		jt = con->jtAcc - jtOld;
		
		// Apply the final impulse.
		cpVect j = cpvadd(cpvmult(n, jn), cpvmult(t, jt));
		cpBodyApplyImpulse(a, cpvneg(j), r1);
		cpBodyApplyImpulse(b, j, r2);
	}
}
void
cpArbiterPreStep(cpArbiter *arb, cpFloat dt_inv)
{
	cpShape *shapea = arb->a;
	cpShape *shapeb = arb->b;
		
	arb->e = shapea->e * shapeb->e;
	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];
		
		// Calculate the offsets.
		con->r1 = cpvsub(con->p, a->p);
		con->r2 = cpvsub(con->p, b->p);
		
		// Calculate the mass normal.
		cpFloat mass_sum = a->m_inv + b->m_inv;
		
		cpFloat r1cn = cpvcross(con->r1, con->n);
		cpFloat r2cn = cpvcross(con->r2, con->n);
		cpFloat kn = mass_sum + a->i_inv*r1cn*r1cn + b->i_inv*r2cn*r2cn;
		con->nMass = 1.0f/kn;
		
		// Calculate the mass tangent.
		cpVect t = cpvperp(con->n);
		cpFloat r1ct = cpvcross(con->r1, t);
		cpFloat r2ct = cpvcross(con->r2, t);
		cpFloat kt = mass_sum + a->i_inv*r1ct*r1ct + b->i_inv*r2ct*r2ct;
		con->tMass = 1.0f/kt;
				
		// Calculate the target bias velocity.
		con->bias = -cp_bias_coef*dt_inv*cpfmin(0.0f, con->dist + cp_collision_slop);
		con->jBias = 0.0f;
		
		// Calculate the target bounce velocity.
		cpVect v1 = cpvadd(a->v, cpvmult(cpvperp(con->r1), a->w));
		cpVect v2 = cpvadd(b->v, cpvmult(cpvperp(con->r2), b->w));
		con->bounce = cpvdot(con->n, cpvsub(v2, v1))*arb->e;
		
		// Apply the previous accumulated impulse.
		cpVect j = cpvadd(cpvmult(con->n, con->jnAcc), cpvmult(t, con->jtAcc));
		cpBodyApplyImpulse(a, cpvneg(j), con->r1);
		cpBodyApplyImpulse(b, j, con->r2);
	}
}
Example #4
0
void
cpArbiterApplyImpulse(cpArbiter *arb, cpFloat eCoef)
{
	cpBody *a = arb->a->body;
	cpBody *b = arb->b->body;

	for(int i=0; i<arb->numContacts; i++){
		cpContact *con = &arb->contacts[i];
		cpVect n = con->n;
		cpVect r1 = con->r1;
		cpVect r2 = con->r2;
		
		// Calculate the relative bias velocities.
		cpVect vb1 = cpvadd(a->v_bias, cpvmult(cpvperp(r1), a->w_bias));
		cpVect vb2 = cpvadd(b->v_bias, cpvmult(cpvperp(r2), b->w_bias));
		cpFloat vbn = cpvdot(cpvsub(vb2, vb1), n);
		
		// Calculate and clamp the bias impulse.
		cpFloat jbn = (con->bias - vbn)*con->nMass;
		cpFloat jbnOld = con->jBias;
		con->jBias = cpfmax(jbnOld + jbn, 0.0f);
		jbn = con->jBias - jbnOld;
		
		// Apply the bias impulse.
		apply_bias_impulses(a, b, r1, r2, cpvmult(n, jbn));

		// Calculate the relative velocity.
		cpVect vr = relative_velocity(a, b, r1, r2);
		cpFloat vrn = cpvdot(vr, n);
		
		// Calculate and clamp the normal impulse.
		cpFloat jn = -(con->bounce*eCoef + vrn)*con->nMass;
		cpFloat jnOld = con->jnAcc;
		con->jnAcc = cpfmax(jnOld + jn, 0.0f);
		jn = con->jnAcc - jnOld;
		
		// Calculate the relative tangent velocity.
		cpFloat vrt = cpvdot(cpvadd(vr, arb->surface_vr), cpvperp(n));
		
		// Calculate and clamp the friction impulse.
		cpFloat jtMax = arb->u*con->jnAcc;
		cpFloat jt = -vrt*con->tMass;
		cpFloat jtOld = con->jtAcc;
		con->jtAcc = cpfclamp(jtOld + jt, -jtMax, jtMax);
		jt = con->jtAcc - jtOld;
		
		// Apply the final impulse.
		apply_impulses(a, b, r1, r2, cpvrotate(n, cpv(jn, jt)));
	}
}
static void
setUpVerts(cpPolyShape *poly, int numVerts, const cpVect *verts, cpVect offset)
{
	// Fail if the user attempts to pass a concave poly, or a bad winding.
	cpAssertHard(cpPolyValidate(verts, numVerts), "Polygon is concave or has a reversed winding. Consider using cpConvexHull() or CP_CONVEX_HULL().");
	
	poly->numVerts = numVerts;
	poly->verts = (cpVect *)cpcalloc(2*numVerts, sizeof(cpVect));
	poly->planes = (cpSplittingPlane *)cpcalloc(2*numVerts, sizeof(cpSplittingPlane));
	poly->tVerts = poly->verts + numVerts;
	poly->tPlanes = poly->planes + numVerts;
	
	for(int i=0; i<numVerts; i++){
		cpVect a = cpvadd(offset, verts[i]);
		cpVect b = cpvadd(offset, verts[(i+1)%numVerts]);
		cpVect n = cpvnormalize(cpvperp(cpvsub(b, a)));

		poly->verts[i] = a;
		poly->planes[i].n = n;
		poly->planes[i].d = cpvdot(n, a);
	}
	
	// TODO: Why did I add this? It duplicates work from above.
	for(int i=0; i<numVerts; i++){
		poly->planes[i] = cpSplittingPlaneNew(poly->verts[(i - 1 + numVerts)%numVerts], poly->verts[i]);
	}
}
static inline struct ClosestPoints
ClosestPointsNew(const struct MinkowskiPoint v0, const struct MinkowskiPoint v1)
{
	cpFloat t = ClosestT(v0.ab, v1.ab);
	cpVect p = LerpT(v0.ab, v1.ab, t);
	
	cpVect pa = LerpT(v0.a, v1.a, t);
	cpVect pb = LerpT(v0.b, v1.b, t);
	cpCollisionID id = (v0.id & 0xFFFF)<<16 | (v1.id & 0xFFFF);
	
	cpVect delta = cpvsub(v1.ab, v0.ab);
	cpVect n = cpvnormalize(cpvperp(delta));
	cpFloat d = -cpvdot(n, p);
	
	if(d <= 0.0f || (0.0f < t && t < 1.0f)){
		struct ClosestPoints points = {pa, pb, cpvneg(n), d, id};
		return points;
	} else {
		cpFloat d2 = cpvlength(p);
		cpVect n = cpvmult(p, 1.0f/(d2 + CPFLOAT_MIN));
		
		struct ClosestPoints points = {pa, pb, n, d2, id};
		return points;
	}
}
Example #7
0
void
cpArbiterPreStep(cpArbiter *arb, cpFloat dt, cpFloat slop, cpFloat bias)
{
	cpBody *a = arb->body_a;
	cpBody *b = arb->body_b;

	for(int i=0; i<arb->numContacts; i++){
		cpContact *con = &arb->contacts[i];

		// Calculate the offsets.
		con->r1 = cpvsub(con->p, a->p);
		con->r2 = cpvsub(con->p, b->p);

		// Calculate the mass normal and mass tangent.
		con->nMass = 1.0f/k_scalar(a, b, con->r1, con->r2, con->n);
		con->tMass = 1.0f/k_scalar(a, b, con->r1, con->r2, cpvperp(con->n));

		// Calculate the target bias velocity.
		con->bias = -bias*cpfmin(0.0f, con->dist + slop)/dt;
		con->jBias = 0.0f;

		// Calculate the target bounce velocity.
		con->bounce = normal_relative_velocity(a, b, con->r1, con->r2, con->n)*arb->e;
	}
}
Example #8
0
static void
add_box(cpSpace *space)
{
	const cpFloat size = 10.0f;
	const cpFloat mass = 1.0f;
	
	cpVect verts[] = {
		cpv(-size,-size),
		cpv(-size, size),
		cpv( size, size),
		cpv( size,-size),
	};
	
	cpFloat radius = cpvlength(cpv(size, size));
	cpVect pos = rand_pos(radius);
	
	cpBody *body = cpSpaceAddBody(space, cpBodyNew(mass, cpMomentForPoly(mass, 4, verts, cpvzero, 0.0f)));
	body->velocity_func = planetGravityVelocityFunc;
	cpBodySetPosition(body, pos);

	// Set the box's velocity to put it into a circular orbit from its
	// starting position.
	cpFloat r = cpvlength(pos);
	cpFloat v = cpfsqrt(gravityStrength / r) / r;
	cpBodySetVelocity(body, cpvmult(cpvperp(pos), v));

	// Set the box's angular velocity to match its orbital period and
	// align its initial angle with its position.
	cpBodySetAngularVelocity(body, v);
	cpBodySetAngle(body, cpfatan2(pos.y, pos.x));

	cpShape *shape = cpSpaceAddShape(space, cpPolyShapeNew(body, 4, verts, cpTransformIdentity, 0.0));
	cpShapeSetElasticity(shape, 0.0f);
	cpShapeSetFriction(shape, 0.7f);
}
Example #9
0
void
cpArbiterPreStep(cpArbiter *arb, cpFloat dt_inv)
{
	cpBody *a = arb->a->body;
	cpBody *b = arb->b->body;
	
	for(int i=0; i<arb->numContacts; i++){
		cpContact *con = &arb->contacts[i];
		
		// Calculate the offsets.
		con->r1 = cpvsub(con->p, a->p);
		con->r2 = cpvsub(con->p, b->p);
		
		// Calculate the mass normal and mass tangent.
		con->nMass = 1.0f/k_scalar(a, b, con->r1, con->r2, con->n);
		con->tMass = 1.0f/k_scalar(a, b, con->r1, con->r2, cpvperp(con->n));
				
		// Calculate the target bias velocity.
		con->bias = -cp_bias_coef*dt_inv*cpfmin(0.0f, con->dist + cp_collision_slop);
		con->jBias = 0.0f;
		
		// Calculate the target bounce velocity.
		con->bounce = normal_relative_velocity(a, b, con->r1, con->r2, con->n)*arb->e;//cpvdot(con->n, cpvsub(v2, v1))*e;
	}
}
Example #10
0
void update_drive()
{
	const cpFloat max_forward_speed = 150;
	const cpFloat max_backward_speed = -20;
	const cpFloat max_drive_force = 100;

	int i;
	for(i=0; i<1; i++) {
		cpFloat desired_speed = 0;

		// find desired speed
		if(controls.forward)
			desired_speed = max_forward_speed;
		else if(controls.back)
			desired_speed = max_backward_speed;

		// find speed
		cpVect forward_normal = cpvperp(cpvforangle(cpBodyGetAngle(tire[i])));
		cpFloat speed = cpvdot(forward_velocity(i), forward_normal);

		// apply force
		cpFloat force = 0;
		if(desired_speed > speed)
			force = max_drive_force;
		else if(desired_speed < speed)
			force = -max_drive_force;
		else
			return;
		cpBodyApplyImpulse(tire[i], cpvmult(forward_normal, force), cpvzero);
	}
}
Example #11
0
// Recursive implementation of the EPA loop.
// Each recursion adds a point to the convex hull until it's known that we have the closest point on the surface.
static struct ClosestPoints
EPARecurse(const struct SupportContext *ctx, const int count, const struct MinkowskiPoint *hull, const int iteration)
{
    int mini = 0;
    cpFloat minDist = INFINITY;

    // TODO: precalculate this when building the hull and save a step.
    // Find the closest segment hull[i] and hull[i + 1] to (0, 0)
    for(int j=0, i=count-1; j<count; i=j, j++) {
        cpFloat d = ClosestDist(hull[i].ab, hull[j].ab);
        if(d < minDist) {
            minDist = d;
            mini = i;
        }
    }

    struct MinkowskiPoint v0 = hull[mini];
    struct MinkowskiPoint v1 = hull[(mini + 1)%count];
    cpAssertSoft(!cpveql(v0.ab, v1.ab), "Internal Error: EPA vertexes are the same (%d and %d)", mini, (mini + 1)%count);

    // Check if there is a point on the minkowski difference beyond this edge.
    struct MinkowskiPoint p = Support(ctx, cpvperp(cpvsub(v1.ab, v0.ab)));

#if DRAW_EPA
    cpVect verts[count];
    for(int i=0; i<count; i++) verts[i] = hull[i].ab;

    ChipmunkDebugDrawPolygon(count, verts, 0.0, RGBAColor(1, 1, 0, 1), RGBAColor(1, 1, 0, 0.25));
    ChipmunkDebugDrawSegment(v0.ab, v1.ab, RGBAColor(1, 0, 0, 1));

    ChipmunkDebugDrawDot(5, p.ab, LAColor(1, 1));
#endif

    if(CheckArea(cpvsub(v1.ab, v0.ab), cpvadd(cpvsub(p.ab, v0.ab), cpvsub(p.ab, v1.ab))) && iteration < MAX_EPA_ITERATIONS) {
        // Rebuild the convex hull by inserting p.
        struct MinkowskiPoint *hull2 = (struct MinkowskiPoint *)alloca((count + 1)*sizeof(struct MinkowskiPoint));
        int count2 = 1;
        hull2[0] = p;

        for(int i=0; i<count; i++) {
            int index = (mini + 1 + i)%count;

            cpVect h0 = hull2[count2 - 1].ab;
            cpVect h1 = hull[index].ab;
            cpVect h2 = (i + 1 < count ? hull[(index + 1)%count] : p).ab;

            if(CheckArea(cpvsub(h2, h0), cpvadd(cpvsub(h1, h0), cpvsub(h1, h2)))) {
                hull2[count2] = hull[index];
                count2++;
            }
        }

        return EPARecurse(ctx, count2, hull2, iteration + 1);
    } else {
        // Could not find a new point to insert, so we have found the closest edge of the minkowski difference.
        cpAssertWarn(iteration < WARN_EPA_ITERATIONS, "High EPA iterations: %d", iteration);
        return ClosestPointsNew(v0, v1);
    }
}
Example #12
0
static struct ClosestPoints
EPARecurse(const struct SupportContext *ctx, const int count, const struct MinkowskiPoint *hull, const int iteration)
{
	int mini = 0;
	cpFloat minDist = INFINITY;
	
	// TODO: precalculate this when building the hull and save a step.
	for(int j=0, i=count-1; j<count; i=j, j++){
		cpFloat d = ClosestDist(hull[i].ab, hull[j].ab);
		if(d < minDist){
			minDist = d;
			mini = i;
		}
	}
	
	struct MinkowskiPoint v0 = hull[mini];
	struct MinkowskiPoint v1 = hull[(mini + 1)%count];
	cpAssertSoft(!cpveql(v0.ab, v1.ab), "Internal Error: EPA vertexes are the same (%d and %d)", mini, (mini + 1)%count);
	
	struct MinkowskiPoint p = Support(ctx, cpvperp(cpvsub(v1.ab, v0.ab)));
	
#if DRAW_EPA
	cpVect verts[count];
	for(int i=0; i<count; i++) verts[i] = hull[i].ab;
	
	ChipmunkDebugDrawPolygon(count, verts, 0.0, RGBAColor(1, 1, 0, 1), RGBAColor(1, 1, 0, 0.25));
	ChipmunkDebugDrawSegment(v0.ab, v1.ab, RGBAColor(1, 0, 0, 1));
	
	ChipmunkDebugDrawDot(5, p.ab, LAColor(1, 1));
#endif
	
	cpFloat area2x = cpvcross(cpvsub(v1.ab, v0.ab), cpvadd(cpvsub(p.ab, v0.ab), cpvsub(p.ab, v1.ab)));
	if(area2x > 0.0f && iteration < MAX_EPA_ITERATIONS){
		int count2 = 1;
		struct MinkowskiPoint *hull2 = (struct MinkowskiPoint *)alloca((count + 1)*sizeof(struct MinkowskiPoint));
		hull2[0] = p;
		
		for(int i=0; i<count; i++){
			int index = (mini + 1 + i)%count;
			
			cpVect h0 = hull2[count2 - 1].ab;
			cpVect h1 = hull[index].ab;
			cpVect h2 = (i + 1 < count ? hull[(index + 1)%count] : p).ab;
			
			// TODO: Should this be changed to an area2x check?
			if(cpvcross(cpvsub(h2, h0), cpvsub(h1, h0)) > 0.0f){
				hull2[count2] = hull[index];
				count2++;
			}
		}
		
		return EPARecurse(ctx, count2, hull2, iteration + 1);
	} else {
		cpAssertWarn(iteration < WARN_EPA_ITERATIONS, "High EPA iterations: %d", iteration);
		return ClosestPointsNew(v0, v1);
	}
}
Example #13
0
void
cpSegmentShapeSetEndpoints(cpShape *shape, cpVect a, cpVect b)
{
	cpAssertHard(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape.");
	cpSegmentShape *seg = (cpSegmentShape *)shape;
	
	seg->a = a;
	seg->b = b;
	seg->n = cpvperp(cpvnormalize(cpvsub(b, a)));
}
Example #14
0
void
cpGrooveJointSetGrooveB(cpConstraint *constraint, cpVect value)
{
	cpGrooveJoint *g = (cpGrooveJoint *)constraint;
	cpConstraintCheckCast(constraint, cpGrooveJoint);
	
	g->grv_b = value;
	g->grv_n = cpvperp(cpvnormalize(cpvsub(value, g->grv_a)));
	
	cpConstraintActivateBodies(constraint);
}
Example #15
0
void
cpGrooveJointSetGrooveB(cpConstraint *constraint, cpVect value)
{
	cpAssertHard(cpConstraintIsGrooveJoint(constraint), "Constraint is not a groove joint.");
	cpGrooveJoint *g = (cpGrooveJoint *)constraint;
	
	g->grv_b = value;
	g->grv_n = cpvperp(cpvnormalize(cpvsub(value, g->grv_a)));
	
	cpConstraintActivateBodies(constraint);
}
Example #16
0
void
cpArbiterApplyImpulse(cpArbiter *arb)
{
	cpBody *a = arb->body_a;
	cpBody *b = arb->body_b;
	cpVect surface_vr = arb->surface_vr;
	cpFloat friction = arb->u;

	for(int i=0; i<arb->numContacts; i++){
		cpContact *con = &arb->contacts[i];
		cpFloat nMass = con->nMass;
		cpVect n = con->n;
		cpVect r1 = con->r1;
		cpVect r2 = con->r2;

		cpVect vb1 = cpvadd(a->v_bias, cpvmult(cpvperp(r1), a->w_bias));
		cpVect vb2 = cpvadd(b->v_bias, cpvmult(cpvperp(r2), b->w_bias));
		cpVect vr = relative_velocity(a, b, r1, r2);

		cpFloat vbn = cpvdot(cpvsub(vb2, vb1), n);
		cpFloat vrn = cpvdot(vr, n);
		cpFloat vrt = cpvdot(cpvadd(vr, surface_vr), cpvperp(n));

		cpFloat jbn = (con->bias - vbn)*nMass;
		cpFloat jbnOld = con->jBias;
		con->jBias = cpfmax(jbnOld + jbn, 0.0f);

		cpFloat jn = -(con->bounce + vrn)*nMass;
		cpFloat jnOld = con->jnAcc;
		con->jnAcc = cpfmax(jnOld + jn, 0.0f);

		cpFloat jtMax = friction*con->jnAcc;
		cpFloat jt = -vrt*con->tMass;
		cpFloat jtOld = con->jtAcc;
		con->jtAcc = cpfclamp(jtOld + jt, -jtMax, jtMax);

		apply_bias_impulses(a, b, r1, r2, cpvmult(n, con->jBias - jbnOld));
		apply_impulses(a, b, r1, r2, cpvrotate(n, cpv(con->jnAcc - jnOld, con->jtAcc - jtOld)));
	}
}
Example #17
0
cpSegmentShape *
cpSegmentShapeInit(cpSegmentShape *seg, cpBody *body, cpVect a, cpVect b, cpFloat r)
{
	seg->a = a;
	seg->b = b;
	seg->n = cpvperp(cpvnormalize(cpvsub(b, a)));
	
	seg->r = r;
	
	cpShapeInit((cpShape *)seg, &cpSegmentShapeClass, body);
	
	return seg;
}
Example #18
0
// Recursive implementatino of the GJK loop.
static inline struct ClosestPoints
GJKRecurse(const struct SupportContext *ctx, const struct MinkowskiPoint v0, const struct MinkowskiPoint v1, const int iteration)
{
    if(iteration > MAX_GJK_ITERATIONS) {
        cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK iterations: %d", iteration);
        return ClosestPointsNew(v0, v1);
    }

    cpVect delta = cpvsub(v1.ab, v0.ab);
    // TODO: should this be an area2x check?
    if(cpvcross(delta, cpvadd(v0.ab, v1.ab)) > 0.0f) {
        // Origin is behind axis. Flip and try again.
        return GJKRecurse(ctx, v1, v0, iteration);
    } else {
        cpFloat t = ClosestT(v0.ab, v1.ab);
        cpVect n = (-1.0f < t && t < 1.0f ? cpvperp(delta) : cpvneg(LerpT(v0.ab, v1.ab, t)));
        struct MinkowskiPoint p = Support(ctx, n);

#if DRAW_GJK
        ChipmunkDebugDrawSegment(v0.ab, v1.ab, RGBAColor(1, 1, 1, 1));
        cpVect c = cpvlerp(v0.ab, v1.ab, 0.5);
        ChipmunkDebugDrawSegment(c, cpvadd(c, cpvmult(cpvnormalize(n), 5.0)), RGBAColor(1, 0, 0, 1));

        ChipmunkDebugDrawDot(5.0, p.ab, LAColor(1, 1));
#endif

        if(
            cpvcross(cpvsub(v1.ab, p.ab), cpvadd(v1.ab, p.ab)) > 0.0f &&
            cpvcross(cpvsub(v0.ab, p.ab), cpvadd(v0.ab, p.ab)) < 0.0f
        ) {
            // The triangle v0, p, v1 contains the origin. Use EPA to find the MSA.
            cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK->EPA iterations: %d", iteration);
            return EPA(ctx, v0, p, v1);
        } else {
            if(cpvdot(p.ab, n) <= cpfmax(cpvdot(v0.ab, n), cpvdot(v1.ab, n))) {
                // The edge v0, v1 that we already have is the closest to (0, 0) since p was not closer.
                cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK iterations: %d", iteration);
                return ClosestPointsNew(v0, v1);
            } else {
                // p was closer to the origin than our existing edge.
                // Need to figure out which existing point to drop.
                if(ClosestDist(v0.ab, p.ab) < ClosestDist(p.ab, v1.ab)) {
                    return GJKRecurse(ctx, v0, p, iteration + 1);
                } else {
                    return GJKRecurse(ctx, p, v1, iteration + 1);
                }
            }
        }
    }
}
Example #19
0
void
cpSegmentShapeSetEndpoints(cpShape *shape, cpVect a, cpVect b)
{
	cpAssertHard(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape.");
	cpSegmentShape *seg = (cpSegmentShape *)shape;
	
	seg->a = a;
	seg->b = b;
	seg->n = cpvperp(cpvnormalize(cpvsub(b, a)));

	cpFloat mass = shape->massInfo.m;
	shape->massInfo = cpSegmentShapeMassInfo(shape->massInfo.m, seg->a, seg->b, seg->r);
	if(mass > 0.0f) cpBodyAccumulateMassFromShapes(shape->body);
}
Example #20
0
cpGrooveJoint *
cpGrooveJointInit(cpGrooveJoint *joint, cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchorB)
{
	cpConstraintInit((cpConstraint *)joint, &klass, a, b);
	
	joint->grv_a = groove_a;
	joint->grv_b = groove_b;
	joint->grv_n = cpvperp(cpvnormalize(cpvsub(groove_b, groove_a)));
	joint->anchorB = anchorB;
	
	joint->jAcc = cpvzero;
	
	return joint;
}
cpVect
cpContactsSumImpulsesWithFriction(cpContact *contacts, int numContacts)
{
	cpVect sum = cpvzero;
	
	for(int i=0; i<numContacts; i++){
		cpContact *con = &contacts[i];
		cpVect t = cpvperp(con->n);
		cpVect j = cpvadd(cpvmult(con->n, con->jnAcc), cpvmult(t, con->jtAcc));
		sum = cpvadd(sum, j);
	}
		
	return sum;
}
Example #22
0
static inline struct ClosestPoints
GJKRecurse(const struct SupportContext *ctx, const struct MinkowskiPoint v0, const struct MinkowskiPoint v1, const int iteration)
{
	if(iteration > MAX_GJK_ITERATIONS){
		cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK iterations: %d", iteration);
		return ClosestPointsNew(v0, v1);
	}
	
	cpVect delta = cpvsub(v1.ab, v0.ab);
	if(cpvcross(delta, cpvadd(v0.ab, v1.ab)) > 0.0f){
		// Origin is behind axis. Flip and try again.
		return GJKRecurse(ctx, v1, v0, iteration + 1);
	} else {
		cpFloat t = ClosestT(v0.ab, v1.ab);
		cpVect n = (-1.0f < t && t < 1.0f ? cpvperp(delta) : cpvneg(LerpT(v0.ab, v1.ab, t)));
		struct MinkowskiPoint p = Support(ctx, n);
		
#if DRAW_GJK
		ChipmunkDebugDrawSegment(v0.ab, v1.ab, RGBAColor(1, 1, 1, 1));
		cpVect c = cpvlerp(v0.ab, v1.ab, 0.5);
		ChipmunkDebugDrawSegment(c, cpvadd(c, cpvmult(cpvnormalize(n), 5.0)), RGBAColor(1, 0, 0, 1));
		
		ChipmunkDebugDrawDot(5.0, p.ab, LAColor(1, 1));
#endif
		
		if(
			cpvcross(cpvsub(v1.ab, p.ab), cpvadd(v1.ab, p.ab)) > 0.0f &&
			cpvcross(cpvsub(v0.ab, p.ab), cpvadd(v0.ab, p.ab)) < 0.0f
		){
			cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK->EPA iterations: %d", iteration);
			// The triangle v0, p, v1 contains the origin. Use EPA to find the MSA.
			return EPA(ctx, v0, p, v1);
		} else {
			// The new point must be farther along the normal than the existing points.
			if(cpvdot(p.ab, n) <= cpfmax(cpvdot(v0.ab, n), cpvdot(v1.ab, n))){
				cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK iterations: %d", iteration);
				return ClosestPointsNew(v0, v1);
			} else {
				if(ClosestDist(v0.ab, p.ab) < ClosestDist(p.ab, v1.ab)){
					return GJKRecurse(ctx, v0, p, iteration + 1);
				} else {
					return GJKRecurse(ctx, p, v1, iteration + 1);
				}
			}
		}
	}
}
Example #23
0
void Slice::SliceShapePostStep(cpSpace *space, cpShape *shape, struct SliceContext *context)
{
	cpVect a = context->a;
	cpVect b = context->b;
	
	// Clipping plane normal and distance.
	cpVect n = cpvnormalize(cpvperp(cpvsub(b, a)));
	cpFloat dist = cpvdot(a, n);
	
	ClipPoly(space, shape, n, dist);
	ClipPoly(space, shape, cpvneg(n), -dist);
	
	cpBody *body = cpShapeGetBody(shape);
	cpSpaceRemoveShape(space, shape);
	cpSpaceRemoveBody(space, body);
	cpShapeFree(shape);
	cpBodyFree(body);
}
Example #24
0
static void
preStep(cpGrooveJoint *joint, cpFloat dt, cpFloat dt_inv)
{
	cpBody *a = joint->constraint.a;
	cpBody *b = joint->constraint.b;
	
	// calculate endpoints in worldspace
	cpVect ta = cpBodyLocal2World(a, joint->grv_a);
	cpVect tb = cpBodyLocal2World(a, joint->grv_b);

	// calculate axis
	cpVect n = cpvrotate(joint->grv_n, a->rot);
	cpFloat d = cpvdot(ta, n);
	
	joint->grv_tn = n;
	joint->r2 = cpvrotate(joint->anchr2, b->rot);
	
	// calculate tangential distance along the axis of r2
	cpFloat td = cpvcross(cpvadd(b->p, joint->r2), n);
	// calculate clamping factor and r2
	if(td <= cpvcross(ta, n)){
		joint->clamp = 1.0f;
		joint->r1 = cpvsub(ta, a->p);
	} else if(td >= cpvcross(tb, n)){
		joint->clamp = -1.0f;
		joint->r1 = cpvsub(tb, a->p);
	} else {
		joint->clamp = 0.0f;
		joint->r1 = cpvsub(cpvadd(cpvmult(cpvperp(n), -td), cpvmult(n, d)), a->p);
	}
	
	// Calculate mass tensor
	k_tensor(a, b, joint->r1, joint->r2, &joint->k1, &joint->k2);	
	
	// compute max impulse
	joint->jMaxLen = J_MAX(joint, dt);
	
	// calculate bias velocity
	cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1));
	joint->bias = cpvclamp(cpvmult(delta, -joint->constraint.biasCoef*dt_inv), joint->constraint.maxBias);
	
	// apply accumulated impulse
	apply_impulses(a, b, joint->r1, joint->r2, joint->jAcc);
}
Example #25
0
static void
setUpVerts(cpPolyShape *poly, int numVerts, cpVect *verts, cpVect offset)
{
	poly->numVerts = numVerts;

	poly->verts = (cpVect *)cpcalloc(numVerts, sizeof(cpVect));
	poly->tVerts = (cpVect *)cpcalloc(numVerts, sizeof(cpVect));
	poly->axes = (cpPolyShapeAxis *)cpcalloc(numVerts, sizeof(cpPolyShapeAxis));
	poly->tAxes = (cpPolyShapeAxis *)cpcalloc(numVerts, sizeof(cpPolyShapeAxis));
	
	for(int i=0; i<numVerts; i++){
		cpVect a = cpvadd(offset, verts[i]);
		cpVect b = cpvadd(offset, verts[(i+1)%numVerts]);
		cpVect n = cpvnormalize(cpvperp(cpvsub(b, a)));

		poly->verts[i] = a;
		poly->axes[i].n = n;
		poly->axes[i].d = cpvdot(n, a);
	}
}
Example #26
0
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);
	}
}
Example #27
0
// Recursive function used by cpPolylineSimplifyCurves().
static cpPolyline *
DouglasPeucker(
	cpVect *verts, cpPolyline *reduced,
	int length, int start, int end,
	cpFloat min, cpFloat tol
){
	// Early exit if the points are adjacent
  if((end - start + length)%length < 2) return reduced;
  
	cpVect a = verts[start];
	cpVect b = verts[end];
	
	// Check if the length is below the threshold
	if(cpvnear(a, b, min) && cpPolylineIsShort(verts, length, start, end, min)) return reduced;
	
	// Find the maximal vertex to split and recurse on
	cpFloat max = 0.0;
	int maxi = start;
	
	cpVect n = cpvnormalize(cpvperp(cpvsub(b, a)));
	cpFloat d = cpvdot(n, a);
	
	for(int i=Next(start, length); i!=end; i=Next(i, length)){
		cpFloat dist = fabs(cpvdot(n, verts[i]) - d);
		
		if(dist > max){
			max = dist;
			maxi = i;
		}
	}
	
	if(max > tol){
    reduced = DouglasPeucker(verts, reduced, length, start, maxi, min, tol);
		reduced = cpPolylinePush(reduced, verts[maxi]);
    reduced = DouglasPeucker(verts, reduced, length, maxi, end, min, tol);
	}
	
	return reduced;
}
Example #28
0
static void
preStep(cpGrooveJoint *joint, cpFloat dt)
{
	cpBody *a = joint->constraint.a;
	cpBody *b = joint->constraint.b;
	
	// calculate endpoints in worldspace
	cpVect ta = cpTransformPoint(a->transform, joint->grv_a);
	cpVect tb = cpTransformPoint(a->transform, joint->grv_b);

	// calculate axis
	cpVect n = cpTransformVect(a->transform, joint->grv_n);
	cpFloat d = cpvdot(ta, n);
	
	joint->grv_tn = n;
	joint->r2 = cpTransformVect(b->transform, cpvsub(joint->anchorB, b->cog));
	
	// calculate tangential distance along the axis of r2
	cpFloat td = cpvcross(cpvadd(b->p, joint->r2), n);
	// calculate clamping factor and r2
	if(td <= cpvcross(ta, n)){
		joint->clamp = 1.0f;
		joint->r1 = cpvsub(ta, a->p);
	} else if(td >= cpvcross(tb, n)){
		joint->clamp = -1.0f;
		joint->r1 = cpvsub(tb, a->p);
	} else {
		joint->clamp = 0.0f;
		joint->r1 = cpvsub(cpvadd(cpvmult(cpvperp(n), -td), cpvmult(n, d)), a->p);
	}
	
	// Calculate mass tensor
	joint->k = k_tensor(a, b, joint->r1, joint->r2);
	
	// calculate bias velocity
	cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1));
	joint->bias = cpvclamp(cpvmult(delta, -bias_coef(joint->constraint.errorBias, dt)/dt), joint->constraint.maxBias);
}
Example #29
0
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;
}
Example #30
0
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;
	}
}