path: root/2d/_collisions/polygon_polygon/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 };
    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 cofOfRestition = 1.f;
    float32 momentOfInertia = 1.f;

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

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

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

            i.timeAppliedSeconds = nextTimeAppliedSeconds;
        }

        Vector2 acceleration = force / mass;
        velocity += (acceleration * deltaTimeSeconds);
        position += (velocity * deltaTimeSeconds);
        rotation += (rotationalVelocity * deltaTimeSeconds);

        // Cleanup any impulses that have expired in the mean time
        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 IntersectionResult {
    bool intersect = false;
    Vector2 collisionNormal;
    Vector2 relativeVelocity;
    Vector2 firstPointOfApplication;
    Vector2 secondPointOfApplication;
};

struct Edge {
	Vector2 normal;
	Vector2 start;
	Vector2 end;
};

struct ConvexPolygon {
	OrthographicShape shape;
	Rigidbody body;
	Rigidbody previousBody;
	Vector4 color;
	int32 numVertices = 3;
	float32 width = 0.f;
	float32 height = 0.f;

    Vector2* originalVertices;
    Vector2* transformedVertices;
    Edge* edges;

	void load(OrthographicRenderer* renderer) {
        transformedVertices = new Vector2[numVertices]; // This will be used for SAT calculations later
        originalVertices = new Vector2[numVertices];

		// Generate the shape with numVertices many sides in a "fan" shape (i.e. 3  vertices per vertex)
		// The shape will have all equal sides, just to make it easier on me. Therefore, it will fit inside
		// the conditions of a circle, which is fun. Before anyone gets mad: I know I can avoid recalculating
		// a lot of these cosines and sines. I will be okay; I will live. The bigger problem with this demo
		// is the 2k lines of JavaScript that are required to display it.
		int32 verticesNeeded = numVertices * 3;
		float32 angleIncrements = (2.f * PI) / static_cast<float32>(numVertices);
		OrthographicVertex* shaderVertices = new OrthographicVertex[verticesNeeded];
		for (int32 vidx = 0; vidx < numVertices; vidx++) {
			int32 indexPosition = vidx * 3;
			
			float32 firstAngle = angleIncrements * vidx;
		    shaderVertices[indexPosition].position = {  cosf(firstAngle) * width, sinf(firstAngle) * height };

            originalVertices[vidx] = shaderVertices[indexPosition].position;

		    shaderVertices[indexPosition + 1].position = {  0.f, 0.f };

			float32 secondAngle = angleIncrements * (vidx + 1);
			shaderVertices[indexPosition + 2].position = {  cosf(secondAngle) * width, sinf(secondAngle) * height };

			// Apply some global stylings
			for (int subIdx = 0; subIdx < 3; subIdx++) {
			    shaderVertices[indexPosition + subIdx].color = color.toNormalizedColor();
			}
		}

		shape.load(shaderVertices, verticesNeeded, renderer);
		delete[] shaderVertices;

        edges = new Edge[numVertices]; // This will be filled in later when we are doing our SAT calculation.

        // Calculate moment of inertia
        body.momentOfInertia = (PI * (width * height * body.mass)) / 4.f;
	}

	void update(float32 dtSeconds) {
		previousBody = body;
		
		body.update(dtSeconds);
		shape.model = Mat4x4().translateByVec2(body.position).rotate2D(body.rotation);

        // Populate the current position of our edges. Note that this might be slow depending
        // on how many edges your shaped have.
        for (int vidx = 0; vidx < numVertices; vidx++) {
            Vector2 start = shape.model * originalVertices[vidx];
            transformedVertices[vidx] = start;

            Vector2 end = shape.model * originalVertices[vidx == numVertices - 1 ? 0 : vidx + 1];
            edges[vidx] = { (end - start).getPerp(), start, end };
        }
	}

    void restorePreviousBody() {
        body = previousBody;
    }

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

	void unload() {
		shape.unload();

        delete[] originalVertices;
        originalVertices = NULL;

        delete[] transformedVertices;
        transformedVertices = NULL;

        delete[] edges;
        edges = NULL;
	}
};

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;
ConvexPolygon polygons[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);

	for (int index = 0; index < 4; index++) {
        polygons[index].body.reset();

		if (index == 0) {
			polygons[index].body.position = { context.width / 4.f, context.height / 4.f };
            polygons[index].body.velocity = { 100.f, 200.f };
            polygons[index].body.mass = 2.f;
		} else if (index == 1) {
			polygons[index].body.position = { context.width / 4.f, context.height * (3.f /4.f) };
            polygons[index].body.velocity = { 50.f, -50.f };
            polygons[index].body.mass = 4.f;
		} else if (index == 2) {
			polygons[index].body.position = { context.width * (3.f / 4.f), context.height * (3.f / 4.f) };
            polygons[index].body.velocity = { -100.f, -50.f };
            polygons[index].body.mass = 6.f;
		} else if (index == 3) {
			polygons[index].body.position = { context.width * (3.f / 4.f), context.height / 4.f };
            polygons[index].body.velocity = { -150.f, 50.f };
            polygons[index].body.mass = 8.f;
		}

        polygons[index].width = (polygons[index].body.mass / 2.f) * 20.f;
		polygons[index].height = (polygons[index].body.mass / 2.f) * 20.f;

		polygons[index].numVertices = (index + 1) * 3;
		polygons[index].color = Vector4 {
			index == 0 || index == 3 ? 255.f : 0.f,
			index == 1 || index == 3 ? 255.f : 0.f,
			index == 2 ? 255.f : 0.f,
			255.f
		};
		polygons[index].load(&renderer);
	}

    printf("Main loop beginning\n");
    mainLoop.run(update);
}

struct SATResult {
    Edge* minOverlapEdge = NULL;        // Edge that caused the intersection
    Vector2 overlapPoint;               // Point that caused the intersection on the other shape (i.e. not minOverlapShape)
    float32 minOverlap = FLT_MAX;       // Smallest projection overlap
};

struct ProjectionResult {
    Vector2 minVertex;
    Vector2 maxVertex;
    Vector2 projection;
};

ProjectionResult getProjection(Vector2* vertices, int numVertices, Vector2 axis) {
    ProjectionResult pr;
	float32 min = axis.dot(vertices[0]);
	float32 max = min;

	for (int v = 1; v < numVertices; v++) {
		float32 d = axis.dot(vertices[v]);

		if (d < min) {
            pr.minVertex = vertices[v];
			min = d;
		} else if (d > max) {
            pr.maxVertex = vertices[v];
			max = d;
		}
	}
    
    pr.projection = Vector2 { min, max };
	return pr;
}

bool projectionsOverlap(Vector2 first, Vector2 second) {
	return first.x <= second.y && second.x <= first.y;
}

float32 getProjectionOverlap(Vector2 first, Vector2 second) {
	float32 e = MIN(first.y, second.y);
    float32 f = MAX(first.x, second.x);
	return e - f;
}

bool runSatForShapesEdges(SATResult* result, ConvexPolygon* first, ConvexPolygon* second) {
    for (int i = 0; i < first->numVertices; i++) {
		Vector2 normal = first->edges[i].normal;

	    ProjectionResult firstProj = getProjection(first->transformedVertices, first->numVertices, normal);
	    ProjectionResult secondProj = getProjection(second->transformedVertices, second->numVertices, normal);

		if (!projectionsOverlap(firstProj.projection, secondProj.projection)) {
			return false;
		}

		float32 overlap = getProjectionOverlap(firstProj.projection, secondProj.projection);
		if (overlap < result->minOverlap) {
			result->minOverlap = overlap;
			result->minOverlapEdge = &first->edges[i];
            
            // The overlapPoint will be the point on the other shape that penetrated the edge.
            // If we caught the intersection reasonably early, it should be the point on 'second'
            // that is nearest to the points on 'first'.
            float32 min1min2 = (firstProj.minVertex - secondProj.minVertex).length();
            float32 min1max2 = (firstProj.minVertex - secondProj.maxVertex).length();
            float32 max1max2 = (firstProj.maxVertex - secondProj.maxVertex).length();
            float32 max1min2 = (firstProj.maxVertex - secondProj.minVertex).length();
            
            float32 closest = MIN(min1min2, MIN(min1max2, MIN(max1max2, max1min2)));
            if (closest == min1min2 || closest == max1min2) {
                result->overlapPoint = secondProj.minVertex;
            } else {
                result->overlapPoint = secondProj.maxVertex;
            }
		}
	}

    return true;
}

const float32 EPSILON = 1.f;
IntersectionResult getIntersection(ConvexPolygon* first, ConvexPolygon* second) {
	IntersectionResult ir;
    SATResult sat;
    
    if (!runSatForShapesEdges(&sat, first, second)) {
        return ir;
    }

    if (!runSatForShapesEdges(&sat, second, first)) {
        return ir;
    }

	ir.intersect = true;
	ir.relativeVelocity = first->body.velocity - second->body.velocity;
	ir.collisionNormal = sat.minOverlapEdge->normal;
    ir.firstPointOfApplication = sat.overlapPoint - first->body.position;
    ir.secondPointOfApplication = sat.overlapPoint - second->body.position;;

	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 cofOfRestition = (first->cofOfRestition + second->cofOfRestition) / 2.f;
    float32 numerator = (relativeVelocity * (-1 * (1.f + cofOfRestition))).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 (int p = 0; p < 4; p++) {
	    polygons[p].update(deltaTimeSeconds);
	}

	// Check collisions with other rectangles
	for (int i = 0; i < 4; i++) {
	    ConvexPolygon* first = &polygons[i];
		for (int j = i + 1; j < 4; j++) {
		    ConvexPolygon* second = &polygons[j];
			
			IntersectionResult ir = getIntersection(first, second);
			if (!ir.intersect) {
				continue;
			}

			// Handle collison here
			IntersectionResult irCopy = ir;
			float32 copyDt = deltaTimeSeconds;
		
			do {
				first->restorePreviousBody();
				second->restorePreviousBody();
				
				ir = irCopy;
				copyDt = copyDt /= 2.f;

			    first->update(copyDt);
				second->update(copyDt);
				
				irCopy = getIntersection(first, second);

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

			} while (irCopy.intersect);

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

			resolveCollision(&first->body, &second->body, &ir);
			float32 frameTimeRemaining = deltaTimeSeconds - copyDt;

			first->update(frameTimeRemaining);
			second->update(frameTimeRemaining);
            i = 0;
		}
    }

    // Check collisions with walls
    for (int p = 0; p < 4; p++) {
        ConvexPolygon* polygon = &polygons[p];
        if (polygon->body.position.x <= 0.f) {
            polygon->body.position.x = 0.f;
            polygon->body.velocity = polygon->body.velocity - Vector2 { 1.f, 0.f } * (2 * (polygon->body.velocity.dot(Vector2 { 1.f, 0.f })));
        }
        if (polygon->body.position.y <= 0.f) {
            polygon->body.position.y = 0.f;
            polygon->body.velocity = polygon->body.velocity - Vector2 { 0.f, 1.f } * (2 * (polygon->body.velocity.dot(Vector2 { 0.f, 1.f })));
        } 
        if (polygon->body.position.x >= 800.f) {
            polygon->body.position.x = 800.f;
            polygon->body.velocity = polygon->body.velocity - Vector2 { -1.f, 0.f } * (2 * (polygon->body.velocity.dot(Vector2{ -1.f, 0.f })));
        }
        if (polygon->body.position.y >= 600.f) {
            polygon->body.position.y = 600.f;
            polygon->body.velocity = polygon->body.velocity - Vector2 { 0.f, -1.f } * (2 * (polygon->body.velocity.dot(Vector2 { 0.f, -1.f }))) ;
        }
    }
	
	// Renderer
	renderer.render();
	for (int p = 0; p < 4; p++) {
	    polygons[p].render(&renderer);
	}
}

void unload() {
    mainLoop.stop();
    renderer.unload();
	for (int p = 0; p < 4; p++) {
	    polygons[p].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;
}