Integrate cute_c2 collision + extension
parent
e70baef781
commit
0106e2673f
|
|
@ -0,0 +1,9 @@
|
|||
add_library(geometry
|
||||
STATIC
|
||||
geometry.c
|
||||
)
|
||||
|
||||
target_include_directories(geometry
|
||||
PUBLIC
|
||||
${CMAKE_CURRENT_LIST_DIR}
|
||||
)
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,35 @@
|
|||
#include "cute_c2.h"
|
||||
double PointToSegmentTOI(c2v p, c2v v, c2v a, c2v b);
|
||||
|
||||
double PointToCircleTOI(c2v p, c2v v, c2v circ_p, double r);
|
||||
|
||||
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);
|
||||
|
||||
|
||||
double PolyToCircleTOI(const c2Poly* pA, const c2x* ax_ptr, c2v vA, c2Circle cB, c2v vB, c2v* out_normal, c2v* out_contact_point);
|
||||
|
||||
|
||||
|
||||
double CircleToPolyTOI(c2Circle cA, c2v vA, const c2Poly* pB, const c2x* bx_ptr, c2v vB, c2v* out_normal, c2v* out_contact_point);
|
||||
|
||||
double CircleToCircleTOI(c2Circle cA, c2v vA, c2Circle cB, c2v vB, c2v* out_normal, c2v* out_contact_point);
|
||||
|
||||
|
||||
double PolyToCapsuleTOI(const c2Poly* pA, const c2x* ax_ptr, c2v vA, c2Capsule cB, c2v vB, c2v* out_normal, c2v* out_contact_point);
|
||||
|
||||
double CapsuleToPolyTOI(c2Capsule cA, c2v vA, const c2Poly* pB, const c2x* bx_ptr, c2v vB, c2v* out_normal, c2v* out_contact_point);
|
||||
|
||||
double CapsuleToCapsuleTOI(c2Capsule cA, c2v vA, c2Capsule cB, c2v vB, c2v* out_normal, c2v* out_contact_point);
|
||||
|
||||
double CapsuleToCircleTOI(c2Capsule cA, c2v vA, c2Circle cB, c2v vB, c2v* out_normal, c2v* out_contact_point);
|
||||
double CircleToCapsuleTOI(c2Circle cA, c2v vA, c2Capsule cB, c2v vB, c2v* out_normal, c2v* out_contact_point);
|
||||
|
||||
//AABB stuff
|
||||
double PolyToAABBTOI(const c2Poly* pA, const c2x* ax_ptr, c2v vA, c2AABB bB, c2v vB, c2v* out_normal, c2v* out_contact_point);
|
||||
double CircleToAABBTOI(c2Circle cA, c2v vA, c2AABB bB, c2v vB, c2v* out_normal, c2v* out_contact_point);
|
||||
double CapsuleToAABBTOI(c2Capsule cA, c2v vA, c2AABB bB, c2v vB, c2v* out_normal, c2v* 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);
|
||||
double AABBToCircleTOI(c2AABB bA, c2v vA, c2Circle cB, c2v vB, c2v* out_normal, c2v* out_contact_point);
|
||||
double AABBToCapsuleTOI(c2AABB bA, c2v vA, c2Capsule cB, c2v vB, c2v* out_normal, c2v* out_contact_point);
|
||||
double AABBToAABBTOI(c2AABB bA, c2v vA, c2AABB bB, c2v vB, c2v* out_normal, c2v* out_contact_point);
|
||||
|
|
@ -0,0 +1,643 @@
|
|||
#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]);
|
||||
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(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){
|
||||
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){
|
||||
//return c2TOI(&cA, C2_TYPE_CIRCLE, NULL, vA, &cB, C2_TYPE_CIRCLE, NULL, vB, true, out_normal, out_contact_point, NULL);
|
||||
|
||||
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){
|
||||
//return c2TOI(pA, C2_TYPE_POLY, ax_ptr, 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};
|
||||
|
||||
//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){
|
||||
//return c2TOI(&cA, C2_TYPE_CAPSULE, 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 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);
|
||||
}
|
||||
|
|
@ -1 +1,2 @@
|
|||
add_subdirectory(base)
|
||||
add_subdirectory(geometry)
|
||||
|
|
|
|||
|
|
@ -0,0 +1 @@
|
|||
add_subdirectory(samples)
|
||||
|
|
@ -0,0 +1,8 @@
|
|||
add_executable(collisionSample collision_sample.c)
|
||||
target_link_libraries(collisionSample PRIVATE
|
||||
engine
|
||||
)
|
||||
set_target_properties(collisionSample
|
||||
PROPERTIES
|
||||
RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin/manual)
|
||||
|
||||
|
|
@ -0,0 +1,209 @@
|
|||
#include "cute_c2_ext.h"
|
||||
#include "raylib.h"
|
||||
#include "raymath.h"
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
const int screenWidth = 800;
|
||||
const int screenHeight = 450;
|
||||
|
||||
typedef struct Shape {
|
||||
Vector2 pos;
|
||||
Vector2 vel;
|
||||
C2_TYPE type;
|
||||
union {
|
||||
c2v boxDim;
|
||||
c2Circle circle;
|
||||
c2Poly poly;
|
||||
} shape;
|
||||
Color colour;
|
||||
} Shape;
|
||||
|
||||
|
||||
void DrawShape(const Shape* shape) {
|
||||
switch (shape->type)
|
||||
{
|
||||
case C2_TYPE_AABB:
|
||||
DrawRectangle(
|
||||
shape->pos.x - shape->shape.boxDim.x / 2,
|
||||
shape->pos.y - shape->shape.boxDim.y / 2,
|
||||
shape->shape.boxDim.x,
|
||||
shape->shape.boxDim.y,
|
||||
shape->colour
|
||||
);
|
||||
break;
|
||||
case C2_TYPE_CIRCLE:
|
||||
DrawCircle(
|
||||
shape->pos.x,
|
||||
shape->pos.y, shape->shape.circle.r,
|
||||
shape->colour
|
||||
);
|
||||
break;
|
||||
case C2_TYPE_POLY:
|
||||
for (int i = 0; i < shape->shape.poly.count; ++i) {
|
||||
int next = (i + 1 + shape->shape.poly.count) % shape->shape.poly.count;
|
||||
DrawLineV(
|
||||
(Vector2){shape->shape.poly.verts[i].x, shape->shape.poly.verts[i].y},
|
||||
(Vector2){shape->shape.poly.verts[next].x, shape->shape.poly.verts[next].y},
|
||||
shape->colour
|
||||
);
|
||||
}
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
struct TOIInfo {
|
||||
double toi;
|
||||
c2v contact;
|
||||
c2v normal;
|
||||
};
|
||||
|
||||
void draw_toi_info(const struct TOIInfo* info)
|
||||
{
|
||||
if (info->toi != INFINITY)
|
||||
{
|
||||
DrawCircle(info->contact.x, info->contact.y, 4, GREEN);
|
||||
DrawLineEx(
|
||||
(Vector2){info->contact.x, info->contact.y},
|
||||
(Vector2){
|
||||
info->contact.x + info->normal.x * 12,
|
||||
info->contact.y + info->normal.y * 12
|
||||
},
|
||||
2, GREEN
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
int main(void)
|
||||
{
|
||||
InitWindow(screenWidth, screenHeight, "raylib");
|
||||
SetTargetFPS(60);
|
||||
|
||||
Shape A = {
|
||||
.pos = {300,150},
|
||||
.type = C2_TYPE_CIRCLE,
|
||||
//.shape.box = {{0, 0}, {64, 64}},
|
||||
.shape.circle = {{0, 0}, 64},
|
||||
.colour = RED,
|
||||
};
|
||||
|
||||
Shape B = {
|
||||
.pos = {43,60},
|
||||
.type = C2_TYPE_AABB,
|
||||
.shape.boxDim = {32, 32},
|
||||
.colour = BLUE,
|
||||
};
|
||||
|
||||
Shape C = {
|
||||
.pos = {100,100},
|
||||
.type = C2_TYPE_AABB,
|
||||
.shape.boxDim = {64, 64},
|
||||
.colour = PURPLE,
|
||||
};
|
||||
|
||||
Shape D = {
|
||||
.pos = {200,300},
|
||||
.type = C2_TYPE_POLY,
|
||||
.shape.poly = {
|
||||
.count = 3,
|
||||
.verts = {
|
||||
{0,0},
|
||||
{100, 0},
|
||||
{50,50},
|
||||
},
|
||||
},
|
||||
.colour = RED,
|
||||
};
|
||||
c2MakePoly(&D.shape.poly);
|
||||
for (int i = 0; i < D.shape.poly.count; ++i) {
|
||||
D.shape.poly.verts[i].x += D.pos.x;
|
||||
D.shape.poly.verts[i].y += D.pos.y;
|
||||
}
|
||||
|
||||
while (!WindowShouldClose())
|
||||
{
|
||||
float frame_time = GetFrameTime();
|
||||
Vector2 move_dir = {0};
|
||||
|
||||
if (IsKeyDown(KEY_LEFT)) {
|
||||
move_dir.x += -1;
|
||||
}
|
||||
if (IsKeyDown(KEY_RIGHT)) {
|
||||
move_dir.x += 1;
|
||||
}
|
||||
if (IsKeyDown(KEY_UP)) {
|
||||
move_dir.y += -1;
|
||||
}
|
||||
if (IsKeyDown(KEY_DOWN)) {
|
||||
move_dir.y += 1;
|
||||
}
|
||||
|
||||
move_dir = Vector2Normalize(move_dir);
|
||||
B.vel = Vector2Scale(move_dir, 100);
|
||||
|
||||
B.pos = Vector2Add(
|
||||
B.pos , Vector2Scale(B.vel, frame_time)
|
||||
);
|
||||
|
||||
c2Circle Acirc= {
|
||||
{A.pos.x, A.pos.y},
|
||||
A.shape.circle.r
|
||||
};
|
||||
c2AABB Bbox= {
|
||||
{B.pos.x - B.shape.boxDim.x / 2, B.pos.y - B.shape.boxDim.y / 2},
|
||||
{B.pos.x + B.shape.boxDim.x / 2, B.pos.y + B.shape.boxDim.y / 2},
|
||||
};
|
||||
c2AABB Cbox= {
|
||||
{C.pos.x - C.shape.boxDim.x / 2, C.pos.y - C.shape.boxDim.y / 2},
|
||||
{C.pos.x + C.shape.boxDim.x / 2, C.pos.y + C.shape.boxDim.y / 2},
|
||||
};
|
||||
|
||||
Vector2 test_spd = Vector2Normalize(Vector2Subtract(C.pos, B.pos));
|
||||
c2v test_velocityB = {test_spd.x * 100, test_spd.y * 100};
|
||||
const c2v static_velocity = {0,0};
|
||||
|
||||
struct TOIInfo toi0 = {0};
|
||||
toi0.toi = AABBToAABBTOI(
|
||||
Bbox, test_velocityB,
|
||||
Cbox, static_velocity,
|
||||
&toi0.normal, &toi0.contact
|
||||
);
|
||||
|
||||
test_spd = Vector2Normalize(Vector2Subtract(A.pos, B.pos));
|
||||
test_velocityB = (c2v){test_spd.x * 100, test_spd.y * 100};
|
||||
struct TOIInfo toi1 = {0};
|
||||
toi1.toi = AABBToCircleTOI(
|
||||
Bbox, test_velocityB,
|
||||
Acirc, static_velocity,
|
||||
&toi1.normal, &toi1.contact
|
||||
);
|
||||
|
||||
test_spd = Vector2Normalize(Vector2Subtract(D.pos, B.pos));
|
||||
test_velocityB = (c2v){test_spd.x * 100, test_spd.y * 100};
|
||||
struct TOIInfo toi2 = {0};
|
||||
toi2.toi = AABBToPolyTOI(
|
||||
Bbox, test_velocityB,
|
||||
&D.shape.poly, NULL, static_velocity,
|
||||
&toi2.normal, &toi2.contact
|
||||
);
|
||||
|
||||
//char buf[32];
|
||||
BeginDrawing();
|
||||
ClearBackground(RAYWHITE);
|
||||
DrawShape(&A);
|
||||
DrawShape(&C);
|
||||
DrawShape(&D);
|
||||
DrawShape(&B);
|
||||
DrawLineEx(B.pos, A.pos, 1, BLACK);
|
||||
DrawLineEx(C.pos, A.pos, 1, BLACK);
|
||||
DrawLineEx(D.pos, A.pos, 1, BLACK);
|
||||
//DrawText(buf, A.pos.x, A.pos.y, 12, BLACK);
|
||||
draw_toi_info(&toi0);
|
||||
draw_toi_info(&toi1);
|
||||
draw_toi_info(&toi2);
|
||||
EndDrawing();
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
Loading…
Reference in New Issue