From 17336be8d63cda1983e78e965a5d70a3d2dbd4ff Mon Sep 17 00:00:00 2001
From: Martin Felis <martin@fysx.org>
Date: Fri, 20 Nov 2020 22:53:30 +0100
Subject: [PATCH] Using boxes instead of spheres for simulation.

---
 include/rbdlsim.h |  1 +
 src/rbdlsim.cc    | 15 ++++++++++++---
 src/simulator.cc  | 26 ++++----------------------
 3 files changed, 17 insertions(+), 25 deletions(-)

diff --git a/include/rbdlsim.h b/include/rbdlsim.h
index dfd2c2f..660f9e1 100644
--- a/include/rbdlsim.h
+++ b/include/rbdlsim.h
@@ -115,6 +115,7 @@ SimBody CreateSphereBody(
 SimBody CreateBoxBody(
     double mass,
     const Vector3d& size,
+    double restitution,
     const Vector3d& pos,
     const Vector3d& vel);
 
diff --git a/src/rbdlsim.cc b/src/rbdlsim.cc
index a39bb51..2025b6e 100644
--- a/src/rbdlsim.cc
+++ b/src/rbdlsim.cc
@@ -38,6 +38,7 @@ void SimBody::calcNextPositions(
     Vector3d omega(in_qdot.block(joint.q_index, 0, 3, 1));
     Quaternion qd = q0.omegaToQDot(omega);
     Quaternion q1 = (q0 + qd * dt).normalize();
+    assert (!isnan(q1.squaredNorm()));
     mModel.SetQuaternion(i, q1, io_q);
   }
 }
@@ -122,7 +123,7 @@ bool CheckPenetration(
     return CheckPenetrationBoxVsPlane(shape_a, shape_b, cinfo);
   }
   if (shape_a.mType == SimShape::Plane && shape_b.mType == SimShape::Box) {
-    bool result = CheckPenetrationBoxVsPlane(shape_a, shape_b, cinfo);
+    bool result = CheckPenetrationBoxVsPlane(shape_b, shape_a, cinfo);
     sSwapCollisionInfoShapeOrder(cinfo);
     return result;
   }
@@ -375,7 +376,7 @@ void SimBody::updateCollisionShapes() {
 
     shape.pos =
         CalcBodyToBaseCoordinates(mModel, q, body_id, Vector3d::Zero(), false);
-    shape.orientation.fromMatrix(
+    shape.orientation = Quaternion::fromMatrix(
         CalcBodyWorldOrientation(mModel, q, body_id, false));
   }
 }
@@ -490,6 +491,7 @@ void CalcImpulseVariables(
   (*jac) = dir.transpose() * G_constr;
 
   *G_MInv_GT = (*jac) * (*MInv) * (*jac).transpose();
+  assert (!isnan(*G_MInv_GT));
 
   *bias_vel = (*jac) * qdot * restitution;
 }
@@ -549,6 +551,7 @@ void CalcConstraintImpulse(
   ref = ref_a + ref_b;
 
   double denom = cinfo.GMInvGTA + cinfo.GMInvGTB;
+  assert (denom > cCollisionEps);
 
   double old_impulse = cinfo.accumImpulse;
   // TODO: is this really needed here??
@@ -564,15 +567,19 @@ void ApplyConstraintImpulse(
   if (body_a && !body_a->mIsStatic) {
     body_a->qdot +=
         cinfo.MInvA * cinfo.jacA.transpose() * (-cinfo.deltaImpulse);
+    assert (!isnan(body_a->qdot.squaredNorm()));
   }
 
   if (body_b && !body_b->mIsStatic) {
     body_b->qdot += cinfo.MInvB * cinfo.jacB.transpose() * (cinfo.deltaImpulse);
+    assert (!isnan(body_b->qdot.squaredNorm()));
   }
 }
 
 void World::calcUnconstrainedVelUpdate(double dt) {
   for (SimBody& body : mBodies) {
+    assert (!isnan(body.q.squaredNorm()));
+
     ForwardDynamics(body.mModel, body.q, body.qdot, body.tau, body.qddot);
 
     // semi-implicit eulers
@@ -696,6 +703,7 @@ SimBody CreateSphereBody(
 SimBody CreateBoxBody(
     double mass,
     const Vector3d& size,
+    double restitution,
     const Vector3d& pos,
     const Vector3d& vel) {
   SimBody result;
@@ -729,10 +737,11 @@ SimBody CreateBoxBody(
   result.tau = VectorNd::Zero(result.mModel.qdot_size);
 
   SimShape shape;
-  shape.mType = SimShape::Sphere;
+  shape.mType = SimShape::Box;
   shape.pos = pos;
   shape.orientation.set(0., 0., 0., 1.);
   shape.scale.set(size[0], size[1], size[2]);
+  shape.restitution = restitution;
 
   result.mCollisionShapes.push_back(
       SimBody::BodyCollisionInfo(sphere_body, shape));
diff --git a/src/simulator.cc b/src/simulator.cc
index fa069c8..40f263c 100644
--- a/src/simulator.cc
+++ b/src/simulator.cc
@@ -17,8 +17,6 @@ using namespace RBDLSim;
 
 static World sWorld;
 static SimShape sGroundShape;
-static SimBody sSphereBody;
-static SimBody sSphereBody2;
 
 typedef SimpleMath::Matrix<float, 3, 3> Matrix33f;
 typedef SimpleMath::Matrix<float, 3, 1> Vector3f;
@@ -47,27 +45,11 @@ void simulator_init() {
 
   double restitution = 0.2;
 
-  sSphereBody = CreateSphereBody(
-      1.,
-      1.,
-      restitution,
-      Vector3d(0., 2.48, 0.),
-      Vector3d::Zero());
-  sWorld.mBodies.push_back(sSphereBody);
-
-  sSphereBody2 = CreateSphereBody(
-      1.,
-      1.,
-      restitution,
-      Vector3d(0., 5.405, 0.),
-      Vector3d::Zero());
-  sWorld.mBodies.push_back(sSphereBody2);
-
-  int num_spheres = 5;
-  for (int i = 0; i < num_spheres; i++) {
-    SimBody sphere_body = CreateSphereBody(
-        1.,
+  int num_bodies = 1;
+  for (int i = 0; i < num_bodies; i++) {
+    SimBody body = CreateBoxBody(
         1.,
+        Vector3d(2., 1., 1.),
         restitution,
         Vector3d::Random() * 5.,
         Vector3d::Zero());