diff --git a/include/hpp/fcl/narrowphase/gjk.h b/include/hpp/fcl/narrowphase/gjk.h index 1d55d3b52f830be92fb3a8147069d8a0621cc52c..46ecc26eab558cef5b35fe4ef43e709c8e956338 100644 --- a/include/hpp/fcl/narrowphase/gjk.h +++ b/include/hpp/fcl/narrowphase/gjk.h @@ -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); diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp index 56d88d16c059e96c9fd15256ad7d8669a5e24c3c..d83803e7d8ba343fdc79b1985b90a2b2f6597e46 100644 --- a/src/narrowphase/gjk.cpp +++ b/src/narrowphase/gjk.cpp @@ -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++]);