Properly detecting collisions with a pill as far as I can tell

1 files changed, 73 insertions, 32 deletions

diff --git a/frontend/2d/_collisions/pill_line/main.cpp b/frontend/2d/_collisions/pill_line/main.cpp
index 9123b50..5aa0059 100644
--- a/frontend/2d/_collisions/pill_line/main.cpp
+++ b/frontend/2d/_collisions/pill_line/main.cpp
@@ -29,7 +29,7 @@ struct Rigidbody {
     }
 
     void applyGravity() {
-        force += Vector2 { 0.f, -25.f };
+        force += Vector2 { 0.f, -100.f };
     }
 
     void update(float32 deltaTimeSeconds) {
@@ -46,7 +46,16 @@ struct Pill {
     OrthographicShape shape;
     Rigidbody body;
     float32 a = 0;
-    float32 b = 0;;
+    float32 b = 0;
+
+    Pill copy() {
+        Pill retval;
+        retval.shape = shape;
+        retval.body = body;
+        retval.a = a;
+        retval.b = b;
+        return retval;
+    }
 
     void load(OrthographicRenderer* renderer, float32 numSegments, float32 width, float32 height) {
         // Note that a so-called "pill" is simply an ellipse.
@@ -114,15 +123,11 @@ struct LineSegment {
     Vector2 start;
     Vector2 end;
     Vector2 normal;
-    float32 slope;
-    float32 yIntercept;
     OrthographicVertex vertices[2];
 
     void load(OrthographicRenderer* renderer, Vector4 color, Vector2 inStart, Vector2 inEnd) {
         start = inStart;
         end = inEnd;
-        slope = (end.y - start.y) / (end.x - start.x);
-        yIntercept = (end.y - slope * end.x);
         vertices[0].position = start;
         vertices[0].color = color;
         vertices[1].position = end;
@@ -203,51 +208,87 @@ float32 areaOfTriangle(Vector2 a, Vector2 b, Vector2 c) {
 IntersectionResult getIntersection(Pill* pill, LineSegment* segment) {
     IntersectionResult result;
 
-    // Solve quadratic equation to see if the imaginary line defined by our line segment intersects the ellipse.
-    // Equation: x^2 / a^2 + y^2 / b^2 = 1
-    // (x^2 / a^2) + (line equation) ^2 / b^2 = 1
-    // => x^2 / (pill->a * pill->a) + (segment->slope * x + segment->yIntercept) / (pill->b * pill ->b) = 1.f;
+    /*float32 rotationAngleOfPill = pill->body.rotation;
+    Vector2 translationOfPill = pill->body.position;
     
-
-    // Build a triangle defined by the following to vectors:
-    Vector2 startToCenter = pill->body.position - segment->start;
-    Vector2 startToEnd = segment->end - segment->start;
-    Vector2 endToCenter = pill->body.position - segment->end;
-
-    // Get the area of this triangle using the properties of the determinant:
-    float32 area = areaOfTriangle(startToCenter, startToEnd, endToCenter);
-    float32 base = startToEnd.length();
-
-    // Knowning that Area = 0.5 Base * Height
-    float32 height = (2.f * area) / base;     // Distance from center of pill to line
-
-    if (height <= MAX(pill->b, pill->a) && height >= MIN(pill->b, pill->a)) {
-        // We at least have an intersection: Half the problem solved!
-        result.intersect = true;
-    } else {
+    float32 cosRotation = cosf(rotationAngleOfPill);
+    float32 sinRotation = sinf(rotationAngleOfPill);
+    float32 cosRotationSquared = cosRotation * cosRotation;
+    float32 sinRotationSquared = sinRotation * sinRotation;
+    float32 aSquared = pill->a * pill->a;
+    float32 bSquared = pill->b * pill->b;
+
+    float32 x = (segment->end.x - segment->start.x) - translationOfPill.x;
+    float32 xSquared = x * x;
+    float32 y = (segment->end.y - segment->start.y) - translationOfPill.y;
+    float32 ySquared = y * y;
+
+    float32 A_XPart = (segment->end.x - segment->start.x - translationOfPill.x) * cosRotationSquared;
+    float32 A_YPart = (segment->end.y - segment->start.y - translationOfPill.y) * sinRotationSquared;
+    float32 A = ((A_XPart * A_XPart) / aSquared) + ((A_YPart * A_YPart) / bSquared);
+
+    float32 B_XPart = 2 * (segment->start.x - translationOfPill.x) * (segment->end.x - segment->start.x - translationOfPill.x);
+    float32 B = 2 * cosRotation * sinRotation * (1.f / aSquared - 1.f / bSquared) * (x * y);
+    float32 C = (sinRotationSquared / aSquared + cosRotationSquared / bSquared) * ySquared;*/
+
+    Mat4x4 inverseModel = pill->shape.model.inverse();
+    Vector2 start = inverseModel * segment->start;
+    Vector2 end = inverseModel * segment->end;
+    float32 A = pow((end.x - start.x), 2.f) / pow(pill->a, 2) + pow((end.y - start.y), 2.f) / pow(pill->b, 2);
+    float32 B = ((2 * start.x) * (end.x - start.x)) / pow(pill->a, 2) + ((2 * start.y) * (end.y - start.y)) / pow(pill->b, 2);
+    float32 C = pow(start.x, 2) / pow(pill->a, 2) + pow(start.y, 2) / pow(pill->b, 2) - 1.f;
+
+    float32 determinant = B * B - 4 * A * C;
+    if (determinant < 0.f) {    
         result.intersect = false;
         return result;
     }
 
-    // Time to find where we intersected. We can do this by getting the intersection depth.
+    float32 t1 = -B + determinant / (2 * A);
+    float32 t2 = -B - determinant / (2 * A);
 
-    
-    Vector2 transformedX = pill->shape.model.multByVec2(Vector2 { pill->a / 2.f, 0.f });
-    Vector2 transformedY = pill->shape.model.multByVec2(Vector2 { pill->b / 2.f, 0.f });
+    Vector2 pointAtT1 = { segment->start.x + (t1 * (segment->end.x - segment->start.x)), segment->start.y + (t1 * (segment->end.y - segment->start.y)) };
+    Vector2 pointAtT2 = { segment->start.x + (t2 * (segment->end.x - segment->start.x)), segment->start.y + (t2 * (segment->end.y - segment->start.y)) };
+
+    printf("Intersecting\n");
+    result.intersect = true;
+    result.pointOfIntersection = pointAtT1 - pill->body.position;;
+    result.relativeVelocity = Vector2() - pill->body.velocity;
+    result.collisionNormal = (pointAtT1 - pill->body.position).normalize();
 
     return result;
 }
 
 void update(float32 deltaTimeSeconds, void* userData) {
+
     // Input
     pill.body.applyGravity();
 
     // Update
+    Pill copyPill = pill.copy();
     pill.update(deltaTimeSeconds);
 
     // Intersections
     for (int32 lineIdx = 0; lineIdx < 4; lineIdx++) {
-        getIntersection(&pill, &segmentList[lineIdx]);
+        IntersectionResult ir = getIntersection(&pill, &segmentList[lineIdx]);
+        if (ir.intersect) {
+
+            // Find the exact moment that the intersection happens by rewinding the simulation until we're not intersecting
+            while (ir.intersect) {
+                pill = copyPill.copy();
+                deltaTimeSeconds /= 2.f;
+                pill.update(deltaTimeSeconds);
+
+                ir = getIntersection(&pill, &segmentList[lineIdx]);
+
+                if (deltaTimeSeconds <= 0.f) {
+                    printf("Error: Should not be happening.\n");
+                    break;
+                }
+            }
+
+            printf("Found intersection at timestamp: %f\n", deltaTimeSeconds);
+        }
     }
 
     // Render