// 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 penetrating vertexes.
static inline int
findVerts(cpContact **arr, cpPolyShape *poly1, cpPolyShape *poly2, cpVect n, cpFloat dist)
{
int max = 0;
int num = 0;
for(int i=0; i<poly1->numVerts; i++){
cpVect v = poly1->tVerts[i];
if(cpPolyShapeContainsVert(poly2, v))
cpContactInit(addContactPoint(arr, &max, &num), v, n, dist, CP_HASH_PAIR(poly1, i));
}
for(int i=0; i<poly2->numVerts; i++){
cpVect v = poly2->tVerts[i];
if(cpPolyShapeContainsVert(poly1, v))
cpContactInit(addContactPoint(arr, &max, &num), v, n, dist, CP_HASH_PAIR(poly2, i));
}
// if(!num)
// addContactPoint(arr, &size, &num, cpContactNew(shape1->body->p, n, dist, 0));
return num;
}
static int
cpPolyShapePointQuery(cpShape *shape, cpVect p){
return cpBBcontainsVect(shape->bb, p) && cpPolyShapeContainsVert((cpPolyShape *)shape, p);
}