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

Fix nearest point calculation in shapeDistance.

parent c9292413
......@@ -234,7 +234,8 @@ namespace fcl
Vec3f w0, w1;
gjk.getClosestPoints (shape, w0, w1);
distance = 0;
p1 = p2 = tf1.transform (.5* (w0 + w1));
p1 = tf1.transform (w0);
p2 = tf1.transform (w1);
normal = Vec3f (0,0,0);
return false;
}
......@@ -261,7 +262,8 @@ namespace fcl
//p1 = tf1.transform (p1);
//p2 = tf1.transform (p2);
normal = (tf1.getRotation() * (p2 - p1)).normalized();
p1 = p2 = tf1.transform(p1);
p1 = tf1.transform(p1);
p2 = tf1.transform(p2);
} else {
details::EPA epa(epa_max_face_num, epa_max_vertex_num,
epa_max_iterations, epa_tolerance);
......@@ -276,12 +278,14 @@ namespace fcl
assert (epa.depth >= -eps);
distance = (std::min) (0., -epa.depth);
normal = tf1.getRotation() * epa.normal;
p1 = p2 = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
p1 = tf1.transform(w0);
p2 = tf1.transform(w1);
return false;
}
distance = -(std::numeric_limits<FCL_REAL>::max)();
gjk.getClosestPoints (shape, p1, p2);
p1 = p2 = tf1.transform (p1);
p1 = tf1.transform(p1);
p2 = tf1.transform(p2);
}
return false;
}
......
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