Basics of the pill-pill simulation

2 files changed, 360 insertions, 4 deletions

diff --git a/frontend/2d/_collisions/pill_line/#main.cpp# b/frontend/2d/_collisions/pill_line/#main.cpp#
new file mode 100644
index 0000000..a3ede40
--- /dev/null
+++ b/frontend/2d/_collisions/pill_line/#main.cpp#
@@ -0,0 +1,360 @@
+#include "../../../shared_cpp/OrthographicRenderer.h"
+#include "../../../shared_cpp/types.h"
+#include "../../../shared_cpp/WebglContext.h"
+#include "../../../shared_cpp/mathlib.h"
+#include "../../../shared_cpp/MainLoop.h"
+#include <cstdio>
+#include <cmath>
+#include <emscripten/html5.h>
+#include <unistd.h>
+#include <pthread.h>
+#include <cmath>
+
+// Side note: It is Eastertime, so I chose this easter color palette. Enjoy: https://htmlcolors.com/palette/144/easter
+
+struct Rigidbody {
+    Vector2 force = { 0, 0 };
+    Vector2 velocity = { 0, 0 };
+    Vector2 position = { 0, 0 };
+    float32 rotationalVelocity  = 0.f;
+    float32 rotation = 0.f;
+    float32 mass = 1.f;
+    float32 cofOfRestition = 1.f;
+    float32 momentOfInertia = 0.f;
+
+    void reset() {
+        force = { 0, 0 };
+        velocity = { 0, 0 };
+        rotationalVelocity = 0.f;
+        rotation = 0.f;
+    }
+
+    void applyForce(Vector2 f) {
+        force += f;
+    }
+
+    void applyGravity() {
+        force += Vector2 { 0.f, -100.f };
+    }
+
+    void update(float32 deltaTimeSeconds) {
+        Vector2 acceleration = force / mass;
+        velocity += (acceleration * deltaTimeSeconds);
+        position += (velocity * deltaTimeSeconds);
+        force = Vector2 { 0.f, 0.f };
+
+        rotation += (rotationalVelocity * deltaTimeSeconds);
+    }
+
+    void setMomentOfInertia(float32 moi) {
+        momentOfInertia = moi;
+    }
+};
+
+struct Pill {
+    OrthographicShape shape;
+    Rigidbody body;
+    float32 a = 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.
+        // Equation of an ellipse is:
+        //
+        //	x^2 / a^2 + y^2 / b^2 = 1
+        //
+        // or, in parametric form:
+        //
+        // x = a * cos(t), y = b * sin(t)
+        //
+        float32 angleIncrements = (2.f * PI) / numSegments;
+        uint32 numVertices = static_cast<uint32>(numSegments * 3.f);
+        OrthographicVertex* vertices = new OrthographicVertex[numVertices];
+
+        a = width / 2.f;
+        b = height / 2.f;
+
+        Vector4 color = Vector4().fromColor(243,166,207, 255);
+
+        for (uint32 vertexIndex = 0; vertexIndex < numVertices; vertexIndex += 3) {
+            // Create a single "slice" of the ellipse (like a pizza)
+            float32 currAngle = (vertexIndex / 3.f) * angleIncrements;
+            float32 nextAngle = (vertexIndex / 3.f + 1.f) * angleIncrements;
+
+            vertices[vertexIndex].position = Vector2 { 0.f, 0.f };
+            vertices[vertexIndex].color = color;
+
+            vertices[vertexIndex + 1].position = Vector2 { a * cosf(currAngle), b * sinf(currAngle) };
+            vertices[vertexIndex + 1].color = color;
+
+            vertices[vertexIndex + 2].position = Vector2 { a * cosf(nextAngle), b * sinf(nextAngle) };
+            vertices[vertexIndex + 2].color = color;
+        }
+
+        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;
+
+        delete[] vertices;
+    }
+
+    void update(float32 deltaTimeSeconds) {
+        body.update(deltaTimeSeconds);
+        shape.model = Mat4x4().translateByVec2(body.position).rotate2D(body.rotation);
+    }
+
+    void render(OrthographicRenderer* renderer) {
+        shape.render(renderer);
+    }
+
+    void unload() {
+        shape.unload();
+    }
+
+    float32 getArea() {
+        return 0.f;
+    }
+};
+
+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 = 1000000000.f;
+        body.cofOfRestition = 1.f;
+        body.rotationalVelocity = 0;
+        body.velocity = Vector2();
+        body.momentOfInertia = body.mass * (length / 2.f);
+    }
+
+    void render(OrthographicRenderer* renderer) {
+        shape.render(renderer, GL_LINES);
+    }
+
+    void unload() {
+        shape.unload();
+    }
+
+    Vector2 getPointOnLine(float32 t) {
+        return {
+            start.x + (end.x - start.x) * t,
+            start.y + (end.y - start.y) * t,
+        };
+    }
+
+    Vector2 getNormal() {
+        return (end - start).getPerp().normalize();
+    }
+};
+
+struct IntersectionResult {
+    bool intersect = false;
+    Vector2 collisionNormal;
+    Vector2 relativeVelocity;
+    Vector2 firstPointOfApplication;
+    Vector2 secondPointOfApplication;
+};
+
+EM_BOOL onPlayClicked(int eventType, const EmscriptenMouseEvent* mouseEvent, void* userData);
+EM_BOOL onStopClicked(int eventType, const EmscriptenMouseEvent* mouseEvent, void* userData);
+
+void load();
+void update(float32 time, void* userData);
+void unload();
+IntersectionResult getIntersection(Pill* pill, LineSegment* segment);
+void resolveCollision(Rigidbody* first, Rigidbody* second, IntersectionResult* ir);
+
+// Global Variables
+WebglContext context;
+OrthographicRenderer renderer;
+Pill pill;
+MainLoop mainLoop;
+LineSegment segmentList[4];
+
+int main() {
+    context.init("#gl_canvas");
+    emscripten_set_click_callback("#gl_canvas_play", NULL, false, onPlayClicked);
+    emscripten_set_click_callback("#gl_canvas_stop", NULL, false, onStopClicked);
+    return 0;
+}
+
+void load() {
+    renderer.load(&context);
+
+    pill.body.position = Vector2 { context.width / 2.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) });
+    segmentList[2].load(&renderer, Vector4().fromColor(248, 255, 156, 255.f), Vector2 { 50.f, 50.f }, Vector2 { context.width - 50.f, 150.f });
+    segmentList[3].load(&renderer, Vector4().fromColor(205, 178, 214, 255.f), Vector2 { 50.f, 150.f }, Vector2 { context.width - 50.f, 50.f });
+
+    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();
+    Vector2 start = inverseModel * segment->start;
+    Vector2 end = inverseModel * segment->end;
+    Vector2 diff = end - start;
+    float32 A = (diff.x * diff.x) / (pill->a * pill->a) + (diff.y * diff.y) / (pill->b * pill->b);
+    float32 B = ((2 * start.x) * (end.x - start.x)) / (pill->a * pill->a) + ((2 * start.y) * (end.y - start.y)) / (pill->b * pill->b);
+    float32 C = (start.x * start.x) / (pill->a * pill->a) + (start.y * start.y) / (pill->b * pill->b) - 1.f;
+
+    float32 determinant = B * B - 4 * A * C;
+    if (determinant < 0.f) {    
+        result.intersect = false;
+        return result;
+    }
+
+    float32 t1 = (-B + sqrtf(determinant)) / (2 * A);
+    float32 t2 = (-B - sqrtf(determinant)) / (2 * A);
+
+    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
+
+    //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();
+    // Or, simply: Vector2 normal = { pointOnEllipse.x * (pill->b / pill->a), pointOnEllipse.y * (pill->a / pill->b) };
+    Vector2 normal = segment->getNormal();
+
+    result.intersect = true;
+    result.relativeVelocity = pill->body.velocity - segment->body.velocity;;
+    result.collisionNormal = normal;
+    result.firstPointOfApplication = pointOnEllipse;
+    result.secondPointOfApplication = pointOnLine;
+
+    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.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;
+
+    float32 lImpulseMagnitude = lNumerator / (lLinearDenomPart + lRotationalDenomPart);
+
+    first->velocity = first->velocity + (collisionNormal * (lImpulseMagnitude / first->mass));
+    second->velocity = second->velocity - (collisionNormal * (lImpulseMagnitude / second->mass));
+
+    printf("%f\n", firstPerp.dot(collisionNormal * lImpulseMagnitude) / first->momentOfInertia);
+    
+    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);
+
+    // Intersections
+    for (int32 lineIdx = 0; lineIdx < 4; 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
+            IntersectionResult subIr = ir;
+            float32 subdividedTimeSeconds = deltaTimeSeconds;
+            do {
+                ir = subIr;
+
+                pill = copyPill.copy();
+                subdividedTimeSeconds /= 2.f;
+                pill.update(subdividedTimeSeconds);
+
+                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", subdividedTimeSeconds);
+            resolveCollision(&pill.body, &segmentList[lineIdx].body, &ir);
+            pill.update(deltaTimeSeconds - subdividedTimeSeconds);
+        }
+    }
+
+    // Render
+    renderer.render();
+    pill.shape.render(&renderer);
+
+    for (int32 segmentIndex = 0; segmentIndex < 4; segmentIndex++) {
+        segmentList[segmentIndex].render(&renderer);
+    }
+}
+
+void unload() {
+    mainLoop.stop();
+    pill.unload();
+    renderer.unload();
+    for (int32 segmentIndex = 0; segmentIndex < 4; segmentIndex++) {
+        segmentList[segmentIndex].unload();
+    }
+}
+
+//
+// Interactions with DOM handled below
+//
+EM_BOOL onPlayClicked(int eventType, const EmscriptenMouseEvent* mouseEvent, void* userData) {
+    printf("Play clicked\n");
+    
+    load();
+    return true;
+}
+
+EM_BOOL onStopClicked(int eventType, const EmscriptenMouseEvent* mouseEvent, void* userData) {
+    printf("Stop clicked\n");
+    unload();
+    return true;
+}
diff --git a/frontend/2d/_collisions/pill_line/main.cpp b/frontend/2d/_collisions/pill_line/main.cpp
index 9f585f1..b9315a8 100644
--- a/frontend/2d/_collisions/pill_line/main.cpp
+++ b/frontend/2d/_collisions/pill_line/main.cpp
@@ -126,10 +126,6 @@ struct Pill {
     void unload() {
         shape.unload();
     }
-
-    float32 getArea() {
-        return 0.f;
-    }
 };
 
 struct LineSegment {