Commit a9943a27 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Clean GJK::evaluate

parent 3840c19c
...@@ -530,47 +530,15 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, ...@@ -530,47 +530,15 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess,
shape = &shape_; shape = &shape_;
distance = 0.0; distance = 0.0;
simplices[0].rank = 0; simplices[0].rank = 0;
ray = guess;
support_hint = supportHint; support_hint = supportHint;
FCL_REAL rl = ray.squaredNorm(); FCL_REAL rl = guess.norm();
if (rl > 0) { if (rl < tolerance) {
rl = std::sqrt(rl);
ray /= rl;
} else {
ray = Vec3f(-1,0,0); ray = Vec3f(-1,0,0);
rl = 1; rl = 1;
} } else
appendVertex(simplices[0], -ray, true, support_hint); ray = guess;
FCL_REAL omega = ray.dot(simplices[0].vertex[0]->w);
if (omega > upper_bound)
{
distance = omega - inflation;
simplex = &simplices[current];
return Valid;
}
if (omega > 0) {
alpha = omega;
if((rl - omega) - tolerance * rl <= 0)
{
removeVertex(simplices[current]);
distance = rl - inflation;
simplex = &simplices[current];
std::cout << "here" << std::endl;
return Valid;
}
}
ray = simplices[0].vertex[0]->w;
rl = ray.norm();
if (rl == 0) {
status = Inside;
distance = - inflation - 1.;
simplex = &simplices[0];
return status;
}
do do
{ {
vertex_id_t next = (vertex_id_t)(1 - current); vertex_id_t next = (vertex_id_t)(1 - current);
...@@ -596,7 +564,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, ...@@ -596,7 +564,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess,
const Vec3f& w = curr_simplex.vertex[curr_simplex.rank - 1]->w; const Vec3f& w = curr_simplex.vertex[curr_simplex.rank - 1]->w;
// check B: no collision if omega > 0 // check B: no collision if omega > 0
omega = ray.dot(w) / rl; FCL_REAL omega = ray.dot(w) / rl;
if (omega > upper_bound) if (omega > upper_bound)
{ {
distance = omega - inflation; distance = omega - inflation;
...@@ -605,9 +573,12 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, ...@@ -605,9 +573,12 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess,
// 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 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); alpha = std::max(alpha, omega);
if((rl - alpha) - tolerance * rl <= 0) FCL_REAL diff (rl - alpha);
if (iterations == 0) diff = std::abs(diff);
if(diff - tolerance * rl <= 0)
{ {
removeVertex(simplices[current]); if (iterations > 0)
removeVertex(simplices[current]);
distance = rl - inflation; distance = rl - inflation;
// TODO When inflation is strictly positive, the distance may be exactly // TODO When inflation is strictly positive, the distance may be exactly
// zero (so the ray is not zero) and we are not in the case rl < tolerance. // zero (so the ray is not zero) and we are not in the case rl < tolerance.
...@@ -621,6 +592,13 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, ...@@ -621,6 +592,13 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess,
bool inside; bool inside;
switch(curr_simplex.rank) switch(curr_simplex.rank)
{ {
case 1: // Only at the first iteration
assert(iterations == 0);
ray = w;
inside = false;
next_simplex.rank = 1;
next_simplex.vertex[0] = curr_simplex.vertex[0];
break;
case 2: case 2:
inside = projectLineOrigin (curr_simplex, next_simplex); inside = projectLineOrigin (curr_simplex, next_simplex);
break; break;
...@@ -630,6 +608,8 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, ...@@ -630,6 +608,8 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess,
case 4: case 4:
inside = projectTetrahedraOrigin (curr_simplex, next_simplex); inside = projectTetrahedraOrigin (curr_simplex, next_simplex);
break; break;
default:
throw std::logic_error("Invalid simplex rank");
} }
assert (nfree+next_simplex.rank == 4); assert (nfree+next_simplex.rank == 4);
current = next; current = next;
...@@ -646,6 +626,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, ...@@ -646,6 +626,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess,
} while(status == Valid); } while(status == Valid);
simplex = &simplices[current]; simplex = &simplices[current];
assert(simplex->rank > 0 && simplex->rank < 5);
return status; return status;
} }
......
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