Nearly have good pill-lin collisions working, just need to get the actual equation right

2 files changed, 84 insertions, 14 deletions

diff --git a/frontend/2d/_collisions/pill_line/dist/output.wasm b/frontend/2d/_collisions/pill_line/dist/output.wasm
index 3b6a673..c671a7e 100755
--- a/frontend/2d/_collisions/pill_line/dist/output.wasm
+++ b/frontend/2d/_collisions/pill_line/dist/output.wasm
diff --git a/frontend/2d/_collisions/pill_line/main.cpp b/frontend/2d/_collisions/pill_line/main.cpp
index 76dc7d4..0476b98 100644
--- a/frontend/2d/_collisions/pill_line/main.cpp
+++ b/frontend/2d/_collisions/pill_line/main.cpp
@@ -19,6 +19,8 @@ struct Rigidbody {
     float32 rotationalVelocity  = 0.f;
     float32 rotation = 0.f;
     float32 mass = 1.f;
+    float32 cofOfRestition = 0.7f;
+    float32 momentOfInertia = 0.f;
 
     void reset() {
         force = { 0, 0 };
@@ -41,6 +43,10 @@ struct Rigidbody {
 
         rotation += (rotationalVelocity * deltaTimeSeconds);
     }
+
+    void setMomentOfInertia(float32 moi) {
+        momentOfInertia = moi;
+    }
 };
 
 struct Pill {
@@ -95,6 +101,9 @@ struct Pill {
         shape.load(vertices, numVertices, renderer);
         body.reset();
 
+        // https://byjus.com/jee/moment-of-inertia-of-ellipse/
+        body.momentOfInertia = (body.mass * (a * a + b * b)) / 4.f;
+
         a = width / 2.f;
         b = height / 2.f;
 
@@ -121,20 +130,30 @@ struct Pill {
 
 struct LineSegment {
     OrthographicShape shape;
+    Rigidbody body;
     Vector2 start;
     Vector2 end;
+    float32 length;
     Vector2 normal;
     OrthographicVertex vertices[2];
 
     void load(OrthographicRenderer* renderer, Vector4 color, Vector2 inStart, Vector2 inEnd) {
         start = inStart;
         end = inEnd;
+        length = (start - end).length();
         vertices[0].position = start;
         vertices[0].color = color;
         vertices[1].position = end;
         vertices[1].color = color;
         normal = (end - start).getPerp().normalize();
         shape.load(vertices, 2, renderer);
+
+        body.reset();
+        body.mass = 100000.f;
+        body.cofOfRestition = 1.f;
+        body.rotationalVelocity = 0;
+        body.velocity = Vector2();
+        body.momentOfInertia = body.mass * (length / 2.f);
     }
 
     void render(OrthographicRenderer* renderer) {
@@ -159,9 +178,10 @@ struct LineSegment {
 
 struct IntersectionResult {
     bool intersect = false;
-    Vector2 pointOfIntersection;
     Vector2 collisionNormal;
     Vector2 relativeVelocity;
+    Vector2 firstPointOfApplication;
+    Vector2 secondPointOfApplication;
 };
 
 EM_BOOL onPlayClicked(int eventType, const EmscriptenMouseEvent* mouseEvent, void* userData);
@@ -209,6 +229,34 @@ float32 areaOfTriangle(Vector2 a, Vector2 b, Vector2 c) {
 
 IntersectionResult getIntersection(Pill* pill, LineSegment* segment) {
     IntersectionResult result;
+    
+    /**
+
+    Formula for finding equation of pill:
+    1) Using the parametrized equation of a line:
+        x(t) = x1 + (x2 - x1) * t
+        y(t) = y1 + (y2 - y1) * t
+    2) Knowing the equation of a translated and rotated ellipse:
+
+        Define x and y as follows:
+
+        x = x_1 + l, where l is the translation in x
+        y = y_1 + h, where h is the translation in y
+
+        Therefore, the general equation of a rotated ellipse is:
+
+        (x * cos(theta) + y * sin(theta)) / (a * a)
+            + (y * cos(theta) - x * sin(theta)) / b * b = 1
+
+        ( + l + x * cos(theta) + y * sin(theta)) / (a * a)
+            + (y + h + y * cos(theta) + y * sin(theta)) / (a * a)
+
+    3) Plug in the parametrized equation of our line for x_1 and y_2.
+
+    4) If the determinant >= 0, we have an intersection, otherwise, nothing.
+
+    **/
+    
 
     /*float32 rotationAngleOfPill = pill->body.rotation;
     Vector2 translationOfPill = pill->body.position;
@@ -249,20 +297,37 @@ IntersectionResult getIntersection(Pill* pill, LineSegment* segment) {
     float32 t1 = -B + determinant / (2 * A);
     float32 t2 = -B - determinant / (2 * A);
 
-    Vector2 pointAtT1 = { segment->start.x + (t1 * (segment->end.x - segment->start.x)), segment->start.y + (t1 * (segment->end.y - segment->start.y)) };
+    Vector2 pointAtT1 = pill->shape.model * Vector2 { 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.relativeVelocity = pill->body.velocity - segment->body.velocity;;
     result.collisionNormal = (pointAtT1 - pill->body.position).normalize();
+    result.firstPointOfApplication = pointAtT1 - pill->body.position;
+    result.secondPointOfApplication = pointAtT1 - ((segment->end - segment->start) / 2.f);
 
     return result;
 }
 
 void resolveCollision(Rigidbody* first, Rigidbody* second, IntersectionResult* ir) {
-    
+    Vector2 relativeVelocity = ir->relativeVelocity;
+    Vector2 collisionNormal  = ir->collisionNormal;
+    Vector2 firstPerp        = ir->firstPointOfApplication.getPerp();
+    Vector2 secondPerp       = ir->secondPointOfApplication.getPerp();
+
+    float32 cofOfRestition = (first->cofOfRestition + second->cofOfRestition) / 2.f;
+    float32 lNumerator = (relativeVelocity * -(1.0 + cofOfRestition)).dot(collisionNormal);
+    float32 lLinearDenomPart = collisionNormal.dot(collisionNormal * (1 / first->mass + 1 / second->mass));
+    float32 lRotationalDenomPart = powf(firstPerp.dot(collisionNormal), 2) / first->momentOfInertia
+        + powf(secondPerp.dot(collisionNormal), 2) / second->momentOfInertia;
+
+    float32 lImpulseMagnitude = lNumerator / (lLinearDenomPart);// + lRotationalDenomPart);
+
+    first->velocity = first->velocity + (collisionNormal * (lImpulseMagnitude / first->mass));
+    second->velocity = second->velocity - (collisionNormal * (lImpulseMagnitude / second->mass));
+
+    //first->rotationalVelocity = first->rotationalVelocity + firstPerp.dot(collisionNormal * lImpulseMagnitude) / first->momentOfInertia;
+    //second->rotationalVelocity = second->rotationalVelocity - secondPerp.dot(collisionNormal * lImpulseMagnitude) / second->momentOfInertia;
 }
 
 void update(float32 deltaTimeSeconds, void* userData) {
@@ -280,20 +345,25 @@ void update(float32 deltaTimeSeconds, void* userData) {
         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);
+            IntersectionResult subIr = ir;
+            float32 subdividedTimeSeconds = deltaTimeSeconds;
+            do {
+                ir = subIr;
 
-                ir = getIntersection(&pill, &segmentList[lineIdx]);
+                pill = copyPill.copy();
+                subdividedTimeSeconds /= 2.f;
+                pill.update(subdividedTimeSeconds);
 
-                if (deltaTimeSeconds <= 0.f) {
+                subIr = getIntersection(&pill, &segmentList[lineIdx]);
+                if (subdividedTimeSeconds == 0.f) {
                     printf("Error: Should not be happening.\n");
                     break;
                 }
-            }
+            } while (subIr.intersect);
 
-            printf("Found intersection at timestamp: %f\n", deltaTimeSeconds);
+            printf("Found intersection at timestamp: %f\n", subdividedTimeSeconds);
+            resolveCollision(&pill.body, &segmentList[lineIdx].body, &ir);
+            pill.update(deltaTimeSeconds - subdividedTimeSeconds);
         }
     }