Inital works of Box vs Plan collision.

include/rbdlsim.h

@@ -15,7 +15,7 @@ struct SimShape;
 struct SimBody;
 struct CollisionInfo;
 
-const double cCollisionEps = 1.0e-4;
+const double cCollisionEps = 1.0e-5;
 
 struct SimShape {
   enum ShapeType { Box = 0, Sphere = 1, Plane = 2 };
@@ -47,6 +47,8 @@ struct CollisionInfo {
   const SimShape* mShapeB = nullptr;
   int mBodyAIndex;
   int mBodyBIndex;
+  Vector3d mManifoldPoints[4];
+  int mNumManifoldPoints = 0;
   Vector3d posA = Vector3d::Zero();
   Vector3d posB = Vector3d::Zero();
   double biasVelocityA = 0.;
@@ -93,6 +95,16 @@ bool CheckPenetrationSphereVsPlane(
     const SimShape& shape_b,
     CollisionInfo& cinfo);
 
+bool CheckPenetrationBoxVsPlane (
+    const SimShape& shape_a,
+    const SimShape& shape_b,
+    CollisionInfo& cinfo);
+
+bool CheckPenetrationAABBVsPlane(
+    const SimShape& shape_a,
+    const SimShape& shape_b,
+    CollisionInfo& cinfo);
+
 SimBody CreateSphereBody(
     double mass,
     double radius,

src/rbdlsim.cc

@@ -1,5 +1,4 @@
 #include "rbdlsim.h"
-#include "utils.h"
 
 #include <ccd/ccd.h>
 #include <ccd/quat.h>
@@ -11,6 +10,8 @@
 #include <iostream>
 #include <vector>
 
+#include "utils.h"
+
 using namespace std;
 
 namespace RBDLSim {
@@ -113,6 +114,9 @@ bool CheckPenetration(
   if (shape_a.mType == SimShape::Sphere && shape_b.mType == SimShape::Sphere) {
     return CheckPenetrationSphereVsSphere(shape_a, shape_b, cinfo);
   }
+  if (shape_a.mType == SimShape::Box && shape_b.mType == SimShape::Plane) {
+    return CheckPenetrationBoxVsPlane(shape_a, shape_b, cinfo);
+  }
 
   ccd_t ccd;
   CCD_INIT(&ccd);
@@ -177,7 +181,7 @@ bool CheckPenetrationSphereVsPlane(
       (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.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;
@@ -197,6 +201,153 @@ bool CheckPenetrationSphereVsPlane(
   return 0;
 }
 
+void CalcIntersectionLineSegmentPlane(
+    const Vector3d& vA,
+    const Vector3d& vB,
+    const Vector3d& plane_point,
+    const Vector3d& plane_normal,
+    Vector3d& result) {
+  double dA = (vA - plane_point).dot(plane_normal);
+  double dB = (vB - plane_point).dot(plane_normal);
+
+  double s = (-dA) / (dB - dA);
+  assert(s >= 0);
+  assert(s <= 1.);
+  result = vA + s * (vB - vA);
+}
+
+bool CheckPenetrationBoxVsPlane (
+    const SimShape& shape_a,
+    const SimShape& shape_b,
+    CollisionInfo& cinfo) {
+  assert(shape_a.mType == SimShape::Box);
+  assert(shape_b.mType == SimShape::Plane);
+
+  SimShape aabb = shape_a;
+  SimShape plane = shape_b;
+
+  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;
+  plane.orientation = rot_rel_box;
+
+  bool result = CheckPenetrationAABBVsPlane(aabb, plane, cinfo);
+
+  // TODO: transform values back into world space
+}
+
+bool CheckPenetrationAABBVsPlane(
+    const SimShape& shape_a,
+    const SimShape& shape_b,
+    CollisionInfo& cinfo) {
+  assert(shape_a.mType == SimShape::Box);
+  assert(shape_b.mType == SimShape::Plane);
+  cinfo.mNumManifoldPoints = 0;
+
+  Vector3d plane_normal = shape_b.orientation.conjugate().rotate(Vector3d (0., 1., 0.));
+  Vector3d plane_pos = shape_b.pos;
+  Vector3d dir_min, dir_max;
+
+  for (int i = 0; i < 3; i++) {
+    double sign = plane_normal[i] >= 0 ? -1. : 1.;
+
+    dir_min[i] = sign * shape_a.scale[i] * 0.5;
+    dir_max[i] = -sign * shape_a.scale[i] * 0.5;
+  }
+
+  double distance = (dir_min - plane_pos).dot(plane_normal);
+  if (distance > cCollisionEps) {
+    // early out: all points above plane
+    return false;
+  }
+
+  // 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;
+  }
+
+  const Vector3d& scale = shape_a.scale;
+  // clang-format off
+  Vector3d vertices[8] = {
+      Vector3d( scale[0] * 0.5, -scale[0] * 0.5,  scale[0] * 0.5),
+      Vector3d( scale[0] * 0.5, -scale[0] * 0.5, -scale[0] * 0.5),
+      Vector3d( scale[0] * 0.5,  scale[0] * 0.5,  scale[0] * 0.5),
+      Vector3d( scale[0] * 0.5,  scale[0] * 0.5, -scale[0] * 0.5),
+      Vector3d(-scale[0] * 0.5, -scale[0] * 0.5,  scale[0] * 0.5),
+      Vector3d(-scale[0] * 0.5, -scale[0] * 0.5, -scale[0] * 0.5),
+      Vector3d(-scale[0] * 0.5,  scale[0] * 0.5,  scale[0] * 0.5),
+      Vector3d(-scale[0] * 0.5,  scale[0] * 0.5, -scale[0] * 0.5)
+  };
+  // clang-format on
+
+  // Check whether any vertices are touching the plane.
+  double distances[8];
+  double max_depth = -cCollisionEps;
+  for (int i = 0; i < 8; i++) {
+    distances[i] = (vertices[i] - plane_pos).dot(plane_normal);
+    if (distances[i] >= 0. && distances[i] < cCollisionEps) {
+      cinfo.mManifoldPoints[cinfo.mNumManifoldPoints++] = vertices[i];
+    }
+    max_depth = distances[i] < max_depth ? distances[i] : max_depth;
+  }
+
+  if (cinfo.mNumManifoldPoints == 0) {
+    // We have no vertex contacts so we have to compute points on the edges.
+    const int num_edges = 12;
+    char edge_pairs[num_edges][2] = {
+        {0, 1},
+        {0, 2},
+        {0, 4},
+        {1, 3},
+        {1, 5},
+        {2, 3},
+        {2, 6},
+        {3, 7},
+        {4, 5},
+        {4, 6},
+        {5, 7},
+        {6, 7}};
+
+    for (int i = 0; i < num_edges; i++) {
+      const double& d0 = distances[edge_pairs[i][0]];
+      const double& d1 = distances[edge_pairs[i][1]];
+
+      if (d0 * d1 < 0) {
+        // we have an intersection on this edge, compute the contact point on
+        // this edge.
+        const Vector3d& v0 = vertices[edge_pairs[i][0]];
+        const Vector3d& v1 = vertices[edge_pairs[i][1]];
+
+        double s = (-d0) / (d1 - d0);
+        assert(s >= 0);
+        assert(s <= 1.);
+
+        cinfo.mManifoldPoints[cinfo.mNumManifoldPoints++] = v0 + s * (v1 - v0);
+      }
+    }
+  }
+
+  // Average manifold points to compute the central contact point.
+  cinfo.posA.setZero();
+  for (int i = 0; i < cinfo.mNumManifoldPoints; i++) {
+    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;
+}
+
 void SimBody::updateCollisionShapes() {
   UpdateKinematicsCustom(mModel, &q, nullptr, nullptr);
 
@@ -393,13 +544,12 @@ void ApplyConstraintImpulse(
     SimBody* body_b,
     CollisionInfo& cinfo) {
   if (body_a && !body_a->mIsStatic) {
-    body_a->qdot += cinfo.MInvA * cinfo.jacA.transpose()
+    body_a->qdot +=
-                    * (-cinfo.deltaImpulse);
+        cinfo.MInvA * cinfo.jacA.transpose() * (-cinfo.deltaImpulse);
   }
 
   if (body_b && !body_b->mIsStatic) {
-    body_b->qdot += cinfo.MInvB * cinfo.jacB.transpose()
+    body_b->qdot += cinfo.MInvB * cinfo.jacB.transpose() * (cinfo.deltaImpulse);
-                    * (cinfo.deltaImpulse);
   }
 }
 

tests/CollisionTests.cc

@@ -45,11 +45,77 @@ TEST_CASE("Simple Box vs Sphere Collision", "[Collision]") {
   }
 }
 
+TEST_CASE ("AABB vs Plane", "[Collision]") {
+  SimShape plane;
+  plane.mType = SimShape::Plane;
+  plane.pos = Vector3d(0., 0., 0.);
+  plane.orientation = Quaternion(0., 0., 0., 1.);
+  plane.scale = Vector3d(1., 1., 1.);
+
+  SimShape box;
+  box.mType = SimShape::Box;
+  box.pos.set(0.0, 0.0, 0.);
+  box.scale.set(1., 1., 1.);
+  box.orientation.set(0., 0., 0., 1.);
+
+  bool cresult = false;
+  CollisionInfo cinfo;
+
+  SECTION("Unit AABB above Plane") {
+    plane.pos.set(0.0, -0.6, 0.);
+    cresult = CheckPenetration(box, plane, cinfo);
+
+    REQUIRE(cresult == false);
+  }
+
+  SECTION("Unit AABB below Plane") {
+    plane.pos.set(0.0, 0.6, 0.);
+    cresult = CheckPenetration(box, plane, cinfo);
+
+    REQUIRE(cresult == true);
+  }
+
+  SECTION("Unit AABB on Plane") {
+    plane.pos.set(0.0, 0.5, 0.);
+    cresult = CheckPenetration(box, plane, cinfo);
+
+    REQUIRE(cresult == true);
+  }
+
+  SECTION("Unit AABB Edge Contact Rotated Plane") {
+    plane.pos.set(10., -0.5, -0.5);
+    plane.orientation = Quaternion::fromAxisAngle(Vector3d (1.0, 0., 0.), M_PI * 0.25);
+    cresult = CheckPenetrationBoxVsPlane (box, plane, cinfo);
+
+    REQUIRE(cresult == true);
+    REQUIRE((cinfo.posA - Vector3d(0, -0.5, -0.5)).norm() < 1.0e-12);
+  }
+
+  SECTION("Unit AABB Intersecting Contact Rotated Plane") {
+    plane.pos.set(10., -0.4, -0.4);
+    plane.orientation = Quaternion::fromAxisAngle(Vector3d (1.0, 0., 0.), M_PI * 0.25);
+    cresult = CheckPenetrationBoxVsPlane(box, plane, cinfo);
+
+    REQUIRE(cresult == true);
+    REQUIRE((cinfo.posA - Vector3d(0, -0.4, -0.4)).norm() < 1.0e-12);
+  }
+
+  SECTION("RotatedBox 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);
+  }
+}
+
 TEST_CASE("CheckCollisionSphereVsPlane", "[Collision]") {
   SimShape plane;
   plane.mType = SimShape::Plane;
   plane.pos = Vector3d(0., 0., 0.);
-  plane.orientation = Quaternion(0., 1., 0., 1.);
+  plane.orientation = Quaternion(0., 0., 0., 1.);
   plane.scale = Vector3d(1., 1., 1.);
 
   SimShape sphere;
@@ -94,7 +160,6 @@ TEST_CASE("CheckCollisionSphereVsPlane", "[Collision]") {
 
     REQUIRE(cresult == true);
   }
-
 }
 
 TEST_CASE("CheckCollisionSphereVsSphere", "[Collision]") {