From a05a021976a840315282080c8908ea40c81912f3 Mon Sep 17 00:00:00 2001
From: En Yi <en.yi@tutanota.com>
Date: Sat, 4 Jan 2025 16:40:53 +0800
Subject: [PATCH] Update collision detection sample

---
 engine/geometry/geometry.c                    | 10 +-
 .../tests/geometry/samples/collision_sample.c | 98 +++++++++++++------
 2 files changed, 74 insertions(+), 34 deletions(-)

diff --git a/engine/geometry/geometry.c b/engine/geometry/geometry.c
index 4fc2a0b..14ec987 100644
--- a/engine/geometry/geometry.c
+++ b/engine/geometry/geometry.c
@@ -84,7 +84,10 @@ double PolyToPolyTOI(const c2Poly* pA, const c2x* ax_ptr, c2v vA, const c2Poly*
             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){
+                    //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;
@@ -102,7 +105,7 @@ double PolyToPolyTOI(const c2Poly* pA, const c2x* ax_ptr, c2v vA, const c2Poly*
             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 - t < -1e-6){
                         if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case
 
                         t = v_t;
@@ -194,7 +197,6 @@ double CircleToPolyTOI(c2Circle cA, c2v vA, const c2Poly* pB, const c2x* bx_ptr,
 }
 
 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);
@@ -212,7 +214,6 @@ double CircleToCircleTOI(c2Circle cA, c2v vA, c2Circle cB, c2v vB, c2v* out_norm
 
 
 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;
@@ -344,7 +345,6 @@ double CapsuleToPolyTOI(c2Capsule cA, c2v vA, const c2Poly* pB, const c2x* bx_pt
 
 
 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;
diff --git a/engine/tests/geometry/samples/collision_sample.c b/engine/tests/geometry/samples/collision_sample.c
index d4b8dd9..8a7f60c 100644
--- a/engine/tests/geometry/samples/collision_sample.c
+++ b/engine/tests/geometry/samples/collision_sample.c
@@ -9,6 +9,7 @@ const int screenHeight = 450;
 
 typedef struct Shape {
     Vector2 pos;
+    Vector2 center;
     Vector2 vel;
     C2_TYPE type;
     union {
@@ -25,8 +26,8 @@ void DrawShape(const Shape* shape) {
     {
         case C2_TYPE_AABB:
             DrawRectangle(
-                shape->pos.x - shape->shape.boxDim.x / 2,
-                shape->pos.y - shape->shape.boxDim.y / 2,
+                shape->center.x - shape->shape.boxDim.x / 2,
+                shape->center.y - shape->shape.boxDim.y / 2,
                 shape->shape.boxDim.x,
                 shape->shape.boxDim.y,
                 shape->colour
@@ -34,8 +35,8 @@ void DrawShape(const Shape* shape) {
         break;
         case C2_TYPE_CIRCLE:
             DrawCircle(
-                shape->pos.x,
-                shape->pos.y, shape->shape.circle.r,
+                shape->center.x,
+                shape->center.y, shape->shape.circle.r,
                 shape->colour
             );
         break;
@@ -82,29 +83,32 @@ int main(void)
     SetTargetFPS(60);
 
     Shape A = {
-        .pos = {300,150},
+        .pos = {450,150},
+        .center = {450,150},
         .type = C2_TYPE_CIRCLE,
-        //.shape.box = {{0, 0}, {64, 64}},
         .shape.circle = {{0, 0}, 64},
         .colour = RED,
     };
 
     Shape B = {
-        .pos = {43,60},
+        .pos = {300,150},
+        .center = {300,150},
         .type = C2_TYPE_AABB,
         .shape.boxDim = {32, 32},
         .colour = BLUE,
     };
 
     Shape C = {
-        .pos = {100,100},
+        .pos = {200,100},
+        .center = {200,100},
         .type = C2_TYPE_AABB,
         .shape.boxDim = {64, 64},
-        .colour = PURPLE,
+        .colour = RED,
     };
 
     Shape D = {
-        .pos = {200,300},
+        .pos = {200,250},
+        .center = {250,266.666},
         .type = C2_TYPE_POLY,
         .shape.poly = {
             .count = 3,
@@ -141,27 +145,30 @@ int main(void)
         }
 
         move_dir = Vector2Normalize(move_dir);
-        B.vel = Vector2Scale(move_dir, 100);
+        B.vel = Vector2Scale(move_dir, 200);
   
         B.pos = Vector2Add(
             B.pos , Vector2Scale(B.vel, frame_time)
         );
+        B.center = B.pos;
         
         c2Circle Acirc= {
-            {A.pos.x, A.pos.y},
+            {A.center.x, A.center.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},
+            {B.center.x - B.shape.boxDim.x / 2, B.center.y - B.shape.boxDim.y / 2},
+            {B.center.x + B.shape.boxDim.x / 2, B.center.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},
+            {C.center.x - C.shape.boxDim.x / 2, C.center.y - C.shape.boxDim.y / 2},
+            {C.center.x + C.shape.boxDim.x / 2, C.center.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 float TEST_VELOCITY = 100;
+
+        Vector2 test_spd = Vector2Normalize(Vector2Subtract(C.center, B.center));
+        c2v test_velocityB = {test_spd.x * TEST_VELOCITY, test_spd.y * TEST_VELOCITY};
         const c2v static_velocity = {0,0};
 
         struct TOIInfo toi0 = {0};
@@ -170,39 +177,72 @@ int main(void)
             Cbox, static_velocity,
             &toi0.normal, &toi0.contact
         );
+        struct Shape projB0 = B;
+        projB0.pos = Vector2Add(
+            projB0.pos ,
+            (Vector2){test_velocityB.x * toi0.toi, test_velocityB.y * toi0.toi}
+        );
+        projB0.center = projB0.pos;
+        projB0.colour = PINK;
 
-        test_spd = Vector2Normalize(Vector2Subtract(A.pos, B.pos));
-        test_velocityB = (c2v){test_spd.x * 100, test_spd.y * 100};
+        test_spd = Vector2Normalize(Vector2Subtract(A.center, B.center));
+        test_velocityB = (c2v){test_spd.x * TEST_VELOCITY, test_spd.y * TEST_VELOCITY};
         struct TOIInfo toi1 = {0};
         toi1.toi = AABBToCircleTOI(
             Bbox, test_velocityB,
             Acirc, static_velocity,
             &toi1.normal, &toi1.contact
         );
+        struct Shape projB1 = B;
+        projB1.pos = Vector2Add(
+            projB1.pos ,
+            (Vector2){test_velocityB.x * toi1.toi, test_velocityB.y * toi1.toi}
+        );
+        projB1.center = projB1.pos;
+        projB1.colour = PINK;
 
-        test_spd = Vector2Normalize(Vector2Subtract(D.pos, B.pos));
-        test_velocityB = (c2v){test_spd.x * 100, test_spd.y * 100};
+        test_spd = Vector2Normalize(Vector2Subtract(D.center, B.center));
+        test_velocityB = (c2v){test_spd.x * TEST_VELOCITY, test_spd.y * TEST_VELOCITY};
         struct TOIInfo toi2 = {0};
         toi2.toi = AABBToPolyTOI(
             Bbox, test_velocityB,
             &D.shape.poly, NULL, static_velocity,
             &toi2.normal, &toi2.contact
         );
+        struct Shape projB2 = B;
+        projB2.pos = Vector2Add(
+            projB2.pos ,
+            (Vector2){test_velocityB.x * toi2.toi, test_velocityB.y * toi2.toi}
+        );
+        projB2.center = projB2.pos;
+        projB2.colour = PINK;
 
-        //char buf[32];
         BeginDrawing();
             ClearBackground(RAYWHITE);
             DrawShape(&A);
             DrawShape(&C);
             DrawShape(&D);
+            if (toi0.toi <= 1)
+            {
+                DrawShape(&projB0);
+                draw_toi_info(&toi0);
+            }
+            if (toi1.toi <= 1)
+            {
+                DrawShape(&projB1);
+                draw_toi_info(&toi1);
+            }
+            if (toi2.toi <= 1)
+            {
+                DrawShape(&projB2);
+                draw_toi_info(&toi2);
+            }
+
             DrawShape(&B);
-            DrawLineEx(B.pos, A.pos, 1, BLACK);
-            DrawLineEx(C.pos, A.pos, 1, BLACK);
-            DrawLineEx(D.pos, A.pos, 1, BLACK);
+            DrawLineEx(A.center, B.center, 1, BLACK);
+            DrawLineEx(C.center, B.center, 1, BLACK);
+            DrawLineEx(D.center, B.center, 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;