Commit 7735a592 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

[GJK] Clean API of struct GJK.

parent 5712c6a4
......@@ -148,13 +148,11 @@ struct GJK
Status evaluate(const MinkowskiDiff& shape, const Vec3f& guess);
/// @brief apply the support function along a direction, the result is return in sv
void getSupport(const Vec3f& d, bool dIsNormalized, SimplexV& sv) const;
/// @brief discard one vertex from the simplex
void removeVertex(Simplex& simplex);
/// @brief append one vertex to the simplex
void appendVertex(Simplex& simplex, const Vec3f& v, bool isNormalized = false);
inline void getSupport(const Vec3f& d, bool dIsNormalized, SimplexV& sv) const
{
shape->support(d, dIsNormalized, sv.w0, sv.w1);
sv.w.noalias() = sv.w0 - sv.w1;
}
/// @brief whether the simplex enclose the origin
bool encloseOrigin();
......@@ -193,6 +191,12 @@ private:
FCL_REAL tolerance;
FCL_REAL distance_upper_bound;
/// @brief discard one vertex from the simplex
inline void removeVertex(Simplex& simplex);
/// @brief append one vertex to the simplex
inline void appendVertex(Simplex& simplex, const Vec3f& v, bool isNormalized = false);
/// @brief Project origin (0) onto line a-b
bool projectLineOrigin(const Simplex& current, Simplex& next);
......
......@@ -486,18 +486,12 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
return status;
}
void GJK::getSupport(const Vec3f& d, bool dIsNormalized, SimplexV& sv) const
{
shape->support(d, dIsNormalized, sv.w0, sv.w1);
sv.w.noalias() = sv.w0 - sv.w1;
}
void GJK::removeVertex(Simplex& simplex)
inline void GJK::removeVertex(Simplex& simplex)
{
free_v[nfree++] = simplex.vertex[--simplex.rank];
}
void GJK::appendVertex(Simplex& simplex, const Vec3f& v, bool isNormalized)
inline void GJK::appendVertex(Simplex& simplex, const Vec3f& v, bool isNormalized)
{
simplex.vertex[simplex.rank] = free_v[--nfree]; // set the memory
getSupport (v, isNormalized, *simplex.vertex[simplex.rank++]);
......
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