diff --git a/include/rbdlsim.h b/include/rbdlsim.h index 4b1ba8e..dfd2c2f 100644 --- a/include/rbdlsim.h +++ b/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, diff --git a/src/rbdlsim.cc b/src/rbdlsim.cc index 2de977f..01a6401 100644 --- a/src/rbdlsim.cc +++ b/src/rbdlsim.cc @@ -1,5 +1,4 @@ #include "rbdlsim.h" -#include "utils.h" #include #include @@ -11,6 +10,8 @@ #include #include +#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() - * (-cinfo.deltaImpulse); + body_a->qdot += + cinfo.MInvA * cinfo.jacA.transpose() * (-cinfo.deltaImpulse); } if (body_b && !body_b->mIsStatic) { - body_b->qdot += cinfo.MInvB * cinfo.jacB.transpose() - * (cinfo.deltaImpulse); + body_b->qdot += cinfo.MInvB * cinfo.jacB.transpose() * (cinfo.deltaImpulse); } } diff --git a/tests/CollisionTests.cc b/tests/CollisionTests.cc index 86877b4..8fdc33e 100644 --- a/tests/CollisionTests.cc +++ b/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]") {