// 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;
}
// 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;
}
// 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 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;
}
}
// Collide circle shapes.
static int
circle2circle(cpShape *shape1, cpShape *shape2, cpContact **arr)
{
cpCircleShape *circ1 = (cpCircleShape *)shape1;
cpCircleShape *circ2 = (cpCircleShape *)shape2;
return circle2circleQuery(circ1->tc, circ2->tc, circ1->r, circ2->r, arr);
}
// 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);
}
}
