1 files changed, 35 insertions, 8 deletions

diff --git a/2d/_collisions/polygon_polygon/main.cpp b/2d/_collisions/polygon_polygon/main.cpp
index 3d8561f..dc0505e 100644
--- a/2d/_collisions/polygon_polygon/main.cpp
+++ b/2d/_collisions/polygon_polygon/main.cpp
@@ -33,6 +33,20 @@ struct Rigidbody {
     float32 cofOfRestition = 1.f;
     float32 momentOfInertia = 1.f;
 
+	Rigidbody copy() {
+		return {
+			numImpulses,
+			{ activeImpulses[0], activeImpulses[1], activeImpulses[2], activeImpulses[3] },
+			velocity,
+			position,
+			mass,
+			rotationalVelocity,
+			rotation,
+			cofOfRestition,
+			momentOfInertia
+		};
+	}
+
     void reset() {
         numImpulses = 0;
         velocity = { 0, 0 };
@@ -160,11 +174,11 @@ struct ConvexPolygon {
         edges = new Edge[numVertices]; // This will be filled in later when we are doing our SAT calculation.
 
         // Calculate moment of inertia
-        body.momentOfInertia = (PI * (radius * radius * body.mass)) / 4.f;
+        body.momentOfInertia = (PI * (radius * radius * body.mass)) / 4.f * 10;
 	}
 
 	void update(float32 dtSeconds) {
-		previousBody = body;
+		previousBody = body.copy();
 		
 		body.update(dtSeconds);
 		shape.model = Mat4x4().translateByVec2(body.position).rotate2D(body.rotation);
@@ -183,7 +197,7 @@ struct ConvexPolygon {
     }
 
     void restorePreviousBody() {
-        body = previousBody;
+        body = previousBody.copy();
     }
 
 	void render(Renderer2d* renderer) {
@@ -429,7 +443,7 @@ void update(float32 deltaTimeSeconds, void* userData) {
 			// Handle collison here
 			IntersectionResult irCopy = ir;
 			float32 copyDt = deltaTimeSeconds;
-            float32 subdivisionDt = copyDt / 8.f;
+            float32 subdivisionDt = copyDt / 16.f;
 		
 			do {
 				first->restorePreviousBody();
@@ -440,6 +454,9 @@ void update(float32 deltaTimeSeconds, void* userData) {
 
 			    first->update(copyDt);
 				second->update(copyDt);
+
+				first->calculateTransformedVertices();
+				second->calculateTransformedVertices();			
 				
 				irCopy = getIntersection(first, second);
 
@@ -453,11 +470,21 @@ void update(float32 deltaTimeSeconds, void* userData) {
 			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;
+			// OK - now we need to avoid the case where we just caused
+			// another collision immediately. We will replay this entire
+			// frame.
+			for (int o = 0; o < 4; o++) {
+				if (o != i && o != j) {
+					auto other = &polygons[o];
+					other->restorePreviousBody();
+					other->update(copyDt);
+				}
+			}
+			
+			float32 frameTimeRemaining = deltaTimeSeconds - copyDt;
+			update(frameTimeRemaining, NULL);
+		    break;
 		}
     }