Tweak collision logic to for second sample

engine/geometry/geometry.c

@@ -87,8 +87,8 @@ double PolyToPolyTOI(const c2Poly* pA, const c2x* ax_ptr, c2v vA, const c2Poly*
                     //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 < t){
-                        //if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case
+                        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]);
@@ -105,8 +105,8 @@ 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 < -1e-6){
+                    if(v_t < t){
-                        //if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case
+                        if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case
 
                         t = v_t;
                         n = A.norms[j];

engine/tests/geometry/samples/collision_sample2.c

@@ -1,3 +1,4 @@
+#include "cute_c2.h"
 #include "cute_c2_ext.h"
 #include "raylib.h"
 #include "raymath.h"
@@ -63,6 +64,8 @@ struct TOIInfo {
 
 void draw_toi_info(const struct TOIInfo* info)
 {
+    if (info->toi > 1) return;
+
     if (info->toi != INFINITY)
     {
         DrawCircle(info->contact.x, info->contact.y, 4, GREEN);
@@ -77,6 +80,25 @@ void draw_toi_info(const struct TOIInfo* info)
     }
 }
 
+void check_collision_and_move(struct Shape* shape, const struct TOIInfo* toi) {
+    if (toi->toi < 1)
+    {
+        Vector2 normal = {-toi->normal.x, -toi->normal.y};
+        Vector2 nV =
+            Vector2Scale(
+                normal,
+                fabs(
+                Vector2DotProduct(
+                    normal,
+                    Vector2Scale(shape->vel, 1.0-toi->toi)
+                ) - 0.2)
+            );
+        shape->vel = Vector2Add(
+            shape->vel, nV
+        );
+    }
+}
+
 int main(void)
 {
     InitWindow(screenWidth, screenHeight, "raylib");
@@ -92,36 +114,44 @@ int main(void)
     };
 
     Shape B = {
-        .pos = {400,300},
+        .pos = {200,300},
-        .center = {400,300},
+        .center = {200,300},
         .type = C2_TYPE_AABB,
         .shape.boxDim = {256, 64},
         .colour = RED,
     };
 
-    //Shape C = {
+    Shape C = {
-    //    .pos = {200,250},
+        .pos = {328,268},
-    //    .center = {250,266.666},
+        .center = {250,266.666},
-    //    .type = C2_TYPE_POLY,
+        .type = C2_TYPE_POLY,
-    //    .shape.poly = {
+        .shape.poly = {
-    //        .count = 3,
+            .count = 3,
-    //        .verts = {
+            .verts = {
-    //            {0,0},
+                {0,0},
-    //            {100, 0},
+                {100, -50},
-    //            {50,50},
+                {100,0},
-    //        },
+            },
-    //    },
+        },
-    //    .colour = RED,
+        .colour = RED,
-    //};
+    };
-    //c2MakePoly(&C.shape.poly);
+    c2MakePoly(&C.shape.poly);
-    //for (int i = 0; i < C.shape.poly.count; ++i) {
+    for (int i = 0; i < C.shape.poly.count; ++i) {
-    //    C.shape.poly.verts[i].x += C.pos.x;
+        C.shape.poly.verts[i].x += C.pos.x;
-    //    C.shape.poly.verts[i].y += C.pos.y;
+        C.shape.poly.verts[i].y += C.pos.y;
-    //}
+    }
+
+    Shape D = {
+        .pos = {460,243},
+        .center = {460,243},
+        .type = C2_TYPE_AABB,
+        .shape.boxDim = {64, 50},
+        .colour = RED,
+    };
 
     while (!WindowShouldClose())
     {
-        float frame_time = GetFrameTime();
+        float frame_time = 1.0/60;
         Vector2 move_dir = {0};
 
         if (IsKeyDown(KEY_LEFT)) {
@@ -138,7 +168,8 @@ int main(void)
         }
 
         move_dir = Vector2Normalize(move_dir);
-        A.vel = Vector2Scale(move_dir, 200);
+        A.vel = Vector2Scale(move_dir, 400);
+        A.vel.y += 15000 * frame_time;
 
         c2AABB Abox= {
             {A.center.x - A.shape.boxDim.x / 2, A.center.y - A.shape.boxDim.y / 2},
@@ -148,7 +179,29 @@ int main(void)
             {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 Dbox= {
+            {D.center.x - D.shape.boxDim.x / 2, D.center.y - D.shape.boxDim.y / 2},
+            {D.center.x + D.shape.boxDim.x / 2, D.center.y + D.shape.boxDim.y / 2},
+        };
 
+        // Actually need to order via TOI and resolve
+        // For each resolve, recheck TOI
+        struct TOIInfo toi1 = {0};
+        toi1.toi = AABBToPolyTOI(
+            Abox, (c2v){A.vel.x *frame_time, A.vel.y * frame_time},
+            &C.shape.poly, NULL, (c2v){0,0},
+            &toi1.normal, &toi1.contact
+        );
+        check_collision_and_move(&A, &toi1);
+
+        struct TOIInfo toi2 = {0};
+        toi2.toi = AABBToAABBTOI(
+            Abox, (c2v){A.vel.x *frame_time, A.vel.y * frame_time},
+            Dbox, (c2v){0,0},
+            &toi2.normal, &toi2.contact
+        );
+        check_collision_and_move(&A, &toi2);
 
         struct TOIInfo toi0 = {0};
         toi0.toi = AABBToAABBTOI(
@@ -157,19 +210,29 @@ int main(void)
             &toi0.normal, &toi0.contact
         );
 
+        check_collision_and_move(&A, &toi0);
+
 
         A.pos = Vector2Add(
             A.pos , Vector2Scale(A.vel, frame_time)
         );
         A.center = A.pos;
 
+        char buf[32];
         BeginDrawing();
             ClearBackground(RAYWHITE);
             DrawShape(&B);
-            if (toi0.toi <= 1 && toi0.toi > 0)
+            DrawShape(&C);
-            {
+            DrawShape(&D);
-                draw_toi_info(&toi0);
+            draw_toi_info(&toi0);
-            }
+            draw_toi_info(&toi1);
+            draw_toi_info(&toi2);
+            sprintf(buf, "TOI: %.3f, %.3f, %.3f", toi0.toi, toi1.toi, toi2.toi);
+            DrawText(buf, 16, 16, 12, BLACK);
+            sprintf(buf, "Velocity: %.3f, %.3f", A.vel.x, A.vel.y);
+            DrawText(buf, 16, 32, 12, BLACK);
+
+
             DrawShape(&A);
         EndDrawing();
     }