path: root/2d/_collisions/pill_line/main.cpp

#include "../../../shared_cpp/Renderer2d.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(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);
        force = Vector2 { 0.f, 0.f };

        rotation += (rotationalVelocity * deltaTimeSeconds);
    }

    void setMomentOfInertia(float32 moi) {
        momentOfInertia = moi;
    }
};

struct Pill {
    Mesh2d 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(Renderer2d* 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);
        Vertex2d* vertices = new Vertex2d[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();

        body.mass = 100.f;
        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(Renderer2d* renderer) {
        shape.render(renderer);
    }

    void unload() {
        shape.unload();
    }
};

struct LineSegment {
    Mesh2d shape;
    Rigidbody body;
    Vector2 start;
    Vector2 end;
    float32 length;
    Vector2 normal;
    Vertex2d vertices[2];

    void load(Renderer2d* 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(Renderer2d* 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;
Renderer2d 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 / 4.f, context.height / 2.f };
    pill.load(&renderer, 64, 100.f, 50.f);

    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);
}

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 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 pointOnTransformedLine = {
        start.x + (end.x - start.x) * t,
        start.y + (end.y - start.y) * t
    };

    Vector2 pointOnEllipse = (pointOnTransformedLine);

    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;

    //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) {
    // 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);
            deltaTimeSeconds = deltaTimeSeconds - subdividedTimeSeconds;
            pill.update(deltaTimeSeconds);
        }
    }

    // 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;
}