From a881ad5ae2bbece7d2587a37d8c4418ba51ca79e Mon Sep 17 00:00:00 2001 From: Martin Felis Date: Tue, 6 Oct 2020 20:22:27 +0200 Subject: [PATCH] Added Collision Test Sphere vs Sphere, Collision returns for each body a separate contact point. --- include/rbdlsim.h | 20 ++++----- src/rbdlsim.cc | 71 ++++++++++++++++++++++++-------- tests/CollisionTests.cc | 90 +++++++++++++++++++++++++++++++++++------ 3 files changed, 142 insertions(+), 39 deletions(-) diff --git a/include/rbdlsim.h b/include/rbdlsim.h index dffa06d..e72f202 100644 --- a/include/rbdlsim.h +++ b/include/rbdlsim.h @@ -18,11 +18,7 @@ struct CollisionInfo; const double cCollisionEps = 1.0e-4; struct SimShape { - enum ShapeType { - Box = 0, - Sphere = 1, - Plane = 2 - }; + enum ShapeType { Box = 0, Sphere = 1, Plane = 2 }; ShapeType mType; Vector3d pos; Quaternion orientation; @@ -32,13 +28,13 @@ struct SimShape { struct SimBody { VectorNd q, qdot, qddot, tau; Model mModel; - + typedef std::pair BodyCollisionInfo; std::vector mCollisionShapes; void step(double ts); - void resolveCollisions (double dt, std::vector& collisions); - void calcNextPositions (double dt, const VectorNd& in_qdot, VectorNd& out_q); + void resolveCollisions(double dt, std::vector& collisions); + 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, diff --git a/src/rbdlsim.cc b/src/rbdlsim.cc index 60cb173..5986ec3 100644 --- a/src/rbdlsim.cc +++ 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; } @@ -204,7 +241,7 @@ bool SolveGaussSeidelProj( return true; } - if (iter > maxiter) { + if (iter > maxiter) { break; } } @@ -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_lo (VectorNd::Constant(nconstraints, 0.)); - VectorNd hlambda_hi (VectorNd::Constant(nconstraints, 1.0e9)); + VectorNd hlambda(VectorNd::Zero(nconstraints)); + VectorNd hlambda_lo(VectorNd::Constant(nconstraints, 0.)); + 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 \ No newline at end of file diff --git a/tests/CollisionTests.cc b/tests/CollisionTests.cc index ddc8eed..ac3d318 100644 --- a/tests/CollisionTests.cc +++ b/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.orientation = Quaternion (0., 1., 0., 1.); - plane.scale = Vector3d (1., 1., 1.); + 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.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.); + 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.); + SECTION("Sphere touching") { + 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 == true); + } + + SECTION("Sphere penetration") { + 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 == 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 ("Sphere penetration") { - sphere.pos = Vector3d (0., -1., 0.); - cresult = CheckPenetrationSphereVsPlane(sphere, plane, cinfo); + 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 == false); + 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)); } } \ No newline at end of file