path: root/3d/rigidbody/main.cpp

#include "../../shared_cpp/Renderer3d.h"
#include "../../shared_cpp/RenderShared.h"
#include "../../shared_cpp/Camera3d.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>
#include <cfloat>

// -- Rigidbody updates
struct Impulse {
    Vector3 force = { 0, 0, 0 };
    Vector3 pointOfApplication = { 0, 0, 0 };
    float32 timeOfApplicationSeconds = 0.016f;
    float32 timeAppliedSeconds = 0.f;
    bool isDead = false;
};

const int32 MAX_IMPULSES = 4;

struct Rigidbody3d {
    int32 numImpulses = 0;
    Impulse activeImpulses[MAX_IMPULSES];
    Vector3 velocity;
    Vector3 position;
    float32 mass = 1.f;

    Vector3 rotationalVelocity;
    Quaternion rotation;
    float32 momentOfInertia = 1.f;
    float32 cofOfRestitution = 1.f;

    void reset() {
        numImpulses = 0;
        velocity = Vector3();
        rotationalVelocity = Vector3();
        position = Vector3();
        rotation = Quaternion();
    }

    void applyImpulse(Impulse i) {
        if (numImpulses > MAX_IMPULSES) {
            printf("Unable to apply impulse. Buffer full.\n");
            return;
        }

        activeImpulses[numImpulses] = i;
        numImpulses++;
    }

    void update(float32 deltaTimeSeconds) {
        // Apply gravity
        velocity += (Vector3 { 0.f, -9.8f, 0.f } * deltaTimeSeconds);

        Vector3 force = { 0.f, 0.f, 0.f };
        Vector3 torque = { 0.f, 0.f, 0.f };
        for (int32 idx = 0; idx < numImpulses; idx++) {
            Impulse& i = activeImpulses[idx];

            float32 nextTimeAppliedSeconds = i.timeAppliedSeconds + deltaTimeSeconds;
            if (nextTimeAppliedSeconds >= i.timeOfApplicationSeconds) {
                nextTimeAppliedSeconds = i.timeOfApplicationSeconds;
                i.isDead = true;
            }
            
            float32 impulseDtSeconds = nextTimeAppliedSeconds - i.timeAppliedSeconds;
            Vector3 forceToApply = i.force * (impulseDtSeconds / i.timeOfApplicationSeconds);
            force += forceToApply;

            torque += i.pointOfApplication.cross(force).dot(forceToApply);
            i.timeAppliedSeconds = nextTimeAppliedSeconds;
        }
        
        Vector3 acceleration = force / mass;
        velocity += (acceleration * deltaTimeSeconds);
        position += (velocity * deltaTimeSeconds);

	    Vector3 rotationalAcceleration = torque / momentOfInertia;
		rotationalVelocity += (rotationalAcceleration * deltaTimeSeconds);

        Vector3 drDt = rotationalVelocity * deltaTimeSeconds;

        Quaternion rotationVelocityQuat = quaternionFromRotation(Vector3(1, 0, 0), drDt.x)
            * quaternionFromRotation(Vector3(0, 1, 0), drDt.y)
            * quaternionFromRotation(Vector3(0, 0, 1), drDt.z);
		rotation = rotation * rotationVelocityQuat;

        for (int32 idx = 0; idx < numImpulses; idx++) {
            if (activeImpulses[idx].isDead) {
                for (int j = idx + 1; j < numImpulses; j++) {
                    activeImpulses[j - 1] = activeImpulses[j];
                }

                idx = idx - 1;
                numImpulses--;
            }
        }
    }
};

// -- Sphere definition
float32 bounds = 50.f;

struct Sphere {
	Mesh3d mesh;
    Rigidbody3d previousBody;
    Rigidbody3d body;
    float32 radius = 1.f;

	void load(Renderer3d* renderer) {
        const float32 angleIncrements = 2.f;
        const int32 numFaces = static_cast<int32>((180.f / angleIncrements + 1) * (360.f / angleIncrements + 1));
        const int32 numVertices = 4.f * numFaces;
        const int32 numIndices = 6 * numFaces;
        Vertex3d* vertices = new Vertex3d[numVertices];
        GLuint* indices = new GLuint[numIndices];

        radius = body.mass; // Set the radius to be proportional to the mass

        // Generate vertices and indices
        GLint index = 0;
        int32 vidx = 0;
        int32 iidx = 0;
        for (float phi = 0.0; phi <= 180; phi += angleIncrements) {
            const auto cosPhi = cos(DEG_TO_RAD(phi));
            const auto sinPhi = sin(DEG_TO_RAD(phi));

            const auto nextCosPhi = cos(DEG_TO_RAD(phi + angleIncrements));
            const auto nextSinPhi = sin(DEG_TO_RAD(phi + angleIncrements));

            for (float theta = 0.0; theta <= 360; theta += angleIncrements) {
                auto color = colorFromHex(randomFloatBetween(0.f, 255.f), randomFloatBetween(0.f, 255.f), randomFloatBetween(0.f, 255.f), 255.f);
                const auto cosTheta = cos(DEG_TO_RAD(theta));
                const auto sinTheta = sin(DEG_TO_RAD(theta));

                const auto nextSinTheta = sin(DEG_TO_RAD(theta + angleIncrements));
                const auto nextCosTheta =  cos(DEG_TO_RAD(theta + angleIncrements));

                // Top Left Point
                auto topLeftPoint = Vector3(radius * sinPhi * cosTheta, radius * sinPhi * sinTheta, radius * cosPhi);
                auto topLeftIdx = index++;
                vertices[vidx++] = { topLeftPoint, topLeftPoint.normalize(), color };

                // Bottom Left Point
                auto bottomLeftPoint = Vector3(radius * nextSinPhi * cosTheta, radius * nextSinPhi * sinTheta, radius * nextCosPhi);
                auto bottomLeftIdx = index++;
                vertices[vidx++] = { bottomLeftPoint, bottomLeftPoint.normalize(), color };

                // Bottom Right Point
                auto bottomRightPoint = Vector3(radius * nextSinPhi * nextCosTheta, radius * nextSinPhi * nextSinTheta, radius * nextCosPhi);
                auto bottomRightIdx = index++;
                vertices[vidx++] = { bottomRightPoint, bottomRightPoint.normalize(), color };

                // Top Right Point
                auto topRightPoint = Vector3(radius * sinPhi * nextCosTheta, radius * sinPhi * nextSinTheta, radius * cosPhi);
                auto topRightIdx = index++;
                vertices[vidx++] = { topRightPoint, topRightPoint.normalize(), color };

                indices[iidx++] = (topLeftIdx);
                indices[iidx++] = (bottomLeftIdx);
                indices[iidx++] = (bottomRightIdx);
                indices[iidx++] = (bottomRightIdx);
                indices[iidx++] = (topLeftIdx);
                indices[iidx++] = (topRightIdx);
            }
        }

		mesh.load(vertices, numVertices, indices, numIndices, renderer);

        body.momentOfInertia = (2.f / 5.f) * (body.mass * (radius * radius));

        delete [] vertices;
        delete [] indices;
	}

	void update(float32 dtSeconds) {
        previousBody = body;
        body.update(dtSeconds);

        Vector3 upper = body.position + Vector3(radius, radius, radius);
        Vector3 lower = body.position - Vector3(radius, radius, radius);

        // -- Constrain inside of the box
        if (upper.x > bounds) {
            body.position.x = bounds - radius;
            body.velocity.x = -body.velocity.x;
        }
        else if (lower.x < -bounds) {
            body.position.x = -bounds + radius;
            body.velocity.x = -body.velocity.x;
        }

        if (upper.y > bounds) {
            body.position.y = bounds - radius;
            body.velocity.y = -body.velocity.y;
        }
        else if (lower.y < -bounds) {
            body.position.y = -bounds + radius;
            body.velocity.y = -body.velocity.y;
        }

        if (upper.z > bounds) {
            body.position.z = bounds - radius;
            body.velocity.z = -body.velocity.z;
        }
        else if (lower.z < -bounds) {
            body.position.z = -bounds + radius;
            body.velocity.z = -body.velocity.z;
        }

        mesh.model = body.rotation.toMatrix().translate(body.position);

	}

	void render(Renderer3d* renderer) {
		mesh.render(renderer);
	}

	void unload() {
        body.reset();
		mesh.unload();
	}
};

struct Cube {
    Mesh3d mesh;

    void load(Renderer3d* renderer) {
        Vertex3d vertices[8];
        GLuint indices[] = {
            2, 6, 7, 2, 7, 3,
            6, 4, 5, 6, 5, 7,
            4, 0, 1, 4, 1, 5,
            0, 4, 6, 0, 6, 2,
            1, 5, 7, 1, 7, 3,
        };

        vertices[0].position = Vector3(-1.f, 1.f, 1.f);
        vertices[1].position = Vector3(-1.f, -1.f, 1.f);
        vertices[2].position = Vector3(1.f, 1.f, 1.f);
        vertices[3].position = Vector3(1.f, -1.f, 1.f);
        vertices[4].position = Vector3(-1.f, 1.f, -1.f);
        vertices[5].position = Vector3(-1.f, -1.f, -1.f);
        vertices[6].position = Vector3(1.f, 1.f, -1.f);
        vertices[7].position = Vector3(1.f, -1.f, -1.f);

        for (int idx = 0; idx < 8; idx++) {
            vertices[idx].normal = Vector3(0, 1, 0);
            vertices[idx].color = Vector4(randomFloatBetween(0, 1), randomFloatBetween(0, 1), randomFloatBetween(0, 1), 0.9f).normalize();
        }

        mesh.load(vertices, 8, indices, 30, renderer);
        float32 scale = bounds;
        mesh.model = Mat4x4().scale(Vector3(scale, scale, scale));//.translate(Vector3(0, 0, scale / 2.f));
    }

    void render(Renderer3d* renderer) {
        mesh.render(renderer);
    }

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

// -- Intersection info
struct IntersectionResult {
    bool intersect = false;
    Vector3 collisionNormal;
    Vector3 relativeVelocity;
    Vector3 firstPointOfApplication;
    Vector3 secondPointOfApplication;
};


EM_BOOL onPlayClicked(int eventType, const EmscriptenMouseEvent* mouseEvent, void* userData);
EM_BOOL onStopClicked(int eventType, const EmscriptenMouseEvent* mouseEvent, void* userData);
EM_BOOL onForceApplicationRequested(int eventType, const EmscriptenMouseEvent* mouseEvent, void* userData);

void load();
IntersectionResult getIntersection(Sphere* first, Sphere* second);
void resolveIntersection(Rigidbody3d* first, Rigidbody3d* second, IntersectionResult* ir);
void update(float32 time, void* userData);
void unload();

WebglContext context;
Renderer3d renderer;
Camera3d camera;
MainLoop mainLoop;
bool isIntersectingPointer = false;

const int32 numSpheres = 3;
Sphere sphereList[numSpheres];
Cube environmentCube;

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);
    emscripten_set_click_callback("#force_apply", NULL, false, onForceApplicationRequested);
    return 0;
}

void load() {
    renderer.load(&context);

    sphereList[0].body.mass = 10.f;
    sphereList[0].body.position = Vector3(10.f, 0, -5);
    sphereList[0].body.velocity = Vector3(-10.f, 10.f, 10.f);
    sphereList[0].body.rotationalVelocity = Vector3(1.f, 1.f, 1.f);
    sphereList[0].load(&renderer);

    sphereList[1].body.mass = 6.f;
    sphereList[1].body.position = Vector3(-10.f, 0, 5);
    sphereList[1].body.velocity = Vector3(10.f, -10.f, -10.f);
    sphereList[1].body.rotationalVelocity = Vector3(1.f, 1.f, 1.f);
    sphereList[1].load(&renderer);

    sphereList[2].body.mass = 3.f;
    sphereList[2].body.position = Vector3(-10.f, -10, 5);
    sphereList[2].body.velocity = Vector3(10.f, 0.f, -10.f);
    sphereList[2].body.rotationalVelocity = Vector3(1.f, 1.f, 1.f);
    sphereList[2].load(&renderer);

    environmentCube.load(&renderer);

	camera.projection  = Mat4x4().getPerspectiveProjection(0.1f, 10000.f, DEG_TO_RAD(45.f), 800.f / 600.f);
	camera.view = Mat4x4().translate({ 0, 0, -175.f });
    mainLoop.run(update);
}

IntersectionResult getIntersection(Sphere* first, Sphere* second) {
    IntersectionResult ir;
    auto distance = (first->body.position - second->body.position).length();
    if (distance <= first->radius + second->radius) {
        ir.intersect = true;
    }
    else {
        ir.intersect = false;
        return ir;
    }

    auto positionDiff = first->body.position - second->body.position;
    auto positionDirection = positionDiff.normalize();
    ir.relativeVelocity = first->body.velocity - second->body.velocity;
    ir.firstPointOfApplication = positionDirection * first->radius;
    ir.secondPointOfApplication = positionDirection * second->radius;
    ir.collisionNormal = (positionDirection + ir.relativeVelocity.negate().normalize()).normalize();

    return ir;
}

void resolveIntersection(Rigidbody3d* first, Rigidbody3d* second, IntersectionResult* ir) {
    Vector3 relativeVelocity = ir->relativeVelocity;
    Vector3 collisionNormal  = ir->collisionNormal;
    Vector3 firstPerp        = ir->firstPointOfApplication.cross(relativeVelocity);
    Vector3 secondPerp       = ir->secondPointOfApplication.cross(relativeVelocity);
	float32 firstPerpNorm    = firstPerp.dot(collisionNormal);
	float32 sndPerpNorm      = secondPerp.dot(collisionNormal);

    float32 cofOfRestitution = (first->cofOfRestitution + second->cofOfRestitution) / 2.f;
    float32 numerator = (relativeVelocity * (-1 * (1.f + cofOfRestitution))).dot(collisionNormal);
    float32 linearDenomPart = collisionNormal.dot(collisionNormal * (1.f / first->mass + 1.f / second->mass));
	float32 rotationalDenomPart = (firstPerpNorm * firstPerpNorm) / first->momentOfInertia + (sndPerpNorm * sndPerpNorm) / second->momentOfInertia;

    float32 impulseMagnitude = numerator / (linearDenomPart + rotationalDenomPart);
    first->velocity = first->velocity + (collisionNormal * (impulseMagnitude / first->mass));
	second->velocity = second->velocity - (collisionNormal * (impulseMagnitude / second->mass));

    first->rotationalVelocity = first->rotationalVelocity + firstPerp.dot(collisionNormal * impulseMagnitude) / first->momentOfInertia;
    second->rotationalVelocity = second->rotationalVelocity - secondPerp.dot(collisionNormal * impulseMagnitude) / second->momentOfInertia;

}

void update(float32 deltaTimeSeconds, void* userData) {
    // -- Update
    for (int32 idx = 0; idx < numSpheres; idx++) {
        sphereList[idx].update(deltaTimeSeconds);
    }

    // -- Check collisions
    for (int32 idx = 0; idx < numSpheres; idx++) {
        auto first = &sphereList[idx];
        for (int32 otherIdx = idx + 1; otherIdx < numSpheres; otherIdx++) {
            auto second = &sphereList[otherIdx];

            auto ir = getIntersection(first, second);
            if (!ir.intersect) { continue; }

            // Find out where the intersection took place. We will rewind the simulation
            // until the spheres are no longer intersecting.
            const float32 numDtSubdivision = 8.f;
            const float32 subdivisionSeconds = deltaTimeSeconds / numDtSubdivision;
            int32 currentDtSubdivision = 1.f;
            IntersectionResult tempIr = ir;
            do {
                ir = tempIr;

                // Restore the bodies to the point before the collision happened
                first->body = first->previousBody;
                second->body = second->previousBody;
                
                // Subdivide the timestep to get a more granular intersection result
                float32 currentUpdateStep = deltaTimeSeconds - (currentDtSubdivision * subdivisionSeconds);

                first->update(currentUpdateStep);
                second->update(currentUpdateStep);

                tempIr = getIntersection(first, second);
                currentDtSubdivision++;
            } while (tempIr.intersect);

            // Exact intersection point is found. Now we can solve it
            resolveIntersection(&first->body, &second->body, &ir);
        }
    }

	// -- Render
	renderer.render(&camera);
    for (int32 idx = 0; idx < numSpheres; idx++) {
        sphereList[idx].render(&renderer);
    }

    environmentCube.render(&renderer);
}

void unload() {
    mainLoop.stop();
    renderer.unload();
    for (int32 idx = 0; idx < numSpheres; idx++) {
        sphereList[idx].unload();
    }

    environmentCube.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;
}

EM_BOOL onForceApplicationRequested(int eventType, const EmscriptenMouseEvent* mouseEvent, void* userData) {
    printf("Force applied\n");
    Impulse base;
    base.force = { 0, 10000, 0 };
    base.pointOfApplication = { -15, -15, 0 };
    for (int32 idx = 0; idx < numSpheres; idx++) {
        sphereList[idx].body.applyImpulse(base);
    }
    return true;
}