Storing MInv * jacT instead of each separately, fixed failing due to size of spheres being now the diameter instead of radius.

include/rbdlsim.h

@@ -58,8 +58,8 @@ struct CollisionInfo {
   Vector3d dir = Vector3d::Zero();
   VectorNd jacA = VectorNd::Zero(1);
   VectorNd jacB = VectorNd::Zero(1);
-  MatrixNd MInvA = MatrixNd::Zero(1, 1);
+  VectorNd MInvJacTA = VectorNd::Zero(1);
-  MatrixNd MInvB = MatrixNd::Zero(1, 1);
+  VectorNd MInvJacTB = VectorNd::Zero(1);
   double GMInvGTA = 0.;
   double GMInvGTB = 0.;
   double tangentAccumImpulse0 = 0.;
@@ -132,9 +132,11 @@ SimBody CreateBoxBody(
     const Vector3d& vel);
 
 void PrepareConstraintImpulse(
+    double dt,
     SimBody* body_a,
     SimBody* body_b,
     CollisionInfo& cinfo);
+
 void CalcCollisions(
     SimBody& body_a,
     SimBody& body_b,

src/rbdlsim.cc

@@ -512,9 +512,7 @@ void CalcImpulseVariables(
     const Vector3d& pos,
     const Vector3d& dir,
     const double depth,
-    const Vector3d& tangent0,
+    VectorNd* MInvJacT,
-    const Vector3d& tangent1,
-    MatrixNd* MInv,
     VectorNd* jac,
     double* G_MInv_GT,
     MatrixNd* tangentJac,
@@ -543,13 +541,13 @@ void CalcImpulseVariables(
   // Compute vectors and matrices of the contact system
   MatrixNd M(MatrixNd::Zero(ndof, ndof));
   CompositeRigidBodyAlgorithm(*model, q, M, false);
-  *MInv = M.inverse();
 
   MatrixNd G_constr(MatrixNd::Zero(3, ndof));
   CalcPointJacobian(*model, q, body_index, point_local_b, G_constr, false);
   (*jac) = dir.transpose() * G_constr;
 
-  *G_MInv_GT = (*jac) * (*MInv) * (*jac).transpose();
+  (*MInvJacT) = M.llt().solve(jac->transpose());
+  *G_MInv_GT = (*jac) * (*MInvJacT);
   assert(!isnan(*G_MInv_GT));
 
   double beta = 0.01;
@@ -574,9 +572,7 @@ void PrepareConstraintImpulse(
       cinfo.posA,
       cinfo.dir,
       cinfo.depth,
-      cinfo.tangent0,
+      &cinfo.MInvJacTA,
-      cinfo.tangent1,
-      &cinfo.MInvA,
       &cinfo.jacA,
       &cinfo.GMInvGTA,
       &cinfo.tangentJacA,
@@ -591,9 +587,7 @@ void PrepareConstraintImpulse(
       cinfo.posB,
       cinfo.dir,
       -cinfo.depth,
-      cinfo.tangent0,
+      &cinfo.MInvJacTB,
-      cinfo.tangent1,
-      &cinfo.MInvB,
       &cinfo.jacB,
       &cinfo.GMInvGTB,
       &cinfo.tangentJacB,
@@ -645,12 +639,12 @@ void ApplyConstraintImpulse(
     CollisionInfo& cinfo) {
   if (body_a && !body_a->mIsStatic) {
     body_a->qdot +=
-        cinfo.MInvA * cinfo.jacA.transpose() * (-cinfo.deltaImpulse);
+        cinfo.MInvJacTA * (-cinfo.deltaImpulse);
     assert(!isnan(body_a->qdot.squaredNorm()));
   }
 
   if (body_b && !body_b->mIsStatic) {
-    body_b->qdot += cinfo.MInvB * cinfo.jacB.transpose() * (cinfo.deltaImpulse);
+    body_b->qdot += cinfo.MInvJacTB * (cinfo.deltaImpulse);
     assert(!isnan(body_b->qdot.squaredNorm()));
   }
 }

tests/CollisionTests.cc

@@ -24,14 +24,14 @@ TEST_CASE("Simple Box vs Sphere Collision", "[Collision]") {
   CollisionInfo cinfo;
 
   SECTION("Box and Sphere Touching") {
-    sphere.pos.set(0., 1.5, 0.);
+    sphere.pos.set(0., 1.0, 0.);
     cresult = CheckPenetration(box, sphere, cinfo);
 
     REQUIRE(cresult == true);
   }
 
   SECTION("Box and Sphere Intersecting") {
-    sphere.pos.set(0., 1.4, 0.);
+    sphere.pos.set(0., 0.9, 0.);
     cresult = CheckPenetration(box, sphere, cinfo);
 
     REQUIRE(cresult == true);
@@ -272,7 +272,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
     REQUIRE(cresult == true);
     REQUIRE((cinfo.dir - Vector3d(0., 1., 0.)).norm() < 1.0e-12);
 
-    PrepareConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
+    PrepareConstraintImpulse(0.001, &ground_body, &sphere_a_body, cinfo);
 
     SECTION("EnsureImpulseDirection") {
       CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo, 0);
@@ -304,7 +304,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
 
     SECTION("CheckBounce") {
       cinfo.effectiveRestitution = 1.0;
-      PrepareConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
+      PrepareConstraintImpulse(0.001, &ground_body, &sphere_a_body, cinfo);
       VectorNd old_vel = sphere_a_body.qdot;
       CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo, 0);
       ApplyConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
@@ -329,7 +329,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
     REQUIRE(cresult == true);
     REQUIRE((cinfo.dir - Vector3d(0., -1., 0.)).norm() < 1.0e-12);
 
-    PrepareConstraintImpulse(&sphere_a_body, &ground_body, cinfo);
+    PrepareConstraintImpulse(0.001, &sphere_a_body, &ground_body, cinfo);
 
     SECTION("EnsureImpulseDirection") {
       CalcConstraintImpulse(&sphere_a_body, &ground_body, cinfo, 0);
@@ -377,7 +377,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
 
     REQUIRE((cinfo.dir - Vector3d(0., -1., 0.)).norm() < 1.0e-12);
 
-    PrepareConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo);
+    PrepareConstraintImpulse(0.001, &sphere_a_body, &sphere_b_body, cinfo);
 
     SECTION("CheckForceAndJacobianDirections") {
       REQUIRE(cinfo.jacA * sphere_a_body.qdot == 1.23);
@@ -398,7 +398,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
 
     SECTION("CheckBounce") {
       cinfo.effectiveRestitution = 1.0;
-      PrepareConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo);
+      PrepareConstraintImpulse(0.001, &sphere_a_body, &sphere_b_body, cinfo);
       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);
@@ -431,7 +431,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
 
     REQUIRE((cinfo.dir - Vector3d(0., 1., 0.)).norm() < 1.0e-12);
 
-    PrepareConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo);
+    PrepareConstraintImpulse(0.001, &sphere_a_body, &sphere_b_body, cinfo);
 
     SECTION("CheckForceAndJacobianDirections") {
       REQUIRE(cinfo.jacA * sphere_a_body.qdot == 1.23);