From 0624a7e1aca89f5ee826e48e29c6e8c244b59746 Mon Sep 17 00:00:00 2001
From: Martin Felis <martin@fysx.org>
Date: Fri, 20 Nov 2020 21:47:17 +0100
Subject: [PATCH] Properly allowing for rotated box vs plane checking.

---
 src/rbdlsim.cc          | 44 +++++++++++++++++++++++++++++------------
 tests/CollisionTests.cc |  8 ++++++--
 2 files changed, 37 insertions(+), 15 deletions(-)

diff --git a/src/rbdlsim.cc b/src/rbdlsim.cc
index 01a6401..a39bb51 100644
--- a/src/rbdlsim.cc
+++ b/src/rbdlsim.cc
@@ -94,6 +94,13 @@ void SimShapeSupport(
   ccdVec3Add(v, &pos);
 }
 
+static void sSwapCollisionInfoShapeOrder(CollisionInfo &cinfo) {
+  cinfo.dir *= -1.;
+  Vector3d temp_pos = cinfo.posA;
+  cinfo.posA = cinfo.posB;
+  cinfo.posB = temp_pos;
+}
+
 bool CheckPenetration(
     const SimShape& shape_a,
     const SimShape& shape_b,
@@ -105,10 +112,7 @@ 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;
+    sSwapCollisionInfoShapeOrder(cinfo);
     return result;
   }
   if (shape_a.mType == SimShape::Sphere && shape_b.mType == SimShape::Sphere) {
@@ -117,6 +121,11 @@ bool CheckPenetration(
   if (shape_a.mType == SimShape::Box && shape_b.mType == SimShape::Plane) {
     return CheckPenetrationBoxVsPlane(shape_a, shape_b, cinfo);
   }
+  if (shape_a.mType == SimShape::Plane && shape_b.mType == SimShape::Box) {
+    bool result = CheckPenetrationBoxVsPlane(shape_a, shape_b, cinfo);
+    sSwapCollisionInfoShapeOrder(cinfo);
+    return result;
+  }
 
   ccd_t ccd;
   CCD_INIT(&ccd);
@@ -181,7 +190,8 @@ bool CheckPenetrationSphereVsPlane(
       (shape_a.orientation - Quaternion(0., 0., 0., 1.)).squaredNorm()
       < cCollisionEps);
 
-  Vector3d plane_normal = shape_b.orientation.conjugate().rotate(Vector3d (0., 1., 0.));
+  Vector3d plane_normal =
+      shape_b.orientation.conjugate().rotate(Vector3d(0., 1., 0.));
   Vector3d plane_point = shape_b.pos;
   Vector3d sphere_point_to_plane =
       shape_a.pos - plane_normal * shape_a.scale[0] * 0.5;
@@ -216,7 +226,7 @@ void CalcIntersectionLineSegmentPlane(
   result = vA + s * (vB - vA);
 }
 
-bool CheckPenetrationBoxVsPlane (
+bool CheckPenetrationBoxVsPlane(
     const SimShape& shape_a,
     const SimShape& shape_b,
     CollisionInfo& cinfo) {
@@ -229,13 +239,19 @@ bool CheckPenetrationBoxVsPlane (
   aabb.pos.setZero();
   aabb.orientation = Quaternion(0., 0., 0., 1.);
 
-  plane.pos = shape_a.orientation.rotate(shape_b.pos - shape_a.pos);
-  Quaternion rot_rel_box = shape_a.orientation.conjugate() * shape_b.orientation;
+  const Matrix3d shape_a_rot = shape_a.orientation.toMatrix();
+  plane.pos = shape_a_rot * (shape_b.pos - shape_a.pos);
+  Quaternion rot_rel_box =
+      shape_a.orientation.conjugate() * shape_b.orientation;
   plane.orientation = rot_rel_box;
 
   bool result = CheckPenetrationAABBVsPlane(aabb, plane, cinfo);
 
-  // TODO: transform values back into world space
+  cinfo.posA = shape_a_rot.transpose() * (cinfo.posA) + shape_a.pos;
+  cinfo.posB = shape_a_rot.transpose() * (cinfo.posB) + shape_a.pos;
+  cinfo.dir = shape_a_rot.transpose() * (cinfo.dir);
+
+  return result;
 }
 
 bool CheckPenetrationAABBVsPlane(
@@ -246,7 +262,8 @@ bool CheckPenetrationAABBVsPlane(
   assert(shape_b.mType == SimShape::Plane);
   cinfo.mNumManifoldPoints = 0;
 
-  Vector3d plane_normal = shape_b.orientation.conjugate().rotate(Vector3d (0., 1., 0.));
+  Vector3d plane_normal =
+      shape_b.orientation.conjugate().rotate(Vector3d(0., 1., 0.));
   Vector3d plane_pos = shape_b.pos;
   Vector3d dir_min, dir_max;
 
@@ -263,12 +280,15 @@ bool CheckPenetrationAABBVsPlane(
     return false;
   }
 
+  // Separation direction clear: we would need to push the plane along its
+  // negative plane normal for separation.
+  cinfo.dir = -plane_normal;
+
   // If center is below plane, return that
   double center_distance = (Vector3d::Zero() - plane_pos).dot(plane_normal);
   if (center_distance < cCollisionEps) {
     cinfo.posA = Vector3d::Zero();
     cinfo.posB = Vector3d::Zero();
-    cinfo.dir = -plane_normal;
     cinfo.depth = center_distance - cCollisionEps;
     return true;
   }
@@ -340,9 +360,7 @@ bool CheckPenetrationAABBVsPlane(
     cinfo.posA += cinfo.mManifoldPoints[i];
   }
   cinfo.posA = cinfo.posA / (double)cinfo.mNumManifoldPoints;
-
   cinfo.posB = cinfo.posA;
-  cinfo.dir = -plane_normal;
   cinfo.depth = max_depth;
 
   return true;
diff --git a/tests/CollisionTests.cc b/tests/CollisionTests.cc
index 8fdc33e..43ac553 100644
--- a/tests/CollisionTests.cc
+++ b/tests/CollisionTests.cc
@@ -89,6 +89,7 @@ TEST_CASE ("AABB vs Plane", "[Collision]") {
 
     REQUIRE(cresult == true);
     REQUIRE((cinfo.posA - Vector3d(0, -0.5, -0.5)).norm() < 1.0e-12);
+    REQUIRE((cinfo.dir - Vector3d(0, -sqrt(2.) * 0.5, -sqrt(2.) * 0.5)).norm() < 1.0e-12);
   }
 
   SECTION("Unit AABB Intersecting Contact Rotated Plane") {
@@ -100,14 +101,17 @@ TEST_CASE ("AABB vs Plane", "[Collision]") {
     REQUIRE((cinfo.posA - Vector3d(0, -0.4, -0.4)).norm() < 1.0e-12);
   }
 
-  SECTION("RotatedBox Touching Plane") {
+  SECTION("Rotated Unit Box Touching Plane") {
     box.orientation = Quaternion::fromAxisAngle(Vector3d (1.0, 0.0, 0.0), M_PI * 0.25);
     plane.pos.set(0., -sqrt(2.) * 0.5, -sqrt(2.) * 0.5);
     plane.orientation = Quaternion(0., 0., 0., 1.);
     cresult = CheckPenetrationBoxVsPlane(box, plane, cinfo);
 
     REQUIRE(cresult == true);
-//    REQUIRE((cinfo.posA - Vector3d(0, -0.4, -0.4)).norm() < 1.0e-12);
+    REQUIRE(fabs(cinfo.depth) <= cCollisionEps);
+    REQUIRE((cinfo.posA - Vector3d(0, -sqrt(2.) * 0.5, 0.)).norm() < 1.0e-12);
+    REQUIRE((cinfo.posB - Vector3d(0, -sqrt(2.) * 0.5, 0.)).norm() < 1.0e-12);
+    REQUIRE((cinfo.dir - Vector3d(0, -1., 0.)).norm() < 1.0e-12);
   }
 }