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

src/rbdlsim.cc

@@ -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);
     }
@@ -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;
 
@@ -260,6 +257,12 @@ bool CheckPenetrationBoxVsPlane(
   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,7 +362,24 @@ 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);
         }
@@ -635,18 +656,25 @@ void World::detectCollisions() {
           if (isnan(cinfo.posA.squaredNorm())) {
             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.posB.squaredNorm()));
+
+          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);
           }
         }
       }
     }
   }
+}
 
 void World::resolveCollisions(double dt) {
   for (CollisionInfo& cinfo : mContactPoints) {

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;
+
+    bool create_sphere = false;
+
+    if (!create_sphere) {
+      body = CreateBoxBody(
           1.,
           Vector3d(2., 1., 1.),
           restitution,
           Vector3d::Random() * 5.,
           Vector3d::Zero());
-
-//    SimBody body = CreateSphereBody(
-//        1.,
-//        1.,
-//        restitution,
-//        Vector3d::Random() * 5.,
-//        Vector3d::Zero());
+    } else {
+      body = CreateSphereBody(
+          1.,
+          1.,
+          restitution,
+          Vector3d::Random() * 5.,
+          Vector3d::Zero());
+    }
 
     sWorld.mBodies.push_back(body);
   }