Commit 1b7d77b2 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Clean API of MinkowskiDiff

parent 7a7c258c
......@@ -54,6 +54,9 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir);
/// @brief Minkowski difference class of two shapes
///
/// \todo template this by the two shapes. The triangle / triangle case can be
/// easily optimized computing once the triangle shapes[1] into frame0
///
/// \note The Minkowski difference is expressed in the frame of the first shape.
struct MinkowskiDiff
{
......@@ -94,27 +97,12 @@ struct MinkowskiDiff
return oR1 * getSupport(shapes[1], oR1.transpose() * d) + ot1;
}
/// @brief support function for the pair of shapes
inline Vec3f support(const Vec3f& d) const
{
return support0(d) - support1(-d);
}
/// @brief support function for the pair of shapes
inline void support(const Vec3f& d, bool dIsNormalized, Vec3f& supp) const
{
assert(getSupportFunc != NULL);
getSupportFunc(*this, d, dIsNormalized, supp);
}
/// @brief support function for the d-th shape (d = 0 or 1)
inline Vec3f support(const Vec3f& d, size_t index) const
{
if(index)
return support1(d);
else
return support0(d);
}
};
/// @brief class for GJK algorithm
......
......@@ -80,7 +80,7 @@ namespace fcl
Vec3f w0 (Vec3f::Zero());
for(size_t i = 0; i < epa.result.rank; ++i)
{
w0 += shape.support(epa.result.vertex[i]->d, 0) *
w0 += shape.support0(epa.result.vertex[i]->d) *
epa.result.coefficient[i];
}
if(penetration_depth) *penetration_depth = -epa.depth;
......@@ -131,7 +131,7 @@ namespace fcl
Vec3f w0 (Vec3f::Zero());
for(size_t i = 0; i < epa.result.rank; ++i)
{
w0 += shape.support(epa.result.vertex[i]->d, 0) *
w0 += shape.support0(epa.result.vertex[i]->d) *
epa.result.coefficient[i];
}
distance = -epa.depth;
......@@ -148,8 +148,8 @@ namespace fcl
for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
{
FCL_REAL p = gjk.getSimplex()->coefficient[i];
w0 += shape.support(gjk.getSimplex()->vertex[i]->d, 0) * p;
w1 += shape.support(-gjk.getSimplex()->vertex[i]->d, 1) * p;
w0 += shape.support0( gjk.getSimplex()->vertex[i]->d) * p;
w1 += shape.support1(-gjk.getSimplex()->vertex[i]->d) * p;
}
distance = (w0 - w1).norm();
p1 = tf1.transform (w0);
......@@ -194,8 +194,8 @@ namespace fcl
for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
{
FCL_REAL p = gjk.getSimplex()->coefficient[i];
w0 += shape.support(gjk.getSimplex()->vertex[i]->d, 0) * p;
w1 += shape.support(-gjk.getSimplex()->vertex[i]->d, 1) * p;
w0 += shape.support0( gjk.getSimplex()->vertex[i]->d) * p;
w1 += shape.support1(-gjk.getSimplex()->vertex[i]->d) * p;
}
distance = 0;
p1 = p2 = tf1.transform (.5* (w0 + w1));
......@@ -208,8 +208,8 @@ namespace fcl
for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
{
FCL_REAL p = gjk.getSimplex()->coefficient[i];
w0 += shape.support(gjk.getSimplex()->vertex[i]->d, 0) * p;
w1 += shape.support(-gjk.getSimplex()->vertex[i]->d, 1) * p;
w0 += shape.support0( gjk.getSimplex()->vertex[i]->d) * p;
w1 += shape.support1(-gjk.getSimplex()->vertex[i]->d) * p;
}
distance = (w0 - w1).norm();
......@@ -231,7 +231,7 @@ namespace fcl
Vec3f w0 (Vec3f::Zero());
for(size_t i = 0; i < epa.result.rank; ++i)
{
w0 += shape.support(epa.result.vertex[i]->d, 0) *
w0 += shape.support0(epa.result.vertex[i]->d) *
epa.result.coefficient[i];
}
assert (epa.depth >= -eps);
......@@ -246,8 +246,8 @@ namespace fcl
for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
{
FCL_REAL p = gjk.getSimplex()->coefficient[i];
w0 += shape.support(gjk.getSimplex()->vertex[i]->d, 0) * p;
w1 += shape.support(-gjk.getSimplex()->vertex[i]->d, 1) * p;
w0 += shape.support0( gjk.getSimplex()->vertex[i]->d) * p;
w1 += shape.support1(-gjk.getSimplex()->vertex[i]->d) * p;
}
distance = 0;
......
......@@ -648,8 +648,8 @@ bool GJKSolver_indep::shapeDistance<Capsule, Capsule>
for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
{
FCL_REAL p = gjk.getSimplex()->coefficient[i];
w0 += shape.support(gjk.getSimplex()->vertex[i]->d, 0) * p;
w1 += shape.support(-gjk.getSimplex()->vertex[i]->d, 1) * p;
w0 += shape.support0( gjk.getSimplex()->vertex[i]->d) * p;
w1 += shape.support1(-gjk.getSimplex()->vertex[i]->d) * p;
}
if((gjk_status == details::GJK::Valid) ||
(gjk_status == details::GJK::Failed))
......
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