Skip to content
GitLab
Commit 35d3109b authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Add hint to the support function 

This allows a near constant support function for class Convex.
parent 7eb86853
Hide whitespace changes
Inline Side-by-side
include/hpp/fcl/narrowphase/gjk.h
View file @ 35d3109b
......@@ -50,7 +50,8 @@ namespace details
{
/// @brief the support function for shape
Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized);
/// \param hint use to initialize the search when shape is a ConvexBase object.
Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized, int& hint);
/// @brief Minkowski difference class of two shapes
///
......@@ -60,6 +61,8 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized)
/// @note The Minkowski difference is expressed in the frame of the first shape.
struct MinkowskiDiff
{
typedef Eigen::Vector2i hint_t;
/// @brief points to two shapes
const ShapeBase* shapes[2];
......@@ -76,7 +79,8 @@ struct MinkowskiDiff
Eigen::Array<FCL_REAL, 1, 2> inflation;
typedef void (*GetSupportFunction) (const MinkowskiDiff& minkowskiDiff,
const Vec3f& dir, bool dirIsNormalized, Vec3f& support0, Vec3f& support1);
const Vec3f& dir, bool dirIsNormalized, Vec3f& support0, Vec3f& support1,
hint_t& hint);
GetSupportFunction getSupportFunc;
MinkowskiDiff() : getSupportFunc (NULL) {}
......@@ -90,22 +94,22 @@ struct MinkowskiDiff
const Transform3f& tf0, const Transform3f& tf1);
/// @brief support function for shape0
inline Vec3f support0(const Vec3f& d, bool dIsNormalized) const
inline Vec3f support0(const Vec3f& d, bool dIsNormalized, int& hint) const
{
return getSupport(shapes[0], d, dIsNormalized);
return getSupport(shapes[0], d, dIsNormalized, hint);
}
/// @brief support function for shape1
inline Vec3f support1(const Vec3f& d, bool dIsNormalized) const
inline Vec3f support1(const Vec3f& d, bool dIsNormalized, int& hint) const
{
return oR1 * getSupport(shapes[1], oR1.transpose() * d, dIsNormalized) + ot1;
return oR1 * getSupport(shapes[1], oR1.transpose() * d, dIsNormalized, hint) + ot1;
}
/// @brief support function for the pair of shapes
inline void support(const Vec3f& d, bool dIsNormalized, Vec3f& supp0, Vec3f& supp1) const
inline void support(const Vec3f& d, bool dIsNormalized, Vec3f& supp0, Vec3f& supp1, hint_t& hint) const
{
assert(getSupportFunc != NULL);
getSupportFunc(*this, d, dIsNormalized, supp0, supp1);
getSupportFunc(*this, d, dIsNormalized, supp0, supp1, hint);
}
};
......@@ -123,6 +127,7 @@ struct GJK
};
typedef unsigned char vertex_id_t;
typedef MinkowskiDiff::hint_t support_hint_t;
struct Simplex
{
......@@ -138,6 +143,7 @@ struct GJK
MinkowskiDiff const* shape;
Vec3f ray;
support_hint_t support_hint;
/// The distance computed by GJK. The possible values are
/// - \f$ d = - R - 1 \f$ when a collision is detected and GJK
/// cannot compute penetration informations.
......@@ -164,12 +170,14 @@ struct GJK
void initialize();
/// @brief GJK algorithm, given the initial value guess
Status evaluate(const MinkowskiDiff& shape, const Vec3f& guess);
Status evaluate(const MinkowskiDiff& shape, const Vec3f& guess,
const support_hint_t& supportHint = support_hint_t::Zero());
/// @brief apply the support function along a direction, the result is return in sv
inline void getSupport(const Vec3f& d, bool dIsNormalized, SimplexV& sv) const
inline void getSupport(const Vec3f& d, bool dIsNormalized, SimplexV& sv,
support_hint_t& hint) const
{
shape->support(d, dIsNormalized, sv.w0, sv.w1);
shape->support(d, dIsNormalized, sv.w0, sv.w1, hint);
sv.w.noalias() = sv.w0 - sv.w1;
}
......@@ -229,7 +237,8 @@ private:
inline void removeVertex(Simplex& simplex);
/// @brief append one vertex to the simplex
inline void appendVertex(Simplex& simplex, const Vec3f& v, bool isNormalized = false);
inline void appendVertex(Simplex& simplex, const Vec3f& v, bool isNormalized,
support_hint_t& hint);
/// @brief Project origin (0) onto line a-b
bool projectLineOrigin(const Simplex& current, Simplex& next);
......
include/hpp/fcl/narrowphase/narrowphase.h
View file @ 35d3109b
......@@ -52,6 +52,8 @@ namespace fcl
/// @brief collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the GJK in bullet)
struct GJKSolver
{
typedef details::GJK::support_hint_t support_func_guess_t;
/// @brief intersection checking between two shapes
template<typename S1, typename S2>
bool shapeIntersect(const S1& s1, const Transform3f& tf1,
......@@ -61,14 +63,22 @@ namespace fcl
Vec3f* contact_points, Vec3f* normal) const
{
Vec3f guess(1, 0, 0);
if(enable_cached_guess) guess = cached_guess;
support_func_guess_t support_hint;
if(enable_cached_guess) {
guess = cached_guess;
support_hint = support_func_cached_guess;
} else
support_hint.setZero();
details::MinkowskiDiff shape;
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);
if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex();
details::GJK::Status gjk_status = gjk.evaluate(shape, -guess, support_hint);
if(enable_cached_guess) {
cached_guess = gjk.getGuessFromSimplex();
support_func_cached_guess = gjk.support_hint;
}
Vec3f w0, w1;
switch(gjk_status) {
......@@ -127,14 +137,22 @@ namespace fcl
tf_1M2.transform (P3));
Vec3f guess(1, 0, 0);
if(enable_cached_guess) guess = cached_guess;
support_func_guess_t support_hint;
if(enable_cached_guess) {
guess = cached_guess;
support_hint = support_func_cached_guess;
} else
support_hint.setZero();
details::MinkowskiDiff shape;
shape.set (&s, &tri);
details::GJK gjk((unsigned int )gjk_max_iterations, gjk_tolerance);
details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex();
details::GJK::Status gjk_status = gjk.evaluate(shape, -guess, support_hint);
if(enable_cached_guess) {
cached_guess = gjk.getGuessFromSimplex();
support_func_cached_guess = gjk.support_hint;
}
Vec3f w0, w1;
switch(gjk_status) {
......@@ -197,14 +215,22 @@ namespace fcl
FCL_REAL eps (sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
#endif
Vec3f guess(1, 0, 0);
if(enable_cached_guess) guess = cached_guess;
support_func_guess_t support_hint;
if(enable_cached_guess) {
guess = cached_guess;
support_hint = support_func_cached_guess;
} else
support_hint.setZero();
details::MinkowskiDiff shape;
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);
if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex();
details::GJK::Status gjk_status = gjk.evaluate(shape, -guess, support_hint);
if(enable_cached_guess) {
cached_guess = gjk.getGuessFromSimplex();
support_func_cached_guess = gjk.support_hint;
}
if(gjk_status == details::GJK::Failed)
{
......@@ -279,6 +305,7 @@ namespace fcl
epa_tolerance = 1e-6;
enable_cached_guess = false;
cached_guess = Vec3f(1, 0, 0);
support_func_cached_guess = support_func_guess_t::Zero();
}
void enableCachedGuess(bool if_enable) const
......@@ -319,6 +346,9 @@ namespace fcl
/// @brief smart guess
mutable Vec3f cached_guess;
/// @brief smart guess for the support function
mutable support_func_guess_t support_func_cached_guess;
};
#if __cplusplus < 201103L
......
src/narrowphase/gjk.cpp
View file @ 35d3109b
......@@ -97,7 +97,7 @@ template <> struct shape_traits<ConvexBase> : shape_traits_base
};
};
void getShapeSupport(const TriangleP* triangle, const Vec3f& dir, Vec3f& support)
void getShapeSupport(const TriangleP* triangle, const Vec3f& dir, Vec3f& support, int&)
{
FCL_REAL dota = dir.dot(triangle->a);
FCL_REAL dotb = dir.dot(triangle->b);
......@@ -118,25 +118,25 @@ void getShapeSupport(const TriangleP* triangle, const Vec3f& dir, Vec3f& support
}
}
inline void getShapeSupport(const Box* box, const Vec3f& dir, Vec3f& support)
inline void getShapeSupport(const Box* box, const Vec3f& dir, Vec3f& support, int&)
{
const FCL_REAL inflate = (dir.array() == 0).any() ? 1.00000001 : 1.;
support.noalias() = (dir.array() > 0).select(inflate * box->halfSide, -inflate * box->halfSide);
}
inline void getShapeSupport(const Sphere*, const Vec3f& /*dir*/, Vec3f& support)
inline void getShapeSupport(const Sphere*, const Vec3f& /*dir*/, Vec3f& support, int&)
{
support.setZero();
}
inline void getShapeSupport(const Capsule* capsule, const Vec3f& dir, Vec3f& support)
inline void getShapeSupport(const Capsule* capsule, const Vec3f& dir, Vec3f& support, int&)
{
support.head<2>().setZero();
if (dir[2] > 0) support[2] = capsule->halfLength;
else support[2] = - capsule->halfLength;
}
void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support)
void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support, int&)
{
// The cone radius is, for -h < z < h, (h - z) * r / (2*h)
static const FCL_REAL inflate = 1.00001;
......@@ -174,7 +174,7 @@ void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support)
}
}
void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, Vec3f& support)
void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, Vec3f& support, int&)
{
// The inflation makes the object look strictly convex to GJK and EPA. This
// helps solving particular cases (e.g. a cylinder with itself at the same
......@@ -196,39 +196,40 @@ void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, Vec3f& support)
< sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
}
void getShapeSupport(const ConvexBase* convex, const Vec3f& dir, Vec3f& support)
void getShapeSupport(const ConvexBase* convex, const Vec3f& dir, Vec3f& support, int& hint)
{
const Vec3f* pts = convex->points;
const ConvexBase::Neighbors* nn = convex->neighbors;
int i = 0;
FCL_REAL maxdot = pts[i].dot(dir);
if (hint < 0 || hint >= convex->num_points)
hint = 0;
FCL_REAL maxdot = pts[hint].dot(dir);
FCL_REAL dot;
bool found = true;
while (found)
{
const ConvexBase::Neighbors& n = nn[i];
const ConvexBase::Neighbors& n = nn[hint];
found = false;
for (int in = 0; in < n.count(); ++in) {
dot = pts[n[in]].dot(dir);
if (dot > maxdot) {
maxdot = dot;
i = n[in];
hint = n[in];
found = true;
}
}
}
support = pts[i];
support = pts[hint];
}
#define CALL_GET_SHAPE_SUPPORT(ShapeType) \
getShapeSupport (static_cast<const ShapeType*>(shape), \
(shape_traits<ShapeType>::NeedNormalizedDir && !dirIsNormalized) \
? dir.normalized() : dir, \
support)
support, hint)
Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized)
Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized, int& hint)
{
Vec3f support;
switch(shape->getNodeType())
......@@ -269,20 +270,22 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized)
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)
const Vec3f& dir, Vec3f& support0, Vec3f& support1,
MinkowskiDiff::hint_t& hint)
{
getShapeSupport (s0, dir, support0);
getShapeSupport (s0, dir, support0, hint[0]);
if (TransformIsIdentity)
getShapeSupport (s1, - dir, support1);
getShapeSupport (s1, - dir, support1, hint[1]);
else {
getShapeSupport (s1, - oR1.transpose() * dir, support1);
getShapeSupport (s1, - oR1.transpose() * dir, support1, hint[1]);
support1 = oR1 * support1 + ot1;
}
}
template <typename Shape0, typename Shape1, bool TransformIsIdentity>
void getSupportFuncTpl (const MinkowskiDiff& md,
const Vec3f& dir, bool dirIsNormalized, Vec3f& support0, Vec3f& support1)
const Vec3f& dir, bool dirIsNormalized, Vec3f& support0, Vec3f& support1,
MinkowskiDiff::hint_t& hint)
{
enum { NeedNormalizedDir =
bool ( (bool)shape_traits<Shape0>::NeedNormalizedDir
......@@ -301,7 +304,7 @@ void getSupportFuncTpl (const MinkowskiDiff& md,
static_cast <const Shape1*>(md.shapes[1]),
md.oR1, md.ot1,
(NeedNormalizedDir && !dirIsNormalized) ? dir.normalized() : dir,
support0, support1);
support0, support1, hint);
}
template <typename Shape0>
......@@ -508,7 +511,8 @@ bool GJK::getClosestPoints (const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1)
return true;
}
GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess,
const MinkowskiDiff::hint_t& supportHint)
{
size_t iterations = 0;
FCL_REAL alpha = 0;
......@@ -527,9 +531,10 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
distance = 0.0;
simplices[0].rank = 0;
ray = guess;
support_hint = supportHint;
if (ray.squaredNorm() > 0) appendVertex(simplices[0], -ray);
else appendVertex(simplices[0], Vec3f(1, 0, 0), true);
if (ray.squaredNorm() > 0) appendVertex(simplices[0], -ray, false, support_hint);
else appendVertex(simplices[0], Vec3f(1, 0, 0), true, support_hint);
ray = simplices[0].vertex[0]->w;
FCL_REAL rl = ray.norm();
......@@ -558,7 +563,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
break;
}
appendVertex(curr_simplex, -ray); // see below, ray points away from origin
appendVertex(curr_simplex, -ray, false, support_hint); // see below, ray points away from origin
// check removed (by ?): when the new support point is close to previous support points, stop (as the new simplex is degenerated)
const Vec3f& w = curr_simplex.vertex[curr_simplex.rank - 1]->w;
......@@ -622,26 +627,27 @@ inline void GJK::removeVertex(Simplex& simplex)
free_v[nfree++] = simplex.vertex[--simplex.rank];
}
inline void GJK::appendVertex(Simplex& simplex, const Vec3f& v, bool isNormalized)
inline void GJK::appendVertex(Simplex& simplex, const Vec3f& v, bool isNormalized, support_hint_t& hint)
{
simplex.vertex[simplex.rank] = free_v[--nfree]; // set the memory
getSupport (v, isNormalized, *simplex.vertex[simplex.rank++]);
getSupport (v, isNormalized, *simplex.vertex[simplex.rank++], hint);
}
bool GJK::encloseOrigin()
{
Vec3f axis(Vec3f::Zero());
support_hint_t hint = support_hint_t::Zero();
switch(simplex->rank)
{
case 1:
for(size_t i = 0; i < 3; ++i)
{
axis[i] = 1;
appendVertex(*simplex, axis, true);
appendVertex(*simplex, axis, true, hint);
if(encloseOrigin()) return true;
removeVertex(*simplex);
axis[i] = -1;
appendVertex(*simplex, -axis, true);
appendVertex(*simplex, -axis, true, hint);
if(encloseOrigin()) return true;
removeVertex(*simplex);
axis[i] = 0;
......@@ -656,10 +662,10 @@ bool GJK::encloseOrigin()
Vec3f p = d.cross(axis);
if(!p.isZero())
{
appendVertex(*simplex, p);
appendVertex(*simplex, p, false, hint);
if(encloseOrigin()) return true;
removeVertex(*simplex);
appendVertex(*simplex, -p);
appendVertex(*simplex, -p, false, hint);
if(encloseOrigin()) return true;
removeVertex(*simplex);
}
......@@ -673,10 +679,10 @@ bool GJK::encloseOrigin()
(simplex->vertex[2]->w - simplex->vertex[0]->w);
if(!axis.isZero())
{
appendVertex(*simplex, axis);
appendVertex(*simplex, axis, false, hint);
if(encloseOrigin()) return true;
removeVertex(*simplex);
appendVertex(*simplex, -axis);
appendVertex(*simplex, -axis, false, hint);
if(encloseOrigin()) return true;
removeVertex(*simplex);
}
......@@ -1258,6 +1264,7 @@ EPA::SimplexF* EPA::findBest()
EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess)
{
GJK::Simplex& simplex = *gjk.getSimplex();
MinkowskiDiff::hint_t hint (gjk.support_hint);
if((simplex.rank > 1) && gjk.encloseOrigin())
{
while(hull.root)
......@@ -1316,7 +1323,7 @@ EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess)
bool valid = true;
best->pass = ++pass;
// At the moment, SimplexF.n is always normalized. This could be revised in the future...
gjk.getSupport(best->n, true, *w);
gjk.getSupport(best->n, true, *w, hint);
FCL_REAL wdist = best->n.dot(w->w) - best->d;
if(wdist <= tolerance) {
status = AccuracyReached;
......
src/narrowphase/narrowphase.cpp
View file @ 35d3109b
......@@ -496,16 +496,25 @@ bool GJKSolver::shapeDistance<Capsule, Capsule>
t2 (tf2.transform(s2.a), tf2.transform(s2.b), tf2.transform(s2.c));
Vec3f guess;
if(enable_cached_guess) guess = cached_guess;
else guess = (t1.a + t1.b + t1.c - t2.a - t2.b - t2.c) / 3;
support_func_guess_t support_hint;
if(enable_cached_guess) {
guess = cached_guess;
support_hint = support_func_cached_guess;
} else {
support_hint.setZero();
guess = (t1.a + t1.b + t1.c - t2.a - t2.b - t2.c) / 3;
}
bool enable_penetration = true;
details::MinkowskiDiff shape;
shape.set (&t1, &t2);
details::GJK gjk((unsigned int) gjk_max_iterations, gjk_tolerance);
details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex();
details::GJK::Status gjk_status = gjk.evaluate(shape, -guess, support_hint);
if(enable_cached_guess) {
cached_guess = gjk.getGuessFromSimplex();
support_func_cached_guess = gjk.support_hint;
}
gjk.getClosestPoints (shape, p1, p2);
......
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