From b4d102f5d2fd543ff35a8a086856f79dc9a443a3 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Fri, 30 Aug 2019 10:11:46 +0200
Subject: [PATCH] [GJK] Store support for each shape in SimplexV.
---
include/hpp/fcl/narrowphase/gjk.h | 12 +++++-------
include/hpp/fcl/narrowphase/narrowphase.h | 9 +++------
src/narrowphase/gjk.cpp | 20 +++++++++-----------
3 files changed, 17 insertions(+), 24 deletions(-)
diff --git a/include/hpp/fcl/narrowphase/gjk.h b/include/hpp/fcl/narrowphase/gjk.h
index 42f39c1a..b7015560 100644
--- a/include/hpp/fcl/narrowphase/gjk.h
+++ b/include/hpp/fcl/narrowphase/gjk.h
@@ -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
diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h
index 20b29cd3..92536e30 100644
--- a/include/hpp/fcl/narrowphase/narrowphase.h
+++ b/include/hpp/fcl/narrowphase/narrowphase.h
@@ -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);
diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp
index 9ae359b5..55198f97 100644
--- a/src/narrowphase/gjk.cpp
+++ b/src/narrowphase/gjk.cpp
@@ -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)
--
GitLab