From 1df482ba4301e9a9de64c31042cdb475f35f9fcb Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Tue, 25 Dec 2018 11:24:17 +0100
Subject: [PATCH] [GJK] Slightly modify function getSupport for cylinders.
- if dir [2] == 0, return a point with z coordinate equal to 0,
- add a test between box and cylinder that did make GJK algorithm fail.
---
src/narrowphase/gjk.cpp | 11 ++++--
test/test_fcl_geometric_shapes.cpp | 56 ++++++++++++++++++++++++++++++
2 files changed, 65 insertions(+), 2 deletions(-)
diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp
index 9862fdbb..e45cd21a 100644
--- a/src/narrowphase/gjk.cpp
+++ b/src/narrowphase/gjk.cpp
@@ -46,6 +46,7 @@ namespace details
Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir)
{
+ FCL_REAL eps (sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
switch(shape->getNodeType())
{
case GEOM_TRIANGLE:
@@ -126,15 +127,21 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir)
const Cylinder* cylinder = static_cast<const Cylinder*>(shape);
FCL_REAL zdist = std::sqrt(dir[0] * dir[0] + dir[1] * dir[1]);
FCL_REAL half_h = cylinder->lz * 0.5;
+ Vec3f res;
if(zdist == 0.0)
{
- return Vec3f(0, 0, (dir[2]>0)? half_h:-half_h);
+ res = Vec3f(0, 0, (dir[2]>0)? half_h:-half_h);
}
else
{
FCL_REAL d = cylinder->radius / zdist;
- return Vec3f(d * dir[0], d * dir[1], (dir[2]>0)?half_h:-half_h);
+ FCL_REAL z (0.);
+ if (dir [2] > eps) z = half_h;
+ else if (dir [2] < -eps) z = -half_h;
+ res = Vec3f(d * dir[0], d * dir[1], z);
}
+ assert (fabs (res [0] * dir [1] - res [1] * dir [0]) < eps);
+ return res;
}
break;
case GEOM_CONVEX:
diff --git a/test/test_fcl_geometric_shapes.cpp b/test/test_fcl_geometric_shapes.cpp
index 5e05a114..808e0b22 100644
--- a/test/test_fcl_geometric_shapes.cpp
+++ b/test/test_fcl_geometric_shapes.cpp
@@ -188,6 +188,62 @@ void testShapeIntersection(const S1& s1, const Transform3f& tf1,
}
}
+BOOST_AUTO_TEST_CASE (shapeIntersection_cylinderbox)
+{
+ Cylinder s1 (0.029, 0.1);
+ Box s2 (1.6, 0.6, 0.025);
+
+ Transform3f tf1
+ (Quaternion3f (0.5279170511703305, -0.50981118132505521,
+ -0.67596178682051911, 0.0668715876735793),
+ Vec3f (0.041218354748013122, 1.2022554710435607, 0.77338855025700015));
+
+ Transform3f tf2
+ (Quaternion3f (0.70738826916719977, 0, 0, 0.70682518110536596),
+ Vec3f (-0.29936284351096382, 0.80023864435868775, 0.71750000000000003));
+
+ GJKSolver_indep solver;
+ FCL_REAL distance;
+ Vec3f p1, p2, normal;
+ bool res = solver.shapeDistance (s1, tf1, s2, tf2, distance, p1, p2, normal);
+ BOOST_CHECK ((res && distance > 0) || (!res && distance <= 0));
+ // If objects are not colliding, p2 should be outside the cylinder and
+ // p1 should be outside the box
+ Vec3f p2Loc (inverse (tf1).transform (p2));
+ bool p2_in_cylinder ((fabs (p2Loc [2]) <= .5*s1.lz) &&
+ (p2Loc [0] * p2Loc [0] + p2Loc [1] * p2Loc [1]
+ <= s1.radius));
+ Vec3f p1Loc (inverse (tf2).transform (p1));
+ bool p1_in_box ((fabs (p1Loc [0]) <= .5 * s2.side [0]) &&
+ (fabs (p1Loc [1]) <= .5 * s2.side [1]) &&
+ (fabs (p1Loc [2]) <= .5 * s2.side [2]));
+ std::cout << "p2 in cylinder = (" << p2Loc.transpose () << ")" << std::endl;
+ std::cout << "p1 in box = (" << p1Loc.transpose () << ")" << std::endl;
+
+ BOOST_CHECK ((res && !p2_in_cylinder && !p1_in_box) ||
+ (!res && p2_in_cylinder && p1_in_box));
+
+ res = solver.shapeDistance (s2, tf2, s1, tf1, distance, p2, p1, normal);
+ BOOST_CHECK ((res && distance > 0) || (!res && distance <= 0));
+ // If objects are not colliding, p2 should be outside the cylinder and
+ // p1 should be outside the box
+
+ p2Loc = inverse (tf1).transform (p2);
+ p2_in_cylinder = (fabs (p2Loc [2]) <= .5*s1.lz) &&
+ (p2Loc [0] * p2Loc [0] + p2Loc [1] * p2Loc [1]
+ <= s1.radius);
+ p1Loc = inverse (tf2).transform (p1);
+ p1_in_box = (fabs (p1Loc [0]) <= .5 * s2.side [0]) &&
+ (fabs (p1Loc [1]) <= .5 * s2.side [1]) &&
+ (fabs (p1Loc [2]) <= .5 * s2.side [2]);
+
+ std::cout << "p2 in cylinder = (" << p2Loc.transpose () << ")" << std::endl;
+ std::cout << "p1 in box = (" << p1.transpose () << ")" << std::endl;
+
+ BOOST_CHECK ((res && !p2_in_cylinder && !p1_in_box) ||
+ (!res && p2_in_cylinder && p1_in_box));
+}
+
BOOST_AUTO_TEST_CASE(shapeIntersection_spheresphere)
{
Sphere s1(20);
--
GitLab