644 lines
21 KiB
C
644 lines
21 KiB
C
#define CUTE_C2_IMPLEMENTATION
|
|
#include "cute_c2_ext.h"
|
|
#include <stdbool.h>
|
|
|
|
double PointToSegmentTOI(c2v p, c2v v, c2v a, c2v b){
|
|
c2v n = c2CCW90(c2Sub(b, a)); //this could be passed in
|
|
c2v PA = c2Sub(a, p);
|
|
c2v PB = c2Sub(b, p);
|
|
|
|
double denom = (c2Dot(v, n));
|
|
if(denom == 0) return INFINITY; //parallel, never collide
|
|
double t = (c2Dot(PA, n))/denom;
|
|
//if(t<0) return INFINITY; //collided in the past
|
|
|
|
if(c2Det2(PA, v)*c2Det2(PB, v) > 0) return INFINITY; //off the sides, never collide
|
|
return t;
|
|
}
|
|
|
|
//adapted from https://stackoverflow.com/a/1084899
|
|
double PointToCircleTOI(c2v p, c2v v, c2v circ_p, double r){
|
|
double a = c2Dot(v, v);
|
|
c2v f = c2Sub(p, circ_p);
|
|
double b = 2*c2Dot(f, v);
|
|
double c = c2Dot(f, f)-r*r;
|
|
|
|
double discriminant = b*b-4*a*c;
|
|
if(discriminant < 0) return INFINITY;
|
|
discriminant = c2Sqrt(discriminant);
|
|
|
|
/*double t1 = (-b - discriminant)/(2*a);
|
|
double t2 = (-b + discriminant)/(2*a); //this is the one you'd use for exit point TOI
|
|
return c2Min(t1, t2);*/
|
|
return (-b - discriminant)/(2*a);
|
|
}
|
|
|
|
double PolyToPolyTOI(const c2Poly* pA, const c2x* ax_ptr, c2v vA, const c2Poly* pB, const c2x* bx_ptr, c2v vB, c2v* out_normal, c2v* out_contact_point){
|
|
//return c2TOI(pA, C2_TYPE_POLY, ax_ptr, vA, pB, C2_TYPE_POLY, bx_ptr, vB, true, out_normal, out_contact_point, NULL);
|
|
|
|
//degenerate cases, zero movement
|
|
if(vA.x==vB.x && vA.y==vB.y) return INFINITY;
|
|
|
|
double t = INFINITY;
|
|
c2v n = {0,0};
|
|
c2v p = {0,0};
|
|
|
|
//pre-transform poly vertices to avoid doing it every access, compute which edges and faces count as "leading"
|
|
c2Poly A = *pA;
|
|
c2Poly B = *pB;
|
|
|
|
c2v vA2B = c2Sub(vA, vB);
|
|
c2v vB2A = c2Sub(vB, vA);
|
|
|
|
bool leading_edges_a[C2_MAX_POLYGON_VERTS] = {0};
|
|
bool leading_verts_a[C2_MAX_POLYGON_VERTS] = {0};
|
|
bool leading_edges_b[C2_MAX_POLYGON_VERTS] = {0};
|
|
bool leading_verts_b[C2_MAX_POLYGON_VERTS] = {0};
|
|
|
|
for(int i = 0; i<A.count; i++){
|
|
if(ax_ptr) A.norms[i] = c2Mulrv(ax_ptr->r, A.norms[i]);
|
|
if(c2Dot(A.norms[i], vB2A) < 0) leading_edges_a[i] = 1;
|
|
}
|
|
for(int i = 0; i<A.count; i++){
|
|
if(leading_edges_a[i] || leading_edges_a[(i-1+A.count)%A.count]){
|
|
if(ax_ptr) A.verts[i] = c2Mulxv(*ax_ptr, A.verts[i]);
|
|
leading_verts_a[i] = 1;
|
|
}
|
|
}
|
|
|
|
for(int i = 0; i<B.count; i++){
|
|
if(bx_ptr) B.norms[i] = c2Mulrv(bx_ptr->r, B.norms[i]);
|
|
if(c2Dot(B.norms[i], vA2B) < 0) leading_edges_b[i] = 1;
|
|
}
|
|
for(int i = 0; i<B.count; i++){
|
|
if(leading_edges_b[i] || leading_edges_b[(i-1+B.count)%B.count]){
|
|
if(bx_ptr) B.verts[i] = c2Mulxv(*bx_ptr, B.verts[i]);
|
|
leading_verts_b[i] = 1;
|
|
}
|
|
}
|
|
|
|
|
|
//TOI of A's vertices against B's edges
|
|
for(int j = 0; j<B.count; j++){
|
|
if(leading_edges_b[j]){
|
|
for(int i = 0; i<A.count; i++){
|
|
if(leading_verts_a[i]){
|
|
double v_t = PointToSegmentTOI(A.verts[i], vA2B, B.verts[j], B.verts[(j+1)%B.count]);
|
|
//Original check is (v_t <= t), but we want to override only if there is a shorter time.
|
|
// Change to check if difference is within some threshold
|
|
// This is fine because the value should fall between 0 and 1
|
|
if(v_t - t < -1e-6){
|
|
//if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case
|
|
|
|
t = v_t;
|
|
n = c2Neg(B.norms[j]);
|
|
p = c2Add(A.verts[i], c2Mulvs(vA, v_t));
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
//TOI of B's vertices against A's edges
|
|
for(int j = 0; j<A.count; j++){
|
|
if(leading_edges_a[j]){
|
|
for(int i = 0; i<B.count; i++){
|
|
if(leading_verts_b[i]){
|
|
double v_t = PointToSegmentTOI(B.verts[i], vB2A, A.verts[j], A.verts[(j+1)%A.count]);
|
|
if(v_t - t < -1e-6){
|
|
//if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case
|
|
|
|
t = v_t;
|
|
n = A.norms[j];
|
|
p = c2Add(B.verts[i], c2Mulvs(vB, v_t));
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if(out_normal) *out_normal = c2SafeNorm(n);
|
|
if(out_contact_point) *out_contact_point = p;
|
|
return t;
|
|
}
|
|
|
|
|
|
double PolyToCircleTOI(const c2Poly* pA, const c2x* ax_ptr, c2v vA, c2Circle cB, c2v vB, c2v* out_normal, c2v* out_contact_point){
|
|
//return c2TOI(pA, C2_TYPE_POLY, ax_ptr, vA, &cB, C2_TYPE_CIRCLE, NULL, vB, true, out_normal, out_contact_point, NULL);
|
|
|
|
//degenerate cases, zero movement
|
|
if(vA.x==vB.x && vA.y==vB.y) return INFINITY;
|
|
|
|
|
|
double t = INFINITY;
|
|
c2v n = {0,0};
|
|
c2v p = {0,0};
|
|
|
|
//pre-transform poly vertices to avoid doing it every access, compute which edges and faces count as "leading"
|
|
c2Poly A = *pA;
|
|
|
|
c2v vA2B = c2Sub(vA, vB);
|
|
c2v vB2A = c2Sub(vB, vA);
|
|
|
|
bool leading_edges_a[C2_MAX_POLYGON_VERTS] = {0};
|
|
for(int i = 0; i<A.count; i++){
|
|
if(ax_ptr) A.norms[i] = c2Mulrv(ax_ptr->r, A.norms[i]);
|
|
if(c2Dot(A.norms[i], vB2A) < 0) leading_edges_a[i] = 1;
|
|
}
|
|
|
|
|
|
//TOI of B against A's vertices
|
|
for(int i = 0; i<A.count; i++){
|
|
if(leading_edges_a[i] || leading_edges_a[(i-1+A.count)%A.count]){ //if this is a leading vert
|
|
if(ax_ptr) A.verts[i] = c2Mulxv(*ax_ptr, A.verts[i]);
|
|
|
|
double v_t = PointToCircleTOI(A.verts[i], vA2B, cB.p, cB.r);
|
|
if(v_t <= t){
|
|
//if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case
|
|
|
|
t = v_t;
|
|
n = c2Sub(
|
|
c2Add(c2Mulvs(vB, t), cB.p),
|
|
c2Add(c2Mulvs(vA, t), A.verts[i])
|
|
);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
//TOI of B's center against offset A edges
|
|
for(int i = 0; i<A.count; i++){
|
|
if(leading_edges_a[i]){
|
|
c2v offset = c2Mulvs(A.norms[i], cB.r);
|
|
double v_t = PointToSegmentTOI(cB.p, vB2A, c2Add(A.verts[i], offset), c2Add(A.verts[(i+1)%A.count], offset));
|
|
if(v_t <= t){
|
|
//if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case
|
|
|
|
t = v_t;
|
|
n = A.norms[i];
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
n = c2SafeNorm(n);
|
|
|
|
if(out_normal) *out_normal = n;
|
|
if(out_contact_point) *out_contact_point = c2Add(cB.p, c2Add(c2Mulvs(c2Neg(n), cB.r), c2Mulvs(vB, t)));
|
|
return t;
|
|
}
|
|
|
|
|
|
|
|
double CircleToPolyTOI(c2Circle cA, c2v vA, const c2Poly* pB, const c2x* bx_ptr, c2v vB, c2v* out_normal, c2v* out_contact_point){
|
|
double t = PolyToCircleTOI(pB, bx_ptr, vB, cA, vA, out_normal, out_contact_point);
|
|
if(out_normal) *out_normal = c2Neg(*out_normal);
|
|
return t;
|
|
}
|
|
|
|
double CircleToCircleTOI(c2Circle cA, c2v vA, c2Circle cB, c2v vB, c2v* out_normal, c2v* out_contact_point){
|
|
|
|
c2v vA2B = c2Sub(vA, vB);
|
|
double t = PointToCircleTOI(cA.p, vA2B, cB.p, cB.r+cA.r);
|
|
|
|
if(t < 0) return INFINITY;
|
|
|
|
c2v pA = c2Add(c2Mulvs(vA, t), cA.p);
|
|
c2v pB = c2Add(c2Mulvs(vB, t), cB.p);
|
|
c2v n = c2SafeNorm(c2Sub(pB, pA));
|
|
|
|
if(out_normal) *out_normal = n;
|
|
if(out_contact_point) *out_contact_point = c2Add(pA, c2Mulvs(n, cA.r));
|
|
return t;
|
|
}
|
|
|
|
|
|
double PolyToCapsuleTOI(const c2Poly* pA, const c2x* ax_ptr, c2v vA, c2Capsule cB, c2v vB, c2v* out_normal, c2v* out_contact_point){
|
|
|
|
//degenerate cases, zero movement
|
|
if(vA.x==vB.x && vA.y==vB.y) return INFINITY;
|
|
|
|
double t = INFINITY;
|
|
c2v n = {0,0};
|
|
c2v p = {0,0};
|
|
|
|
//pre-transform poly vertices to avoid doing it every access, compute which edges and faces count as "leading"
|
|
c2Poly A = *pA;
|
|
|
|
c2v vA2B = c2Sub(vA, vB);
|
|
c2v vB2A = c2Sub(vB, vA);
|
|
|
|
bool leading_edges_a[C2_MAX_POLYGON_VERTS] = {0};
|
|
for(int i = 0; i<A.count; i++){
|
|
if(ax_ptr) A.norms[i] = c2Mulrv(ax_ptr->r, A.norms[i]);
|
|
if(c2Dot(A.norms[i], vB2A) < 0) leading_edges_a[i] = 1;
|
|
}
|
|
|
|
bool p_on_cap = true;
|
|
|
|
//TOI of B's endcircles against A's vertices
|
|
for(int i = 0; i<A.count; i++){
|
|
if(leading_edges_a[i] || leading_edges_a[(i-1+A.count)%A.count]){ //if this is a leading vert
|
|
if(ax_ptr) A.verts[i] = c2Mulxv(*ax_ptr, A.verts[i]);
|
|
|
|
{ //a
|
|
double v_t = PointToCircleTOI(A.verts[i], vA2B, cB.a, cB.r);
|
|
if(v_t <= t){
|
|
if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case
|
|
|
|
t = v_t;
|
|
n = c2Sub(
|
|
c2Add(c2Mulvs(vB, t), cB.a),
|
|
c2Add(c2Mulvs(vA, t), A.verts[i])
|
|
);
|
|
p = cB.a;
|
|
}
|
|
}
|
|
{ //b
|
|
double v_t = PointToCircleTOI(A.verts[i], vA2B, cB.b, cB.r);
|
|
if(v_t <= t){
|
|
if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case
|
|
|
|
t = v_t;
|
|
n = c2Sub(
|
|
c2Add(c2Mulvs(vB, t), cB.b),
|
|
c2Add(c2Mulvs(vA, t), A.verts[i])
|
|
);
|
|
p = cB.b;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
//TOI of B's endpoints against offset A edges
|
|
for(int i = 0; i<A.count; i++){
|
|
if(leading_edges_a[i]){
|
|
{ //a
|
|
c2v offset = c2Mulvs(A.norms[i], cB.r);
|
|
double v_t = PointToSegmentTOI(cB.a, vB2A, c2Add(A.verts[i], offset), c2Add(A.verts[(i+1)%A.count], offset));
|
|
if(v_t <= t){
|
|
if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case
|
|
|
|
t = v_t;
|
|
n = A.norms[i];
|
|
p = cB.a;
|
|
}
|
|
}
|
|
{ //b
|
|
c2v offset = c2Mulvs(A.norms[i], cB.r);
|
|
double v_t = PointToSegmentTOI(cB.b, vB2A, c2Add(A.verts[i], offset), c2Add(A.verts[(i+1)%A.count], offset));
|
|
if(v_t <= t){
|
|
if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case
|
|
|
|
t = v_t;
|
|
n = A.norms[i];
|
|
p = cB.b;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
//TOI of A's vertices against B's leading edge
|
|
|
|
c2v leading_capsule_n = c2SafeNorm(c2CCW90(c2Sub(cB.b, cB.a)));
|
|
double d = c2Dot(leading_capsule_n, vA2B);
|
|
if(d > 0){
|
|
leading_capsule_n = c2Neg(leading_capsule_n);
|
|
}
|
|
|
|
if(d != 0){
|
|
c2v cap_offset = c2Mulvs(leading_capsule_n, cB.r);
|
|
|
|
for(int i = 0; i<A.count; i++){
|
|
if(leading_edges_a[i] || leading_edges_a[(i-1+A.count)%A.count]){ //if this is a leading vert
|
|
double v_t = PointToSegmentTOI(A.verts[i], vA2B, c2Add(cap_offset, cB.a), c2Add(cap_offset, cB.b));
|
|
if(v_t <= t){
|
|
if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case
|
|
|
|
t = v_t;
|
|
p = c2Add(A.verts[i], c2Mulvs(vA, v_t));
|
|
n = c2Neg(leading_capsule_n);
|
|
p_on_cap = false;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
n = c2SafeNorm(n);
|
|
|
|
if(out_normal) *out_normal = n;
|
|
if(p_on_cap){
|
|
if(out_contact_point) *out_contact_point = c2Add(p, c2Add(c2Mulvs(c2Neg(n), cB.r), c2Mulvs(vB, t)));
|
|
} else {
|
|
if(out_contact_point) *out_contact_point = p;
|
|
}
|
|
return t;
|
|
}
|
|
|
|
double CapsuleToPolyTOI(c2Capsule cA, c2v vA, const c2Poly* pB, const c2x* bx_ptr, c2v vB, c2v* out_normal, c2v* out_contact_point){
|
|
double t = PolyToCapsuleTOI(pB, bx_ptr, vB, cA, vA, out_normal, out_contact_point);
|
|
if(out_normal) *out_normal = c2Neg(*out_normal);
|
|
return t;
|
|
}
|
|
|
|
|
|
|
|
double CapsuleToCapsuleTOI(c2Capsule cA, c2v vA, c2Capsule cB, c2v vB, c2v* out_normal, c2v* out_contact_point){
|
|
|
|
//degenerate cases, zero movement
|
|
if(vA.x==vB.x && vA.y==vB.y) return INFINITY;
|
|
|
|
double t = INFINITY;
|
|
c2v n = {0,0};
|
|
c2v p = {0,0};
|
|
|
|
c2v vA2B = c2Sub(vA, vB);
|
|
c2v vB2A = c2Sub(vB, vA);
|
|
|
|
double sum_radius = cA.r+cB.r;
|
|
|
|
//TOI of circles of B against leading edge of A
|
|
c2v leading_edge = c2SafeNorm(c2CCW90(c2Sub(cA.b, cA.a)));
|
|
double d = c2Dot(leading_edge, vB2A);
|
|
if(d > 0){
|
|
leading_edge = c2Neg(leading_edge);
|
|
}
|
|
bool p_on_a = true;
|
|
|
|
if(d != 0){
|
|
c2v cap_offset = c2Mulvs(leading_edge, sum_radius);
|
|
|
|
{//a
|
|
double v_t = PointToSegmentTOI(cB.a, vB2A, c2Add(cap_offset, cA.a), c2Add(cap_offset, cA.b));
|
|
if(v_t <= t){
|
|
if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case
|
|
|
|
t = v_t;
|
|
p = cB.a;
|
|
p_on_a = false;
|
|
n = leading_edge;
|
|
}
|
|
}
|
|
|
|
{//b
|
|
double v_t = PointToSegmentTOI(cB.b, vB2A, c2Add(cap_offset, cA.a), c2Add(cap_offset, cA.b));
|
|
if(v_t <= t){
|
|
if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case
|
|
|
|
t = v_t;
|
|
p = cB.b;
|
|
p_on_a = false;
|
|
n = leading_edge;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
//TOI of circles of A against leading edge of B
|
|
leading_edge = c2SafeNorm(c2CCW90(c2Sub(cB.b, cB.a)));
|
|
d = c2Dot(leading_edge, vA2B);
|
|
if(d > 0){
|
|
leading_edge = c2Neg(leading_edge);
|
|
}
|
|
if(d != 0){
|
|
c2v cap_offset = c2Mulvs(leading_edge, sum_radius);
|
|
|
|
{//a
|
|
double v_t = PointToSegmentTOI(cA.a, vA2B, c2Add(cap_offset, cB.a), c2Add(cap_offset, cB.b));
|
|
if(v_t <= t){
|
|
if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case
|
|
|
|
t = v_t;
|
|
p = cA.a;
|
|
p_on_a = true;
|
|
n = c2Neg(leading_edge);
|
|
}
|
|
}
|
|
|
|
{//a
|
|
double v_t = PointToSegmentTOI(cA.b, vA2B, c2Add(cap_offset, cB.a), c2Add(cap_offset, cB.b));
|
|
if(v_t <= t){
|
|
if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case
|
|
|
|
t = v_t;
|
|
p = cA.b;
|
|
p_on_a = true;
|
|
n = c2Neg(leading_edge);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
//TOI circles of B against circles of A
|
|
|
|
{//aa
|
|
double v_t = PointToCircleTOI(cB.a, vB2A, cA.a, sum_radius);
|
|
if(v_t <= t){
|
|
if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case
|
|
|
|
t = v_t;
|
|
p = cA.a;
|
|
p_on_a = true;
|
|
c2v pA = c2Add(c2Mulvs(vA, t), cA.a);
|
|
c2v pB = c2Add(c2Mulvs(vB, t), cB.a);
|
|
n = c2Sub(pB, pA);
|
|
}
|
|
}
|
|
|
|
{//ba
|
|
double v_t = PointToCircleTOI(cB.b, vB2A, cA.a, sum_radius);
|
|
if(v_t <= t){
|
|
if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case
|
|
|
|
t = v_t;
|
|
p = cA.a;
|
|
p_on_a = true;
|
|
c2v pA = c2Add(c2Mulvs(vA, t), cA.a);
|
|
c2v pB = c2Add(c2Mulvs(vB, t), cB.b);
|
|
n = c2Sub(pB, pA);
|
|
}
|
|
}
|
|
{//ab
|
|
double v_t = PointToCircleTOI(cB.a, vB2A, cA.b, sum_radius);
|
|
if(v_t <= t){
|
|
if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case
|
|
|
|
t = v_t;
|
|
p = cA.b;
|
|
p_on_a = true;
|
|
c2v pA = c2Add(c2Mulvs(vA, t), cA.b);
|
|
c2v pB = c2Add(c2Mulvs(vB, t), cB.a);
|
|
n = c2Sub(pB, pA);
|
|
}
|
|
}
|
|
|
|
{//bb
|
|
double v_t = PointToCircleTOI(cB.b, vB2A, cA.b, sum_radius);
|
|
if(v_t <= t){
|
|
if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case
|
|
|
|
t = v_t;
|
|
p = cA.b;
|
|
p_on_a = true;
|
|
c2v pA = c2Add(c2Mulvs(vA, t), cA.b);
|
|
c2v pB = c2Add(c2Mulvs(vB, t), cB.b);
|
|
n = c2Sub(pB, pA);
|
|
}
|
|
}
|
|
|
|
|
|
n = c2SafeNorm(n);
|
|
if(out_normal) *out_normal = n;
|
|
if(out_contact_point) {
|
|
if(p_on_a){
|
|
*out_contact_point = c2Add(p, c2Add(c2Mulvs(n, cA.r), c2Mulvs(vA, t)));
|
|
} else {
|
|
*out_contact_point = c2Add(p, c2Add(c2Mulvs(c2Neg(n), cB.r), c2Mulvs(vB, t)));
|
|
}
|
|
}
|
|
return t;
|
|
}
|
|
|
|
|
|
double CapsuleToCircleTOI(c2Capsule cA, c2v vA, c2Circle cB, c2v vB, c2v* out_normal, c2v* out_contact_point){
|
|
double t = CircleToCapsuleTOI(cB, vB, cA, vA, out_normal, out_contact_point);
|
|
if(out_normal) *out_normal = c2Neg(*out_normal);
|
|
return t;
|
|
}
|
|
double CircleToCapsuleTOI(c2Circle cA, c2v vA, c2Capsule cB, c2v vB, c2v* out_normal, c2v* out_contact_point){
|
|
//return c2TOI(&cA, C2_TYPE_CIRCLE, NULL, vA, &cB, C2_TYPE_CAPSULE, NULL, vB, true, out_normal, out_contact_point, NULL);
|
|
|
|
//degenerate cases, zero movement
|
|
if(vA.x==vB.x && vA.y==vB.y) return INFINITY;
|
|
|
|
double t = INFINITY;
|
|
c2v n = {0,0};
|
|
c2v p = {0,0};
|
|
|
|
c2v vA2B = c2Sub(vA, vB);
|
|
c2v vB2A = c2Sub(vB, vA);
|
|
|
|
double sum_radius = cA.r+cB.r;
|
|
|
|
|
|
|
|
//TOI of A against leading edge of B
|
|
c2v leading_edge = c2SafeNorm(c2CCW90(c2Sub(cB.b, cB.a)));
|
|
double d = c2Dot(leading_edge, vA2B);
|
|
if(d > 0){
|
|
leading_edge = c2Neg(leading_edge);
|
|
}
|
|
if(d != 0){
|
|
c2v cap_offset = c2Mulvs(leading_edge, sum_radius);
|
|
|
|
double v_t = PointToSegmentTOI(cA.p, vA2B, c2Add(cap_offset, cB.a), c2Add(cap_offset, cB.b));
|
|
if(v_t <= t){
|
|
if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case
|
|
|
|
t = v_t;
|
|
n = c2Neg(leading_edge);
|
|
}
|
|
}
|
|
|
|
//TOI of A against circles of B
|
|
|
|
{//a
|
|
double v_t = PointToCircleTOI(cA.p, vA2B, cB.a, sum_radius);
|
|
if(v_t <= t){
|
|
if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case
|
|
|
|
t = v_t;
|
|
c2v pA = c2Add(c2Mulvs(vA, t), cA.p);
|
|
c2v pB = c2Add(c2Mulvs(vB, t), cB.a);
|
|
n = c2Sub(pB, pA);
|
|
}
|
|
}
|
|
|
|
{//b
|
|
double v_t = PointToCircleTOI(cA.p, vA2B, cB.b, sum_radius);
|
|
if(v_t <= t){
|
|
if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case
|
|
|
|
t = v_t;
|
|
c2v pA = c2Add(c2Mulvs(vA, t), cA.p);
|
|
c2v pB = c2Add(c2Mulvs(vB, t), cB.b);
|
|
n = c2Sub(pB, pA);
|
|
}
|
|
}
|
|
|
|
|
|
|
|
n = c2SafeNorm(n);
|
|
if(out_normal) *out_normal = n;
|
|
if(out_contact_point) *out_contact_point = c2Add(cA.p, c2Add(c2Mulvs(n, cA.r), c2Mulvs(vA, t)));
|
|
return t;
|
|
|
|
}
|
|
|
|
|
|
|
|
//AABB stuff
|
|
double PolyToAABBTOI(const c2Poly* pA, const c2x* ax_ptr, c2v vA, c2AABB bB, c2v vB, c2v* out_normal, c2v* out_contact_point){
|
|
c2Poly pB;
|
|
c2BBVerts(pB.verts, &bB);
|
|
pB.count = 4;
|
|
c2Norms(pB.verts, pB.norms, 4);
|
|
|
|
return PolyToPolyTOI(pA, ax_ptr, vA, &pB, NULL, vB, out_normal, out_contact_point);
|
|
}
|
|
double CircleToAABBTOI(c2Circle cA, c2v vA, c2AABB bB, c2v vB, c2v* out_normal, c2v* out_contact_point){
|
|
c2Poly pB;
|
|
c2BBVerts(pB.verts, &bB);
|
|
pB.count = 4;
|
|
c2Norms(pB.verts, pB.norms, 4);
|
|
|
|
return CircleToPolyTOI(cA, vA, &pB, NULL, vB, out_normal, out_contact_point);
|
|
}
|
|
double CapsuleToAABBTOI(c2Capsule cA, c2v vA, c2AABB bB, c2v vB, c2v* out_normal, c2v* out_contact_point){
|
|
c2Poly pB;
|
|
c2BBVerts(pB.verts, &bB);
|
|
pB.count = 4;
|
|
c2Norms(pB.verts, pB.norms, 4);
|
|
|
|
return CapsuleToPolyTOI(cA, vA, &pB, NULL, vB, out_normal, out_contact_point);
|
|
}
|
|
|
|
double AABBToPolyTOI(c2AABB bA, c2v vA, const c2Poly* pB, const c2x* bx_ptr, c2v vB, c2v* out_normal, c2v* out_contact_point){
|
|
c2Poly pA;
|
|
c2BBVerts(pA.verts, &bA);
|
|
pA.count = 4;
|
|
c2Norms(pA.verts, pA.norms, 4);
|
|
|
|
return PolyToPolyTOI(&pA, NULL, vA, pB, bx_ptr, vB, out_normal, out_contact_point);
|
|
}
|
|
double AABBToCircleTOI(c2AABB bA, c2v vA, c2Circle cB, c2v vB, c2v* out_normal, c2v* out_contact_point){
|
|
c2Poly pA;
|
|
c2BBVerts(pA.verts, &bA);
|
|
pA.count = 4;
|
|
c2Norms(pA.verts, pA.norms, 4);
|
|
|
|
return PolyToCircleTOI(&pA, NULL, vA, cB, vB, out_normal, out_contact_point);
|
|
}
|
|
double AABBToCapsuleTOI(c2AABB bA, c2v vA, c2Capsule cB, c2v vB, c2v* out_normal, c2v* out_contact_point){
|
|
c2Poly pA;
|
|
c2BBVerts(pA.verts, &bA);
|
|
pA.count = 4;
|
|
c2Norms(pA.verts, pA.norms, 4);
|
|
|
|
return PolyToCapsuleTOI(&pA, NULL, vA, cB, vB, out_normal, out_contact_point);
|
|
}
|
|
double AABBToAABBTOI(c2AABB bA, c2v vA, c2AABB bB, c2v vB, c2v* out_normal, c2v* out_contact_point){
|
|
c2Poly pA;
|
|
c2BBVerts(pA.verts, &bA);
|
|
pA.count = 4;
|
|
c2Norms(pA.verts, pA.norms, 4);
|
|
|
|
c2Poly pB;
|
|
c2BBVerts(pB.verts, &bB);
|
|
pB.count = 4;
|
|
c2Norms(pB.verts, pB.norms, 4);
|
|
|
|
return PolyToPolyTOI(&pA, NULL, vA, &pB, NULL, vB, out_normal, out_contact_point);
|
|
}
|