From c02255d64c957470fda8e37687da207e3b27665d Mon Sep 17 00:00:00 2001
From: Martin Felis <martin@fysx.org>
Date: Mon, 5 Oct 2020 21:47:25 +0200
Subject: [PATCH] Added plane vs sphere penetration test.

---
 include/rbdlsim.h       |  8 ++++++++
 src/main.cc             |  8 ++++----
 src/rbdlsim.cc          | 41 ++++++++++++++++++++++++++++++++++++++++-
 tests/CollisionTests.cc | 38 ++++++++++++++++++++++++++++++++++++++
 4 files changed, 90 insertions(+), 5 deletions(-)

diff --git a/include/rbdlsim.h b/include/rbdlsim.h
index ee02870..dffa06d 100644
--- a/include/rbdlsim.h
+++ b/include/rbdlsim.h
@@ -15,10 +15,13 @@ struct SimShape;
 struct SimBody;
 struct CollisionInfo;
 
+const double cCollisionEps = 1.0e-4;
+
 struct SimShape {
   enum ShapeType {
     Box = 0,
     Sphere = 1,
+    Plane = 2
   };
   ShapeType mType;
   Vector3d pos;
@@ -68,6 +71,11 @@ bool CheckPenetration(
     const SimShape& shape_b,
     CollisionInfo& cinfo);
 
+bool CheckPenetrationSphereVsPlane(
+    const SimShape& shape_a,
+    const SimShape& shape_b,
+    CollisionInfo& cinfo);
+
 SimBody CreateSphereBody(
     double mass,
     double radius,
diff --git a/src/main.cc b/src/main.cc
index 8daa957..7db693f 100644
--- a/src/main.cc
+++ b/src/main.cc
@@ -12,10 +12,10 @@ void simplesim() {
   world.mBodies.push_back(sphere_body);
 
   SimShape ground_shape;
-  ground_shape.mType = SimShape::Box;
-  ground_shape.pos.set(0., -5.0, 0.);
+  ground_shape.mType = SimShape::Plane;
+  ground_shape.pos.set(0., 0., 0.);
   ground_shape.orientation.set(0., 0., 0., 1.);
-  ground_shape.scale.set(25.0, 10.0, 25.0);
+  ground_shape.scale.set(1.0, 1.0, 1.0);
 
   world.mStaticShapes.push_back(ground_shape);
 
@@ -28,7 +28,7 @@ void simplesim() {
     world.detectCollisions();
     world.resolveCollisions(dt);
     world.integrateWorld(dt);
-  } while (world.mSimTime < 1.01);
+  } while (world.mSimTime < 10.01);
 }
 
 int main(int argc, char* argv[]) {
diff --git a/src/rbdlsim.cc b/src/rbdlsim.cc
index 6ceef21..60cb173 100644
--- a/src/rbdlsim.cc
+++ b/src/rbdlsim.cc
@@ -82,6 +82,9 @@ void SimShapeSupport(
     } else {
       ccdVec3Set(v, CCD_ZERO, CCD_ZERO, CCD_ZERO);
     }
+  } else {
+    cerr << "Unknown shape type: " << shape->mType << endl;
+    abort();
   }
 
   // transform support point according to position and rotation of object
@@ -93,6 +96,15 @@ bool CheckPenetration(
     const SimShape& shape_a,
     const SimShape& shape_b,
     CollisionInfo& cinfo) {
+  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);
+    cinfo.dir *= -1.;
+    return result;
+  }
+
   ccd_t ccd;
   CCD_INIT(&ccd);
   ccd.support1 = SimShapeSupport;
@@ -114,6 +126,31 @@ bool CheckPenetration(
   return !intersect;
 }
 
+bool CheckPenetrationSphereVsPlane(
+    const SimShape& shape_a,
+    const SimShape& shape_b,
+    CollisionInfo& cinfo) {
+  assert(shape_a.mType == SimShape::Sphere);
+  assert(shape_b.mType == SimShape::Plane);
+
+  // For now only support aligned spheres
+  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_point = shape_b.pos;
+  Vector3d sphere_point_to_plane = shape_a.pos - plane_normal * shape_a.scale[0];
+
+  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;
+    return 1;
+  }
+
+  return 0;
+}
+
 void SimBody::updateCollisionShapes() {
   UpdateKinematicsCustom(mModel, &q, nullptr, nullptr);
 
@@ -241,7 +278,7 @@ void SimBody::resolveCollisions(
 
   // Solve for the impules hlambda
   A = G * Minv * G.transpose();
-  b = (constr_value + VectorNd::Constant(nconstraints, 0.0001) ) * 1. / dt + G * (qdot + Minv * dt * (tau - N)) * -1.0;
+  b = (constr_value ) * 1. / dt + G * (qdot + Minv * dt * (tau - N));
 
   VectorNd hlambda (VectorNd::Zero(nconstraints));
   VectorNd hlambda_lo (VectorNd::Constant(nconstraints, 0.));
@@ -438,4 +475,6 @@ SimBody CreateBoxBody(
   return result;
 }
 
+
+
 }  // namespace RBDLSim
\ No newline at end of file
diff --git a/tests/CollisionTests.cc b/tests/CollisionTests.cc
index dd6740b..ddc8eed 100644
--- a/tests/CollisionTests.cc
+++ b/tests/CollisionTests.cc
@@ -44,3 +44,41 @@ TEST_CASE("Simple Box vs Sphere Collision", "[Collision]") {
     REQUIRE(cresult == false);
   }
 }
+
+TEST_CASE ("CheckCollisionSphereVsPlane", "[Collision]") {
+  SimShape plane;
+  plane.mType = SimShape::Plane;
+  plane.pos = Vector3d (0., 0., 0.);
+  plane.orientation = Quaternion (0., 1., 0., 1.);
+  plane.scale = Vector3d (1., 1., 1.);
+
+  SimShape sphere;
+  sphere.mType = SimShape::Sphere;
+
+  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") {
+    sphere.pos = Vector3d (0., 2.0, 0.);
+    cresult = CheckPenetrationSphereVsPlane(sphere, plane, cinfo);
+
+    REQUIRE(cresult == false);
+  }
+
+  SECTION ("Sphere touching") {
+    sphere.pos = Vector3d (0., 1.5, 0.);
+    cresult = CheckPenetrationSphereVsPlane(sphere, plane, cinfo);
+
+    REQUIRE(cresult == false);
+  }
+
+  SECTION ("Sphere penetration") {
+    sphere.pos = Vector3d (0., -1., 0.);
+    cresult = CheckPenetrationSphereVsPlane(sphere, plane, cinfo);
+
+    REQUIRE(cresult == false);
+  }
+}
\ No newline at end of file