Trying to get collisions in simulation right.

include/simulator.h

@@ -9,7 +9,9 @@
 typedef struct srcmdbuf srcmdbuf;
 
 void simulator_init();
+void simulator_reset();
 void simulator_update(double dt);
+void simulator_step(double dt);
 void simulator_draw(srcmdbuf* cmdbuf);
 
 #endif  //RBDLSIM_SIMULATOR_H

src/simulator.cc

@@ -24,6 +24,11 @@ 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 nSteps = 0;
+static double sSimTime = 0.;
+static double sSimTimeAccumulator = 0.;
+static double sSimTimeStep = 1.0e-2;
 
 void simulator_init() {
   gLog("Initializing Simulator");
@@ -32,24 +37,66 @@ void simulator_init() {
   sGroundShape.pos.set(0., 0., 0.);
   sGroundShape.orientation.set(0., 0., 0., 1.);
   sGroundShape.scale.set(1.0, 1.0, 1.0);
+  sGroundShape.restitution = 1.0;
 
   sWorld.mStaticShapes.push_back(sGroundShape);
 
-  sSphereBody =
+  double restitution = 0.97;
-      CreateSphereBody(1., 1., 0., Vector3d(0., 1.405, 0.), Vector3d::Zero());
+
+  sSphereBody = CreateSphereBody(
+      1.,
+      1.,
+      restitution,
+      Vector3d(2.00, 2.48, 0.),
+      Vector3d::Zero());
   sWorld.mBodies.push_back(sSphereBody);
 
-  sSphereBody2 =
+  sSphereBody2 = CreateSphereBody(
-      CreateSphereBody(1., 1., 0., Vector3d(0.3, 2.405, 0.), Vector3d::Zero());
+      1.,
+      1.,
+      restitution,
+      Vector3d(2.05, 5.405, 0.),
+      Vector3d::Zero());
   //sWorld.mBodies.push_back(sSphereBody2);
 
   sWorld.mSimTime = 0.;
+
+  simulator_reset();
+}
+
+void simulator_reset() {
+  // Reset all Quaternions:
+  for (SimBody& body : sWorld.mBodies) {
+    body.q.setZero();
+    body.qdot.setZero();
+    body.qddot.setZero();
+    for (int i = 0; i < body.mModel.mBodies.size(); i++) {
+      if (body.mModel.mJoints[i].mJointType
+          == RigidBodyDynamics::JointTypeSpherical) {
+        body.mModel.SetQuaternion(i, Quaternion(0., 0., 0., 1.), body.q);
+      }
+    }
+  }
+
+  sWorld.mBodies[0].q[0] = 0.0;
+  sWorld.mBodies[0].q[1] = 1.50;
+
+//  sWorld.mBodies[1].q[0] = 0.0;
+//  sWorld.mBodies[1].q[1] = 5.50;
 }
 
 void simulator_update(double dt) {
-  gLog ("Updating Simulator");
+  gLog("dt = %f", dt);
 
   ImGui::Begin("Simulator");
+  if (ImGui::Button("Reset")) {
+    simulator_reset();
+  }
+  if (ImGui::Button("Step")) {
+    simulator_step(sSimTimeStep);
+  }
+  ImGui::Checkbox("Paused", &sIsPaused);
+
   ImGui::Text("Ground Plane");
   Vector3f ground_pos = sGroundShape.pos;
   ImGui::DragFloat3("Position", ground_pos.data(), 0.1f, -5.0f, 5.0f);
@@ -58,7 +105,8 @@ void simulator_update(double dt) {
   Vector4f orientation = sGroundShape.orientation;
   ImGui::DragFloat4("Normal", orientation.data(), 0.1f, -1.0f, 1.0f);
   orientation.normalize();
-  sGroundShape.orientation.set(orientation[0], orientation[1], orientation[2], orientation[3]);
+  sGroundShape.orientation
+      .set(orientation[0], orientation[1], orientation[2], orientation[3]);
 
   ImGui::Text("Bodies");
   for (int i = 0; i < sWorld.mBodies.size(); i++) {
@@ -66,21 +114,31 @@ void simulator_update(double dt) {
     Vector3f body_pos = body.q.block(0, 0, 3, 1);
     ImGui::DragFloat3("Pos", body_pos.data(), 0.01f, -5.0f, 5.0f);
     body.q.block(0, 0, 3, 1) = body_pos;
+
+    Vector3f body_vel = body.qdot.block(0, 0, 3, 1);
+    ImGui::DragFloat3("Vel", body_vel.data(), 0.01f, -20.0f, 20.0f);
+    body.qdot.block(0, 0, 3, 1) = body_vel;
+
     body.updateCollisionShapes();
   }
 
   ImGui::End();
 
+  if (!sIsPaused) {
+    simulator_step(dt);
+  }
+}
+
+void simulator_step(double dt) {
   sWorld.calcUnconstrainedVelUpdate(dt);
   sWorld.updateCollisionShapes();
   sWorld.detectCollisions();
+
   sWorld.resolveCollisions(dt);
   sWorld.integrateWorld(dt);
 }
 
 void simulator_draw(srcmdbuf* cmdbuf) {
-  gLog ("Drawing Simulator");
-
   srcmd rcmd;
   srcmd_clear(&rcmd);
 
@@ -90,7 +148,11 @@ void simulator_draw(srcmdbuf* cmdbuf) {
   rcmd.type = SRndrCmdTypeGrid;
   for (int i = -1; i <= 1; i++) {
     for (int j = -1; j <= 1; j++) {
-      simd4x4f_translation(&rcmd.mat, ground_pos[0] + i * 10.0f, ground_pos[1], ground_pos[2] + j * 10.f);
+      simd4x4f_translation(
+          &rcmd.mat,
+          ground_pos[0] + i * 10.0f,
+          ground_pos[1],
+          ground_pos[2] + j * 10.f);
       srcmdbuf_add(cmdbuf, &rcmd);
     }
   }
@@ -101,9 +163,14 @@ void simulator_draw(srcmdbuf* cmdbuf) {
     for (int j = 0; j < body.mCollisionShapes.size(); j++) {
       const SimBody::BodyCollisionInfo& cinfo = body.mCollisionShapes[j];
       switch (cinfo.second.mType) {
-        case SimShape::Box: rcmd.type = SRndrCmdTypeCube; break;
+        case SimShape::Box:
-        case SimShape::Sphere: rcmd.type = SRndrCmdTypeSphere; break;
+          rcmd.type = SRndrCmdTypeCube;
-        default: gLog ("Error: cannot render shape of type %d", cinfo.second.mType);
+          break;
+        case SimShape::Sphere:
+          rcmd.type = SRndrCmdTypeSphere;
+          break;
+        default:
+          gLog("Error: cannot render shape of type %d", cinfo.second.mType);
       }
 
       rcmd.mat = simd4x4f_create(