From 7a7c258c4c690c0af4a8830f81a70119e47df385 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Wed, 28 Aug 2019 16:59:04 +0200
Subject: [PATCH] Reduce memory footprint of MinkowskiDiff
---
include/hpp/fcl/narrowphase/gjk.h | 20 ++++++++++++++------
include/hpp/fcl/narrowphase/narrowphase.h | 9 +++------
src/narrowphase/gjk.cpp | 9 ++++-----
src/narrowphase/narrowphase.cpp | 3 +--
4 files changed, 22 insertions(+), 19 deletions(-)
diff --git a/include/hpp/fcl/narrowphase/gjk.h b/include/hpp/fcl/narrowphase/gjk.h
index 87785747..0fa6edf7 100644
--- a/include/hpp/fcl/narrowphase/gjk.h
+++ b/include/hpp/fcl/narrowphase/gjk.h
@@ -60,11 +60,13 @@ struct MinkowskiDiff
/// @brief points to two shapes
const ShapeBase* shapes[2];
- /// @brief rotation from shape0 to shape1
- Matrix3f toshape1;
+ /// @brief rotation from shape1 to shape0
+ /// such that \f$ p_in_0 = oR1 * p_in_1 + ot1 \f$.
+ Matrix3f oR1;
- /// @brief transform from shape1 to shape0
- Transform3f toshape0;
+ /// @brief translation from shape1 to shape0
+ /// such that \f$ p_in_0 = oR1 * p_in_1 + ot1 \f$.
+ Vec3f ot1;
typedef void (*GetSupportFunction) (const MinkowskiDiff& minkowskiDiff,
const Vec3f& dir, bool dirIsNormalized, Vec3f& support);
@@ -74,16 +76,22 @@ struct MinkowskiDiff
void set (const ShapeBase* shape0, const ShapeBase* shape1);
+ void set (const Transform3f& tf0, const Transform3f& tf1)
+ {
+ oR1 = tf0.getRotation().transpose() * tf1.getRotation();
+ ot1 = tf0.getRotation().transpose() * (tf1.getTranslation() - tf0.getTranslation());
+ }
+
/// @brief support function for shape0
inline Vec3f support0(const Vec3f& d) const
{
return getSupport(shapes[0], d);
}
-
+
/// @brief support function for shape1
inline Vec3f support1(const Vec3f& d) const
{
- return toshape0.transform(getSupport(shapes[1], toshape1 * d));
+ return oR1 * getSupport(shapes[1], oR1.transpose() * d) + ot1;
}
/// @brief support function for the pair of shapes
diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h
index dc0720c9..2c3b536e 100644
--- a/include/hpp/fcl/narrowphase/narrowphase.h
+++ b/include/hpp/fcl/narrowphase/narrowphase.h
@@ -63,8 +63,7 @@ namespace fcl
details::MinkowskiDiff shape;
shape.set (&s1, &s2);
- shape.toshape1 = tf2.getRotation().transpose() * tf1.getRotation();
- shape.toshape0 = tf1.inverseTimes(tf2);
+ shape.set (tf1, tf2);
details::GJK gjk((unsigned int )gjk_max_iterations, gjk_tolerance);
details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
@@ -115,8 +114,7 @@ namespace fcl
details::MinkowskiDiff shape;
shape.set (&s, &tri);
- shape.toshape1 = tf2.getRotation().transpose() * tf1.getRotation();
- shape.toshape0 = tf1.inverseTimes(tf2);
+ shape.set (tf1, tf2);
details::GJK gjk((unsigned int )gjk_max_iterations, gjk_tolerance);
details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
@@ -182,8 +180,7 @@ namespace fcl
details::MinkowskiDiff shape;
shape.set (&s1, &s2);
- shape.toshape1 = tf2.getRotation().transpose() * tf1.getRotation();
- shape.toshape0 = tf1.inverseTimes(tf2);
+ shape.set (tf1, tf2);
details::GJK gjk((unsigned int) gjk_max_iterations, gjk_tolerance);
details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp
index af596e66..53691b28 100644
--- a/src/narrowphase/gjk.cpp
+++ b/src/narrowphase/gjk.cpp
@@ -203,13 +203,13 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir)
template <typename Shape0, typename Shape1>
void getSupportTpl (const Shape0* s0, const Shape1* s1,
- const Matrix3f& oM1, const Vec3f& ot1,
+ const Matrix3f& oR1, const Vec3f& ot1,
const Vec3f& dir, Vec3f& support)
{
getShapeSupport (s0, dir, support);
Vec3f support1;
- getShapeSupport (s1, - oM1.transpose() * dir, support1);
- support.noalias() -= oM1 * support1 + ot1;
+ getShapeSupport (s1, - oR1.transpose() * dir, support1);
+ support.noalias() -= oR1 * support1 + ot1;
}
template <typename Shape0, typename Shape1>
@@ -223,8 +223,7 @@ void getSupportFuncTpl (const MinkowskiDiff& md,
getSupportTpl<Shape0, Shape1> (
static_cast <const Shape0*>(md.shapes[0]),
static_cast <const Shape1*>(md.shapes[1]),
- md.toshape0.getRotation(),
- md.toshape0.getTranslation(),
+ md.oR1, md.ot1,
(NeedNormalizedDir && !dirIsNormalized) ? dir.normalized() : dir,
support);
}
diff --git a/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp
index 32de8492..b38850bf 100644
--- a/src/narrowphase/narrowphase.cpp
+++ b/src/narrowphase/narrowphase.cpp
@@ -631,8 +631,7 @@ bool GJKSolver_indep::shapeDistance<Capsule, Capsule>
details::MinkowskiDiff shape;
shape.set (&s1, &s2);
- shape.toshape1 = tf2.getRotation().transpose() * tf1.getRotation();
- shape.toshape0 = tf1.inverseTimes(tf2);
+ shape.set (tf1, tf2);
const Vec3f& P1 (s1.a);
const Vec3f& P2 (s1.b);
--
GitLab