1 files changed, 60 insertions, 17 deletions

diff --git a/2d/_collisions/polygon_polygon/main.cpp b/2d/_collisions/polygon_polygon/main.cpp
index 1633530..c458299 100644
--- a/2d/_collisions/polygon_polygon/main.cpp
+++ b/2d/_collisions/polygon_polygon/main.cpp
@@ -11,44 +11,86 @@
 #include <cmath>
 #include <cfloat>
 
-struct Rigidbody {
+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 mass = 1.f;
     float32 cofOfRestition = 1.f;
     float32 momentOfInertia = 1.f;
 
     void reset() {
-        force = { 0, 0 };
+        numImpulses = 0;
         velocity = { 0, 0 };
-        rotationalVelocity = 0.f;
-        rotation = 0.f;
     }
 
-    void applyForce(Vector2 f) {
-        force += 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, -50.f } * 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);
-        force = Vector2 { 0.f, 0.f };
-
         rotation += (rotationalVelocity * deltaTimeSeconds);
-    }
 
-    void setMomentOfInertia(float32 moi) {
-        momentOfInertia = moi;
+        // 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--;
+            }
+        }
     }
 };
 
@@ -125,13 +167,14 @@ struct ConvexPolygon {
 		body.update(dtSeconds);
 		shape.model = Mat4x4().translateByVec2(body.position).rotate2D(body.rotation);
 
-        // Populate the current position of our edges
+        // 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().normalize(), start, end };
+            edges[vidx] = { (end - start).getPerp(), start, end };
         }
 	}
 
@@ -287,6 +330,7 @@ IntersectionResult getIntersection(ConvexPolygon* first, ConvexPolygon* second)
 		if (overlap < minOverlap) {
 			minOverlap = overlap;
 			minOverlapEdge = &second->edges[i];
+            minOverlapWasFirst = false;
 		}
 	}
 
@@ -297,7 +341,6 @@ IntersectionResult getIntersection(ConvexPolygon* first, ConvexPolygon* second)
     // Time to find just where we intersected
 	Vector2 closestPoint;
 	float32 minDistance = FLT_MAX;
-
     
     for (int p = 0; p < (minOverlapWasFirst ? second->numVertices : first->numVertices); p++) {
 		Vector2 point = minOverlapWasFirst ? second->transformedVertices[p] : first->transformedVertices[p];