path: root/2d/rigidbody/rigidbody_3/main.cpp

#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>
#include <cfloat>


struct Impulse {
    Vector2 force = { 0, 0 };
    Vector2 pointOfApplication = { 0, 0 };
    float32 timeOfApplicationSeconds = 0.25f;
    float32 timeAppliedSeconds = 0.f;
    bool isDead = false;
};

const int32 NUM_IMPULSES = 4;

struct Rigidbody {
    int32 numImpulses = 0;
    Impulse activeImpulses[NUM_IMPULSES];
    Vector2 velocity = { 0, 0 };
    Vector2 position = { 0, 0 };
    float32 mass = 1.f;

	float32 rotationalVelocity = 0.f;
	float32 rotation = 0.f;
	float32 momentOfInertia = 1.f;

    float32 cofOfRestitution = 1.f;

    void reset() {
        numImpulses = 0;
        velocity = { 0, 0 };
		rotationalVelocity = 0.f;
		rotation = 0.f;
    }

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

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

    void applyGravity(float32 deltaTimeSeconds) {
        velocity += (Vector2 { 0.f, -9.8f } * deltaTimeSeconds);
    }

    void update(float32 deltaTimeSeconds) {
        applyGravity(deltaTimeSeconds);

        Vector2 force;
        float32 torque = 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; // Do the remainder of the time
                i.isDead = true;
            }
            
            float32 impulseDtSeconds = nextTimeAppliedSeconds - i.timeAppliedSeconds;
            Vector2 forceToApply = i.force * (impulseDtSeconds / i.timeOfApplicationSeconds);
            force += forceToApply * impulseDtSeconds;
            torque += i.pointOfApplication.getPerp().dot(forceToApply);

            i.timeAppliedSeconds = nextTimeAppliedSeconds;
        }
        
        Vector2 acceleration = force / mass;
        velocity += (acceleration * deltaTimeSeconds);
        position += (velocity * deltaTimeSeconds);

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

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

struct Circle {
	OrthographicShape shape;
    Rigidbody body;
    Rigidbody previousBody;
    Vector2 force;
    float32 radius;

	void load(OrthographicRenderer* renderer, float32 inRadius, Vector4 startColor, Vector4 endColor) {
        radius = inRadius;
		const int32 numSegments = 36;
		const float32 radiansPerSegment = (2.f * PI) / static_cast<float>(numSegments);
		const int32 numVertices = numSegments * 3;
		
        startColor = startColor.toNormalizedColor();
        endColor = endColor.toNormalizedColor();

		OrthographicVertex vertices[numSegments * 3];
		for (int idx = 0; idx < numSegments; idx++) {
			int vIdx = idx * 3;
			
            Vector4 color;
            if (idx >= numSegments / 2) {
                color = endColor;
            } else {
                color = startColor;
            }
			
            vertices[vIdx].color = color;
			vertices[vIdx].position = Vector2 { radius * cosf(radiansPerSegment * idx), radius * sinf(radiansPerSegment * idx) };
			vertices[vIdx + 1].color = color;
			vertices[vIdx + 1].position = Vector2 { 0.f, 0.f };
			vertices[vIdx + 2].color = color;
			vertices[vIdx + 2].position = Vector2 { radius * cosf(radiansPerSegment * (idx + 1)), radius * sinf(radiansPerSegment * (idx + 1)) };
		}

		shape.load(vertices, numVertices, renderer);
		body.reset();
        body.momentOfInertia = (PI * (radius * radius)) / 4.f;
	}

	void update(float32 dtSeconds) {
        previousBody = body;

		shape.model = Mat4x4().translateByVec2(body.position).rotate2D(body.rotation);
        body.update(dtSeconds);
	}

	void render(OrthographicRenderer* renderer) {
		shape.render(renderer);
	}

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

    void restorePreviousBody() {
        body = previousBody;
    }
};

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

WebglContext context;
OrthographicRenderer renderer;
MainLoop mainLoop;
Circle c1;
Circle c2;

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

    c1.load(&renderer, 32.f, Vector4 { 55.f, 235.f, 35.f, 255.f }, Vector4 { 235.f, 5.f, 235.f, 255.f });
	c1.body.mass = 3.f;
    c1.body.position = Vector2 { context.width / 4.f, context.height / 4.f };
	c1.body.velocity = Vector2 { 100.f, 250.f };

	c2.load(&renderer, 64.f, Vector4 { 235.f, 5.f, 35.f, 255.f }, Vector4 { 5.f, 35.f, 235.f, 255.f });
	c2.body.mass = 1.f;
    c2.body.position = Vector2 { context.width * (3.f / 4.f), context.height * (3.f / 4.f) };
	c2.body.velocity = Vector2 { -300.f, -150.f };

    mainLoop.run(update);
}

void handleCollisionWithWall(Circle* c) {
	if (c->body.position.x <= 0.f) {
        c->body.position.x = 0.f;
        c->body.velocity = c->body.velocity - Vector2 { 1.f, 0.f } * (2 * (c->body.velocity.dot(Vector2 { 1.f, 0.f })));
    }
    if (c->body.position.y <= 0.f) {
        c->body.position.y = 0.f;
        c->body.velocity = c->body.velocity - Vector2 { 0.f, 1.f } * (2 * (c->body.velocity.dot(Vector2 { 0.f, 1.f })));
    } 
    if (c->body.position.x >= 800.f) {
        c->body.position.x = 800.f;
        c->body.velocity = c->body.velocity - Vector2 { -1.f, 0.f } * (2 * (c->body.velocity.dot(Vector2{ -1.f, 0.f })));
    }
    if (c->body.position.y >= 600.f) {
        c->body.position.y = 600.f;
        c->body.velocity = c->body.velocity - Vector2 { 0.f, -1.f } * (2 * (c->body.velocity.dot(Vector2 { 0.f, -1.f }))) ;
    }
}

IntersectionResult getIntersection(Circle* first, Circle* second) {
    IntersectionResult ir;
    Vector2 positionDiff = (first->body.position - second->body.position);
    if (positionDiff.length() > first->radius + second->radius) {
        return ir; // Not intersecting
    }

    Vector2 positionDirection = positionDiff.normalize();
    ir.relativeVelocity = first->body.velocity - second->body.velocity;

    // The positionDirection could represent the normal at which our circles intersect, but then we would
    // never get any rotation on them. At the same time, this is not an entirely great selection because, in the real
    // world, two circles wouldn't hit one another at exactly the normal. To fix this, we offset the positionDirection
    // by the relative velocity. This gives a normal that is slightly more believable, and allows our spin to take place.
    ir.collisionNormal = (positionDirection + ir.relativeVelocity.negate().normalize()).normalize();
    ir.firstPointOfApplication = positionDirection * first->radius;
    ir.secondPointOfApplication = positionDirection * second->radius;
    ir.intersect = true;
    
	return ir;
}

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 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) {
    c1.update(deltaTimeSeconds);
	c2.update(deltaTimeSeconds);

	// Let's backtrack the simulation to find the precise point at which we collided.
	// There exists many ways to find this precise point. This is by far the most
	// expensive, but it gets the job done.
	IntersectionResult ir = getIntersection(&c1, &c2);
	if (ir.intersect) {
		IntersectionResult irCopy = ir;
		float32 copyDt = deltaTimeSeconds;
		float32 subdivisionAmountSeconds = deltaTimeSeconds / 16.f;
		
		do {
		    c1.restorePreviousBody();
			c2.restorePreviousBody();
				
			ir = irCopy;
			copyDt = copyDt - subdivisionAmountSeconds;

			c1.update(copyDt);
		    c2.update(copyDt);
				
			irCopy = getIntersection(&c1, &c2);

			if (copyDt <= 0.f) {
				printf("Error: Should not be happening.\n");
				break;
			}

		} while (irCopy.intersect);

		printf("Found intersection at timestamp: %f\n", copyDt);

		// The following function is the main one that we're talking about in this tutorial.
		// This function will take the collision data, and repel the objects away from one
		// another using what we know from physics.
		resolveCollision(&c1.body, &c2.body, &ir);
		float32 frameTimeRemaining = deltaTimeSeconds - copyDt;

		c1.update(frameTimeRemaining);
	    c2.update(frameTimeRemaining);
	}

	// Keep within the bounds
	handleCollisionWithWall(&c1);
	handleCollisionWithWall(&c2);
	
	// Renderer
	renderer.render();
    c1.render(&renderer);
	c2.render(&renderer);
}

void unload() {
    mainLoop.stop();
    renderer.unload();
    c1.unload();
	c2.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;
}