Commit 5712c6a4 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

[GJK] Put shapes in the same frame when possible.

parent 4cb0ed89
......@@ -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
......
......@@ -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);
......
......@@ -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
......
......@@ -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;
......
......@@ -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
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment