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); }