diff --git a/include/hpp/fcl/narrowphase/gjk.h b/include/hpp/fcl/narrowphase/gjk.h
index 1b31b5d631f387d6bab9336802adba219a7d7b05..1d55d3b52f830be92fb3a8147069d8a0621cc52c 100644
--- a/include/hpp/fcl/narrowphase/gjk.h
+++ b/include/hpp/fcl/narrowphase/gjk.h
@@ -77,13 +77,13 @@ struct MinkowskiDiff
MinkowskiDiff() : getSupportFunc (NULL) {}
+ /// Set the two shapes,
+ /// assuming the relative transformation between them is identity.
void set (const ShapeBase* shape0, const ShapeBase* shape1);
- void set (const Transform3f& tf0, const Transform3f& tf1)
- {
- oR1 = tf0.getRotation().transpose() * tf1.getRotation();
- ot1 = tf0.getRotation().transpose() * (tf1.getTranslation() - tf0.getTranslation());
- }
+ /// Set the two shapes, with a relative transformation.
+ void set (const ShapeBase* shape0, const ShapeBase* shape1,
+ const Transform3f& tf0, const Transform3f& tf1);
/// @brief support function for shape0
inline Vec3f support0(const Vec3f& d, bool dIsNormalized) const
diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h
index b2df87f8f76fcaf5067a9e91d506c678ba87fbf5..5285233068caab91512c808ad9d788da8cf6f4e9 100644
--- a/include/hpp/fcl/narrowphase/narrowphase.h
+++ b/include/hpp/fcl/narrowphase/narrowphase.h
@@ -62,8 +62,7 @@ namespace fcl
if(enable_cached_guess) guess = cached_guess;
details::MinkowskiDiff shape;
- shape.set (&s1, &s2);
- shape.set (tf1, tf2);
+ shape.set (&s1, &s2, tf1, tf2);
details::GJK gjk((unsigned int )gjk_max_iterations, gjk_tolerance);
details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
@@ -103,14 +102,18 @@ namespace fcl
Vec3f& p1, Vec3f& p2, Vec3f& normal) const
{
bool col;
- TriangleP tri(P1, P2, P3);
+ // Express everything in frame 1
+ const Transform3f tf_1M2 (tf1.inverseTimes(tf2));
+ TriangleP tri(
+ tf_1M2.transform (P1),
+ tf_1M2.transform (P2),
+ tf_1M2.transform (P3));
Vec3f guess(1, 0, 0);
if(enable_cached_guess) guess = cached_guess;
details::MinkowskiDiff shape;
shape.set (&s, &tri);
- shape.set (tf1, tf2);
details::GJK gjk((unsigned int )gjk_max_iterations, gjk_tolerance);
details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
@@ -170,8 +173,7 @@ namespace fcl
if(enable_cached_guess) guess = cached_guess;
details::MinkowskiDiff shape;
- shape.set (&s1, &s2);
- shape.set (tf1, tf2);
+ shape.set (&s1, &s2, tf1, tf2);
details::GJK gjk((unsigned int) gjk_max_iterations, gjk_tolerance);
details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
diff --git a/src/narrowphase/details.h b/src/narrowphase/details.h
index 8ff1abf9ae54404883d78586bc57ec0f31cb477d..b8cfeb0f863fda002107c3d11304efb72589646b 100644
--- a/src/narrowphase/details.h
+++ b/src/narrowphase/details.h
@@ -2626,6 +2626,21 @@ namespace fcl {
return true;
}
+
+ /// See the prototype below
+ inline FCL_REAL computePenetration
+ (const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
+ const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
+ Vec3f& normal)
+ {
+ Vec3f u ((P2-P1).cross (P3-P1));
+ normal = u.normalized ();
+ FCL_REAL depth1 ((P1-Q1).dot (normal));
+ FCL_REAL depth2 ((P1-Q2).dot (normal));
+ FCL_REAL depth3 ((P1-Q3).dot (normal));
+ return std::max (depth1, std::max (depth2, depth3));
+ }
+
// Compute penetration distance and normal of two triangles in collision
// Normal is normal of triangle 1 (P1, P2, P3), penetration depth is the
// minimal distance (Q1, Q2, Q3) should be translated along the normal so
@@ -2644,14 +2659,8 @@ namespace fcl {
Vec3f globalQ1 (tf2.transform (Q1));
Vec3f globalQ2 (tf2.transform (Q2));
Vec3f globalQ3 (tf2.transform (Q3));
- Vec3f u ((globalP2-globalP1).cross (globalP3-globalP1));
- normal = u.normalized ();
- FCL_REAL depth;
- FCL_REAL depth1 ((globalP1-globalQ1).dot (normal));
- FCL_REAL depth2 ((globalP1-globalQ2).dot (normal));
- FCL_REAL depth3 ((globalP1-globalQ3).dot (normal));
- depth = std::max (depth1, std::max (depth2, depth3));
- return depth;
+ return computePenetration (globalP1, globalP2, globalP3,
+ globalQ1, globalQ2, globalQ3, normal);
}
} // details
} // namespace fcl
diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp
index e0d7c0cd3b81aeb8f65eb965c7b0053ff06f7d6b..56d88d16c059e96c9fd15256ad7d8669a5e24c3c 100644
--- a/src/narrowphase/gjk.cpp
+++ b/src/narrowphase/gjk.cpp
@@ -210,17 +210,21 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized)
#undef CALL_GET_SHAPE_SUPPORT
-template <typename Shape0, typename Shape1>
+template <typename Shape0, typename Shape1, bool TransformIsIdentity>
void getSupportTpl (const Shape0* s0, const Shape1* s1,
const Matrix3f& oR1, const Vec3f& ot1,
const Vec3f& dir, Vec3f& support0, Vec3f& support1)
{
getShapeSupport (s0, dir, support0);
- getShapeSupport (s1, - oR1.transpose() * dir, support1);
- support1 = oR1 * support1 + ot1;
+ if (TransformIsIdentity)
+ getShapeSupport (s1, - dir, support1);
+ else {
+ getShapeSupport (s1, - oR1.transpose() * dir, support1);
+ support1 = oR1 * support1 + ot1;
+ }
}
-template <typename Shape0, typename Shape1>
+template <typename Shape0, typename Shape1, bool TransformIsIdentity>
void getSupportFuncTpl (const MinkowskiDiff& md,
const Vec3f& dir, bool dirIsNormalized, Vec3f& support0, Vec3f& support1)
{
@@ -228,7 +232,7 @@ void getSupportFuncTpl (const MinkowskiDiff& md,
bool ( (bool)shape_traits<Shape0>::NeedNormalizedDir
|| (bool)shape_traits<Shape1>::NeedNormalizedDir)
};
- getSupportTpl<Shape0, Shape1> (
+ getSupportTpl<Shape0, Shape1, TransformIsIdentity> (
static_cast <const Shape0*>(md.shapes[0]),
static_cast <const Shape1*>(md.shapes[1]),
md.oR1, md.ot1,
@@ -237,62 +241,91 @@ void getSupportFuncTpl (const MinkowskiDiff& md,
}
template <typename Shape0>
-MinkowskiDiff::GetSupportFunction makeGetSupportFunction1 (const ShapeBase* s1)
+MinkowskiDiff::GetSupportFunction makeGetSupportFunction1 (const ShapeBase* s1, bool identity)
{
switch(s1->getNodeType())
{
case GEOM_TRIANGLE:
- return getSupportFuncTpl<Shape0, TriangleP>;
+ if (identity) return getSupportFuncTpl<Shape0, TriangleP, true >;
+ else return getSupportFuncTpl<Shape0, TriangleP, false>;
case GEOM_BOX:
- return getSupportFuncTpl<Shape0, Box>;
+ if (identity) return getSupportFuncTpl<Shape0, Box, true >;
+ else return getSupportFuncTpl<Shape0, Box, false>;
case GEOM_SPHERE:
- return getSupportFuncTpl<Shape0, Sphere>;
+ if (identity) return getSupportFuncTpl<Shape0, Sphere, true >;
+ else return getSupportFuncTpl<Shape0, Sphere, false>;
case GEOM_CAPSULE:
- return getSupportFuncTpl<Shape0, Capsule>;
+ if (identity) return getSupportFuncTpl<Shape0, Capsule, true >;
+ else return getSupportFuncTpl<Shape0, Capsule, false>;
case GEOM_CONE:
- return getSupportFuncTpl<Shape0, Cone>;
+ if (identity) return getSupportFuncTpl<Shape0, Cone, true >;
+ else return getSupportFuncTpl<Shape0, Cone, false>;
case GEOM_CYLINDER:
- return getSupportFuncTpl<Shape0, Cylinder>;
+ if (identity) return getSupportFuncTpl<Shape0, Cylinder, true >;
+ else return getSupportFuncTpl<Shape0, Cylinder, false>;
case GEOM_CONVEX:
- return getSupportFuncTpl<Shape0, Convex>;
+ if (identity) return getSupportFuncTpl<Shape0, Convex, true >;
+ else return getSupportFuncTpl<Shape0, Convex, false>;
default:
throw std::logic_error ("Unsupported geometric shape");
}
}
-void MinkowskiDiff::set (const ShapeBase* shape0, const ShapeBase* shape1)
+MinkowskiDiff::GetSupportFunction makeGetSupportFunction0 (const ShapeBase* s0, const ShapeBase* s1, bool identity)
{
- shapes[0] = shape0;
- shapes[1] = shape1;
-
- switch(shape0->getNodeType())
+ switch(s0->getNodeType())
{
case GEOM_TRIANGLE:
- getSupportFunc = makeGetSupportFunction1<TriangleP> (shape1);
+ return makeGetSupportFunction1<TriangleP> (s1, identity);
break;
case GEOM_BOX:
- getSupportFunc = makeGetSupportFunction1<Box> (shape1);
+ return makeGetSupportFunction1<Box> (s1, identity);
break;
case GEOM_SPHERE:
- getSupportFunc = makeGetSupportFunction1<Sphere> (shape1);
+ return makeGetSupportFunction1<Sphere> (s1, identity);
break;
case GEOM_CAPSULE:
- getSupportFunc = makeGetSupportFunction1<Capsule> (shape1);
+ return makeGetSupportFunction1<Capsule> (s1, identity);
break;
case GEOM_CONE:
- getSupportFunc = makeGetSupportFunction1<Cone> (shape1);
+ return makeGetSupportFunction1<Cone> (s1, identity);
break;
case GEOM_CYLINDER:
- getSupportFunc = makeGetSupportFunction1<Cylinder> (shape1);
+ return makeGetSupportFunction1<Cylinder> (s1, identity);
break;
case GEOM_CONVEX:
- getSupportFunc = makeGetSupportFunction1<Convex> (shape1);
+ return makeGetSupportFunction1<Convex> (s1, identity);
break;
default:
throw std::logic_error ("Unsupported geometric shape");
}
}
+void MinkowskiDiff::set (const ShapeBase* shape0, const ShapeBase* shape1,
+ const Transform3f& tf0, const Transform3f& tf1)
+{
+ shapes[0] = shape0;
+ shapes[1] = shape1;
+
+ oR1 = tf0.getRotation().transpose() * tf1.getRotation();
+ ot1 = tf0.getRotation().transpose() * (tf1.getTranslation() - tf0.getTranslation());
+
+ bool identity = (oR1.isIdentity() && ot1.isZero());
+
+ getSupportFunc = makeGetSupportFunction0 (shape0, shape1, identity);
+}
+
+void MinkowskiDiff::set (const ShapeBase* shape0, const ShapeBase* shape1)
+{
+ shapes[0] = shape0;
+ shapes[1] = shape1;
+
+ oR1.setIdentity();
+ ot1.setZero();
+
+ getSupportFunc = makeGetSupportFunction0 (shape0, shape1, true);
+}
+
void GJK::initialize()
{
nfree = 0;
diff --git a/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp
index 943c4dd7f968f265105b7d89ba7e055a1f570ecd..123d9f2ee78e585ed1f5da2b163dd00bb03dc287 100644
--- a/src/narrowphase/narrowphase.cpp
+++ b/src/narrowphase/narrowphase.cpp
@@ -625,20 +625,16 @@ bool GJKSolver_indep::shapeDistance<Capsule, Capsule>
const TriangleP& s2, const Transform3f& tf2,
FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal) const
{
+ const TriangleP
+ t1 (tf1.transform(s1.a), tf1.transform(s1.b), tf1.transform(s1.c)),
+ t2 (tf2.transform(s2.a), tf2.transform(s2.b), tf2.transform(s2.c));
+
Vec3f guess(1, 0, 0);
if(enable_cached_guess) guess = cached_guess;
bool enable_penetration (true);
details::MinkowskiDiff shape;
- shape.set (&s1, &s2);
- shape.set (tf1, tf2);
-
- const Vec3f& P1 (s1.a);
- const Vec3f& P2 (s1.b);
- const Vec3f& P3 (s1.c);
- const Vec3f& Q1 (s2.a);
- const Vec3f& Q2 (s2.b);
- const Vec3f& Q3 (s2.c);
+ shape.set (&t1, &t2);
details::GJK gjk((unsigned int) gjk_max_iterations, gjk_tolerance);
details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
@@ -653,19 +649,14 @@ bool GJKSolver_indep::shapeDistance<Capsule, Capsule>
// (i.e. an object face normal is colinear to gjk.ray
// assert (dist == (w0 - w1).norm());
dist = gjk.distance;
- p1 = tf1.transform (p1);
- p2 = tf1.transform (p2);
return true;
}
else if (gjk_status == details::GJK::Inside)
{
- p1 = tf1.transform (p1);
- p2 = tf1.transform (p2);
-
if (enable_penetration) {
FCL_REAL penetrationDepth = details::computePenetration
- (P1, P2, P3, Q1, Q2, Q3, tf1, tf2, normal);
+ (t1.a, t1.b, t1.c, t2.a, t2.b, t2.c, normal);
dist = -penetrationDepth;
assert (dist <= 1e-6);
// GJK says Inside when below GJK.tolerance. So non intersecting