2 files changed, 205 insertions, 3 deletions

diff --git a/2d/rigidbody/rigidbody_3/dist/output.wasm b/2d/rigidbody/rigidbody_3/dist/output.wasm
index 9af2c31..5df757d 100755
--- a/2d/rigidbody/rigidbody_3/dist/output.wasm
+++ b/2d/rigidbody/rigidbody_3/dist/output.wasm
diff --git a/2d/rigidbody/rigidbody_3/main.cpp b/2d/rigidbody/rigidbody_3/main.cpp
index 06f77dc..2063718 100644
--- a/2d/rigidbody/rigidbody_3/main.cpp
+++ b/2d/rigidbody/rigidbody_3/main.cpp
@@ -21,6 +21,7 @@ struct Rigidbody {
 	float32 rotationalVelocity = 0.f;
 	float32 rotation = 0.f;
 	float32 momentOfInertia = 1.f;
+	float32 cofOfRestitution = 1.f;
 
     void reset() {
         linearForce = { 0, 0 };
@@ -54,11 +55,19 @@ struct Rigidbody {
     }
 };
 
+struct Edge {
+	Vector2 normal;
+	Vector2 start;
+	Vector2 end;
+};
+
 struct Rectangle {
 	OrthographicShape shape;
     Rigidbody body;
+	Rigidbody previousBody;
 	Vector2 originalPoints[4];
 	Vector2 transformedPoints[4];
+	Edge edges[4];
 
 	void load(OrthographicRenderer* renderer, Vector4 color, float32 width, float32 height) {
         color = color.toNormalizedColor();
@@ -90,14 +99,26 @@ struct Rectangle {
 	}
 
 	void update(float32 dtSeconds) {
+		previousBody = body;
 		body.update(dtSeconds);
 		shape.model = Mat4x4().translateByVec2(body.position).rotate2D(body.rotation);
 
-		// Note: This helps us check if the cursor is within the bounds of the
-		// rectangle later on.
+		// Note: This helps us check rectangle collisions using SAT later on.
+		// This is probably a slightly slow way of doing this, but we will ignore
+		// that for now.
 		for (int idx = 0; idx < 4; idx++) {
 			transformedPoints[idx] = shape.model * originalPoints[idx];
 		}
+
+		for (int eidx = 0; eidx < 4; eidx++) {
+			edges[eidx].start = transformedPoints[eidx];
+			edges[eidx].end = transformedPoints[eidx == 3 ? 0 : eidx + 1];
+			edges[eidx].normal = (edges[eidx].end - edges[eidx].start).getPerp().normalize();
+		}
+	}
+
+	void restorePreviousBody() {
+		body = previousBody;
 	}
 
 	void render(OrthographicRenderer* renderer) {
@@ -109,6 +130,13 @@ struct Rectangle {
 	}
 };
 
+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);
@@ -165,13 +193,187 @@ void handleCollisionWithWall(Rectangle* r) {
     }
 }
 
+Vector2 getProjection(Vector2* vertices, Vector2 axis) {
+	float32 min = axis.dot(vertices[0]);
+	float32 max = min;
+
+	for (int v = 1; v < 4; v++) {
+		float32 d = axis.dot(vertices[v]);
+
+		if (d < min) {
+			min = d;
+		} else if (d > max) {
+			max = d;
+		}
+	}
+
+	return Vector2 { min, max };
+}
+
+bool projectionsOverlap(Vector2 first, Vector2 second) {
+	return first.x <= second.y && second.x <= first.y;
+}
+
+float32 getProjectionOverlap(Vector2 first, Vector2 second) {
+	float32 firstOverlap = fabs(first.x - second.y);
+	float32 secondOverlap = fabs(second.x - first.y);
+	return firstOverlap > secondOverlap ? secondOverlap : firstOverlap;
+}
+
+const float32 EPSILON = 1.f;
+IntersectionResult getIntersection(Rectangle* first, Rectangle* second) {
+	IntersectionResult ir;
+
+	// For two rectangles to overlap, it means that at least one of the corners of one is inside of the other
+    Edge* firstEdges = first->edges;
+	Vector2* firstPoints = first->transformedPoints;
+
+    Edge* secondEdges = second->edges;
+	Vector2* secondPoints = second->transformedPoints;
+
+	float32 minOverlap = FLT_MAX;
+	Vector2 minOverlapAxis;
+	Edge* minOverlapEdge = NULL;
+	bool minOverlapWasFirstRect = false;
+    
+	for (int i = 0; i < 4; i++) {
+		Vector2 normal = firstEdges[i].normal;
+
+	    Vector2 firstProj = getProjection(firstPoints, normal);
+		Vector2 secondProj = getProjection(secondPoints, normal);
+
+		if (!projectionsOverlap(firstProj, secondProj)) {
+			return ir;
+		}
+
+		float32 overlap = getProjectionOverlap(firstProj, secondProj);
+		if (overlap < minOverlap) {
+			minOverlap = overlap;
+			minOverlapAxis = normal;
+			minOverlapEdge = &firstEdges[i];
+			minOverlapWasFirstRect = true;
+		}
+	}
+
+	for (int i = 0; i < 4; i++) {
+		Vector2 normal = secondEdges[i].normal;
+
+	    Vector2 firstProj = getProjection(firstPoints, normal);
+		Vector2 secondProj = getProjection(secondPoints, normal);
+
+		if (!projectionsOverlap(firstProj, secondProj)) {
+			return ir;
+		}
+
+		float32 overlap = getProjectionOverlap(firstProj, secondProj);
+		if (overlap < minOverlap) {
+			minOverlap = overlap;
+			minOverlapAxis = normal;
+			minOverlapEdge = &secondEdges[i];
+		}
+	}
+
+	ir.intersect = true;
+	ir.relativeVelocity = first->body.velocity - second->body.velocity;
+	ir.collisionNormal = minOverlapAxis;
+
+    // Find the point of collision, this is kind of tricky, and it is just an approximation for now.
+    // At this point, we know that we intersected along the minOverlapAxis, but we do not know where
+    // that exactly happened. To remedy this will, we create two parallel lines: one at the top of the
+    // normal area, and one at the bottom. For point on both of the Rectangles, we will check:
+    // (1) if it is between these two planes
+    // (2) if, for that rectangle, it is the closest point to the original normal vector
+    // (3) or if it is equally distant from normal vector as another point (then this is a "flat" collision)
+    //
+    // The collision point MUST be between these two planes. We can then say the corner/face of the non-monoverlapAxis 
+    // Rectangle is the collision point. This enables us to then solve for their respective points of application fairly
+    // easily. If the collision "point" is an entire face, we make the collision point be the center point.
+    //
+
+	Vector2 closestPoint;
+	float32 minDistance = FLT_MAX;
+
+    for (int p = 0; p < 4; p++) {
+		Vector2 point = minOverlapWasFirstRect ? secondPoints[p] : firstPoints[p];
+
+		float32 distFromPointToStart = (minOverlapEdge->start - point).length();
+		float32 distFromPointToEnd = (minOverlapEdge->end - point).length();
+		float32 potentialMin = MIN(distFromPointToStart, distFromPointToEnd);
+
+		if (potentialMin < minDistance) {
+			closestPoint = point;
+			minDistance = potentialMin; 
+		}
+    }
+
+    ir.firstPointOfApplication = closestPoint - first->body.position;
+    ir.secondPointOfApplication = closestPoint - 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 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) {
     r1.update(deltaTimeSeconds);
 	r2.update(deltaTimeSeconds);
 
+	// Handle intersection between the two rectangles here
+	IntersectionResult ir = getIntersection(&r1, &r2);
+	if (ir.intersect) {
+		IntersectionResult irCopy = ir;
+		float32 copyDt = deltaTimeSeconds;
+		
+		do {
+		    r1.restorePreviousBody();
+			r2.restorePreviousBody();
+				
+			ir = irCopy;
+			copyDt = copyDt /= 2.f;
+
+			r1.update(copyDt);
+		    r2.update(copyDt);
+				
+			irCopy = getIntersection(&r1, &r2);
+
+			if (copyDt <= 0.f) {
+				printf("Error: Should not be happening.\n");
+				break;
+			}
+
+		} while (irCopy.intersect);
+
+		printf("Found intersection at timestamp: %f\n", copyDt);
+
+		resolveCollision(&r1.body, &r2.body, &ir);
+		float32 frameTimeRemaining = deltaTimeSeconds - copyDt;
+
+		r1.update(frameTimeRemaining);
+	    r2.update(frameTimeRemaining);
+	}
+
+	// Keep within the bounds
 	handleCollisionWithWall(&r1);
 	handleCollisionWithWall(&r2);
-
 	
 	// Renderer
 	renderer.render();