From 4389dd19fc3126ce76e04eb6536e2d4e53d91a29 Mon Sep 17 00:00:00 2001
From: Martin Felis <martin@fysx.org>
Date: Fri, 13 Nov 2020 21:54:08 +0100
Subject: [PATCH] Working on unilateral contact constraints.

---
 src/rbdlsim.cc   |  3 ++-
 src/simulator.cc | 40 ++++++++++++++++++++++++----------------
 2 files changed, 26 insertions(+), 17 deletions(-)

diff --git a/src/rbdlsim.cc b/src/rbdlsim.cc
index 83abe64..c40c12a 100644
--- a/src/rbdlsim.cc
+++ b/src/rbdlsim.cc
@@ -363,7 +363,8 @@ void CalcConstraintImpulse(
   double denom = cinfo.GMInvGTA + cinfo.GMInvGTB;
 
   double old_impulse = cinfo.accumImpulse;
-  cinfo.deltaImpulse = rhs / denom;
+  // TODO: is this really needed here??
+  cinfo.deltaImpulse = std::max(0., rhs / denom);
   cinfo.accumImpulse = std::max(0., cinfo.accumImpulse + cinfo.deltaImpulse);
   cinfo.deltaImpulse = cinfo.accumImpulse - old_impulse;
 }
diff --git a/src/simulator.cc b/src/simulator.cc
index c9a2ba0..e38fd25 100644
--- a/src/simulator.cc
+++ b/src/simulator.cc
@@ -25,7 +25,7 @@ typedef SimpleMath::Matrix<float, 3, 1> Vector3f;
 typedef SimpleMath::Matrix<float, 4, 4> Matrix44f;
 typedef SimpleMath::Matrix<float, 4, 1> Vector4f;
 
-static bool sIsPaused = false;
+static bool sIsPaused = true;
 static bool nSteps = 0;
 static double sSimTime = 0.;
 static double sSimTimeAccumulator = 0.;
@@ -45,13 +45,13 @@ void simulator_init() {
 
   sWorld.mStaticShapes.push_back(sGroundShape);
 
-  double restitution = 0.97;
+  double restitution = 0.9;
 
   sSphereBody = CreateSphereBody(
       1.,
       1.,
       restitution,
-      Vector3d(2.00, 2.48, 0.),
+      Vector3d(0., 2.48, 0.),
       Vector3d::Zero());
   sWorld.mBodies.push_back(sSphereBody);
 
@@ -59,18 +59,22 @@ void simulator_init() {
       1.,
       1.,
       restitution,
-      Vector3d(2.05, 5.405, 0.),
+      Vector3d(0., 5.405, 0.),
       Vector3d::Zero());
-  //sWorld.mBodies.push_back(sSphereBody2);
+  sWorld.mBodies.push_back(sSphereBody2);
+
+  for (int i = 0; i < sWorld.mBodies.size(); i++) {
+    SimBody& body = sWorld.mBodies[i];
+    sthstry_register(
+        sStateHistory,
+        body.q.data(),
+        body.q.size() * sizeof(double));
+    sthstry_register(
+        sStateHistory,
+        body.qdot.data(),
+        body.qdot.size() * sizeof(double));
+  }
 
-  sthstry_register(
-      sStateHistory,
-      sWorld.mBodies[0].q.data(),
-      sSphereBody.q.size() * sizeof(double));
-  sthstry_register(
-      sStateHistory,
-      sWorld.mBodies[0].qdot.data(),
-      sSphereBody.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));
 
@@ -96,6 +100,10 @@ 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;
+
   sthstry_reset_storage(sStateHistory);
   sthstry_store(sStateHistory);
 
@@ -105,12 +113,12 @@ void simulator_reset() {
 
 void simulator_update(double dt) {
   ImGui::Begin("Simulator");
+
+  sStateHistoryCurrent = std::min (sStateHistoryCurrent, sthstry_get_num_states(sStateHistory) - 1);
+
   if (ImGui::Button("Reset")) {
     simulator_reset();
   }
-  if (ImGui::Button("Step")) {
-    simulator_step(sSimTimeStep);
-  }
   ImGui::Checkbox("Paused", &sIsPaused);
 
   if (ImGui::Button("<")) {