(mkosarek) Fix for wrong timestep

2 files changed, 104 insertions, 6 deletions

diff --git a/3d/rigidbody/dist/output.wasm b/3d/rigidbody/dist/output.wasm
index 98d25e5..8ebe0dd 100755
--- a/3d/rigidbody/dist/output.wasm
+++ b/3d/rigidbody/dist/output.wasm
diff --git a/3d/rigidbody/main.cpp b/3d/rigidbody/main.cpp
index c83018b..90c6bc8 100644
--- a/3d/rigidbody/main.cpp
+++ b/3d/rigidbody/main.cpp
@@ -13,17 +13,96 @@
 #include <cmath>
 #include <cfloat>
 
+struct Impulse {
+    Vector3 force = { 0, 0, 0 };
+    Vector3 pointOfApplication = { 0, 0, 0 };
+    float32 timeOfApplicationSeconds = 0.016f;
+    float32 timeAppliedSeconds = 0.f;
+    bool isDead = false;
+};
+
+const int32 MAX_IMPULSES = 4;
+
 struct Rigidbody3d {
+    int32 numImpulses = 0;
+    Impulse activeImpulses[MAX_IMPULSES];
     Vector3 velocity;
     Vector3 position;
+    float32 mass = 1.f;
+
+    Vector3 rotationalVelocity;
+    Quaternion rotation;
+    float32 momentOfInertia = 1.f;
+
+    void reset() {
+        numImpulses = 0;
+        velocity = { 0, 0, 0};
+        rotationalVelocity = { 0, 0, 0 };
+    }
+
+    void applyImpulse(Impulse i) {
+        if (numImpulses > MAX_IMPULSES) {
+            printf("Unable to apply impulse. Buffer full.\n");
+            return;
+        }
+
+        activeImpulses[numImpulses] = i;
+        numImpulses++;
+    }
+
+    void update(float32 deltaTimeSeconds) {
+        // Apply gravity
+        //velocity += (Vector3 { 0.f, -9.8f, 0.f } * deltaTimeSeconds);
+
+        Vector3 force = { 0.f, 0.f, 0.f };
+        Vector3 torque = { 0.f, 0.f, 0.f };
+        for (int32 idx = 0; idx < numImpulses; idx++) {
+            Impulse& i = activeImpulses[idx];
 
-    void update(float32 dtSeconds) {
+            float32 nextTimeAppliedSeconds = i.timeAppliedSeconds + deltaTimeSeconds;
+            if (nextTimeAppliedSeconds >= i.timeOfApplicationSeconds) {
+                nextTimeAppliedSeconds = i.timeOfApplicationSeconds;
+                i.isDead = true;
+            }
+            
+            float32 impulseDtSeconds = nextTimeAppliedSeconds - i.timeAppliedSeconds;
+            Vector3 forceToApply = i.force * (impulseDtSeconds / i.timeOfApplicationSeconds);
+            force += forceToApply;
+
+            force.printDebug("Force applied\n");
+
+            torque += i.pointOfApplication.cross(force).dot(forceToApply);
+            i.timeAppliedSeconds = nextTimeAppliedSeconds;
+        }
         
+        Vector3 acceleration = force / mass;
+        velocity += (acceleration * deltaTimeSeconds);
+        position += (velocity * deltaTimeSeconds);
+
+	    Vector3 rotationalAcceleration = torque / momentOfInertia;
+		rotationalVelocity += (rotationalAcceleration * deltaTimeSeconds);
+
+        Quaternion rotationVelocityQuat = { 0, rotationalVelocity.x, rotationalVelocity.y, rotationalVelocity.z };
+        Quaternion dqDt = (rotationVelocityQuat  * rotation) * 0.5f;
+		rotation = rotation + (dqDt * deltaTimeSeconds);
+
+        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--;
+            }
+        }
     }
 };
 
 struct Cube {
 	Mesh3d mesh;
+    Rigidbody3d body;
+    float32 scale = 10.f;
 
 	void load(Renderer3d* renderer) {
 	    Vertex3d cubeVertices[] = {
@@ -87,12 +166,18 @@ struct Cube {
 
 
 		mesh.load(&cubeVertices[0], 8, cubeIndices, 36, renderer);
+
+        body.position = Vector3 { 0.f, 0.f, 0.f };
+        body.velocity = Vector3 { 0.f, 0.f, 0.f };
+        mesh.model = Mat4x4().scale(scale);
+
+        float32 singleFaceArea = scale * scale;
+        body.momentOfInertia = (body.mass * singleFaceArea) / 6.f;
 	}
 
 	void update(float32 dtSeconds) {
-        float32 multiplier = (PI / 8.f) * dtSeconds;
-        Vector3 currentRotation = Vector3 { multiplier, multiplier, multiplier };
-		mesh.model = mesh.model.rotate(currentRotation.x, currentRotation.y, currentRotation.z);
+        body.update(dtSeconds);
+		mesh.model = mesh.model.translate(body.position) * body.rotation.toMatrix();
 	}
 
 	void render(Renderer3d* renderer) {
@@ -106,6 +191,7 @@ struct Cube {
 
 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();
 void update(float32 time, void* userData);
@@ -122,14 +208,17 @@ int main() {
 	context.init("#gl_canvas");
     emscripten_set_click_callback("#gl_canvas_play", NULL, false, onPlayClicked);
     emscripten_set_click_callback("#gl_canvas_stop", NULL, false, onStopClicked);
+    emscripten_set_click_callback("#force_apply", NULL, false, onForceApplicationRequested);
     return 0;
 }
 
 void load() {
     renderer.load(&context);
+    cube.scale = 12.f;
+    cube.body.mass = 100.f;
 	cube.load(&renderer);
-	camera.projection  = Mat4x4().getPerspectiveProjection(0.1f, 100.f, PI / 2.f, 800.f / 600.f);
-	camera.view = Mat4x4().translate({ 0, 0, -10 });
+	camera.projection  = Mat4x4().getPerspectiveProjection(0.1f, 10000.f, DEG_TO_RAD(60.f), 800.f / 600.f);
+	camera.view = Mat4x4().translate({ 0, 0, -250 });
     mainLoop.run(update);
 }
 
@@ -162,3 +251,12 @@ EM_BOOL onStopClicked(int eventType, const EmscriptenMouseEvent* mouseEvent, voi
     unload();
     return true;
 }
+
+EM_BOOL onForceApplicationRequested(int eventType, const EmscriptenMouseEvent* mouseEvent, void* userData) {
+    printf("Force applied\n");
+    Impulse base;
+    base.force = { 0, 1000, 0 };
+    base.pointOfApplication = { 0, 1, 0 };
+    cube.body.applyImpulse(base);
+    return true;
+}