// 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; }
// This one is complicated and gross. Just don't go there... // TODO: Comment me! static int seg2poly(cpShape *shape1, cpShape *shape2, cpContact **arr) { cpSegmentShape *seg = (cpSegmentShape *)shape1; cpPolyShape *poly = (cpPolyShape *)shape2; cpPolyShapeAxis *axes = poly->tAxes; 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, axes->n, axes->d); if(poly_min > 0.0f) return 0; for(int i=0; i<poly->numVerts; i++){ cpFloat dist = segValueOnAxis(seg, axes[i].n, axes[i].d); if(dist > 0.0f){ return 0; } else if(dist > poly_min){ poly_min = dist; mini = i; } } int max = 0; int num = 0; cpVect poly_n = cpvneg(axes[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(addContactPoint(arr, &max, &num), va, poly_n, poly_min, CP_HASH_PAIR(seg, 0)); if(cpPolyShapeContainsVert(poly, vb)) cpContactInit(addContactPoint(arr, &max, &num), vb, poly_n, poly_min, CP_HASH_PAIR(seg, 1)); // Floating point precision problems here. // This will have to do for now. poly_min -= cp_collision_slop; if(minNorm >= poly_min || minNeg >= poly_min) { if(minNorm > minNeg) findPointsBehindSeg(arr, &max, &num, seg, poly, minNorm, 1.0f); else findPointsBehindSeg(arr, &max, &num, seg, poly, minNeg, -1.0f); } return num; }
// Add contacts for penetrating vertexes. static inline int findVerts(cpContact *arr, const cpPolyShape *poly1, const cpPolyShape *poly2, const cpVect n, const cpFloat dist) { int num = 0; for(int i=0; i<poly1->numVerts; i++){ cpVect v = poly1->tVerts[i]; if(cpPolyShapeContainsVert(poly2, v)) cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly1->shape.hashid, i)); } for(int i=0; i<poly2->numVerts; i++){ cpVect v = poly2->tVerts[i]; if(cpPolyShapeContainsVert(poly1, v)) cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly2->shape.hashid, i)); } return (num ? num : findVertsFallback(arr, poly1, poly2, n, dist)); }
// Add contacts for probably penetrating vertexes. // This handles the degenerate case where an overlap was detected, but no vertexes fall inside // the opposing polygon. (like a star of david) static /*inline*/ int findVertsFallback(cpContact *arr, const cpPolyShape *poly1, const cpPolyShape *poly2, const cpVect n, const cpFloat dist) { int num = 0; for(int i=0; i<poly1->numVerts; i++){ cpVect v = poly1->tVerts[i]; if(cpPolyShapeContainsVertPartial(poly2, v, cpvneg(n))) cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly1->shape.hashid, i)); } for(int i=0; i<poly2->numVerts; i++){ cpVect v = poly2->tVerts[i]; if(cpPolyShapeContainsVertPartial(poly1, v, n)) cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly2->shape.hashid, i)); } return num; }
// Add contacts for penetrating vertexes. static inline int findVerts(cpContact *arr, cpPolyShape *poly1, cpPolyShape *poly2, cpVect n, cpFloat dist) { int num = 0; for(int i=0; i<poly1->numVerts; i++){ cpVect v = poly1->tVerts[i]; if(cpPolyShapeContainsVertPartial(poly2, v, cpvneg(n))) cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly1->shape.hashid, i)); } for(int i=0; i<poly2->numVerts; i++){ cpVect v = poly2->tVerts[i]; if(cpPolyShapeContainsVertPartial(poly1, v, n)) cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly2->shape.hashid, i)); } // if(!num) // addContactPoint(arr, &size, &num, cpContactNew(shape1->body->p, n, dist, 0)); return num; }
// 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; }
// 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)); } } } }
// 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); } }
// This one is less gross, but still gross. // TODO: Comment me! static int circle2poly(const cpShape *shape1, const cpShape *shape2, cpContact *con) { cpCircleShape *circ = (cpCircleShape *)shape1; cpPolyShape *poly = (cpPolyShape *)shape2; cpSplittingPlane *planes = poly->tPlanes; int mini = 0; cpFloat min = cpSplittingPlaneCompare(planes[0], circ->tc) - circ->r; for(int i=0; i<poly->numVerts; i++){ cpFloat dist = cpSplittingPlaneCompare(planes[i], circ->tc) - circ->r; if(dist > 0.0f){ return 0; } else if(dist > min) { min = dist; mini = i; } } cpVect n = planes[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) { 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); } }
// 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; }
// 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; }