From 7735a592f0bc08456e5d7ee090bac6b0dda9801e Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Sat, 31 Aug 2019 18:11:53 +0200
Subject: [PATCH] [GJK] Clean API of struct GJK.
---
include/hpp/fcl/narrowphase/gjk.h | 18 +++++++++++-------
src/narrowphase/gjk.cpp | 10 ++--------
2 files changed, 13 insertions(+), 15 deletions(-)
diff --git a/include/hpp/fcl/narrowphase/gjk.h b/include/hpp/fcl/narrowphase/gjk.h
index 1d55d3b5..46ecc26e 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 56d88d16..d83803e7 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++]);
--
GitLab