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
...@@ -50,7 +50,8 @@ namespace details ...@@ -50,7 +50,8 @@ namespace details
{ {
/// @brief the support function for shape /// @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 /// @brief Minkowski difference class of two shapes
/// ///
...@@ -60,6 +61,8 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized) ...@@ -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. /// @note The Minkowski difference is expressed in the frame of the first shape.
struct MinkowskiDiff struct MinkowskiDiff
{ {
typedef Eigen::Vector2i hint_t;
/// @brief points to two shapes /// @brief points to two shapes
const ShapeBase* shapes[2]; const ShapeBase* shapes[2];
...@@ -76,7 +79,8 @@ struct MinkowskiDiff ...@@ -76,7 +79,8 @@ struct MinkowskiDiff
Eigen::Array<FCL_REAL, 1, 2> inflation; Eigen::Array<FCL_REAL, 1, 2> inflation;
typedef void (*GetSupportFunction) (const MinkowskiDiff& minkowskiDiff, 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; GetSupportFunction getSupportFunc;
MinkowskiDiff() : getSupportFunc (NULL) {} MinkowskiDiff() : getSupportFunc (NULL) {}
...@@ -90,22 +94,22 @@ struct MinkowskiDiff ...@@ -90,22 +94,22 @@ struct MinkowskiDiff
const Transform3f& tf0, const Transform3f& tf1); const Transform3f& tf0, const Transform3f& tf1);
/// @brief support function for shape0 /// @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 /// @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 /// @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); assert(getSupportFunc != NULL);
getSupportFunc(*this, d, dIsNormalized, supp0, supp1); getSupportFunc(*this, d, dIsNormalized, supp0, supp1, hint);
} }
}; };
...@@ -123,6 +127,7 @@ struct GJK ...@@ -123,6 +127,7 @@ struct GJK
}; };
typedef unsigned char vertex_id_t; typedef unsigned char vertex_id_t;
typedef MinkowskiDiff::hint_t support_hint_t;
struct Simplex struct Simplex
{ {
...@@ -138,6 +143,7 @@ struct GJK ...@@ -138,6 +143,7 @@ struct GJK
MinkowskiDiff const* shape; MinkowskiDiff const* shape;
Vec3f ray; Vec3f ray;
support_hint_t support_hint;
/// The distance computed by GJK. The possible values are /// The distance computed by GJK. The possible values are
/// - \f$ d = - R - 1 \f$ when a collision is detected and GJK /// - \f$ d = - R - 1 \f$ when a collision is detected and GJK
/// cannot compute penetration informations. /// cannot compute penetration informations.
...@@ -164,12 +170,14 @@ struct GJK ...@@ -164,12 +170,14 @@ struct GJK
void initialize(); void initialize();
/// @brief GJK algorithm, given the initial value guess /// @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 /// @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; sv.w.noalias() = sv.w0 - sv.w1;
} }
...@@ -229,7 +237,8 @@ private: ...@@ -229,7 +237,8 @@ private:
inline void removeVertex(Simplex& simplex); inline void removeVertex(Simplex& simplex);
/// @brief append one vertex to the 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 /// @brief Project origin (0) onto line a-b
bool projectLineOrigin(const Simplex& current, Simplex& next); bool projectLineOrigin(const Simplex& current, Simplex& next);
......
...@@ -52,6 +52,8 @@ namespace fcl ...@@ -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) /// @brief collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the GJK in bullet)
struct GJKSolver struct GJKSolver
{ {
typedef details::GJK::support_hint_t support_func_guess_t;
/// @brief intersection checking between two shapes /// @brief intersection checking between two shapes
template<typename S1, typename S2> template<typename S1, typename S2>
bool shapeIntersect(const S1& s1, const Transform3f& tf1, bool shapeIntersect(const S1& s1, const Transform3f& tf1,
...@@ -61,14 +63,22 @@ namespace fcl ...@@ -61,14 +63,22 @@ namespace fcl
Vec3f* contact_points, Vec3f* normal) const Vec3f* contact_points, Vec3f* normal) const
{ {
Vec3f guess(1, 0, 0); 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; details::MinkowskiDiff shape;
shape.set (&s1, &s2, tf1, tf2); shape.set (&s1, &s2, tf1, tf2);
details::GJK gjk((unsigned int )gjk_max_iterations, gjk_tolerance); details::GJK gjk((unsigned int )gjk_max_iterations, gjk_tolerance);
details::GJK::Status gjk_status = gjk.evaluate(shape, -guess); details::GJK::Status gjk_status = gjk.evaluate(shape, -guess, support_hint);
if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex(); if(enable_cached_guess) {
cached_guess = gjk.getGuessFromSimplex();
support_func_cached_guess = gjk.support_hint;
}
Vec3f w0, w1; Vec3f w0, w1;
switch(gjk_status) { switch(gjk_status) {
...@@ -127,14 +137,22 @@ namespace fcl ...@@ -127,14 +137,22 @@ namespace fcl
tf_1M2.transform (P3)); tf_1M2.transform (P3));
Vec3f guess(1, 0, 0); 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; details::MinkowskiDiff shape;
shape.set (&s, &tri); shape.set (&s, &tri);
details::GJK gjk((unsigned int )gjk_max_iterations, gjk_tolerance); details::GJK gjk((unsigned int )gjk_max_iterations, gjk_tolerance);
details::GJK::Status gjk_status = gjk.evaluate(shape, -guess); details::GJK::Status gjk_status = gjk.evaluate(shape, -guess, support_hint);
if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex(); if(enable_cached_guess) {
cached_guess = gjk.getGuessFromSimplex();
support_func_cached_guess = gjk.support_hint;
}
Vec3f w0, w1; Vec3f w0, w1;
switch(gjk_status) { switch(gjk_status) {
...@@ -197,14 +215,22 @@ namespace fcl ...@@ -197,14 +215,22 @@ namespace fcl
FCL_REAL eps (sqrt(std::numeric_limits<FCL_REAL>::epsilon())); FCL_REAL eps (sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
#endif #endif
Vec3f guess(1, 0, 0); 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; details::MinkowskiDiff shape;
shape.set (&s1, &s2, tf1, tf2); shape.set (&s1, &s2, tf1, tf2);
details::GJK gjk((unsigned int) gjk_max_iterations, gjk_tolerance); details::GJK gjk((unsigned int) gjk_max_iterations, gjk_tolerance);
details::GJK::Status gjk_status = gjk.evaluate(shape, -guess); details::GJK::Status gjk_status = gjk.evaluate(shape, -guess, support_hint);
if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex(); if(enable_cached_guess) {
cached_guess = gjk.getGuessFromSimplex();
support_func_cached_guess = gjk.support_hint;
}
if(gjk_status == details::GJK::Failed) if(gjk_status == details::GJK::Failed)
{ {
...@@ -279,6 +305,7 @@ namespace fcl ...@@ -279,6 +305,7 @@ namespace fcl
epa_tolerance = 1e-6; epa_tolerance = 1e-6;
enable_cached_guess = false; enable_cached_guess = false;
cached_guess = Vec3f(1, 0, 0); cached_guess = Vec3f(1, 0, 0);
support_func_cached_guess = support_func_guess_t::Zero();
} }
void enableCachedGuess(bool if_enable) const void enableCachedGuess(bool if_enable) const
...@@ -319,6 +346,9 @@ namespace fcl ...@@ -319,6 +346,9 @@ namespace fcl
/// @brief smart guess /// @brief smart guess
mutable Vec3f cached_guess; mutable Vec3f cached_guess;
/// @brief smart guess for the support function
mutable support_func_guess_t support_func_cached_guess;
}; };
#if __cplusplus < 201103L #if __cplusplus < 201103L
......
...@@ -97,7 +97,7 @@ template <> struct shape_traits<ConvexBase> : shape_traits_base ...@@ -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 dota = dir.dot(triangle->a);
FCL_REAL dotb = dir.dot(triangle->b); FCL_REAL dotb = dir.dot(triangle->b);
...@@ -118,25 +118,25 @@ void getShapeSupport(const TriangleP* triangle, const Vec3f& dir, Vec3f& support ...@@ -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.; const FCL_REAL inflate = (dir.array() == 0).any() ? 1.00000001 : 1.;
support.noalias() = (dir.array() > 0).select(inflate * box->halfSide, -inflate * box->halfSide); 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(); 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(); support.head<2>().setZero();
if (dir[2] > 0) support[2] = capsule->halfLength; if (dir[2] > 0) support[2] = capsule->halfLength;
else 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) // The cone radius is, for -h < z < h, (h - z) * r / (2*h)
static const FCL_REAL inflate = 1.00001; static const FCL_REAL inflate = 1.00001;
...@@ -174,7 +174,7 @@ void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support) ...@@ -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 // 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 // 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) ...@@ -196,39 +196,40 @@ void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, Vec3f& support)
< sqrt(std::numeric_limits<FCL_REAL>::epsilon())); < 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 Vec3f* pts = convex->points;
const ConvexBase::Neighbors* nn = convex->neighbors; const ConvexBase::Neighbors* nn = convex->neighbors;
int i = 0; if (hint < 0 || hint >= convex->num_points)
FCL_REAL maxdot = pts[i].dot(dir); hint = 0;
FCL_REAL maxdot = pts[hint].dot(dir);
FCL_REAL dot; FCL_REAL dot;
bool found = true; bool found = true;
while (found) while (found)
{ {
const ConvexBase::Neighbors& n = nn[i]; const ConvexBase::Neighbors& n = nn[hint];
found = false; found = false;
for (int in = 0; in < n.count(); ++in) { for (int in = 0; in < n.count(); ++in) {
dot = pts[n[in]].dot(dir); dot = pts[n[in]].dot(dir);
if (dot > maxdot) { if (dot > maxdot) {
maxdot = dot; maxdot = dot;
i = n[in]; hint = n[in];
found = true; found = true;
} }
} }
} }
support = pts[i]; support = pts[hint];
} }
#define CALL_GET_SHAPE_SUPPORT(ShapeType) \ #define CALL_GET_SHAPE_SUPPORT(ShapeType) \
getShapeSupport (static_cast<const ShapeType*>(shape), \ getShapeSupport (static_cast<const ShapeType*>(shape), \
(shape_traits<ShapeType>::NeedNormalizedDir && !dirIsNormalized) \ (shape_traits<ShapeType>::NeedNormalizedDir && !dirIsNormalized) \
? dir.normalized() : dir, \ ? 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; Vec3f support;
switch(shape->getNodeType()) switch(shape->getNodeType())
...@@ -269,20 +270,22 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized) ...@@ -269,20 +270,22 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized)
template <typename Shape0, typename Shape1, bool TransformIsIdentity> template <typename Shape0, typename Shape1, bool TransformIsIdentity>
void getSupportTpl (const Shape0* s0, const Shape1* s1, void getSupportTpl (const Shape0* s0, const Shape1* s1,
const Matrix3f& oR1, const Vec3f& ot1, 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) if (TransformIsIdentity)
getShapeSupport (s1, - dir, support1); getShapeSupport (s1, - dir, support1, hint[1]);
else { else {
getShapeSupport (s1, - oR1.transpose() * dir, support1); getShapeSupport (s1, - oR1.transpose() * dir, support1, hint[1]);
support1 = oR1 * support1 + ot1; support1 = oR1 * support1 + ot1;
} }
} }
template <typename Shape0, typename Shape1, bool TransformIsIdentity> template <typename Shape0, typename Shape1, bool TransformIsIdentity>
void getSupportFuncTpl (const MinkowskiDiff& md, 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 = enum { NeedNormalizedDir =
bool ( (bool)shape_traits<Shape0>::NeedNormalizedDir bool ( (bool)shape_traits<Shape0>::NeedNormalizedDir
...@@ -301,7 +304,7 @@ void getSupportFuncTpl (const MinkowskiDiff& md, ...@@ -301,7 +304,7 @@ void getSupportFuncTpl (const MinkowskiDiff& md,
static_cast <const Shape1*>(md.shapes[1]), static_cast <const Shape1*>(md.shapes[1]),
md.oR1, md.ot1, md.oR1, md.ot1,
(NeedNormalizedDir && !dirIsNormalized) ? dir.normalized() : dir, (NeedNormalizedDir && !dirIsNormalized) ? dir.normalized() : dir,
support0, support1); support0, support1, hint);
} }
template <typename Shape0> template <typename Shape0>
...@@ -508,7 +511,8 @@ bool GJK::getClosestPoints (const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1) ...@@ -508,7 +511,8 @@ bool GJK::getClosestPoints (const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1)
return true; 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; size_t iterations = 0;
FCL_REAL alpha = 0; FCL_REAL alpha = 0;
...@@ -527,9 +531,10 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess) ...@@ -527,9 +531,10 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
distance = 0.0; distance = 0.0;
simplices[0].rank = 0; simplices[0].rank = 0;
ray = guess; ray = guess;
support_hint = supportHint;
if (ray.squaredNorm() > 0) appendVertex(simplices[0], -ray); if (ray.squaredNorm() > 0) appendVertex(simplices[0], -ray, false, support_hint);
else appendVertex(simplices[0], Vec3f(1, 0, 0), true); else appendVertex(simplices[0], Vec3f(1, 0, 0), true, support_hint);
ray = simplices[0].vertex[0]->w; ray = simplices[0].vertex[0]->w;
FCL_REAL rl = ray.norm(); FCL_REAL rl = ray.norm();
...@@ -558,7 +563,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess) ...@@ -558,7 +563,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
break; 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) // 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; const Vec3f& w = curr_simplex.vertex[curr_simplex.rank - 1]->w;
...@@ -622,26 +627,27 @@ inline void GJK::removeVertex(Simplex& simplex) ...@@ -622,26 +627,27 @@ inline void GJK::removeVertex(Simplex& simplex)
free_v[nfree++] = simplex.vertex[--simplex.rank]; 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 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() bool GJK::encloseOrigin()
{ {
Vec3f axis(Vec3f::Zero()); Vec3f axis(Vec3f::Zero());
support_hint_t hint = support_hint_t::Zero();
switch(simplex->rank) switch(simplex->rank)
{ {
case 1: case 1:
for(size_t i = 0; i < 3; ++i) for(size_t i = 0; i < 3; ++i)
{ {
axis[i] = 1; axis[i] = 1;
appendVertex(*simplex, axis, true); appendVertex(*simplex, axis, true, hint);
if(encloseOrigin()) return true; if(encloseOrigin()) return true;
removeVertex(*simplex); removeVertex(*simplex);
axis[i] = -1; axis[i] = -1;
appendVertex(*simplex, -axis, true); appendVertex(*simplex, -axis, true, hint);
if(encloseOrigin()) return true; if(encloseOrigin()) return true;
removeVertex(*simplex); removeVertex(*simplex);
axis[i] = 0; axis[i] = 0;
...@@ -656,10 +662,10 @@ bool GJK::encloseOrigin() ...@@ -656,10 +662,10 @@ bool GJK::encloseOrigin()
Vec3f p = d.cross(axis); Vec3f p = d.cross(axis);
if(!p.isZero()) if(!p.isZero())
{ {
appendVertex(*simplex, p); appendVertex(*simplex, p, false, hint);
if(encloseOrigin()) return true; if(encloseOrigin()) return true;
removeVertex(*simplex); removeVertex(*simplex);
appendVertex(*simplex, -p); appendVertex(*simplex, -p, false, hint);
if(encloseOrigin()) return true; if(encloseOrigin()) return true;
removeVertex(*simplex); removeVertex(*simplex);
} }
...@@ -673,10 +679,10 @@ bool GJK::encloseOrigin() ...@@ -673,10 +679,10 @@ bool GJK::encloseOrigin()
(simplex->vertex[2]->w - simplex->vertex[0]->w); (simplex->vertex[2]->w - simplex->vertex[0]->w);
if(!axis.isZero())