Commit 7a7c258c authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Reduce memory footprint of MinkowskiDiff

parent 89c0170c
......@@ -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
......
......@@ -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);
......
......@@ -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);
}
......
......@@ -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);
......
Supports Markdown
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