Example #1
0
static void
cpPolyShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info)
{
	cpPolyShape *poly = (cpPolyShape *)shape;
	cpPolyShapeAxis *axes = poly->tAxes;
	cpVect *verts = poly->tVerts;
	int numVerts = poly->numVerts;
	
	for(int i=0; i<numVerts; i++){
		cpVect n = axes[i].n;
		cpFloat an = cpvdot(a, n);
		if(axes[i].d > an) continue;
		
		cpFloat bn = cpvdot(b, n);
		cpFloat t = (axes[i].d - an)/(bn - an);
		if(t < 0.0f || 1.0f < t) continue;
		
		cpVect point = cpvlerp(a, b, t);
		cpFloat dt = -cpvcross(n, point);
		cpFloat dtMin = -cpvcross(n, verts[i]);
		cpFloat dtMax = -cpvcross(n, verts[(i+1)%numVerts]);
		
		if(dtMin <= dt && dt <= dtMax){
			info->shape = shape;
			info->t = t;
			info->n = n;
		}
	}
}
Example #2
0
static struct Notch
DeepestNotch(int count, cpVect *verts, int hullCount, cpVect *hullVerts, int first, cpFloat tol)
{
	struct Notch notch = {};
	int j = Next(first, count);
	
	for(int i=0; i<hullCount; i++){
		cpVect a = hullVerts[i];
		cpVect b = hullVerts[Next(i, hullCount)];
		
		// TODO use a cross check instead?
		cpVect n = cpvnormalize(cpvrperp(cpvsub(a, b)));
		cpFloat d = cpvdot(n, a);
		
		cpVect v = verts[j];
		while(!cpveql(v, b)){
			cpFloat depth = cpvdot(n, v) - d;
			
			if(depth > notch.d){
				notch.d = depth;
				notch.i = j;
				notch.v = v;
				notch.n = n;
			}
			
			j = Next(j, count);
			v = verts[j];
		}
		
		j = Next(j, count);
	}
	
	return notch;
}
Example #3
0
static cpBool
cpSegmentShapePointQuery(cpSegmentShape *seg, cpVect p){
	if(!cpBBContainsVect(seg->shape.bb, p)) return cpFalse;
	
	// Calculate normal distance from segment.
	cpFloat dn = cpvdot(seg->tn, p) - cpvdot(seg->ta, seg->tn);
	cpFloat dist = cpfabs(dn) - seg->r;
	if(dist > 0.0f) return cpFalse;
	
	// Calculate tangential distance along segment.
	cpFloat dt = -cpvcross(seg->tn, p);
	cpFloat dtMin = -cpvcross(seg->tn, seg->ta);
	cpFloat dtMax = -cpvcross(seg->tn, seg->tb);
	
	// Decision tree to decide which feature of the segment to collide with.
	if(dt <= dtMin){
		if(dt < (dtMin - seg->r)){
			return cpFalse;
		} else {
			return cpvlengthsq(cpvsub(seg->ta, p)) < (seg->r*seg->r);
		}
	} else {
		if(dt < dtMax){
			return cpTrue;
		} else {
			if(dt < (dtMax + seg->r)) {
				return cpvlengthsq(cpvsub(seg->tb, p)) < (seg->r*seg->r);
			} else {
				return cpFalse;
			}
		}
	}
	
	return cpTrue;	
}
Example #4
0
static int
circle2segment(const cpCircleShape *circleShape, const cpSegmentShape *segmentShape, cpContact *con)
{
	cpVect seg_a = segmentShape->ta;
	cpVect seg_b = segmentShape->tb;
	cpVect center = circleShape->tc;
	
	cpVect seg_delta = cpvsub(seg_b, seg_a);
	cpFloat closest_t = cpfclamp01(cpvdot(seg_delta, cpvsub(center, seg_a))/cpvlengthsq(seg_delta));
	cpVect closest = cpvadd(seg_a, cpvmult(seg_delta, closest_t));
	
	if(circle2circleQuery(center, closest, circleShape->r, segmentShape->r, con)){
		cpVect n = con[0].n;
		
		// Reject endcap collisions if tangents are provided.
		if(
			(closest_t == 0.0f && cpvdot(n, segmentShape->a_tangent) < 0.0) ||
			(closest_t == 1.0f && cpvdot(n, segmentShape->b_tangent) < 0.0)
		) return 0;
		
		return 1;
	} else {
		return 0;
	}
}
Example #5
0
// Like cpPolyValueOnAxis(), but for segments.
static inline cpFloat
segValueOnAxis(cpSegmentShape *seg, cpVect n, cpFloat d)
{
	cpFloat a = cpvdot(n, seg->ta) - seg->r;
	cpFloat b = cpvdot(n, seg->tb) - seg->r;
	return cpfmin(a, b) - d;
}
Example #6
0
static void
applyImpulse(cpPulleyJoint *joint)
{
	cpBody* b1 = joint->constraint.a;
	cpBody* b2 = joint->constraint.b;
	cpVect r1 = joint->r1;
	cpVect r2 = joint->r2;

	// The magic and mystery below
	if (joint->state)
	{
		cpVect v1 = cpvadd(b1->v, cpv(-b1->w * r1.y, b1->w * r1.x));
		cpVect v2 = cpvadd(b2->v, cpv(-b2->w * r2.y, b2->w * r2.x));

		cpFloat Cdot = -cpvdot(joint->u1, v1) - joint->ratio * cpvdot(joint->u2, v2);
		cpFloat impulse = joint->pulleyMass * (-Cdot);
		cpFloat oldImpulse = joint->jnAcc;
		joint->jnAcc = cpfmax(0.0f, joint->jnAcc + impulse);
		impulse = joint->jnAcc - oldImpulse;

		cpVect P1 = cpvmult(joint->u1, -impulse);
		cpVect P2 = cpvmult(joint->u2, -joint->ratio * impulse);
		
		cpBodyApplyImpulse(b1, P1, r1);
		cpBodyApplyImpulse(b2, P2, r2);
	}

	if (joint->limitState1)
	{
		cpVect v1 = cpvadd(b1->v, cpv(-b1->w * r1.y, b1->w * r1.x));

		cpFloat Cdot = -cpvdot(joint->u1, v1);
		cpFloat impulse = -joint->limitMass1 * Cdot;
		cpFloat oldImpulse = joint->jnAccLim1;
		joint->jnAccLim1 = cpfmax(0.0f, joint->jnAccLim1 + impulse);
		impulse = joint->jnAccLim1 - oldImpulse;

		cpVect P1 = cpvmult(joint->u1, -impulse);

		cpBodyApplyImpulse(b1, P1, r1);
	}

	if (joint->limitState2)
	{	
		cpVect v2 = cpvadd(b2->v, cpv(-b2->w * r2.y, b2->w * r2.x));

		cpFloat Cdot = -cpvdot(joint->u2, v2);
		cpFloat impulse = -joint->limitMass2 * Cdot;
		cpFloat oldImpulse = joint->jnAccLim2;
		joint->jnAccLim2 = cpfmax(0.0f, joint->jnAccLim2 + impulse);
		impulse = joint->jnAccLim2 - oldImpulse;

		cpVect P2 = cpvmult(joint->u2, -impulse);

		cpBodyApplyImpulse(b2, P2, 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);
	}
}
Example #8
0
static inline struct SupportPoint
SegmentSupportPoint(const cpSegmentShape *seg, const cpVect n)
{
    if(cpvdot(seg->ta, n) > cpvdot(seg->tb, n)) {
        return SupportPointNew(seg->ta, 0);
    } else {
        return SupportPointNew(seg->tb, 1);
    }
}
Example #9
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 #10
0
static void
cpSegmentShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info)
{
    cpSegmentShape *seg = (cpSegmentShape *)shape;
    cpVect n = seg->tn;
    // flip n if a is behind the axis
    if(cpvdot(a, n) < cpvdot(seg->ta, n))
        n = cpvneg(n);

    cpFloat an = cpvdot(a, n);
    cpFloat bn = cpvdot(b, n);
    cpFloat d = cpvdot(seg->ta, n) + seg->r;

    cpFloat t = (d - an)/(bn - an);
    if(0.0f < t && t < 1.0f) {
        cpVect point = cpvlerp(a, b, t);
        cpFloat dt = -cpvcross(seg->tn, point);
        cpFloat dtMin = -cpvcross(seg->tn, seg->ta);
        cpFloat dtMax = -cpvcross(seg->tn, seg->tb);

        if(dtMin < dt && dt < dtMax) {
            info->shape = shape;
            info->t = t;
            info->n = n;

            return; // don't continue on and check endcaps
        }
    }

    if(seg->r) {
        cpSegmentQueryInfo info1;
        info1.shape = NULL;
        cpSegmentQueryInfo info2;
        info2.shape = NULL;
        circleSegmentQuery(shape, seg->ta, seg->r, a, b, &info1);
        circleSegmentQuery(shape, seg->tb, seg->r, a, b, &info2);

        if(info1.shape && !info2.shape) {
            (*info) = info1;
        } else if(info2.shape && !info1.shape) {
            (*info) = info2;
        } else if(info1.shape && info2.shape) {
            if(info1.t < info2.t) {
                (*info) = info1;
            } else {
                (*info) = info2;
            }
        }
    }
}
Example #11
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)));
	}
}
Example #12
0
// Collide circles to segment shapes.
static int
circle2segment(cpShape *circleShape, cpShape *segmentShape, cpContact **con)
{
	cpCircleShape *circ = (cpCircleShape *)circleShape;
	cpSegmentShape *seg = (cpSegmentShape *)segmentShape;
	
	// Radius sum
	cpFloat rsum = circ->r + seg->r;
	
	// Calculate normal distance from segment.
	cpFloat dn = cpvdot(seg->tn, circ->tc) - cpvdot(seg->ta, seg->tn);
	cpFloat dist = cpfabs(dn) - rsum;
	if(dist > 0.0f) return 0;
	
	// Calculate tangential distance along segment.
	cpFloat dt = -cpvcross(seg->tn, circ->tc);
	cpFloat dtMin = -cpvcross(seg->tn, seg->ta);
	cpFloat dtMax = -cpvcross(seg->tn, seg->tb);
	
	// Decision tree to decide which feature of the segment to collide with.
	if(dt < dtMin){
		if(dt < (dtMin - rsum)){
			return 0;
		} else {
			return circle2circleQuery(circ->tc, seg->ta, circ->r, seg->r, con);
		}
	} else {
		if(dt < dtMax){
			cpVect n = (dn < 0.0f) ? seg->tn : cpvneg(seg->tn);
			(*con) = (cpContact *)cpmalloc(sizeof(cpContact));
			cpContactInit(
				(*con),
				cpvadd(circ->tc, cpvmult(n, circ->r + dist*0.5f)),
				n,
				dist,
				0				 
			);
			return 1;
		} else {
			if(dt < (dtMax + rsum)) {
				return circle2circleQuery(circ->tc, seg->tb, circ->r, seg->r, con);
			} else {
				return 0;
			}
		}
	}
	
	return 1;
}
Example #13
0
static int
segmentEncapQuery(cpVect p1, cpVect p2, cpFloat r1, cpFloat r2, cpContact *con, cpVect tangent)
{
	int count = circle2circleQuery(p1, p2, r1, r2, con);
//	printf("dot %5.2f\n", cpvdot(con[0].n, tangent));
	return (cpvdot(con[0].n, tangent) >= 0.0 ? count : 0);
}
Example #14
0
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 #15
0
static cpFloat
FindSteiner(int count, cpVect *verts, struct Notch notch)
{
	cpFloat min = INFINITY;
	cpFloat feature = -1.0;
	
	for(int i=1; i<count-1; i++){
		int index = (notch.i + i)%count;
		
		cpVect seg_a = verts[index];
		cpVect seg_b = verts[Next(index, count)];
		
		cpFloat thing_a = cpvcross(notch.n, cpvsub(seg_a, notch.v));
		cpFloat thing_b = cpvcross(notch.n, cpvsub(seg_b, notch.v));
		if(thing_a*thing_b <= 0.0){
			cpFloat t = thing_a/(thing_a - thing_b);
			cpFloat dist = cpvdot(notch.n, cpvsub(cpvlerp(seg_a, seg_b, t), notch.v));
			
			if(dist >= 0.0 && dist <= min){
				min = dist;
				feature = index + t;
			}
		}
	}
	
	return feature;
}
Example #16
0
// This one is complicated and gross. Just don't go there...
// TODO: Comment me!
static int
seg2poly(const cpShape *shape1, const cpShape *shape2, cpContact *arr)
{
	cpSegmentShape *seg = (cpSegmentShape *)shape1;
	cpPolyShape *poly = (cpPolyShape *)shape2;
	cpSplittingPlane *planes = poly->tPlanes;
	
	cpFloat segD = cpvdot(seg->tn, seg->ta);
	cpFloat minNorm = cpPolyShapeValueOnAxis(poly, seg->tn, segD) - seg->r;
	cpFloat minNeg = cpPolyShapeValueOnAxis(poly, cpvneg(seg->tn), -segD) - seg->r;
	if(minNeg > 0.0f || minNorm > 0.0f) return 0;
	
	int mini = 0;
	cpFloat poly_min = segValueOnAxis(seg, planes->n, planes->d);
	if(poly_min > 0.0f) return 0;
	for(int i=0; i<poly->numVerts; i++){
		cpFloat dist = segValueOnAxis(seg, planes[i].n, planes[i].d);
		if(dist > 0.0f){
			return 0;
		} else if(dist > poly_min){
			poly_min = dist;
			mini = i;
		}
	}
	
	int num = 0;
	
	cpVect poly_n = cpvneg(planes[mini].n);
	
	cpVect va = cpvadd(seg->ta, cpvmult(poly_n, seg->r));
	cpVect vb = cpvadd(seg->tb, cpvmult(poly_n, seg->r));
	if(cpPolyShapeContainsVert(poly, va))
		cpContactInit(nextContactPoint(arr, &num), va, poly_n, poly_min, CP_HASH_PAIR(seg->shape.hashid, 0));
	if(cpPolyShapeContainsVert(poly, vb))
		cpContactInit(nextContactPoint(arr, &num), vb, poly_n, poly_min, CP_HASH_PAIR(seg->shape.hashid, 1));
	
	// Floating point precision problems here.
	// This will have to do for now.
//	poly_min -= cp_collision_slop; // TODO is this needed anymore?
	
	if(minNorm >= poly_min || minNeg >= poly_min) {
		if(minNorm > minNeg)
			findPointsBehindSeg(arr, &num, seg, poly, minNorm, 1.0f);
		else
			findPointsBehindSeg(arr, &num, seg, poly, minNeg, -1.0f);
	}
	
	// If no other collision points are found, try colliding endpoints.
	if(num == 0){
		cpVect poly_a = poly->tVerts[mini];
		cpVect poly_b = poly->tVerts[(mini + 1)%poly->numVerts];
		
		if(circle2circleQuery(seg->ta, poly_a, seg->r, 0.0f, arr)) return 1;
		if(circle2circleQuery(seg->tb, poly_a, seg->r, 0.0f, arr)) return 1;
		if(circle2circleQuery(seg->ta, poly_b, seg->r, 0.0f, arr)) return 1;
		if(circle2circleQuery(seg->tb, poly_b, seg->r, 0.0f, arr)) return 1;
	}

	return num;
}
Example #17
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 #18
0
static void
applyImpulse(cpSlideJoint *joint)
{
	if(cpveql(joint->n, cpvzero)) return;  // early exit

	cpBody *a = joint->constraint.a;
	cpBody *b = joint->constraint.b;
	
	cpVect n = joint->n;
	cpVect r1 = joint->r1;
	cpVect r2 = joint->r2;
		
	// compute relative velocity
	cpVect vr = relative_velocity(a, b, r1, r2);
	cpFloat vrn = cpvdot(vr, n);
	
	// compute normal impulse
	cpFloat jn = (joint->bias - vrn)*joint->nMass;
	cpFloat jnOld = joint->jnAcc;
	joint->jnAcc = cpfclamp(jnOld + jn, -joint->jnMax, 0.0f);
	jn = joint->jnAcc - jnOld;
	
	// apply impulse
	apply_impulses(a, b, joint->r1, joint->r2, cpvmult(n, jn));
}
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]);
	}
}
Example #20
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);
}
Example #21
0
cpContactPointSet
cpShapesCollide(const cpShape *a, const cpShape *b)
{
	struct cpContact contacts[CP_MAX_CONTACTS_PER_ARBITER];
	struct cpCollisionInfo info = cpCollide(a, b, 0, contacts);
	
	cpContactPointSet set;
	set.count = info.count;
	
	// cpCollideShapes() may have swapped the contact order. Flip the normal.
	cpBool swapped = (a != info.a);
	set.normal = (swapped ? cpvneg(info.n) : info.n);
	
	for(int i=0; i<info.count; i++){
		// cpCollideShapesInfo() returns contacts with absolute positions.
		cpVect p1 = contacts[i].r1;
		cpVect p2 = contacts[i].r2;
		
		set.points[i].point1 = (swapped ? p2 : p1);
		set.points[i].point2 = (swapped ? p1 : p2);
		set.points[i].distance = cpvdot(cpvsub(p2, p1), set.normal);
	}
	
	return set;
}
Example #22
0
// Calculate the closest points on two shapes given the closest edge on their minkowski difference to (0, 0)
static inline struct ClosestPoints
ClosestPointsNew(const struct MinkowskiPoint v0, const struct MinkowskiPoint v1)
{
    // Find the closest p(t) on the minkowski difference to (0, 0)
    cpFloat t = ClosestT(v0.ab, v1.ab);
    cpVect p = LerpT(v0.ab, v1.ab, t);

    // Interpolate the original support points using the same 't' value as above.
    // This gives you the closest surface points in absolute coordinates. NEAT!
    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);

    // First try calculating the MSA from the minkowski difference edge.
    // This gives us a nice, accurate MSA when the surfaces are close together.
    cpVect delta = cpvsub(v1.ab, v0.ab);
    cpVect n = cpvnormalize(cpvrperp(delta));
    cpFloat d = cpvdot(n, p);

    if(d <= 0.0f || (-1.0f < t && t < 1.0f)) {
        // If the shapes are overlapping, or we have a regular vertex/edge collision, we are done.
        struct ClosestPoints points = {pa, pb, n, d, id};
        return points;
    } else {
        // Vertex/vertex collisions need special treatment since the MSA won't be shared with an axis of the minkowski difference.
        cpFloat d2 = cpvlength(p);
        cpVect n2 = cpvmult(p, 1.0f/(d2 + CPFLOAT_MIN));

        struct ClosestPoints points = {pa, pb, n2, d2, id};
        return points;
    }
}
Example #23
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 #24
0
void
cpArbiterUpdate(cpArbiter *arb, struct cpCollisionInfo *info, cpSpace *space)
{
	const cpShape *a = info->a, *b = info->b;
	
	// For collisions between two similar primitive types, the order could have been swapped since the last frame.
	arb->a = a; arb->body_a = a->body;
	arb->b = b; arb->body_b = b->body;
	
	// Iterate over the possible pairs to look for hash value matches.
	for(int i=0; i<info->count; i++){
		struct cpContact *con = &info->arr[i];
		
		// r1 and r2 store absolute offsets at init time.
		// Need to convert them to relative offsets.
		con->r1 = cpvsub(con->r1, a->body->p);
		con->r2 = cpvsub(con->r2, b->body->p);
		
		// Cached impulses are not zeroed at init time.
		con->jnAcc = con->jtAcc = 0.0f;
		
		for(int j=0; j<arb->count; j++){
			struct cpContact *old = &arb->contacts[j];
			
			// This could trigger false positives, but is fairly unlikely nor serious if it does.
			if(con->hash == old->hash){
				// Copy the persistant contact information.
				con->jnAcc = old->jnAcc;
				con->jtAcc = old->jtAcc;
			}
		}
	}
	
	arb->contacts = info->arr;
	arb->count = info->count;
	arb->n = info->n;
	
	arb->e = a->e * b->e;
	arb->u = a->u * b->u;
	
	cpVect surface_vr = cpvsub(b->surfaceV, a->surfaceV);
	arb->surface_vr = cpvsub(surface_vr, cpvmult(info->n, cpvdot(surface_vr, info->n)));
	
	cpCollisionType typeA = info->a->type, typeB = info->b->type;
	cpCollisionHandler *defaultHandler = &space->defaultHandler;
	cpCollisionHandler *handler = arb->handler = cpSpaceLookupHandler(space, typeA, typeB, defaultHandler);
	
	// Check if the types match, but don't swap for a default handler which use the wildcard for type A.
	cpBool swapped = arb->swapped = (typeA != handler->typeA && handler->typeA != CP_WILDCARD_COLLISION_TYPE);
	
	if(handler != defaultHandler || space->usesWildcards){
		// The order of the main handler swaps the wildcard handlers too. Uffda.
		arb->handlerA = cpSpaceLookupHandler(space, (swapped ? typeB : typeA), CP_WILDCARD_COLLISION_TYPE, &cpCollisionHandlerDoNothing);
		arb->handlerB = cpSpaceLookupHandler(space, (swapped ? typeA : typeB), CP_WILDCARD_COLLISION_TYPE, &cpCollisionHandlerDoNothing);
	}
		
	// mark it as new if it's been cached
	if(arb->state == CP_ARBITER_STATE_CACHED) arb->state = CP_ARBITER_STATE_FIRST_COLLISION;
}
Example #25
0
cpVect
cpvslerpconst(const cpVect v1, const cpVect v2, const cpFloat a)
{
    cpFloat dot = cpvdot(cpvnormalize(v1), cpvnormalize(v2));
    cpFloat omega = cpfacos(cpfclamp(dot, -1.0f, 1.0f));

    return cpvslerp(v1, v2, cpfmin(a, omega)/omega);
}
Example #26
0
cpFloat
cpArbiterGetDepth(const cpArbiter *arb, int i)
{
	cpAssertHard(0 <= i && i < cpArbiterGetCount(arb), "Index error: The specified contact index is invalid for this arbiter");
	
	struct cpContact *con = &arb->contacts[i];
	return cpvdot(cpvadd(cpvsub(con->r2, con->r1), cpvsub(arb->body_b->p, arb->body_a->p)), arb->n);
}
// Submitted by LegoCyclon
static int
seg2seg(const cpShape* shape1, const cpShape* shape2, cpContact* con)
{
	cpSegmentShape* seg1 = (cpSegmentShape *)shape1;
	cpSegmentShape* seg2 = (cpSegmentShape *)shape2;
	
	cpVect v1 = cpvsub(seg1->tb, seg1->ta);
	cpVect v2 = cpvsub(seg2->tb, seg2->ta);
	cpFloat v1lsq = cpvlengthsq(v1);
	cpFloat v2lsq = cpvlengthsq(v2);
	// project seg2 onto seg1
	cpVect p1a = cpvproject(cpvsub(seg2->ta, seg1->ta), v1);
	cpVect p1b = cpvproject(cpvsub(seg2->tb, seg1->ta), v1);
	// project seg1 onto seg2
	cpVect p2a = cpvproject(cpvsub(seg1->ta, seg2->ta), v2);
	cpVect p2b = cpvproject(cpvsub(seg1->tb, seg2->ta), v2);
	
	// clamp projections to segment endcaps
	if (cpvdot(p1a, v1) < 0.0f)
	 p1a = cpvzero;
	else if (cpvdot(p1a, v1) > 0.0f && cpvlengthsq(p1a) > v1lsq)
	 p1a = v1;
	if (cpvdot(p1b, v1) < 0.0f)
	 p1b = cpvzero;
	else if (cpvdot(p1b, v1) > 0.0f && cpvlengthsq(p1b) > v1lsq)
	 p1b = v1;
	if (cpvdot(p2a, v2) < 0.0f)
	 p2a = cpvzero;
	else if (cpvdot(p2a, v2) > 0.0f && cpvlengthsq(p2a) > v2lsq)
	 p2a = v2;
	if (cpvdot(p2b, v2) < 0.0f)
	 p2b = cpvzero;
	else if (cpvdot(p2b, v2) > 0.0f && cpvlengthsq(p2b) > v2lsq)
	 p2b = v2;
	
	p1a = cpvadd(p1a, seg1->ta);
	p1b = cpvadd(p1b, seg1->ta);
	p2a = cpvadd(p2a, seg2->ta);
	p2b = cpvadd(p2b, seg2->ta);
	
	int num = 0;
	
	if (!circle2circleQuery(p1a, p2a, seg1->r, seg2->r, nextContactPoint(con, &num)))
	 --num;
	
	if (!circle2circleQuery(p1b, p2b, seg1->r, seg2->r, nextContactPoint(con, &num)))
	 --num;
	
	if (!circle2circleQuery(p1a, p2b, seg1->r, seg2->r, nextContactPoint(con, &num)))
	 --num;
	
	if (!circle2circleQuery(p1b, p2a, seg1->r, seg2->r, nextContactPoint(con, &num)))
	 --num;
	
	return num;
}
Example #28
0
// Identify vertexes that have penetrated the segment.
static inline void
findPointsBehindSeg(cpContact **arr, int *max, int *num, cpSegmentShape *seg, cpPolyShape *poly, cpFloat pDist, cpFloat coef) 
{
	cpFloat dta = cpvcross(seg->tn, seg->ta);
	cpFloat dtb = cpvcross(seg->tn, seg->tb);
	cpVect n = cpvmult(seg->tn, coef);
	
	for(int i=0; i<poly->numVerts; i++){
		cpVect v = poly->tVerts[i];
		if(cpvdot(v, n) < cpvdot(seg->tn, seg->ta)*coef + seg->r){
			cpFloat dt = cpvcross(seg->tn, v);
			if(dta >= dt && dt >= dtb){
				cpContactInit(addContactPoint(arr, max, num), v, n, pDist, CP_HASH_PAIR(poly->shape.hashid, i));
			}
		}
	}
}
Example #29
0
cpFloat
cpMomentForPoly(cpFloat m, const int numVerts, const cpVect *verts, cpVect offset)
{
	cpFloat sum1 = 0.0f;
	cpFloat sum2 = 0.0f;
	for(int i=0; i<numVerts; i++){
		cpVect v1 = cpvadd(verts[i], offset);
		cpVect v2 = cpvadd(verts[(i+1)%numVerts], offset);
		
		cpFloat a = cpvcross(v2, v1);
		cpFloat b = cpvdot(v1, v1) + cpvdot(v1, v2) + cpvdot(v2, v2);
		
		sum1 += a*b;
		sum2 += a;
	}
	
	return (m*sum1)/(6.0f*sum2);
}
Example #30
0
// This one is less gross, but still gross.
// TODO: Comment me!
static int
circle2poly(cpShape *shape1, cpShape *shape2, cpContact **con)
{
	cpCircleShape *circ = (cpCircleShape *)shape1;
	cpPolyShape *poly = (cpPolyShape *)shape2;
	cpPolyShapeAxis *axes = poly->tAxes;
	
	int mini = 0;
	cpFloat min = cpvdot(axes->n, circ->tc) - axes->d - circ->r;
	for(int i=0; i<poly->numVerts; i++){
		cpFloat dist = cpvdot(axes[i].n, circ->tc) - axes[i].d - circ->r;
		if(dist > 0.0f){
			return 0;
		} else if(dist > min) {
			min = dist;
			mini = i;
		}
	}
	
	cpVect n = axes[mini].n;
	cpVect a = poly->tVerts[mini];
	cpVect b = poly->tVerts[(mini + 1)%poly->numVerts];
	cpFloat dta = cpvcross(n, a);
	cpFloat dtb = cpvcross(n, b);
	cpFloat dt = cpvcross(n, circ->tc);
		
	if(dt < dtb){
		return circle2circleQuery(circ->tc, b, circ->r, 0.0f, con);
	} else if(dt < dta) {
		(*con) = (cpContact *)cpmalloc(sizeof(cpContact));
		cpContactInit(
			(*con),
			cpvsub(circ->tc, cpvmult(n, circ->r + min/2.0f)),
			cpvneg(n),
			min,
			0				 
		);
	
		return 1;
	} else {
		return circle2circleQuery(circ->tc, a, circ->r, 0.0f, con);
	}
}