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;
}
}
}
}
static void
cpSegmentShapeSegmentQuery(cpSegmentShape *seg, cpVect a, cpVect b, cpSegmentQueryInfo *info)
{
cpVect n = seg->tn;
cpFloat d = cpvdot(cpvsub(seg->ta, a), n);
cpFloat r = seg->r;
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){
info->shape = (cpShape *)seg;
info->t = ad/(ad - bd);
info->n = flipped_n;
}
} else if(r != 0.0f){
cpSegmentQueryInfo info1 = {NULL, 1.0f, cpvzero};
cpSegmentQueryInfo info2 = {NULL, 1.0f, cpvzero};
circleSegmentQuery((cpShape *)seg, seg->ta, seg->r, a, b, &info1);
circleSegmentQuery((cpShape *)seg, seg->tb, seg->r, a, b, &info2);
if(info1.t < info2.t){
(*info) = info1;
} else {
(*info) = info2;
}
}
}
static void
cpCircleShapeSegmentQuery(cpCircleShape *circle, cpVect a, cpVect b, cpSegmentQueryInfo *info)
{
circleSegmentQuery((cpShape *)circle, circle->tc, circle->r, a, b, info);
}
