Commit 88cc70a3 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Fix caculation of normals when using GJK / EPA.

parent ae22d16a
......@@ -156,7 +156,7 @@ namespace fcl
if (gjk.hasPenetrationInformation(shape)) {
gjk.getClosestPoints (shape, w0, w1);
distance = gjk.distance;
normal = tf1.getRotation() * (w1 - w0).normalized();
normal = tf1.getRotation() * (w0 - w1).normalized();
p1 = p2 = tf1.transform((w0 + w1) / 2);
} else {
details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance);
......@@ -168,7 +168,7 @@ namespace fcl
{
epa.getClosestPoints (shape, w0, w1);
distance = -epa.depth;
normal = -epa.normal;
normal = tf1.getRotation() * epa.normal;
p1 = p2 = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
assert (distance <= 1e-6);
} else {
......@@ -261,7 +261,7 @@ namespace fcl
// Return contact points in case of collision
//p1 = tf1.transform (p1);
//p2 = tf1.transform (p2);
normal = (tf1.getRotation() * (p2 - p1)).normalized();
normal = (tf1.getRotation() * (p1 - p2)).normalized();
p1 = tf1.transform(p1);
p2 = tf1.transform(p2);
} else {
......
......@@ -575,7 +575,10 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess,
alpha = std::max(alpha, omega);
FCL_REAL diff (rl - alpha);
if (iterations == 0) diff = std::abs(diff);
if(diff - tolerance * rl <= 0)
// TODO here, we can stop at iteration 0 if this condition is met.
// We stopping at iteration 0, the closest point will not be valid.
// if(diff - tolerance * rl <= 0)
if(iterations > 0 && diff - tolerance * rl <= 0)
{
if (iterations > 0)
removeVertex(simplices[current]);
......
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