Added Collision Test Sphere vs Sphere, Collision returns for each body a separate contact point.

include/rbdlsim.h

@@ -18,11 +18,7 @@ struct CollisionInfo;
 const double cCollisionEps = 1.0e-4;
 
 struct SimShape {
-  enum ShapeType {
+  enum ShapeType { Box = 0, Sphere = 1, Plane = 2 };
-    Box = 0,
-    Sphere = 1,
-    Plane = 2
-  };
   ShapeType mType;
   Vector3d pos;
   Quaternion orientation;
@@ -37,8 +33,8 @@ struct SimBody {
   std::vector<BodyCollisionInfo> mCollisionShapes;
 
   void step(double ts);
-  void resolveCollisions (double dt, std::vector<CollisionInfo>& collisions);
+  void resolveCollisions(double dt, std::vector<CollisionInfo>& collisions);
-  void calcNextPositions (double dt, const VectorNd& in_qdot, VectorNd& out_q);
+  void calcNextPositions(double dt, const VectorNd& in_qdot, VectorNd& out_q);
   void updateCollisionShapes();
 };
 
@@ -49,7 +45,8 @@ struct CollisionInfo {
   const SimShape* mShapeB;
   int mBodyAIndex;
   int mBodyBIndex;
-  Vector3d pos;
+  Vector3d posA;
+  Vector3d posB;
   Vector3d dir;
   double depth;
 };
@@ -71,6 +68,11 @@ bool CheckPenetration(
     const SimShape& shape_b,
     CollisionInfo& cinfo);
 
+bool CheckPenetrationSphereVsSphere(
+    const SimShape& shape_a,
+    const SimShape& shape_b,
+    CollisionInfo& cinfo);
+
 bool CheckPenetrationSphereVsPlane(
     const SimShape& shape_a,
     const SimShape& shape_b,

src/rbdlsim.cc

@@ -118,7 +118,8 @@ bool CheckPenetration(
       ccdGJKPenetration(&shape_a, &shape_b, &ccd, &depth, &dir, &pos);
 
   if (intersect == 0) {
-    cinfo.pos.set(pos.v[0], pos.v[1], pos.v[2]);
+    cinfo.posA.set(pos.v[0], pos.v[1], pos.v[2]);
+    cinfo.posB.set(pos.v[0], pos.v[1], pos.v[2]);
     cinfo.dir.set(dir.v[0], dir.v[1], dir.v[2]);
     cinfo.depth = depth;
   }
@@ -126,6 +127,35 @@ bool CheckPenetration(
   return !intersect;
 }
 
+bool CheckPenetrationSphereVsSphere(
+    const SimShape& shape_a,
+    const SimShape& shape_b,
+    CollisionInfo& cinfo) {
+  assert(shape_a.mType == SimShape::Sphere);
+  assert(
+      shape_a.scale[0] == shape_a.scale[1]
+      && shape_a.scale[1] == shape_a.scale[2]);
+  assert(shape_b.mType == SimShape::Sphere);
+  assert(
+      shape_b.scale[0] == shape_b.scale[1]
+      && shape_b.scale[1] == shape_b.scale[2]);
+
+  Vector3d diff_pos = shape_b.pos - shape_a.pos;
+  double diff_pos_norm = diff_pos.norm();
+  double distance = diff_pos_norm - (shape_a.scale[0] + shape_b.scale[0]) * 0.5;
+
+  if (distance < cCollisionEps) {
+    cinfo.dir = diff_pos / diff_pos_norm;
+    cinfo.posA = shape_a.pos + cinfo.dir * shape_a.scale[0] * 0.5;
+    cinfo.posB = shape_b.pos - cinfo.dir * shape_b.scale[0] * 0.5;
+    cinfo.depth = fabs(distance);
+
+    return true;
+  }
+
+  return false;
+}
+
 bool CheckPenetrationSphereVsPlane(
     const SimShape& shape_a,
     const SimShape& shape_b,
@@ -134,17 +164,24 @@ bool CheckPenetrationSphereVsPlane(
   assert(shape_b.mType == SimShape::Plane);
 
   // For now only support aligned spheres
-  assert((shape_a.orientation - Quaternion(0., 0., 0., 1.)).squaredNorm() < cCollisionEps);
+  assert(
+      (shape_a.orientation - Quaternion(0., 0., 0., 1.)).squaredNorm()
+      < cCollisionEps);
 
-  Vector3d plane_normal = shape_b.orientation.toMatrix().block(0,1,3,1);
+  Vector3d plane_normal = shape_b.orientation.toMatrix().block(0, 1, 3, 1);
   Vector3d plane_point = shape_b.pos;
-  Vector3d sphere_point_to_plane = shape_a.pos - plane_normal * shape_a.scale[0];
+  Vector3d sphere_point_to_plane =
+      shape_a.pos - plane_normal * shape_a.scale[0] * 0.5;
 
-  double sphere_center_height = plane_normal.transpose() * (sphere_point_to_plane - plane_point);
+  double sphere_center_height =
+      plane_normal.transpose() * (sphere_point_to_plane - plane_point);
   if (sphere_center_height < cCollisionEps) {
-    cinfo.pos = sphere_point_to_plane;
     cinfo.dir = plane_normal;
     cinfo.depth = sphere_center_height;
+
+    cinfo.posA = sphere_point_to_plane;
+    cinfo.posB = sphere_point_to_plane - sphere_center_height * plane_normal;
+
     return 1;
   }
 
@@ -247,7 +284,7 @@ void SimBody::resolveCollisions(
         mModel,
         q,
         collisions[i].mBodyAIndex,
-        collisions[i].pos,
+        collisions[i].posA,
         false));
   }
 
@@ -278,16 +315,16 @@ void SimBody::resolveCollisions(
 
   // Solve for the impules hlambda
   A = G * Minv * G.transpose();
-  b = (constr_value ) * 1. / dt + G * (qdot + Minv * dt * (tau - N));
+  b = (constr_value)*1. / dt + G * (qdot + Minv * dt * (tau - N));
 
-  VectorNd hlambda (VectorNd::Zero(nconstraints));
+  VectorNd hlambda(VectorNd::Zero(nconstraints));
-  VectorNd hlambda_lo (VectorNd::Constant(nconstraints, 0.));
+  VectorNd hlambda_lo(VectorNd::Constant(nconstraints, 0.));
-  VectorNd hlambda_hi (VectorNd::Constant(nconstraints, 1.0e9));
+  VectorNd hlambda_hi(VectorNd::Constant(nconstraints, 1.0e9));
 
   bool solve_result = false;
   solve_result = SolveGaussSeidelProj(A, b, hlambda, hlambda_lo, hlambda_hi);
 
-//  VectorNd hlambda = A.colPivHouseholderQr().solve(b) * -1.;
+  //  VectorNd hlambda = A.colPivHouseholderQr().solve(b) * -1.;
 
   if (!solve_result) {
     cout << "Impulse Solve Failed!! " << endl;
@@ -352,7 +389,9 @@ void World::resolveCollisions(double dt) {
         rev_cinfo.mShapeB = cinfo.mShapeA;
         rev_cinfo.mBodyBIndex = cinfo.mBodyAIndex;
 
-        rev_cinfo.pos = cinfo.pos;
+        Vector3d temp = cinfo.posA;
+        rev_cinfo.posA = cinfo.posB;
+        rev_cinfo.posA = temp;
         rev_cinfo.dir = -cinfo.dir;
         rev_cinfo.depth = cinfo.depth;
 
@@ -362,7 +401,7 @@ void World::resolveCollisions(double dt) {
 
     if (body_collisions.size() > 0) {
       cout << "Collision at t = " << mSimTime
-           << ", pos = " << body_collisions[0].pos.transpose()
+           << ", pos = " << body_collisions[0].posA.transpose()
            << ", depth = " << body_collisions[0].depth << endl
            << " qdotpre  = " << mBodies[0].qdot.transpose() << endl;
     }
@@ -475,6 +514,4 @@ SimBody CreateBoxBody(
   return result;
 }
 
-
-
 }  // namespace RBDLSim

tests/CollisionTests.cc

@@ -45,40 +45,104 @@ TEST_CASE("Simple Box vs Sphere Collision", "[Collision]") {
   }
 }
 
-TEST_CASE ("CheckCollisionSphereVsPlane", "[Collision]") {
+TEST_CASE("CheckCollisionSphereVsPlane", "[Collision]") {
   SimShape plane;
   plane.mType = SimShape::Plane;
-  plane.pos = Vector3d (0., 0., 0.);
+  plane.pos = Vector3d(0., 0., 0.);
-  plane.orientation = Quaternion (0., 1., 0., 1.);
+  plane.orientation = Quaternion(0., 1., 0., 1.);
-  plane.scale = Vector3d (1., 1., 1.);
+  plane.scale = Vector3d(1., 1., 1.);
 
   SimShape sphere;
   sphere.mType = SimShape::Sphere;
 
-  sphere.scale = Vector3d (1.5, 1.5, 1.5);
+  sphere.scale = Vector3d(1.5, 1.5, 1.5);
   sphere.orientation = Quaternion(0., 0., 0., 1.);
 
   CollisionInfo cinfo;
   bool cresult = false;
 
-  SECTION ("Sphere above plane") {
+  SECTION("Sphere above plane") {
-    sphere.pos = Vector3d (0., 2.0, 0.);
+    sphere.pos = Vector3d(0., 2.0, 0.);
     cresult = CheckPenetrationSphereVsPlane(sphere, plane, cinfo);
 
     REQUIRE(cresult == false);
   }
 
-  SECTION ("Sphere touching") {
+  SECTION("Sphere touching") {
-    sphere.pos = Vector3d (0., 1.5, 0.);
+    sphere.pos = Vector3d(0., 0.75, 0.);
     cresult = CheckPenetrationSphereVsPlane(sphere, plane, cinfo);
+    REQUIRE((cinfo.posA - Vector3d(0., 0.0, 0.)).norm() < 1.0e-12);
+    REQUIRE((cinfo.posB - Vector3d(0., 0.0, 0.)).norm() < 1.0e-12);
 
-    REQUIRE(cresult == false);
+    REQUIRE(cresult == true);
   }
 
-  SECTION ("Sphere penetration") {
+  SECTION("Sphere penetration") {
-    sphere.pos = Vector3d (0., -1., 0.);
+    sphere.pos = Vector3d(1., -1., 0.);
     cresult = CheckPenetrationSphereVsPlane(sphere, plane, cinfo);
+    REQUIRE((cinfo.posA - Vector3d(1., -1.75, 0.)).norm() < 1.0e-12);
+    REQUIRE((cinfo.posB - Vector3d(1., 0.0, 0.)).norm() < 1.0e-12);
 
-    REQUIRE(cresult == false);
+    REQUIRE(cresult == true);
+  }
+}
+
+TEST_CASE("CheckCollisionSphereVsSphere", "[Collision]") {
+  SimShape sphere_a;
+  sphere_a.mType = SimShape::Sphere;
+  sphere_a.scale = Vector3d(1.4, 1.4, 1.4);
+  sphere_a.orientation = Quaternion(0., 0., 0., 1.);
+
+  SimShape sphere_b;
+  sphere_b.mType = SimShape::Sphere;
+  sphere_b.scale = Vector3d(1.6, 1.6, 1.6);
+  sphere_b.orientation = Quaternion(0., 0., 0., 1.);
+
+  CollisionInfo cinfo;
+  bool cresult = false;
+
+  SECTION("Spheres non-overlapping") {
+    sphere_a.pos = Vector3d(0., 4.0, 0.);
+    sphere_b.pos = Vector3d(0., 0.0, 0.);
+    cresult = CheckPenetrationSphereVsSphere(sphere_a, sphere_b, cinfo);
+
+    REQUIRE(cresult == false);
+  }
+
+  SECTION("Spheres touching") {
+    sphere_a.pos = Vector3d(0., 1.5, 0.);
+    sphere_b.pos = Vector3d(0., 0.0, 0.);
+    cresult = CheckPenetrationSphereVsSphere(sphere_a, sphere_b, cinfo);
+
+    REQUIRE(cresult == true);
+  }
+
+  SECTION("Spheres overlapping") {
+    sphere_a.pos = Vector3d(0., 1.0, 0.);
+    sphere_b.pos = Vector3d(0., 0.0, 0.);
+    cresult = CheckPenetrationSphereVsSphere(sphere_a, sphere_b, cinfo);
+
+    REQUIRE(cresult == true);
+    double err_pos_A = (cinfo.posA - Vector3d(0., 0.3, 0.)).norm();
+    REQUIRE_THAT(
+        (cinfo.dir - Vector3d(0., -1., 0.)).norm(),
+        Catch::WithinRel(0., 1.0e-12));
+    REQUIRE((cinfo.posA - Vector3d(0., 0.3, 0.)).norm() < 1.0e-12);
+    REQUIRE((cinfo.posB - Vector3d(0., 0.8, 0.)).norm() < 1.0e-12);
+    REQUIRE_THAT(cinfo.depth, Catch::WithinRel(0.5, 1.0e-12));
+  }
+
+  SECTION("Spheres overlapping reversed") {
+    sphere_a.pos = Vector3d(0., 1.0, 0.);
+    sphere_b.pos = Vector3d(0., 0.0, 0.);
+    cresult = CheckPenetrationSphereVsSphere(sphere_b, sphere_a, cinfo);
+
+    REQUIRE(cresult == true);
+    REQUIRE_THAT(
+        (cinfo.dir - Vector3d(0., 1., 0.)).norm(),
+        Catch::WithinRel(0., 1.0e-12));
+    REQUIRE((cinfo.posA - Vector3d(0., 0.8, 0.)).norm() < 1.0e-12);
+    REQUIRE((cinfo.posB - Vector3d(0., 0.3, 0.)).norm() < 1.0e-12);
+    REQUIRE_THAT(cinfo.depth, Catch::WithinRel(0.5, 1.0e-12));
   }
 }