Commit 4cb0ed89 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

[GJK] Add GJK::setDistanceEarlyBreak

parent c60dd501
......@@ -172,6 +172,15 @@ struct GJK
/// @brief get the guess from current simplex
Vec3f getGuessFromSimplex() const;
/// @brief Distance threshold for early break.
/// GJK stops when it proved the distance is more than this threshold.
/// \note The closest points will be erroneous in this case.
/// If you want the closest points, set this to infinity (the default).
void setDistanceEarlyBreak (const FCL_REAL& dup)
{
distance_upper_bound = dup;
}
private:
SimplexV store_v[4];
SimplexV* free_v[4];
......@@ -182,6 +191,7 @@ private:
unsigned int max_iterations;
FCL_REAL tolerance;
FCL_REAL distance_upper_bound;
/// @brief Project origin (0) onto line a-b
bool projectLineOrigin(const Simplex& current, Simplex& next);
......
......@@ -297,6 +297,7 @@ void GJK::initialize()
{
nfree = 0;
status = Failed;
distance_upper_bound = std::numeric_limits<FCL_REAL>::max();
simplex = NULL;
}
......@@ -404,8 +405,15 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
// 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;
// check C: when the new support point is close to the sub-simplex where the ray point lies, stop (as the new simplex again is degenerated)
// check B: no collision if omega > 0
FCL_REAL omega = ray.dot(w) / rl;
if (omega > distance_upper_bound)
{
distance = omega;
break;
}
// check C: when the new support point is close to the sub-simplex where the ray point lies, stop (as the new simplex again is degenerated)
alpha = std::max(alpha, omega);
if((rl - alpha) - tolerance * rl <= 0)
{
......
Markdown is supported
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