Proper intersections of rects using SAT

4 files changed, 70 insertions, 39 deletions

diff --git a/frontend/2d/_collisions/rectangle_line/dist/output.wasm b/frontend/2d/_collisions/rectangle_line/dist/output.wasm
index dd44607..f23eebd 100755
--- a/frontend/2d/_collisions/rectangle_line/dist/output.wasm
+++ b/frontend/2d/_collisions/rectangle_line/dist/output.wasm
diff --git a/frontend/2d/_collisions/rectangle_line/main.cpp b/frontend/2d/_collisions/rectangle_line/main.cpp
index 9ccbc7e..74e1ec2 100644
--- a/frontend/2d/_collisions/rectangle_line/main.cpp
+++ b/frontend/2d/_collisions/rectangle_line/main.cpp
@@ -246,7 +246,7 @@ void resolveCollision(Rigidbody* first, Rigidbody* second, IntersectionResult* i
     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);
+    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;
diff --git a/frontend/2d/_collisions/rectangle_rectangle/dist/output.wasm b/frontend/2d/_collisions/rectangle_rectangle/dist/output.wasm
index 9038de5..c13cd15 100755
--- a/frontend/2d/_collisions/rectangle_rectangle/dist/output.wasm
+++ b/frontend/2d/_collisions/rectangle_rectangle/dist/output.wasm
diff --git a/frontend/2d/_collisions/rectangle_rectangle/main.cpp b/frontend/2d/_collisions/rectangle_rectangle/main.cpp
index b4e307e..6b7b8dd 100644
--- a/frontend/2d/_collisions/rectangle_rectangle/main.cpp
+++ b/frontend/2d/_collisions/rectangle_rectangle/main.cpp
@@ -9,6 +9,7 @@
 #include <unistd.h>
 #include <pthread.h>
 #include <cmath>
+#include <cfloat>
 
 struct Rigidbody {
     Vector2 force = { 0, 0 };
@@ -62,6 +63,7 @@ struct IntersectionResult {
 struct Rectangle {
 	OrthographicShape shape;
 	Rigidbody body;
+	Rigidbody previousBody;
 	Vector4 color;
 	float32 width = 0.f;
 	float32 height = 0.f;
@@ -92,6 +94,8 @@ struct Rectangle {
 	}
 
 	void update(float32 dtSeconds) {
+		previousBody = body;
+		
 		body.update(dtSeconds);
 		shape.model = Mat4x4().translateByVec2(body.position).rotate2D(body.rotation);
 	}
@@ -104,6 +108,10 @@ struct Rectangle {
 		shape.unload();
 	}
 
+	void restorePreviousBody() {
+		body = previousBody;
+	}
+
 	// Note that these getters are needlessly verbose for demonstration's sake
 	void getPoints(Vector2* pointList) {
 		Vector2 botLeft = shape.model * Vector2 { -width / 2.f, -height / 2.f };
@@ -163,21 +171,21 @@ int main() {
 void load() {
     renderer.load(&context);
 
-	rectangleList[0].load(&renderer, Vector4 { 135.f, 135.f, 35.f, 255.f }, 24.f, 32.f);
+	rectangleList[0].load(&renderer, Vector4 { 235.f, 35.f, 35.f, 255.f }, 24.f, 32.f);
 	rectangleList[0].body.position = Vector2 { context.width / 3.f, context.height / 3.f };
 	rectangleList[0].body.velocity = Vector2 { 100.f, 250.f };
 	rectangleList[0].body.rotation = 0.2f;
 	
-	rectangleList[1].load(&renderer, Vector4 { 35.f, 135.f, 135.f, 255.f }, 64.f, 96.f);
+	rectangleList[1].load(&renderer, Vector4 { 35.f, 235.f, 35.f, 255.f }, 64.f, 96.f);
 	rectangleList[1].body.position = Vector2 { context.width / 3.f, context.height * (2.f / 3.f) };
 	rectangleList[1].body.rotation = 1.3f;
 	
-	rectangleList[2].load(&renderer, Vector4 { 135.f, 35.f, 135.f, 255.f }, 64.f, 32.f);
+	rectangleList[2].load(&renderer, Vector4 { 235.f, 35.f, 235.f, 255.f }, 64.f, 32.f);
 	rectangleList[2].body.position = Vector2 { context.width * (2.f / 3.f), context.height / 3.f };
 	rectangleList[2].body.velocity = Vector2 { -100.f, 250.f };
 	rectangleList[2].body.rotation = -0.5f;
 	
-	rectangleList[3].load(&renderer, Vector4 { 35.f, 135.f, 35.f, 255.f }, 8.f, 16.f);
+	rectangleList[3].load(&renderer, Vector4 { 35.f, 235.f, 235.f, 255.f }, 8.f, 16.f);
 	rectangleList[3].body.position = Vector2 { context.width * (2.f / 3.f), context.height * (2.f / 3.f) };
 	rectangleList[3].body.rotation = -3.23f;
 
@@ -205,6 +213,12 @@ 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;
@@ -220,6 +234,9 @@ IntersectionResult getIntersection(Rectangle* first, Rectangle* second) {
 	Vector2 secondPoints[4];
     second->getPoints(secondPoints);
 
+	float32 minOverlap = FLT_MAX;
+	Vector2 minOverlapAxis;
+
 	for (int i = 0; i < 4; i++) {
 		Vector2 normal = firstNormals[i];
 
@@ -229,6 +246,12 @@ IntersectionResult getIntersection(Rectangle* first, Rectangle* second) {
 		if (!projectionsOverlap(firstProj, secondProj)) {
 			return ir;
 		}
+
+		float32 overlap = getProjectionOverlap(firstProj, secondProj);
+		if (overlap < minOverlap) {
+			minOverlap = overlap;
+			minOverlapAxis = normal;
+		}
 	}
 
 	for (int i = 0; i < 4; i++) {
@@ -240,10 +263,17 @@ IntersectionResult getIntersection(Rectangle* first, Rectangle* second) {
 		if (!projectionsOverlap(firstProj, secondProj)) {
 			return ir;
 		}
+
+		float32 overlap = getProjectionOverlap(firstProj, secondProj);
+		if (overlap < minOverlap) {
+			minOverlap = overlap;
+			minOverlapAxis = normal;
+		}
 	}
 
-	printf("We are overlapping\n");
-	
+	ir.intersect = true;
+	ir.relativeVelocity = first->body.velocity - second->body.velocity;
+	ir.collisionNormal = minOverlapAxis;
 
 	return ir;
 }
@@ -259,13 +289,14 @@ void resolveCollision(Rigidbody* first, Rigidbody* second, IntersectionResult* i
     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 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;
+
+    //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) {
@@ -281,42 +312,42 @@ void update(float32 deltaTimeSeconds, void* userData) {
 			Rectangle* second = &rectangleList[j];
 			
 			IntersectionResult ir = getIntersection(first, second);
-		}
-	}
-
-	/*for (int32 segmentIndex = 0; segmentIndex < 4; segmentIndex++) {
-		IntersectionResult ir = getIntersection(&rectangle, &segmentList[segmentIndex]);
-		if (!ir.intersect) {
-		    continue;
-		}
+			if (!ir.intersect) {
+				continue;
+			}
 
-		// Handle collison here
-		IntersectionResult irCopy = ir;
-		float32 copyDt = deltaTimeSeconds;
+			// Handle collison here
+			IntersectionResult irCopy = ir;
+			float32 copyDt = deltaTimeSeconds;
 		
-		do {
-			ir = irCopy;
-			rectangle = rectCopy;
-			copyDt = copyDt /= 2.f;
+			do {
+				first->restorePreviousBody();
+				second->restorePreviousBody();
+				
+				ir = irCopy;
+				copyDt = copyDt /= 2.f;
 
-			rectangle.update(copyDt);
-			irCopy = getIntersection(&rectangle, &segmentList[segmentIndex]);
+			    first->update(copyDt);
+				second->update(copyDt);
+				
+				irCopy = getIntersection(first, second);
 
-			if (copyDt <= 0.f) {
-				printf("Error: Should not be happening.\n");
-				break;
-			}
+				if (copyDt <= 0.f) {
+					printf("Error: Should not be happening.\n");
+					break;
+				}
 
-		} while (irCopy.intersect);
+			} while (irCopy.intersect);
 
-		printf("Found intersection at timestamp: %f\n", copyDt);
+			printf("Found intersection at timestamp: %f\n", copyDt);
 
-		resolveCollision(&rectangle.body, &segmentList[segmentIndex].body, &ir);
-		float32 frameTimeRemaining = deltaTimeSeconds - copyDt;
-		
-		update(frameTimeRemaining, userData);
-		return;
-		}*/
+			resolveCollision(&first->body, &second->body, &ir);
+			float32 frameTimeRemaining = deltaTimeSeconds - copyDt;
+
+			first->update(frameTimeRemaining);
+			second->update(frameTimeRemaining);
+		}
+	}
 	
 	// Renderer
 	renderer.render();