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; } } }
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; }
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; }
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; } }
// 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; }
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); } }
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); } }
// 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); } } } } }
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; } } } }
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))); } }
// 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; }
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); }
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; } }
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; }
// 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; }
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); } }
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]); } }
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); }
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; }
// 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; } }
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); } } } } }
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; }
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); }
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; }
// 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)); } } } }
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); }
// 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); } }