Using manifold points instead of posA, posB for adding constraints.

src/rbdlsim.cc

@@ -38,7 +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()));
+    assert(!isnan(q1.squaredNorm()));
     mModel.SetQuaternion(i, q1, io_q);
   }
 }
@@ -81,7 +81,7 @@ void SimShapeSupport(
     len = ccdVec3Len2(&dir);
     if (len - CCD_EPS > CCD_ZERO) {
       ccdVec3Copy(v, &dir);
-      ccdVec3Scale(v, shape->scale[0] / CCD_SQRT(len));
+      ccdVec3Scale(v, shape->scale[0] * CCD_REAL(0.5) / CCD_SQRT(len));
     } else {
       ccdVec3Set(v, CCD_ZERO, CCD_ZERO, CCD_ZERO);
     }
@@ -95,7 +95,7 @@ void SimShapeSupport(
   ccdVec3Add(v, &pos);
 }
 
-static void sSwapCollisionInfoShapeOrder(CollisionInfo &cinfo) {
+static void sSwapCollisionInfoShapeOrder(CollisionInfo& cinfo) {
   cinfo.dir *= -1.;
   Vector3d temp_pos = cinfo.posA;
   cinfo.posA = cinfo.posB;
@@ -186,11 +186,6 @@ bool CheckPenetrationSphereVsPlane(
   assert(shape_a.mType == SimShape::Sphere);
   assert(shape_b.mType == SimShape::Plane);
 
-  // For now only support aligned spheres
-  assert(
-      (shape_a.orientation - Quaternion(0., 0., 0., 1.)).squaredNorm()
-      < cCollisionEps);
-
   Vector3d plane_normal =
       shape_b.orientation.conjugate().rotate(Vector3d(0., 1., 0.));
   Vector3d plane_point = shape_b.pos;
@@ -203,6 +198,8 @@ bool CheckPenetrationSphereVsPlane(
     cinfo.dir = -plane_normal;
     cinfo.depth = sphere_center_height;
 
+    cinfo.mManifoldPoints[cinfo.mNumManifoldPoints++] =
+        sphere_point_to_plane - sphere_center_height * plane_normal;
     cinfo.posA = sphere_point_to_plane;
     cinfo.posB = sphere_point_to_plane - sphere_center_height * plane_normal;
 
@@ -249,17 +246,23 @@ bool CheckPenetrationBoxVsPlane(
   bool result = CheckPenetrationAABBVsPlane(aabb, plane, cinfo);
 
   if (isnan(cinfo.posA.squaredNorm())) {
-    gLog ("NaN error!");
+    gLog("NaN error!");
   }
 
-  assert (!isnan(cinfo.dir.squaredNorm()));
+  assert(!isnan(cinfo.dir.squaredNorm()));
-  assert (!isnan(cinfo.posA.squaredNorm()));
+  assert(!isnan(cinfo.posA.squaredNorm()));
-  assert (!isnan(cinfo.posB.squaredNorm()));
+  assert(!isnan(cinfo.posB.squaredNorm()));
 
   cinfo.posA = shape_a_rot.transpose() * (cinfo.posA) + shape_a.pos;
   cinfo.posB = shape_a_rot.transpose() * (cinfo.posB) + shape_a.pos;
   cinfo.dir = shape_a_rot.transpose() * (cinfo.dir);
 
+  // also transform all contact manifold points
+  for (int i = 0; i < cinfo.mNumManifoldPoints; i++) {
+    cinfo.mManifoldPoints[i] =
+        shape_a_rot.transpose() * cinfo.mManifoldPoints[i] + shape_a.pos;
+  }
+
   return result;
 }
 
@@ -322,7 +325,8 @@ bool CheckPenetrationAABBVsPlane(
   for (int i = 0; i < 8; i++) {
     distances[i] = (vertices[i] - plane_pos).dot(plane_normal);
     if (distances[i] >= 0. && distances[i] < cCollisionEps) {
-      cinfo.mManifoldPoints[cinfo.mNumManifoldPoints++] = vertices[i];
+      cinfo.mManifoldPoints[cinfo.mNumManifoldPoints] = vertices[i];
+      cinfo.mNumManifoldPoints++;
     }
     max_depth = distances[i] < max_depth ? distances[i] : max_depth;
   }
@@ -358,9 +362,26 @@ bool CheckPenetrationAABBVsPlane(
         assert(s >= 0);
         assert(s <= 1.);
 
-        cinfo.mManifoldPoints[cinfo.mNumManifoldPoints++] = v0 + s * (v1 - v0);
+        s = s < cCollisionEps ? 0. : s;
+        s = s > 1.0 - cCollisionEps ? 1.0 : s;
+
+        Vector3d vc = v0 + s * (v1 - v0);
+        bool found_duplicate_point = false;
+        for (int j = 0; j < cinfo.mNumManifoldPoints; j++) {
+          if ((cinfo.mManifoldPoints[j] - vc).squaredNorm() < cCollisionEps) {
+            gLog("Removing duplicate point");
+            found_duplicate_point = true;
+            break;
+          }
+        }
+
+        if (!found_duplicate_point) {
+          cinfo.mManifoldPoints[cinfo.mNumManifoldPoints++] =
+              v0 + s * (v1 - v0);
+        }
+
         if (cinfo.mNumManifoldPoints > 4) {
-          gLog ("Have %d manifold points?!", cinfo.mNumManifoldPoints);
+          gLog("Have %d manifold points?!", cinfo.mNumManifoldPoints);
         }
       }
     }
@@ -487,7 +508,7 @@ void CalcImpulseVariables(
   const VectorNd& q = body->q;
   const VectorNd& qdot = body->qdot;
 
-  assert (!isnan(q.squaredNorm()));
+  assert(!isnan(q.squaredNorm()));
 
   // Calculate local coordinates of the contact point
   UpdateKinematicsCustom(*model, &q, nullptr, nullptr);
@@ -504,7 +525,7 @@ void CalcImpulseVariables(
   (*jac) = dir.transpose() * G_constr;
 
   *G_MInv_GT = (*jac) * (*MInv) * (*jac).transpose();
-  assert (!isnan(*G_MInv_GT));
+  assert(!isnan(*G_MInv_GT));
 
   *bias_vel = (*jac) * qdot * restitution;
 }
@@ -564,7 +585,7 @@ void CalcConstraintImpulse(
   ref = ref_a + ref_b;
 
   double denom = cinfo.GMInvGTA + cinfo.GMInvGTB;
-  assert (denom > cCollisionEps);
+  assert(denom > cCollisionEps);
 
   double old_impulse = cinfo.accumImpulse;
   // TODO: is this really needed here??
@@ -580,18 +601,18 @@ void ApplyConstraintImpulse(
   if (body_a && !body_a->mIsStatic) {
     body_a->qdot +=
         cinfo.MInvA * cinfo.jacA.transpose() * (-cinfo.deltaImpulse);
-    assert (!isnan(body_a->qdot.squaredNorm()));
+    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()));
+    assert(!isnan(body_b->qdot.squaredNorm()));
   }
 }
 
 void World::calcUnconstrainedVelUpdate(double dt) {
   for (SimBody& body : mBodies) {
-    assert (!isnan(body.q.squaredNorm()));
+    assert(!isnan(body.q.squaredNorm()));
 
     ForwardDynamics(body.mModel, body.q, body.qdot, body.tau, body.qddot);
 
@@ -633,15 +654,22 @@ void World::detectCollisions() {
 
         if (has_penetration) {
           if (isnan(cinfo.posA.squaredNorm())) {
-            gLog ("NaN error!");
+            gLog("NaN error!");
           }
+
           cinfo.mBodyA = nullptr;
           cinfo.mBodyAIndex = -1;
           cinfo.mBodyB = &ref_body;
           cinfo.mBodyBIndex = body_col_info.first;
-          assert (!isnan(cinfo.posA.squaredNorm()));
+          assert(!isnan(cinfo.posA.squaredNorm()));
-          assert (!isnan(cinfo.posB.squaredNorm()));
+          assert(!isnan(cinfo.posB.squaredNorm()));
-          mContactPoints.push_back(cinfo);
+
+          for (int i = 0; i < cinfo.mNumManifoldPoints; i++) {
+            CollisionInfo cpinfo(cinfo);
+            cinfo.posA = cinfo.mManifoldPoints[i];
+            cinfo.posB = cinfo.mManifoldPoints[i];
+            mContactPoints.push_back(cinfo);
+          }
         }
       }
     }

src/simulator.cc

@@ -43,23 +43,29 @@ void simulator_init() {
 
   sWorld.mStaticShapes.push_back(sGroundShape);
 
-  double restitution = 0.01;
+  double restitution = 0.3;
 
   int num_bodies = 5;
   for (int i = 0; i < num_bodies; i++) {
-    SimBody body = CreateBoxBody(
+    SimBody body;
-        1.,
-        Vector3d(2., 1., 1.),
-        restitution,
-        Vector3d::Random() * 5.,
-        Vector3d::Zero());
 
-//    SimBody body = CreateSphereBody(
+    bool create_sphere = false;
-//        1.,
+
-//        1.,
+    if (!create_sphere) {
-//        restitution,
+      body = CreateBoxBody(
-//        Vector3d::Random() * 5.,
+          1.,
-//        Vector3d::Zero());
+          Vector3d(2., 1., 1.),
+          restitution,
+          Vector3d::Random() * 5.,
+          Vector3d::Zero());
+    } else {
+      body = CreateSphereBody(
+          1.,
+          1.,
+          restitution,
+          Vector3d::Random() * 5.,
+          Vector3d::Zero());
+    }
 
     sWorld.mBodies.push_back(body);
   }
@@ -232,14 +238,14 @@ void simulator_draw(srcmdbuf* cmdbuf) {
           gLog("Error: cannot render shape of type %d", cinfo.second.mType);
       }
 
-      simd4x4f trans = simd4x4f_create (
+      simd4x4f trans = simd4x4f_create(
           // clang-format off
           simd4f_create(1.f, 0.f, 0.f, 0.f),
           simd4f_create(0.f, 1.f, 0.f, 0.f),
           simd4f_create(0.f, 0.f, 1.f, 0.f),
           simd4f_create(cinfo.second.pos[0], cinfo.second.pos[1], cinfo.second.pos[2], 1.f)
           // clang-format on
-          );
+      );
 
       simd4x4f scale = simd4x4f_create(
           // clang-format off