static void
cpSegmentShapeSegmentQuery(cpSegmentShape *seg, cpVect a, cpVect b, cpFloat r2, cpSegmentQueryInfo *info)
{
cpVect n = seg->tn;
cpFloat d = cpvdot(cpvsub(seg->ta, a), n);
cpFloat r = seg->r + r2;
cpVect flipped_n = (d > 0.0f ? cpvneg(n) : n);
cpVect seg_offset = cpvsub(cpvmult(flipped_n, r), a);
// Make the endpoints relative to 'a' and move them by the thickness of the segment.
cpVect seg_a = cpvadd(seg->ta, seg_offset);
cpVect seg_b = cpvadd(seg->tb, seg_offset);
cpVect delta = cpvsub(b, a);
if(cpvcross(delta, seg_a)*cpvcross(delta, seg_b) <= 0.0f){
cpFloat d_offset = d + (d > 0.0f ? -r : r);
cpFloat ad = -d_offset;
cpFloat bd = cpvdot(delta, n) - d_offset;
if(ad*bd < 0.0f){
cpFloat t = ad/(ad - bd);
info->shape = (cpShape *)seg;
info->point = cpvsub(cpvlerp(a, b, t), cpvmult(flipped_n, r2));
info->normal = flipped_n;
info->alpha = t;
}
} else if(r != 0.0f){
cpSegmentQueryInfo info1 = {NULL, b, cpvzero, 1.0f};
cpSegmentQueryInfo info2 = {NULL, b, cpvzero, 1.0f};
CircleSegmentQuery((cpShape *)seg, seg->ta, seg->r, a, b, r2, &info1);
CircleSegmentQuery((cpShape *)seg, seg->tb, seg->r, a, b, r2, &info2);
if(info1.alpha < info2.alpha){
(*info) = info1;
} else {
(*info) = info2;
}
}
}
static void
cpPolyShapeSegmentQuery(cpPolyShape *poly, cpVect a, cpVect b, cpFloat r2, cpSegmentQueryInfo *info)
{
struct cpSplittingPlane *planes = poly->planes;
int count = poly->count;
cpFloat r = poly->r;
cpFloat rsum = r + r2;
for(int i=0; i<count; i++){
cpVect n = planes[i].n;
cpFloat an = cpvdot(a, n);
cpFloat d = an - cpvdot(planes[i].v0, n) - rsum;
if(d < 0.0f) continue;
cpFloat bn = cpvdot(b, n);
cpFloat t = d/(an - bn);
if(t < 0.0f || 1.0f < t) continue;
cpVect point = cpvlerp(a, b, t);
cpFloat dt = cpvcross(n, point);
cpFloat dtMin = cpvcross(n, planes[(i - 1 + count)%count].v0);
cpFloat dtMax = cpvcross(n, planes[i].v0);
if(dtMin <= dt && dt <= dtMax){
info->shape = (cpShape *)poly;
info->point = cpvsub(cpvlerp(a, b, t), cpvmult(n, r2));
info->normal = n;
info->alpha = t;
}
}
// Also check against the beveled vertexes.
if(rsum > 0.0f){
for(int i=0; i<count; i++){
cpSegmentQueryInfo circle_info = {NULL, b, cpvzero, 1.0f};
CircleSegmentQuery(&poly->shape, planes[i].v0, r, a, b, r2, &circle_info);
if(circle_info.alpha < info->alpha) (*info) = circle_info;
}
}
}
static void
cpPolyShapeSegmentQuery(cpPolyShape *poly, cpVect a, cpVect b, cpSegmentQueryInfo *info)
{
cpSplittingPlane *axes = poly->tPlanes;
cpVect *verts = poly->tVerts;
int numVerts = poly->numVerts;
cpFloat r = poly->r;
for(int i=0; i<numVerts; i++){
cpVect n = axes[i].n;
cpFloat an = cpvdot(a, n);
cpFloat d = axes[i].d + r - an;
if(d > 0.0f) continue;
cpFloat bn = cpvdot(b, n);
cpFloat t = d/(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 - 1 + numVerts)%numVerts]);
cpFloat dtMax = -cpvcross(n, verts[i]);
if(dtMin <= dt && dt <= dtMax){
info->shape = (cpShape *)poly;
info->t = t;
info->n = n;
}
}
// Also check against the beveled vertexes.
if(r > 0.0f){
for(int i=0; i<numVerts; i++){
cpSegmentQueryInfo circle_info = {NULL, 1.0f, cpvzero};
CircleSegmentQuery(&poly->shape, verts[i], r, a, b, &circle_info);
if(circle_info.t < info->t) (*info) = circle_info;
}
}
}
static void
cpCircleShapeSegmentQuery(cpCircleShape *circle, cpVect a, cpVect b, cpFloat radius, cpSegmentQueryInfo *info)
{
CircleSegmentQuery((cpShape *)circle, circle->tc, circle->r, a, b, radius, info);
}
