Added applying slight separation impulse in case of penetration, also started working on friction along tangents.

include/rbdlsim.h

@@ -62,6 +62,18 @@ struct CollisionInfo {
   MatrixNd MInvB = MatrixNd::Zero(1, 1);
   double GMInvGTA = 0.;
   double GMInvGTB = 0.;
+  double tangentAccumImpulse0 = 0.;
+  double tangentAccumImpulse1 = 0.;
+  double tangentDeltaImpulse0 = 0.;
+  double tangentDeltaImpulse1 = 0.;
+  Vector3d tangent0 = Vector3d::Zero();
+  Vector3d tangent1 = Vector3d::Zero();
+  VectorNd tangentJacA = MatrixNd::Zero(2,2);
+  VectorNd tangentJacB = MatrixNd::Zero(2,2);
+  MatrixNd tangentMInvA = MatrixNd::Zero(2, 2);
+  MatrixNd tangentMInvB = MatrixNd::Zero(2, 2);
+  MatrixNd tangentGMInvGTA = MatrixNd::Zero(2,2);
+  MatrixNd tangentGMInvGTB = MatrixNd::Zero(2,2);
   double effectiveRestitution = 1.0;
 
   double depth = 0.;

src/rbdlsim.cc

@@ -102,29 +102,48 @@ static void sSwapCollisionInfoShapeOrder(CollisionInfo& cinfo) {
   cinfo.posB = temp_pos;
 }
 
+static void sCalcTangentVectors(const Vector3d &normal, Vector3d* tangent0, Vector3d* tangent1) {
+  if (fabs(normal.dot(Vector3d(0., 0., 1.))) < 0.6) {
+    *tangent0 = normal.cross(Vector3d(0., 0., 1.));
+    *tangent1 = tangent0->cross(normal);
+  } else {
+    *tangent0 = normal.cross(Vector3d(1., 0., 0.));
+    *tangent1 = tangent0->cross(normal);
+  }
+}
+
 bool CheckPenetration(
     const SimShape& shape_a,
     const SimShape& shape_b,
     CollisionInfo& cinfo) {
   cinfo.mShapeA = &shape_a;
   cinfo.mShapeB = &shape_b;
+  bool result = false;
   if (shape_a.mType == SimShape::Sphere && shape_b.mType == SimShape::Plane) {
-    return CheckPenetrationSphereVsPlane(shape_a, shape_b, cinfo);
-  }
-  if (shape_b.mType == SimShape::Sphere && shape_a.mType == SimShape::Plane) {
-    bool result = CheckPenetrationSphereVsPlane(shape_b, shape_a, cinfo);
-    sSwapCollisionInfoShapeOrder(cinfo);
+    result = CheckPenetrationSphereVsPlane(shape_a, shape_b, cinfo);
+    sCalcTangentVectors(cinfo.dir, &cinfo.tangent0, &cinfo.tangent1);
     return result;
-  }
-  if (shape_a.mType == SimShape::Sphere && shape_b.mType == SimShape::Sphere) {
-    return CheckPenetrationSphereVsSphere(shape_a, shape_b, cinfo);
-  }
-  if (shape_a.mType == SimShape::Box && shape_b.mType == SimShape::Plane) {
-    return CheckPenetrationBoxVsPlane(shape_a, shape_b, cinfo);
-  }
-  if (shape_a.mType == SimShape::Plane && shape_b.mType == SimShape::Box) {
+  } else if (
+      shape_b.mType == SimShape::Sphere && shape_a.mType == SimShape::Plane) {
+    result = CheckPenetrationSphereVsPlane(shape_b, shape_a, cinfo);
+    sSwapCollisionInfoShapeOrder(cinfo);
+    sCalcTangentVectors(cinfo.dir, &cinfo.tangent0, &cinfo.tangent1);
+    return result;
+  } else if (
+      shape_a.mType == SimShape::Sphere && shape_b.mType == SimShape::Sphere) {
+    result = CheckPenetrationSphereVsSphere(shape_a, shape_b, cinfo);
+    sCalcTangentVectors(cinfo.dir, &cinfo.tangent0, &cinfo.tangent1);
+    return result;
+  } else if (
+      shape_a.mType == SimShape::Box && shape_b.mType == SimShape::Plane) {
+    result = CheckPenetrationBoxVsPlane(shape_a, shape_b, cinfo);
+    sCalcTangentVectors(cinfo.dir, &cinfo.tangent0, &cinfo.tangent1);
+    return result;
+  } else if (
+      shape_a.mType == SimShape::Plane && shape_b.mType == SimShape::Box) {
     bool result = CheckPenetrationBoxVsPlane(shape_b, shape_a, cinfo);
     sSwapCollisionInfoShapeOrder(cinfo);
+    sCalcTangentVectors(cinfo.dir, &cinfo.tangent0, &cinfo.tangent1);
     return result;
   }
 
@@ -487,13 +506,19 @@ void CalcCollisions(
 }
 
 void CalcImpulseVariables(
+    double dt,
     SimBody* body,
     unsigned int body_index,
     const Vector3d& pos,
     const Vector3d& dir,
+    const double depth,
+    const Vector3d& tangent0,
+    const Vector3d& tangent1,
     MatrixNd* MInv,
     VectorNd* jac,
     double* G_MInv_GT,
+    MatrixNd* tangentJac,
+    MatrixNd* tangentGMInvGT,
     double* bias_vel,
     double restitution) {
   if (body == nullptr || body->mIsStatic) {
@@ -527,32 +552,52 @@ void CalcImpulseVariables(
   *G_MInv_GT = (*jac) * (*MInv) * (*jac).transpose();
   assert(!isnan(*G_MInv_GT));
 
-  *bias_vel = (*jac) * qdot * restitution;
+  double beta = 0.01;
+  double delta_slop = cCollisionEps;
+  *bias_vel = (*jac) * qdot * restitution - beta / dt * std::max (0., -depth - 0.05);
+
+  (*tangentJac).resize(2,ndof);
+  (*tangentJac).block(0,0,1,ndof) = tangent0.transpose() * G_constr;
+  (*tangentJac).block(1,0,1,ndof) = tangent0.transpose() * G_constr;
+  (*tangentGMInvGT) = (*tangentJac) * (*MInv) * (*tangentJac).transpose();
 }
 
 void PrepareConstraintImpulse(
+    double dt,
     SimBody* body_a,
     SimBody* body_b,
     CollisionInfo& cinfo) {
   CalcImpulseVariables(
+      dt,
       body_a,
       cinfo.mBodyAIndex,
       cinfo.posA,
       cinfo.dir,
+      cinfo.depth,
+      cinfo.tangent0,
+      cinfo.tangent1,
       &cinfo.MInvA,
       &cinfo.jacA,
       &cinfo.GMInvGTA,
+      &cinfo.tangentJacA,
+      &cinfo.tangentGMInvGTA,
       &cinfo.biasVelocityA,
       cinfo.effectiveRestitution);
 
   CalcImpulseVariables(
+      dt,
       body_b,
       cinfo.mBodyBIndex,
       cinfo.posB,
       cinfo.dir,
+      -cinfo.depth,
+      cinfo.tangent0,
+      cinfo.tangent1,
       &cinfo.MInvB,
       &cinfo.jacB,
       &cinfo.GMInvGTB,
+      &cinfo.tangentJacB,
+      &cinfo.tangentGMInvGTB,
       &cinfo.biasVelocityB,
       cinfo.effectiveRestitution);
 }
@@ -678,7 +723,7 @@ void World::detectCollisions() {
 
 void World::resolveCollisions(double dt) {
   for (CollisionInfo& cinfo : mContactPoints) {
-    PrepareConstraintImpulse(cinfo.mBodyA, cinfo.mBodyB, cinfo);
+    PrepareConstraintImpulse(dt, cinfo.mBodyA, cinfo.mBodyB, cinfo);
   }
 
   int num_iter = 20;

src/simulator.cc

@@ -102,7 +102,7 @@ void simulator_reset() {
 
   for (int i = 0; i < sWorld.mBodies.size(); i++) {
     sWorld.mBodies[i].q.block(0, 0, 3, 1) =
-        Vector3d::Random() * 3. + Vector3d(0., 5., 0.);
+        Vector3d::Random() * 0.3 + Vector3d(0., 5., 0.);
     sWorld.mBodies[i].q[2] = 0.;
   }