Properly compute impulse for simple cases (sphere on plane, sphere vs sphere).

include/rbdlsim.h

@@ -49,10 +49,8 @@ struct CollisionInfo {
   int mBodyBIndex;
   Vector3d posA = Vector3d::Zero();
   Vector3d posB = Vector3d::Zero();
-  double accumImpulseA = 0.;
+  double accumImpulse = 0.;
-  double accumImpulseB = 0.;
+  double deltaImpulse = 0.;
-  double deltaImpulseA = 0.;
-  double deltaImpulseB = 0.;
   Vector3d dir = Vector3d::Zero();
   VectorNd jacA = VectorNd::Zero(1);
   VectorNd jacB = VectorNd::Zero(1);

src/rbdlsim.cc

@@ -105,10 +105,13 @@ bool CheckPenetration(
   if (shape_b.mType == SimShape::Sphere && shape_a.mType == SimShape::Plane) {
     bool result = CheckPenetrationSphereVsPlane(shape_b, shape_a, cinfo);
     cinfo.dir *= -1.;
+    Vector3d temp_pos = cinfo.posA;
+    cinfo.posA = cinfo.posB;
+    cinfo.posB = temp_pos;
     return result;
   }
   if (shape_a.mType == SimShape::Sphere && shape_b.mType == SimShape::Sphere) {
-    return CheckPenetrationSphereVsSphere(shape_b, shape_a, cinfo);
+    return CheckPenetrationSphereVsSphere(shape_a, shape_b, cinfo);
   }
 
   ccd_t ccd;
@@ -327,7 +330,7 @@ void PrepareConstraintImpulse(
       body_a,
       cinfo.mBodyAIndex,
       cinfo.posA,
-      -cinfo.dir,
+      cinfo.dir,
       &cinfo.MInvA,
       &cinfo.jacA,
       &cinfo.GMInvGTA);
@@ -339,11 +342,9 @@ void PrepareConstraintImpulse(
       &cinfo.MInvB,
       &cinfo.jacB,
       &cinfo.GMInvGTB);
-  cout << "jacA = " << cinfo.jacA << endl;
-  cout << "jacB = " << cinfo.jacB << endl;
-
 }
 
+/// Calculates the impulse that we apply on body_b to resolve the contact.
 void CalcConstraintImpulse(
     SimBody* body_a,
     SimBody* body_b,
@@ -353,29 +354,18 @@ void CalcConstraintImpulse(
 
   double rhs = 0.;
   if (body_a && !body_a->mIsStatic) {
-    rhs += (1.0 + cinfo.effectiveRestitution) * cinfo.jacA * body_a->qdot;
+    rhs += cinfo.jacA * body_a->qdot * (1.0 + cinfo.effectiveRestitution);
   }
   if (body_b && !body_b->mIsStatic) {
-    rhs -= (1.0 + cinfo.effectiveRestitution) * cinfo.jacB * (body_b->qdot);
+    rhs += -cinfo.jacB * (body_b->qdot) * (1.0 + cinfo.effectiveRestitution);
   }
 
   double denom = cinfo.GMInvGTA + cinfo.GMInvGTB;
 
-  if (body_a && !body_a->mIsStatic) {
+  double old_impulse = cinfo.accumImpulse;
-    double old_impulse = cinfo.accumImpulseA;
+  cinfo.deltaImpulse = rhs / denom;
-    cinfo.deltaImpulseA = rhs / denom;
+  cinfo.accumImpulse = std::max(0., cinfo.accumImpulse + cinfo.deltaImpulse);
-    cinfo.accumImpulseA += cinfo.deltaImpulseA;
+  cinfo.deltaImpulse = cinfo.accumImpulse - old_impulse;
-    cinfo.accumImpulseA = std::max(0., cinfo.accumImpulseA);
-    cinfo.deltaImpulseA = cinfo.accumImpulseA - old_impulse;
-  }
-
-  if (body_b && !body_b->mIsStatic) {
-    double old_impulse = cinfo.accumImpulseB;
-    cinfo.deltaImpulseB = rhs / denom;
-    cinfo.accumImpulseB += cinfo.deltaImpulseB;
-    cinfo.accumImpulseB = std::max(0., cinfo.accumImpulseB);
-    cinfo.deltaImpulseB = cinfo.accumImpulseB - old_impulse;
-  }
 }
 
 void ApplyConstraintImpulse(
@@ -384,12 +374,12 @@ void ApplyConstraintImpulse(
     CollisionInfo& cinfo) {
   if (body_a && !body_a->mIsStatic) {
     body_a->qdot += cinfo.MInvA * cinfo.jacA.transpose()
-                    * (cinfo.deltaImpulseA);
+                    * (-cinfo.deltaImpulse);
   }
 
   if (body_b && !body_b->mIsStatic) {
-    body_b->qdot -= cinfo.MInvB * cinfo.jacB.transpose()
+    body_b->qdot += cinfo.MInvB * cinfo.jacB.transpose()
-                    * (cinfo.deltaImpulseB);
+                    * (cinfo.deltaImpulse);
   }
 }
 
@@ -464,7 +454,6 @@ bool World::integrateWorld(double dt) {
     body.step(dt);
   }
 
-  mSimTime += dt;
   return true;
 }
 

tests/CollisionTests.cc

@@ -85,6 +85,16 @@ TEST_CASE("CheckCollisionSphereVsPlane", "[Collision]") {
 
     REQUIRE(cresult == true);
   }
+
+  SECTION("Sphere touching shifted") {
+    sphere.pos = Vector3d(3., 0.75, 0.);
+    cresult = CheckPenetrationSphereVsPlane(sphere, plane, cinfo);
+    REQUIRE((cinfo.posA - Vector3d(3., 0.0, 0.)).norm() < 1.0e-12);
+    REQUIRE((cinfo.posB - Vector3d(3., 0.0, 0.)).norm() < 1.0e-12);
+
+    REQUIRE(cresult == true);
+  }
+
 }
 
 TEST_CASE("CheckCollisionSphereVsSphere", "[Collision]") {
@@ -153,6 +163,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
   ground_shape.mType = SimShape::Plane;
   ground_shape.pos = Vector3d::Zero();
   ground_shape.orientation = Quaternion(0., 0., 0., 1.);
+  ground_shape.restitution = 1.0;
   ground_body.mCollisionShapes.push_back(
       SimBody::BodyCollisionInfo(-1, ground_shape));
   ground_body.mIsStatic = true;
@@ -193,13 +204,42 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
     REQUIRE((cinfo.dir - Vector3d(0., 1., 0.)).norm() < 1.0e-12);
 
     PrepareConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
-    CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo, 0);
-    double reference_impulseB = -sphere_a_mass * sphere_a_body.qdot[1];
-    REQUIRE(cinfo.accumImpulseA < 1.0e-12);
-    REQUIRE(cinfo.accumImpulseB - reference_impulseB < 1.0e-12);
 
-    ApplyConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
+    SECTION("EnsureImpulseDirection") {
-    REQUIRE(sphere_a_body.qdot.norm() < 1.0e-12);
+      CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo, 0);
+      REQUIRE(fabs(cinfo.deltaImpulse) > 1.0e-3);
+      REQUIRE(cinfo.deltaImpulse > -1.0e-12);
+    }
+
+    SECTION("CheckForceAndJacobianDirections") {
+      REQUIRE(cinfo.jacB * sphere_a_body.qdot == -1.23);
+      cinfo.deltaImpulse = 1.0;
+      VectorNd qdot_old = sphere_a_body.qdot;
+      ApplyConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
+      REQUIRE(sphere_a_body.qdot[1] > qdot_old[1]);
+    }
+
+    SECTION("CalculateImpulse") {
+      CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo, 0);
+      double reference_impulse = -sphere_a_mass * sphere_a_body.qdot[1];
+      REQUIRE(fabs(cinfo.accumImpulse - reference_impulse) < 1.0e-12);
+      ApplyConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
+      REQUIRE(sphere_a_body.qdot[1] > -1.0e-12);
+    }
+
+    SECTION("ImpulseMustNotPull") {
+      sphere_a_body.qdot[1] = 1.23;
+      CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo, 0);
+      REQUIRE(fabs(cinfo.accumImpulse) < 1.0e-12);
+    }
+
+    SECTION("CheckBounce") {
+      cinfo.effectiveRestitution = 1.0;
+      VectorNd old_vel = sphere_a_body.qdot;
+      CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo, 0);
+      ApplyConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
+      REQUIRE(fabs(sphere_a_body.qdot[1] + old_vel[1]) < 1.0e-12);
+    }
   }
 
   SECTION("SphereOnGroundButCollidingReverseBodyOrder") {
@@ -220,17 +260,31 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
     REQUIRE((cinfo.dir - Vector3d(0., -1., 0.)).norm() < 1.0e-12);
 
     PrepareConstraintImpulse(&sphere_a_body, &ground_body, cinfo);
-    CalcConstraintImpulse(&sphere_a_body, &ground_body, cinfo, 0);
-    double reference_impulseA = -sphere_a_mass * sphere_a_body.qdot[1];
-    REQUIRE(cinfo.accumImpulseA - reference_impulseA < 1.0e-12);
-    REQUIRE(cinfo.accumImpulseB < 1.0e-12);
 
-    ApplyConstraintImpulse(&sphere_a_body, &ground_body, cinfo);
+    SECTION("EnsureImpulseDirection") {
-    REQUIRE(sphere_a_body.qdot.norm() < 1.0e-12);
+      CalcConstraintImpulse(&sphere_a_body, &ground_body, cinfo, 0);
+      REQUIRE(fabs(cinfo.deltaImpulse) > 1.0e-3);
+      REQUIRE(cinfo.deltaImpulse > -1.0e-12);
+    }
+
+    SECTION("CheckForceAndJacobianDirections") {
+      REQUIRE(cinfo.jacA * sphere_a_body.qdot == 1.23);
+      cinfo.deltaImpulse = 1.0;
+      VectorNd qdot_old = sphere_a_body.qdot;
+      ApplyConstraintImpulse(&sphere_a_body, &ground_body, cinfo);
+      REQUIRE(sphere_a_body.qdot[1] > qdot_old[1]);
+    }
+
+    SECTION("CalculateImpulse") {
+      CalcConstraintImpulse(&sphere_a_body, &ground_body, cinfo, 0);
+      double reference_impulse = -sphere_a_mass * sphere_a_body.qdot[1];
+      REQUIRE(fabs(cinfo.accumImpulse - reference_impulse) < 1.0e-12);
+      ApplyConstraintImpulse(&sphere_a_body, &ground_body, cinfo);
+      REQUIRE(sphere_a_body.qdot[1] > -1.0e-12);
+    }
   }
 
   SECTION("SphereVsSphereCollision") {
-    double sphere_b_mass = 1.5;
     SimBody sphere_b_body = CreateSphereBody(
         sphere_b_mass,
         1.0,
@@ -251,20 +305,100 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
     REQUIRE(collisions.size() == 1);
     cinfo = collisions[0];
 
+    REQUIRE((cinfo.dir - Vector3d(0., -1., 0.)).norm() < 1.0e-12);
+
+    PrepareConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo);
+
+    SECTION("CheckForceAndJacobianDirections") {
+      REQUIRE(cinfo.jacA * sphere_a_body.qdot == 1.23);
+      cinfo.deltaImpulse = -1.0;
+      VectorNd qdot_a_old = sphere_a_body.qdot;
+      VectorNd qdot_b_old = sphere_b_body.qdot;
+      ApplyConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo);
+      REQUIRE(sphere_a_body.qdot[1] < qdot_a_old[1]);
+      REQUIRE(sphere_b_body.qdot[1] > qdot_b_old[1]);
+    }
+
+    SECTION("CalculateImpulse") {
+      for (int i = 0; i < 1; i++) {
+        cout << "Iter " << i << endl;
+        CalcConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo, 0);
+        //    REQUIRE((cinfo.accumImpulse - Vector3d(0., 0., 0.)).norm() < 1.0e-12);
+        //    REQUIRE((cinfo.accumImpulse - Vector3d(0., -sphere_mass * sphere_a_body.qdot[1], 0.)).norm() < 1.0e-12);
+
+        cout << "pre impulse:  " << sphere_a_body.qdot.transpose() << endl;
+        cout << "pre impulse2: " << sphere_b_body.qdot.transpose() << endl;
+        cout << "impulse: " << cinfo.deltaImpulse << endl;
+        ApplyConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo);
+        cout << "pst impulse: " << sphere_a_body.qdot.transpose() << endl;
+        cout << "pst impulse2: " << sphere_b_body.qdot.transpose() << endl;
+      }
+      REQUIRE(sphere_a_body.qdot[1] > -0.1);
+      REQUIRE(sphere_b_body.qdot[1] <  0.1);
+    }
+
+    SECTION("CheckBounce") {
+      cinfo.effectiveRestitution = 1.0;
+      VectorNd old_vel_a = sphere_a_body.qdot;
+      VectorNd old_vel_b = sphere_b_body.qdot;
+      CalcConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo, 0);
+      ApplyConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo);
+      REQUIRE(fabs(sphere_a_body.qdot[1] + old_vel_a[1]) < 1.0e-12);
+      REQUIRE(fabs(sphere_b_body.qdot[1] + old_vel_b[1]) < 1.0e-12);
+    }
+  }
+
+  SECTION("SphereVsSphereCollisionReversed") {
+    SimBody sphere_b_body = CreateSphereBody(
+        sphere_b_mass,
+        1.0,
+        0.,
+        Vector3d(0., -0.5, 0.),
+        Vector3d(0., 1., 0.));
+
+    sphere_a_body.q[1] = -0.5;
+    sphere_a_body.qdot[1] = 1.23;
+    sphere_a_body.updateCollisionShapes();
+
+    sphere_b_body.q[1] = 0.5;
+    sphere_b_body.qdot[1] = -1.23;
+    sphere_b_body.updateCollisionShapes();
+
+    std::vector<CollisionInfo> collisions;
+    CalcCollisions(sphere_a_body, sphere_b_body, collisions);
+    REQUIRE(collisions.size() == 1);
+    cinfo = collisions[0];
+
     REQUIRE((cinfo.dir - Vector3d(0., 1., 0.)).norm() < 1.0e-12);
 
     PrepareConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo);
-    CalcConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo, 0);
-    //    REQUIRE((cinfo.accumImpulseA - Vector3d(0., 0., 0.)).norm() < 1.0e-12);
-    //    REQUIRE((cinfo.accumImpulseB - Vector3d(0., -sphere_mass * sphere_a_body.qdot[1], 0.)).norm() < 1.0e-12);
 
-    cout << "pre impulse:  " << sphere_a_body.qdot.transpose() << endl;
+    SECTION("CheckForceAndJacobianDirections") {
-    cout << "pre impulse2: " << sphere_b_body.qdot.transpose() << endl;
+      REQUIRE(cinfo.jacA * sphere_a_body.qdot == 1.23);
-    ApplyConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo);
+      cinfo.deltaImpulse = -1.0;
-    cout << "pst impulse: " << sphere_a_body.qdot.transpose() << endl;
+      VectorNd qdot_a_old = sphere_a_body.qdot;
-    cout << "pst impulse2: " << sphere_b_body.qdot.transpose() << endl;
+      VectorNd qdot_b_old = sphere_b_body.qdot;
+      ApplyConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo);
+      REQUIRE(sphere_a_body.qdot[1] > qdot_a_old[1]);
+      REQUIRE(sphere_b_body.qdot[1] < qdot_b_old[1]);
+    }
 
-    REQUIRE(sphere_a_body.qdot.norm() < 1.0e-12);
+    SECTION("CalculateImpulse") {
-    REQUIRE(sphere_b_body.qdot.norm() < 1.0e-12);
+      for (int i = 0; i < 1; i++) {
+        cout << "Iter " << i << endl;
+        CalcConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo, 0);
+        //    REQUIRE((cinfo.accumImpulse - Vector3d(0., 0., 0.)).norm() < 1.0e-12);
+        //    REQUIRE((cinfo.accumImpulse - Vector3d(0., -sphere_mass * sphere_a_body.qdot[1], 0.)).norm() < 1.0e-12);
+
+        cout << "pre impulse:  " << sphere_a_body.qdot.transpose() << endl;
+        cout << "pre impulse2: " << sphere_b_body.qdot.transpose() << endl;
+        cout << "impulse: " << cinfo.deltaImpulse << endl;
+        ApplyConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo);
+        cout << "pst impulse: " << sphere_a_body.qdot.transpose() << endl;
+        cout << "pst impulse2: " << sphere_b_body.qdot.transpose() << endl;
+      }
+      REQUIRE(sphere_a_body.qdot[1] < 0.1);
+      REQUIRE(sphere_b_body.qdot[1] > -0.1);
+    }
   }
 }