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

[GJK] Store support for each shape in SimplexV.

parent 1bac302a
......@@ -72,7 +72,7 @@ struct MinkowskiDiff
Vec3f ot1;
typedef void (*GetSupportFunction) (const MinkowskiDiff& minkowskiDiff,
const Vec3f& dir, bool dirIsNormalized, Vec3f& support);
const Vec3f& dir, bool dirIsNormalized, Vec3f& support0, Vec3f& support1);
GetSupportFunction getSupportFunc;
MinkowskiDiff() : getSupportFunc (NULL) {}
......@@ -98,10 +98,10 @@ struct MinkowskiDiff
}
/// @brief support function for the pair of shapes
inline void support(const Vec3f& d, bool dIsNormalized, Vec3f& supp) const
inline void support(const Vec3f& d, bool dIsNormalized, Vec3f& supp0, Vec3f& supp1) const
{
assert(getSupportFunc != NULL);
getSupportFunc(*this, d, dIsNormalized, supp);
getSupportFunc(*this, d, dIsNormalized, supp0, supp1);
}
};
......@@ -112,12 +112,10 @@ struct GJK
{
struct SimplexV
{
/// @brief support direction
Vec3f d;
/// @brief support vector for shape 0 and 1.
Vec3f w0, w1;
/// @brieg support vector (i.e., the furthest point on the shape along the support direction)
Vec3f w;
SimplexV () : d(Vec3f::Zero()), w(Vec3f::Zero()) {}
};
struct Simplex
......
......@@ -80,8 +80,7 @@ namespace fcl
Vec3f w0 (Vec3f::Zero());
for(size_t i = 0; i < epa.result.rank; ++i)
{
w0 += shape.support0(epa.result.vertex[i]->d, false) *
epa.result.coefficient[i];
w0 += epa.result.vertex[i]->w0 * epa.result.coefficient[i];
}
if(penetration_depth) *penetration_depth = -epa.depth;
if(normal) *normal = tf2.getRotation() * epa.normal;
......@@ -131,8 +130,7 @@ namespace fcl
Vec3f w0 (Vec3f::Zero());
for(size_t i = 0; i < epa.result.rank; ++i)
{
w0 += shape.support0(epa.result.vertex[i]->d, false) *
epa.result.coefficient[i];
w0 += epa.result.vertex[i]->w0 * epa.result.coefficient[i];
}
distance = -epa.depth;
normal = -epa.normal;
......@@ -221,8 +219,7 @@ namespace fcl
Vec3f w0 (Vec3f::Zero());
for(size_t i = 0; i < epa.result.rank; ++i)
{
w0 += shape.support0(epa.result.vertex[i]->d, false) *
epa.result.coefficient[i];
w0 += epa.result.vertex[i]->w0 * epa.result.coefficient[i];
}
assert (epa.depth >= -eps);
distance = std::min (0., -epa.depth);
......
......@@ -213,17 +213,16 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized)
template <typename Shape0, typename Shape1>
void getSupportTpl (const Shape0* s0, const Shape1* s1,
const Matrix3f& oR1, const Vec3f& ot1,
const Vec3f& dir, Vec3f& support)
const Vec3f& dir, Vec3f& support0, Vec3f& support1)
{
getShapeSupport (s0, dir, support);
Vec3f support1;
getShapeSupport (s0, dir, support0);
getShapeSupport (s1, - oR1.transpose() * dir, support1);
support.noalias() -= oR1 * support1 + ot1;
support1 = oR1 * support1 + ot1;
}
template <typename Shape0, typename Shape1>
void getSupportFuncTpl (const MinkowskiDiff& md,
const Vec3f& dir, bool dirIsNormalized, Vec3f& support)
const Vec3f& dir, bool dirIsNormalized, Vec3f& support0, Vec3f& support1)
{
enum { NeedNormalizedDir =
bool ( (bool)shape_traits<Shape0>::NeedNormalizedDir
......@@ -234,7 +233,7 @@ void getSupportFuncTpl (const MinkowskiDiff& md,
static_cast <const Shape1*>(md.shapes[1]),
md.oR1, md.ot1,
(NeedNormalizedDir && !dirIsNormalized) ? dir.normalized() : dir,
support);
support0, support1);
}
template <typename Shape0>
......@@ -316,8 +315,8 @@ bool GJK::getClosestPoints (const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1) co
for(size_t i = 0; i < getSimplex()->rank; ++i)
{
FCL_REAL p = getSimplex()->coefficient[i];
w0 += shape.support0( getSimplex()->vertex[i]->d, false) * p;
w1 += shape.support1(-getSimplex()->vertex[i]->d, false) * p;
w0 += getSimplex()->vertex[i]->w0 * p;
w1 += getSimplex()->vertex[i]->w1 * p;
}
return true;
}
......@@ -489,9 +488,8 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
void GJK::getSupport(const Vec3f& d, bool dIsNormalized, SimplexV& sv) const
{
// Was sv.d.noalias() = d.normalized();
sv.d.noalias() = d;
shape.support(sv.d, dIsNormalized, sv.w);
shape.support(d, dIsNormalized, sv.w0, sv.w1);
sv.w.noalias() = sv.w0 - sv.w1;
}
void GJK::removeVertex(Simplex& simplex)
......
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