Implemented contact impulse calculation.

include/rbdlsim.h

@@ -28,6 +28,7 @@ struct SimShape {
 struct SimBody {
   VectorNd q, qdot, qddot, tau;
   Model mModel;
+  bool mIsStatic = false;
 
   typedef std::pair<unsigned int, SimShape> BodyCollisionInfo;
   std::vector<BodyCollisionInfo> mCollisionShapes;
@@ -39,16 +40,25 @@ struct SimBody {
 };
 
 struct CollisionInfo {
-  const SimBody* mBodyA;
-  const SimBody* mBodyB;
-  const SimShape* mShapeA;
-  const SimShape* mShapeB;
+  const SimBody* mBodyA = nullptr;
+  const SimBody* mBodyB = nullptr;
+  const SimShape* mShapeA = nullptr;
+  const SimShape* mShapeB = nullptr;
   int mBodyAIndex;
   int mBodyBIndex;
-  Vector3d posA;
-  Vector3d posB;
-  Vector3d dir;
-  double depth;
+  Vector3d posA = Vector3d::Zero();
+  Vector3d posB = Vector3d::Zero();
+  Vector3d accumImpulseA = Vector3d::Zero();
+  Vector3d accumImpulseB = Vector3d::Zero();
+  Vector3d dir = Vector3d::Zero();
+  VectorNd jacA = VectorNd::Zero(1);
+  VectorNd jacB = VectorNd::Zero(1);
+  MatrixNd MInvA = MatrixNd::Zero(1, 1);
+  MatrixNd MInvB = MatrixNd::Zero(1, 1);
+  double GMInvGTA = 0.;
+  double GMInvGTB = 0.;
+
+  double depth = 0.;
 };
 
 struct World {
@@ -90,6 +100,24 @@ SimBody CreateBoxBody(
     const Vector3d& pos,
     const Vector3d& vel);
 
+void PrepareConstraintImpulse(
+    SimBody* body_a,
+    SimBody* body_b,
+    CollisionInfo& cinfo);
+void CalcCollisions(
+    const SimBody& body_a,
+    const SimBody& body_b,
+    std::vector<CollisionInfo>& collisions);
+void CalcConstraintImpulse(
+    SimBody* body_a,
+    SimBody* body_b,
+    CollisionInfo& cinfo,
+    const double dt);
+void ApplyConstraintImpulse(
+    SimBody* body_a,
+    SimBody* body_b,
+    CollisionInfo& cinfo);
+
 }  // namespace RBDLSim
 
 #endif  //RBDLSIM_RBDLSIM_H

src/rbdlsim.cc

@@ -96,6 +96,8 @@ bool CheckPenetration(
     const SimShape& shape_a,
     const SimShape& shape_b,
     CollisionInfo& cinfo) {
+  cinfo.mShapeA = &shape_a;
+  cinfo.mShapeB = &shape_b;
   if (shape_a.mType == SimShape::Sphere && shape_b.mType == SimShape::Plane) {
     return CheckPenetrationSphereVsPlane(shape_a, shape_b, cinfo);
   }
@@ -104,6 +106,9 @@ bool CheckPenetration(
     cinfo.dir *= -1.;
     return result;
   }
+  if (shape_a.mType == SimShape::Sphere && shape_b.mType == SimShape::Sphere) {
+    return CheckPenetrationSphereVsSphere(shape_b, shape_a, cinfo);
+  }
 
   ccd_t ccd;
   CCD_INIT(&ccd);
@@ -176,7 +181,7 @@ bool CheckPenetrationSphereVsPlane(
   double sphere_center_height =
       plane_normal.transpose() * (sphere_point_to_plane - plane_point);
   if (sphere_center_height < cCollisionEps) {
-    cinfo.dir = plane_normal;
+    cinfo.dir = -plane_normal;
     cinfo.depth = sphere_center_height;
 
     cinfo.posA = sphere_point_to_plane;
@@ -249,6 +254,128 @@ bool SolveGaussSeidelProj(
   return false;
 }
 
+void CalcCollisions(
+    const SimBody& body_a,
+    const SimBody& body_b,
+    std::vector<CollisionInfo>& collisions) {
+  collisions.clear();
+
+  for (int i = 0, ni = body_a.mCollisionShapes.size(); i < ni; ++i) {
+    const SimShape& shape_a = body_a.mCollisionShapes[i].second;
+
+    for (int j = 0, nj = body_b.mCollisionShapes.size(); j < nj; ++j) {
+      const SimShape& shape_b = body_b.mCollisionShapes[j].second;
+
+      CollisionInfo cinfo;
+      bool has_penetration = false;
+      has_penetration = CheckPenetration(shape_a, shape_b, cinfo);
+
+      if (has_penetration) {
+        cinfo.mBodyA = &body_a;
+        cinfo.mBodyAIndex = body_a.mCollisionShapes[i].first;
+        cinfo.mBodyB = &body_b;
+        cinfo.mBodyBIndex = body_b.mCollisionShapes[j].first;
+        collisions.push_back(cinfo);
+      }
+    }
+  }
+}
+
+void CalcImpulseVariables(
+    SimBody* body,
+    unsigned int body_index,
+    const Vector3d& pos,
+    const Vector3d& dir,
+    MatrixNd* MInv,
+    VectorNd* jac,
+    double* G_MInv_GT) {
+  if (body->mIsStatic) {
+    *G_MInv_GT = 0.;
+    return;
+  }
+
+  Model* model = &body->mModel;
+  int ndof = model->qdot_size;
+  const VectorNd& q = body->q;
+  const VectorNd& qdot = body->qdot;
+
+
+  // Calculate local coordinates of the contact point
+  UpdateKinematicsCustom(*model, &q, nullptr, nullptr);
+  Vector3d point_local_b =
+      CalcBodyToBaseCoordinates(*model, q, body_index, pos, false);
+
+  // 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();
+}
+
+void PrepareConstraintImpulse (SimBody* body_a, SimBody* body_b, CollisionInfo& cinfo) {
+  CalcImpulseVariables (body_a, cinfo.mBodyAIndex, cinfo.posA, cinfo.dir, &cinfo.MInvA, &cinfo.jacA, &cinfo.GMInvGTA);
+  CalcImpulseVariables (body_b, cinfo.mBodyBIndex, cinfo.posB, -cinfo.dir, &cinfo.MInvB, &cinfo.jacB, &cinfo.GMInvGTB);
+}
+
+void CalcConstraintImpulse(
+    SimBody* body_a,
+    SimBody* body_b,
+    CollisionInfo& cinfo,
+    const double dt) {
+  // Todo: add nonlinear effects * dt
+
+  double rhs = 0.;
+  if (!body_a->mIsStatic) {
+    rhs += cinfo.jacA * body_a->qdot;
+  }
+  if (!body_b->mIsStatic) {
+    rhs += cinfo.jacB * body_b->qdot;
+  }
+
+  double denom = cinfo.GMInvGTA + cinfo.GMInvGTB;
+
+  if (!body_a->mIsStatic) {
+    cinfo.accumImpulseA = -rhs / denom * cinfo.dir;
+  }
+
+  if (!body_b->mIsStatic) {
+    cinfo.accumImpulseB = rhs / denom * cinfo.dir;
+  }
+}
+
+void ApplyConstraintImpulse(
+    SimBody* body,
+    const MatrixNd& MInv,
+    const VectorNd& jac,
+    double impulse
+) {
+
+}
+
+void ApplyConstraintImpulse(
+    SimBody* body_a,
+    SimBody* body_b,
+    CollisionInfo& cinfo) {
+  if (!body_a->mIsStatic) {
+    body_a->qdot += cinfo.MInvA * cinfo.jacA.transpose() * (-cinfo.dir.transpose() * cinfo.accumImpulseB);
+  }
+
+  if (!body_b->mIsStatic) {
+    body_b->qdot += cinfo.MInvB * cinfo.jacB.transpose() * (-cinfo.dir.transpose() * cinfo.accumImpulseB);
+  }
+}
+
 void SimBody::resolveCollisions(
     double dt,
     std::vector<CollisionInfo>& collisions) {

tests/CollisionTests.cc

@@ -146,3 +146,76 @@ TEST_CASE("CheckCollisionSphereVsSphere", "[Collision]") {
     REQUIRE_THAT(cinfo.depth, Catch::WithinRel(0.5, 1.0e-12));
   }
 }
+
+TEST_CASE("CalcConstraintImpulse", "[Collision]") {
+  SimBody ground_body;
+  SimShape ground_shape;
+  ground_shape.mType = SimShape::Plane;
+  ground_shape.pos = Vector3d::Zero();
+  ground_shape.orientation = Quaternion(0., 0., 0., 1.);
+  ground_body.mCollisionShapes.push_back(SimBody::BodyCollisionInfo(-1, ground_shape));
+  ground_body.mIsStatic = true;
+
+  double sphere_mass = 1.5;
+  SimBody sphere_body = CreateSphereBody(sphere_mass, 1.0, Vector3d (0., 0.5, 0.), Vector3d (0., -1., 0.));
+  CollisionInfo cinfo;
+
+  SECTION ("SphereOnGroundButColliding") {
+    sphere_body.q[1] = 0.5;
+    sphere_body.qdot[1] = -1.23;
+
+    sphere_body.updateCollisionShapes();
+    std::vector<CollisionInfo> collisions;
+    CalcCollisions(ground_body, sphere_body, collisions);
+    REQUIRE(collisions.size() == 1);
+    cinfo = collisions[0];
+
+    bool cresult = CheckPenetration(ground_shape, sphere_body.mCollisionShapes[0].second, cinfo);
+    REQUIRE(cresult == true);
+    REQUIRE((cinfo.dir - Vector3d(0., 1., 0.)).norm() < 1.0e-12);
+
+    PrepareConstraintImpulse(&ground_body, &sphere_body, cinfo);
+    CalcConstraintImpulse(&ground_body, &sphere_body, cinfo, 0);
+    REQUIRE((cinfo.accumImpulseA - Vector3d(0., 0., 0.)).norm() < 1.0e-12);
+    REQUIRE((cinfo.accumImpulseB - Vector3d(0., -sphere_mass * sphere_body.qdot[1], 0.)).norm() < 1.0e-12);
+
+    ApplyConstraintImpulse(&ground_body, &sphere_body, cinfo);
+    REQUIRE(sphere_body.qdot.norm() < 1.0e-12);
+  }
+
+  SECTION ("SphereVsSphereCollision") {
+    double sphere2_mass = 1.5;
+    SimBody sphere2_body = CreateSphereBody(sphere2_mass, 1.0, Vector3d (0., -0.5, 0.), Vector3d (0., 1., 0.));
+
+    sphere_body.q[1] = 0.5;
+    sphere_body.qdot[1] = -1.23;
+    sphere_body.updateCollisionShapes();
+
+    sphere2_body.q[1] = -0.5;
+    sphere2_body.qdot[1] = 1.23;
+    sphere2_body.updateCollisionShapes();
+
+    std::vector<CollisionInfo> collisions;
+    CalcCollisions(sphere_body, sphere2_body, collisions);
+    REQUIRE(collisions.size() == 1);
+    cinfo = collisions[0];
+
+    bool cresult = CheckPenetration(sphere_body.mCollisionShapes[0].second, sphere2_body.mCollisionShapes[0].second, cinfo);
+    REQUIRE(cresult == true);
+    REQUIRE((cinfo.dir - Vector3d(0., 1., 0.)).norm() < 1.0e-12);
+
+    PrepareConstraintImpulse(&sphere_body, &sphere2_body, cinfo);
+    CalcConstraintImpulse(&sphere_body, &sphere2_body, cinfo, 0);
+//    REQUIRE((cinfo.accumImpulseA - Vector3d(0., 0., 0.)).norm() < 1.0e-12);
+//    REQUIRE((cinfo.accumImpulseB - Vector3d(0., -sphere_mass * sphere_body.qdot[1], 0.)).norm() < 1.0e-12);
+
+    cout << "pre impulse:  " << sphere_body.qdot.transpose() << endl;
+    cout << "pre impulse2: " << sphere2_body.qdot.transpose() << endl;
+    ApplyConstraintImpulse(&sphere_body, &sphere2_body, cinfo);
+    cout << "pst impulse: " << sphere_body.qdot.transpose() << endl;
+    cout << "pst impulse2: " << sphere2_body.qdot.transpose() << endl;
+
+    REQUIRE(sphere_body.qdot.norm() < 1.0e-12);
+    REQUIRE(sphere2_body.qdot.norm() < 1.0e-12);
+  }
+}