1 files changed, 40 insertions, 33 deletions

diff --git a/frontend/2d/_collisions/pill_line/main.cpp b/frontend/2d/_collisions/pill_line/main.cpp
index 8700649..9f585f1 100644
--- a/frontend/2d/_collisions/pill_line/main.cpp
+++ b/frontend/2d/_collisions/pill_line/main.cpp
@@ -33,11 +33,13 @@ struct Rigidbody {
         force += f;
     }
 
-    void applyGravity() {
-        force += Vector2 { 0.f, -100.f };
+    void applyGravity(float32 deltaTimeSeconds) {
+        velocity += (Vector2 { 0.f, -96.f } * deltaTimeSeconds);
     }
 
     void update(float32 deltaTimeSeconds) {
+        applyGravity(deltaTimeSeconds);
+        
         Vector2 acceleration = force / mass;
         velocity += (acceleration * deltaTimeSeconds);
         position += (velocity * deltaTimeSeconds);
@@ -103,7 +105,7 @@ struct Pill {
         shape.load(vertices, numVertices, renderer);
         body.reset();
 
-        // https://byjus.com/jee/moment-of-inertia-of-ellipse/
+        body.mass = 100.f;
         body.momentOfInertia = (body.mass * (a * a + b * b)) / 4.f;
 
         a = width / 2.f;
@@ -151,7 +153,7 @@ struct LineSegment {
         shape.load(vertices, 2, renderer);
 
         body.reset();
-        body.mass = 100000.f;
+        body.mass = 1000000000.f;
         body.cofOfRestition = 1.f;
         body.rotationalVelocity = 0;
         body.velocity = Vector2();
@@ -172,6 +174,10 @@ struct LineSegment {
             start.y + (end.y - start.y) * t,
         };
     }
+
+    Vector2 getNormal() {
+        return (end - start).getPerp().normalize();
+    }
 };
 
 struct IntersectionResult {
@@ -208,9 +214,8 @@ int main() {
 void load() {
     renderer.load(&context);
 
-    pill.body.position = Vector2 { context.width / 2.f, context.height / 2.f };
+    pill.body.position = Vector2 { context.width / 4.f, context.height / 2.f };
     pill.load(&renderer, 64, 100.f, 50.f);
-	pill.body.rotationalVelocity = 0.3f;
 
     segmentList[0].load(&renderer, Vector4().fromColor(191, 251, 146, 255.f), Vector2 { 50.f, 0.f }, Vector2 { 50.f, static_cast<float>(context.height) });
     segmentList[1].load(&renderer, Vector4().fromColor(159, 224, 210, 255.f), Vector2 { context.width - 50.f, 0.f }, Vector2 { context.width - 50.f, static_cast<float>(context.height) });
@@ -220,11 +225,6 @@ void load() {
     mainLoop.run(update);
 }
 
-float32 areaOfTriangle(Vector2 a, Vector2 b, Vector2 c) {
-    // Refernce for this for the formula: https://www.onlinemath4all.com/area-of-triangle-using-determinant-formula.html
-    return ABS(0.5 * (a.x * b.y - b.x * a.y + b.x * c.y - c.x * b.y + c.x * a.y - a.x * c.y));
-}
-
 IntersectionResult getIntersection(Pill* pill, LineSegment* segment) {
     IntersectionResult result;
     Mat4x4 inverseModel = pill->shape.model.inverse();
@@ -241,22 +241,28 @@ IntersectionResult getIntersection(Pill* pill, LineSegment* segment) {
         return result;
     }
 
-    float32 t1 = (-B + sqrtf(determinant)) / (2 * A);
-    float32 t2 = (-B - sqrtf(determinant)) / (2 * A);
+    float32 t;
+    if (determinant < 0.001f) {
+        t = -B / (2.f * A);
+    } else {
+        t = (-B + sqrtf(determinant)) / (2.f * A);
+        // Or: t = (-B - sqrtf(determinant)) / (2 * A);
+    }
+
+    Vector2 pointOnLine = segment->getPointOnLine(t); // This point is in world space and line space, since the line is in world space
 
-    Vector2 pointOnLine = segment->getPointOnLine(t1); // This point is in world space and line space, since the line is in world space
-    Vector2 pointOnEllipse = pointOnLine - pill->body.position; // This point is in ellipse space
-    pointOnEllipse.printDebug("Point on ellipse");
+    Vector2 pointOnTransformedLine = {
+        start.x + (end.x - start.x) * t,
+        start.y + (end.y - start.y) * t
+    };
 
-    float32 parametricTEllipse = atan2f( pointOnEllipse.y / pill->b , pointOnEllipse.x / pill->a );
-    Vector2 tangent = { -pill->a * sinf(parametricTEllipse), pill->b * cosf(parametricTEllipse) };
-    Vector2 normal = tangent.getPerp();
-	normal.printDebug("Normal");
-    // or: Vector2 normal = { pointOnEllipse.x * (pill->b / pill->a), pointOnEllipse.y * (pill->a / pill->b) };
+    Vector2 pointOnEllipse = (pointOnTransformedLine);
+
+    Vector2 normal = segment->getNormal();
 
     result.intersect = true;
-    result.relativeVelocity = pill->body.velocity - segment->body.velocity;;
-    result.collisionNormal = normal.normalize();
+    result.relativeVelocity = pill->body.velocity - segment->body.velocity;
+    result.collisionNormal = normal;
     result.firstPointOfApplication = pointOnEllipse;
     result.secondPointOfApplication = pointOnLine;
 
@@ -270,24 +276,24 @@ void resolveCollision(Rigidbody* first, Rigidbody* second, IntersectionResult* i
     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 lNumerator = (relativeVelocity * -(1.f + cofOfRestition)).dot(collisionNormal);
+    float32 lLinearDenomPart = collisionNormal.dot(collisionNormal * (1.f / first->mass + 1.f / second->mass));
+    float32 lRotationalDenomPart = powf(firstPerp.dot(collisionNormal), 2.f) / first->momentOfInertia + powf(secondPerp.dot(collisionNormal), 2) / second->momentOfInertia;
+
+    //ir->firstPointOfApplication.printDebug("PoA");
+    //firstPerp.printDebug("PoA Perp");
+    //collisionNormal.printDebug("Normal");
+    //printf("Dot: %f\n", firstPerp.dot(collisionNormal));
 
     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) {
-
-    // Input
-    pill.body.applyGravity();
-
     // Update
     Pill copyPill = pill.copy();
     pill.update(deltaTimeSeconds);
@@ -308,7 +314,7 @@ void update(float32 deltaTimeSeconds, void* userData) {
                 pill.update(subdividedTimeSeconds);
 
                 subIr = getIntersection(&pill, &segmentList[lineIdx]);
-                if (subdividedTimeSeconds == 0.f) {
+                if (subdividedTimeSeconds <= 0.f) {
                     printf("Error: Should not be happening.\n");
                     break;
                 }
@@ -316,7 +322,8 @@ void update(float32 deltaTimeSeconds, void* userData) {
 
             printf("Found intersection at timestamp: %f\n", subdividedTimeSeconds);
             resolveCollision(&pill.body, &segmentList[lineIdx].body, &ir);
-            pill.update(deltaTimeSeconds - subdividedTimeSeconds);
+            deltaTimeSeconds = deltaTimeSeconds - subdividedTimeSeconds;
+            pill.update(deltaTimeSeconds);
         }
     }