1 files changed, 108 insertions, 9 deletions

diff --git a/3d/rigidbody/main.cpp b/3d/rigidbody/main.cpp
index bf6af58..f95e634 100644
--- a/3d/rigidbody/main.cpp
+++ b/3d/rigidbody/main.cpp
@@ -13,6 +13,7 @@
 #include <cmath>
 #include <cfloat>
 
+// -- Rigidbody updates
 struct Impulse {
     Vector3 force = { 0, 0, 0 };
     Vector3 pointOfApplication = { 0, 0, 0 };
@@ -33,6 +34,7 @@ struct Rigidbody3d {
     Vector3 rotationalVelocity;
     Quaternion rotation;
     float32 momentOfInertia = 1.f;
+    float32 cofOfRestitution = 1.f;
 
     void reset() {
         numImpulses = 0;
@@ -100,14 +102,16 @@ struct Rigidbody3d {
     }
 };
 
+// -- Sphere definition
 float32 bounds = 20.f;
 
 struct Sphere {
 	Mesh3d mesh;
+    Rigidbody3d previousBody;
     Rigidbody3d body;
+    float32 radius = 5.f;
 
 	void load(Renderer3d* renderer) {
-        const float32 scale = 3.f;
         const float32 angleIncrements = 2.f;
         const int32 numFaces = static_cast<int32>((180.f / angleIncrements + 1) * (360.f / angleIncrements + 1));
         const int32 numVertices = 4.f * numFaces;
@@ -135,22 +139,22 @@ struct Sphere {
                 const auto nextCosTheta =  cos(DEG_TO_RAD(theta + angleIncrements));
 
                 // Top Left Point
-                auto topLeftPoint = Vector3(scale * sinPhi * cosTheta, scale * sinPhi * sinTheta, scale * cosPhi);
+                auto topLeftPoint = Vector3(radius * sinPhi * cosTheta, radius * sinPhi * sinTheta, radius * cosPhi);
                 auto topLeftIdx = index++;
                 vertices[vidx++] = { topLeftPoint, topLeftPoint.normalize(), color };
 
                 // Bottom Left Point
-                auto bottomLeftPoint = Vector3(scale * nextSinPhi * cosTheta, scale * nextSinPhi * sinTheta, scale * nextCosPhi);
+                auto bottomLeftPoint = Vector3(radius * nextSinPhi * cosTheta, radius * nextSinPhi * sinTheta, radius * nextCosPhi);
                 auto bottomLeftIdx = index++;
                 vertices[vidx++] = { bottomLeftPoint, bottomLeftPoint.normalize(), color };
 
                 // Bottom Right Point
-                auto bottomRightPoint = Vector3(scale * nextSinPhi * nextCosTheta, scale * nextSinPhi * nextSinTheta, scale * nextCosPhi);
+                auto bottomRightPoint = Vector3(radius * nextSinPhi * nextCosTheta, radius * nextSinPhi * nextSinTheta, radius * nextCosPhi);
                 auto bottomRightIdx = index++;
                 vertices[vidx++] = { bottomRightPoint, bottomRightPoint.normalize(), color };
 
                 // Top Right Point
-                auto topRightPoint = Vector3(scale * sinPhi * nextCosTheta, scale * sinPhi * nextSinTheta, scale * cosPhi);
+                auto topRightPoint = Vector3(radius * sinPhi * nextCosTheta, radius * sinPhi * nextSinTheta, radius * cosPhi);
                 auto topRightIdx = index++;
                 vertices[vidx++] = { topRightPoint, topRightPoint.normalize(), color };
 
@@ -168,7 +172,7 @@ struct Sphere {
         body.position = Vector3 { 0.f, 0.f, 0.f };
         body.velocity = Vector3 { 0.f, 0.f, 0.f };
 
-        float32 singleFaceArea = scale;
+        float32 singleFaceArea = radius;
         body.momentOfInertia = (body.mass * singleFaceArea) / 6.f;
 
         delete [] vertices;
@@ -176,8 +180,10 @@ struct Sphere {
 	}
 
 	void update(float32 dtSeconds) {
+        previousBody = body;
         body.update(dtSeconds);
 
+        // -- Constrain inside of the box
         if (body.position.x > bounds) {
             body.position.x = bounds;
             body.velocity.x = -body.velocity.x;
@@ -218,11 +224,23 @@ struct Sphere {
 	}
 };
 
+// -- Intersection info
+struct IntersectionResult {
+    bool intersect = false;
+    Vector3 collisionNormal;
+    Vector3 relativeVelocity;
+    Vector3 firstPointOfApplication;
+    Vector3 secondPointOfApplication;
+};
+
+
 EM_BOOL onPlayClicked(int eventType, const EmscriptenMouseEvent* mouseEvent, void* userData);
 EM_BOOL onStopClicked(int eventType, const EmscriptenMouseEvent* mouseEvent, void* userData);
 EM_BOOL onForceApplicationRequested(int eventType, const EmscriptenMouseEvent* mouseEvent, void* userData);
 
 void load();
+IntersectionResult getIntersection(Sphere* first, Sphere* second);
+void resolveIntersection(Rigidbody3d* first, Rigidbody3d* second, IntersectionResult* ir);
 void update(float32 time, void* userData);
 void unload();
 
@@ -255,20 +273,101 @@ void load() {
     sphereList[1].load(&renderer);
     sphereList[1].body.mass = 6.f;
     sphereList[1].body.position = Vector3(-10.f, 0, 5);
-    sphereList[1].body.velocity = Vector3(10.f, 10.f, -10.f);
+    sphereList[1].body.velocity = Vector3(10.f, -10.f, -10.f);
     sphereList[1].body.rotationalVelocity = Vector3(1.f, 1.f, 1.f);
 
 	camera.projection  = Mat4x4().getPerspectiveProjection(0.1f, 10000.f, DEG_TO_RAD(45.f), 800.f / 600.f);
-	camera.view = Mat4x4().translate({ 0, 0, -40.f });
+	camera.view = Mat4x4().translate({ 0, 0, -60.f });
     mainLoop.run(update);
 }
 
+IntersectionResult getIntersection(Sphere* first, Sphere* second) {
+    IntersectionResult ir;
+    auto distance = (first->body.position - second->body.position).length();
+    if (distance <= first->radius + second->radius) {
+        ir.intersect = true;
+    }
+    else {
+        ir.intersect = false;
+        return ir;
+    }
+
+    auto positionDiff = first->body.position - second->body.position;
+    auto positionDirection = positionDiff.normalize();
+    ir.relativeVelocity = first->body.velocity - second->body.velocity;
+    ir.firstPointOfApplication = positionDirection * first->radius;
+    ir.secondPointOfApplication = positionDirection * second->radius;
+    ir.collisionNormal = (positionDirection + ir.relativeVelocity.negate().normalize()).normalize();
+
+    return ir;
+}
+
+void resolveIntersection(Rigidbody3d* first, Rigidbody3d* second, IntersectionResult* ir) {
+    Vector3 relativeVelocity = ir->relativeVelocity;
+    Vector3 collisionNormal  = ir->collisionNormal;
+    Vector3 firstPerp        = ir->firstPointOfApplication.cross(relativeVelocity);
+    Vector3 secondPerp       = ir->secondPointOfApplication.cross(relativeVelocity);
+	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) {
+    // -- Update
     for (int32 idx = 0; idx < numSpheres; idx++) {
         sphereList[idx].update(deltaTimeSeconds);
     }
 
-	// Renderer
+    // -- Check collisions
+    for (int32 idx = 0; idx < numSpheres; idx++) {
+        auto first = &sphereList[idx];
+        for (int32 otherIdx = idx + 1; otherIdx < numSpheres; otherIdx++) {
+            auto second = &sphereList[otherIdx];
+
+            auto ir = getIntersection(first, second);
+            if (!ir.intersect) { continue; }
+
+            // Find out where the intersection took place. We will rewind the simulation
+            // until the spheres are no longer intersecting.
+            const float32 numDtSubdivision = 8.f;
+            const float32 subdivisionSeconds = deltaTimeSeconds / numDtSubdivision;
+            int32 currentDtSubdivision = 1.f;
+            IntersectionResult tempIr = ir;
+            do {
+                ir = tempIr;
+
+                // Restore the bodies to the point before the collision happened
+                first->body = first->previousBody;
+                second->body = second->previousBody;
+                
+                // Subdivide the timestep to get a more granular intersection result
+                float32 currentUpdateStep = deltaTimeSeconds - (currentDtSubdivision * subdivisionSeconds);
+
+                first->update(currentUpdateStep);
+                second->update(currentUpdateStep);
+
+                tempIr = getIntersection(first, second);
+                currentDtSubdivision++;
+            } while (tempIr.intersect);
+
+            // Exact intersection point is found. Now we can solve it
+            resolveIntersection(&first->body, &second->body, &ir);
+        }
+    }
+
+	// -- Render
 	renderer.render(&camera);
     for (int32 idx = 0; idx < numSpheres; idx++) {
         sphereList[idx].render(&renderer);