From 67cc892f14847b212b4c98eb22d4bddf2b1e1c62 Mon Sep 17 00:00:00 2001
From: Martin Felis <martin@fysx.org>
Date: Sun, 15 Nov 2020 22:23:32 +0100
Subject: [PATCH] Collision resolution works. Finally!

---
 src/rbdlsim.cc   |  5 +----
 src/simulator.cc | 45 +++++++++++++++++++++++++++++----------------
 2 files changed, 30 insertions(+), 20 deletions(-)

diff --git a/src/rbdlsim.cc b/src/rbdlsim.cc
index 4d2c91b..2de977f 100644
--- a/src/rbdlsim.cc
+++ b/src/rbdlsim.cc
@@ -262,8 +262,6 @@ void CalcCollisions(
     SimBody& body_a,
     SimBody& body_b,
     std::vector<CollisionInfo>& collisions) {
-  collisions.clear();
-
   for (int i = 0, ni = body_a.mCollisionShapes.size(); i < ni; ++i) {
     const SimShape& shape_a = body_a.mCollisionShapes[i].second;
 
@@ -311,7 +309,7 @@ void CalcImpulseVariables(
   // Calculate local coordinates of the contact point
   UpdateKinematicsCustom(*model, &q, nullptr, nullptr);
   Vector3d point_local_b =
-      CalcBodyToBaseCoordinates(*model, q, body_index, pos, false);
+      CalcBaseToBodyCoordinates(*model, q, body_index, pos, false);
 
   // Compute vectors and matrices of the contact system
   MatrixNd M(MatrixNd::Zero(ndof, ndof));
@@ -467,7 +465,6 @@ void World::resolveCollisions(double dt) {
     for (CollisionInfo& cinfo : mContactPoints) {
       CalcConstraintImpulse(cinfo.mBodyA, cinfo.mBodyB, cinfo, dt);
       ApplyConstraintImpulse(cinfo.mBodyA, cinfo.mBodyB, cinfo);
-      gLog ("Iter %d: Apply impulse: %f", i, cinfo.deltaImpulse);
     }
   }
 }
diff --git a/src/simulator.cc b/src/simulator.cc
index e38fd25..a110aa7 100644
--- a/src/simulator.cc
+++ b/src/simulator.cc
@@ -45,7 +45,7 @@ void simulator_init() {
 
   sWorld.mStaticShapes.push_back(sGroundShape);
 
-  double restitution = 0.9;
+  double restitution = 0.2;
 
   sSphereBody = CreateSphereBody(
       1.,
@@ -63,6 +63,17 @@ void simulator_init() {
       Vector3d::Zero());
   sWorld.mBodies.push_back(sSphereBody2);
 
+  int num_spheres = 5;
+  for (int i = 0; i < num_spheres; i++) {
+    SimBody sphere_body = CreateSphereBody(
+        1.,
+        1.,
+        restitution,
+        Vector3d::Random() * 5.,
+        Vector3d::Zero());
+    sWorld.mBodies.push_back(sphere_body);
+  }
+
   for (int i = 0; i < sWorld.mBodies.size(); i++) {
     SimBody& body = sWorld.mBodies[i];
     sthstry_register(
@@ -75,11 +86,6 @@ void simulator_init() {
         body.qdot.size() * sizeof(double));
   }
 
-  //  sthstry_register(sStateHistory, sWorld.mBodies[1].q.data(), sSphereBody2.q.size() * sizeof(double));
-  //  sthstry_register(sStateHistory, sWorld.mBodies[1].qdot.data(), sSphereBody2.qdot.size() * sizeof(double));
-
-  sWorld.mSimTime = 0.;
-
   simulator_reset();
 }
 
@@ -97,24 +103,24 @@ void simulator_reset() {
     }
   }
 
-  sWorld.mBodies[0].q[0] = 0.0;
-  sWorld.mBodies[0].q[1] = 1.50;
-
-  sWorld.mBodies[1].q[0] = 0.2;
-  sWorld.mBodies[1].q[1] = 3.50;
-  sWorld.mBodies[1].q[2] = 0.0;
+  for (int i = 0; i < sWorld.mBodies.size(); i++) {
+    sWorld.mBodies[i].q.block(0, 0, 3, 1) =
+        Vector3d::Random() * 3. + Vector3d(0., 5., 0.);
+    sWorld.mBodies[i].q[2] = 0.;
+  }
 
   sthstry_reset_storage(sStateHistory);
   sthstry_store(sStateHistory);
 
-  //  sWorld.mBodies[1].q[0] = 0.0;
-  //  sWorld.mBodies[1].q[1] = 5.50;
+  sSimTime = 0.;
+  sSimTimeAccumulator = 0.;
 }
 
 void simulator_update(double dt) {
   ImGui::Begin("Simulator");
 
-  sStateHistoryCurrent = std::min (sStateHistoryCurrent, sthstry_get_num_states(sStateHistory) - 1);
+  sStateHistoryCurrent =
+      std::min(sStateHistoryCurrent, sthstry_get_num_states(sStateHistory) - 1);
 
   if (ImGui::Button("Reset")) {
     simulator_reset();
@@ -145,6 +151,8 @@ void simulator_update(double dt) {
           sthstry_get_num_states(sStateHistory) - 1)) {
     sthstry_restore(sStateHistory, sStateHistoryCurrent);
   }
+  sSimTime = sSimTimeStep * sStateHistoryCurrent;
+  ImGui::Text("Time: %f", sSimTime);
 
   ImGui::Text("Ground Plane");
   Vector3f ground_pos = sGroundShape.pos;
@@ -174,7 +182,11 @@ void simulator_update(double dt) {
   ImGui::End();
 
   if (!sIsPaused) {
-    simulator_step(dt);
+    sSimTimeAccumulator += dt;
+    while (sSimTimeAccumulator > sSimTimeStep) {
+      simulator_step(sSimTimeStep);
+      sSimTimeAccumulator -= sSimTimeStep;
+    }
   }
 }
 
@@ -188,6 +200,7 @@ void simulator_step(double dt) {
 
   sthstry_store(sStateHistory);
   sStateHistoryCurrent = sthstry_get_num_states(sStateHistory);
+  sSimTime += dt;
 }
 
 void simulator_draw(srcmdbuf* cmdbuf) {