Commit 2cb5c718 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

[GJK] Make GJK::getClosestPoints non-static. Add EPA::getClosestPoints

parent de8ab23f
......@@ -167,7 +167,7 @@ struct GJK
/// Get the closest points on each object.
/// @return true on success
static bool getClosestPoints (const Simplex& simplex, Vec3f& w0, Vec3f& w1);
bool getClosestPoints (const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1);
/// @brief get the guess from current simplex
Vec3f getGuessFromSimplex() const;
......@@ -306,6 +306,10 @@ public:
Status evaluate(GJK& gjk, const Vec3f& guess);
/// Get the closest points on each object.
/// @return true on success
bool getClosestPoints (const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1);
private:
bool getEdgeDist(SimplexF* face, SimplexV* a, SimplexV* b, FCL_REAL& dist);
......
......@@ -77,7 +77,7 @@ namespace fcl
if(epa_status != details::EPA::Failed)
{
Vec3f w0, w1;
details::GJK::getClosestPoints (epa.result, w0, w1);
epa.getClosestPoints (shape, w0, w1);
if(penetration_depth) *penetration_depth = -epa.depth;
if(normal) *normal = tf2.getRotation() * epa.normal;
if(contact_points) *contact_points = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
......@@ -128,7 +128,7 @@ namespace fcl
details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
assert (epa_status != details::EPA::Failed); (void) epa_status;
Vec3f w0, w1;
details::GJK::getClosestPoints (epa.result, w0, w1);
epa.getClosestPoints (shape, w0, w1);
distance = -epa.depth;
normal = -epa.normal;
p1 = p2 = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
......@@ -140,7 +140,7 @@ namespace fcl
{
col = false;
details::GJK::getClosestPoints (*gjk.getSimplex(), p1, p2);
gjk.getClosestPoints (shape, p1, p2);
// TODO On degenerated case, the closest point may be wrong
// (i.e. an object face normal is colinear to gjk.ray
// assert (distance == (w0 - w1).norm());
......@@ -184,7 +184,7 @@ namespace fcl
// TODO: understand why GJK fails between cylinder and box
assert (distance * distance < sqrt (eps));
Vec3f w0, w1;
details::GJK::getClosestPoints (*gjk.getSimplex(), w0, w1);
gjk.getClosestPoints (shape, w0, w1);
distance = 0;
p1 = p2 = tf1.transform (.5* (w0 + w1));
normal = Vec3f (0,0,0);
......@@ -192,7 +192,7 @@ namespace fcl
}
else if(gjk_status == details::GJK::Valid)
{
details::GJK::getClosestPoints (*gjk.getSimplex(), p1, p2);
gjk.getClosestPoints (shape, p1, p2);
// TODO On degenerated case, the closest point may be wrong
// (i.e. an object face normal is colinear to gjk.ray
// assert (distance == (w0 - w1).norm());
......@@ -213,7 +213,7 @@ namespace fcl
if(epa_status != details::EPA::Failed)
{
Vec3f w0, w1;
details::GJK::getClosestPoints (epa.result, w0, w1);
epa.getClosestPoints (shape, w0, w1);
assert (epa.depth >= -eps);
distance = std::min (0., -epa.depth);
normal = tf2.getRotation() * epa.normal;
......@@ -222,7 +222,7 @@ namespace fcl
}
else
{
details::GJK::getClosestPoints (*gjk.getSimplex(), p1, p2);
gjk.getClosestPoints (shape, p1, p2);
distance = 0;
p1 = tf1.transform (p1);
......
......@@ -374,11 +374,13 @@ Vec3f GJK::getGuessFromSimplex() const
return ray;
}
bool GJK::getClosestPoints (const Simplex& simplex, Vec3f& w0, Vec3f& w1)
namespace details {
bool getClosestPoints (const GJK::Simplex& simplex, Vec3f& w0, Vec3f& w1)
{
SimplexV* const* vs = simplex.vertex;
GJK::SimplexV* const* vs = simplex.vertex;
for (vertex_id_t i = 0; i < simplex.rank; ++i) {
for (GJK::vertex_id_t i = 0; i < simplex.rank; ++i) {
assert (vs[i]->w.isApprox (vs[i]->w0 - vs[i]->w1));
}
......@@ -424,13 +426,22 @@ bool GJK::getClosestPoints (const Simplex& simplex, Vec3f& w0, Vec3f& w1)
}
w0.setZero();
w1.setZero();
for (vertex_id_t i = 0; i < simplex.rank; ++i) {
for (GJK::vertex_id_t i = 0; i < simplex.rank; ++i) {
w0 += projection.parameterization[i] * vs[i]->w0;
w1 += projection.parameterization[i] * vs[i]->w1;
}
return true;
}
} // namespace details
bool GJK::getClosestPoints (const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1)
{
bool res = details::getClosestPoints(*simplex, w0, w1);
if (!res) return false;
return true;
}
GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
{
size_t iterations = 0;
......@@ -1296,6 +1307,13 @@ bool EPA::expand(size_t pass, SimplexV* w, SimplexF* f, size_t e, SimplexHorizon
return false;
}
bool EPA::getClosestPoints (const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1)
{
bool res = details::getClosestPoints(result, w0, w1);
if (!res) return false;
return true;
}
} // details
} // fcl
......
......@@ -643,7 +643,7 @@ bool GJKSolver::shapeDistance<Capsule, Capsule>
details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex();
details::GJK::getClosestPoints (*gjk.getSimplex(), p1, p2);
gjk.getClosestPoints (shape, p1, p2);
if((gjk_status == details::GJK::Valid) ||
(gjk_status == details::GJK::Failed))
......
Supports Markdown
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