Unverified Commit 41eac1b2 authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #187 from jmirabel/devel

Logarithmic or linear support function for Convex depending of its size + Python update
parents d07376d9 320772ff
......@@ -55,15 +55,20 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized,
/// @brief Minkowski difference class of two shapes
///
/// @todo template this by the two shapes. The triangle / triangle case can be
/// easily optimized computing once the triangle shapes[1] into frame0
///
/// @note The Minkowski difference is expressed in the frame of the first shape.
struct HPP_FCL_DLLAPI MinkowskiDiff
{
/// @brief points to two shapes
const ShapeBase* shapes[2];
struct ShapeData {
std::vector<int8_t> visited;
};
/// @brief Store temporary data for the computation of the support point for
/// each shape.
ShapeData data[2];
/// @brief rotation from shape1 to shape0
/// such that @f$ p_in_0 = oR1 * p_in_1 + ot1 @f$.
Matrix3f oR1;
......@@ -76,12 +81,18 @@ struct HPP_FCL_DLLAPI MinkowskiDiff
/// The 2 values correspond to the inflation of shape 0 and shape 1.
Eigen::Array<FCL_REAL, 1, 2> inflation;
/// @brief Number of points in a Convex object from which using a logarithmic
/// support function is faster than a linear one.
/// It defaults to 32.
/// \note It must set before the call to \ref set.
int linear_log_convex_threshold;
typedef void (*GetSupportFunction) (const MinkowskiDiff& minkowskiDiff,
const Vec3f& dir, bool dirIsNormalized, Vec3f& support0, Vec3f& support1,
support_func_guess_t& hint);
support_func_guess_t& hint, ShapeData data[2]);
GetSupportFunction getSupportFunc;
MinkowskiDiff() : getSupportFunc (NULL) {}
MinkowskiDiff() : linear_log_convex_threshold (32), getSupportFunc (NULL) {}
/// Set the two shapes,
/// assuming the relative transformation between them is identity.
......@@ -107,7 +118,7 @@ struct HPP_FCL_DLLAPI MinkowskiDiff
inline void support(const Vec3f& d, bool dIsNormalized, Vec3f& supp0, Vec3f& supp1, support_func_guess_t& hint) const
{
assert(getSupportFunc != NULL);
getSupportFunc(*this, d, dIsNormalized, supp0, supp1, hint);
getSupportFunc(*this, d, dIsNormalized, supp0, supp1, hint, const_cast<ShapeData*>(data));
}
};
......@@ -158,6 +169,13 @@ struct HPP_FCL_DLLAPI GJK
Simplex simplices[2];
/// \param max_iterations_ number of iteration before GJK returns failure.
/// \param tolerance_ precision of the algorithm.
///
/// The tolerance argument is useful for continuous shapes and for polyhedron
/// with some vertices closer than this threshold.
///
/// Suggested values are 100 iterations and a tolerance of 1e-6.
GJK(unsigned int max_iterations_, FCL_REAL tolerance_) : max_iterations(max_iterations_),
tolerance(tolerance_)
{
......
......@@ -76,9 +76,8 @@ void exposeGJK()
if(!eigenpy::register_symbolic_link_to_registered_type<MinkowskiDiff>())
{
class_ <MinkowskiDiff> ("MinkowskiDiff",
doxygen::class_doc<MinkowskiDiff>(), init<>(
doxygen::constructor_doc<MinkowskiDiff>()))
class_ <MinkowskiDiff> ("MinkowskiDiff", doxygen::class_doc<MinkowskiDiff>(), no_init)
.def (doxygen::visitor::init<MinkowskiDiff>())
.def ("set", static_cast < void (MinkowskiDiff::*)(
const ShapeBase*, const ShapeBase*)> (&MinkowskiDiff::set),
doxygen::member_func_doc(
......@@ -100,16 +99,17 @@ void exposeGJK()
if(!eigenpy::register_symbolic_link_to_registered_type<GJK>())
{
class_ <GJK> ("GJK",
doxygen::class_doc<GJK>(), init<unsigned int, FCL_REAL>(
doxygen::constructor_doc<GJK, unsigned int, FCL_REAL>()))
class_ <GJK> ("GJK", doxygen::class_doc<GJK>(), no_init)
.def (doxygen::visitor::init<GJK, unsigned int, FCL_REAL>())
.DEF_RW_CLASS_ATTRIB (GJK, distance)
.DEF_RW_CLASS_ATTRIB (GJK, ray )
.DEF_RW_CLASS_ATTRIB (GJK, ray)
.DEF_RW_CLASS_ATTRIB (GJK, support_hint)
.DEF_CLASS_FUNC(GJK, evaluate)
.DEF_CLASS_FUNC(GJK, hasClosestPoints)
.DEF_CLASS_FUNC(GJK, hasPenetrationInformation)
.DEF_CLASS_FUNC(GJK, getClosestPoints)
.DEF_CLASS_FUNC(GJK, setDistanceEarlyBreak)
.DEF_CLASS_FUNC(GJK, getGuessFromSimplex)
;
}
}
......@@ -81,13 +81,13 @@ void exposeMaths ()
eigenpy::enableEigenPySpecific<Matrix3f>();
eigenpy::enableEigenPySpecific<Vec3f >();
class_ <Transform3f> ("Transform3f", doxygen::class_doc<Transform3f>(), init<>(arg("self"),doxygen::constructor_doc<Transform3f>()))
.def (init<Matrix3f, Vec3f> (doxygen::constructor_doc<Transform3f, const Matrix3f::MatrixBase&, const Vec3f::MatrixBase&>()))
.def (init<Quaternion3f, Vec3f>(doxygen::constructor_doc<Transform3f, const Quaternion3f& , const Vec3f::MatrixBase&>()))
.def (init<Matrix3f> (doxygen::constructor_doc<Transform3f, const Matrix3f& >()))
.def (init<Quaternion3f> (doxygen::constructor_doc<Transform3f, const Quaternion3f& >()))
.def (init<Vec3f> (doxygen::constructor_doc<Transform3f, const Vec3f& >()))
.def (init<Transform3f> (doxygen::constructor_doc<Transform3f, const Transform3f& >()))
class_ <Transform3f> ("Transform3f", doxygen::class_doc<Transform3f>(), no_init)
.def (dv::init<Transform3f>())
.def (dv::init<Transform3f, const Matrix3f::MatrixBase&, const Vec3f::MatrixBase&>())
.def (dv::init<Transform3f, const Quaternion3f& , const Vec3f::MatrixBase&>())
.def (dv::init<Transform3f, const Matrix3f& >())
.def (dv::init<Transform3f, const Quaternion3f& >())
.def (dv::init<Transform3f, const Vec3f& >())
.def (dv::member_func("getQuatRotation", &Transform3f::getQuatRotation))
.def ("getTranslation", &Transform3f::getTranslation, doxygen::member_func_doc(&Transform3f::getTranslation), return_value_policy<copy_const_reference>())
......
......@@ -97,7 +97,7 @@ template <> struct HPP_FCL_LOCAL shape_traits<ConvexBase> : shape_traits_base
};
};
void getShapeSupport(const TriangleP* triangle, const Vec3f& dir, Vec3f& support, int&)
void getShapeSupport(const TriangleP* triangle, const Vec3f& dir, Vec3f& support, int&, MinkowskiDiff::ShapeData*)
{
FCL_REAL dota = dir.dot(triangle->a);
FCL_REAL dotb = dir.dot(triangle->b);
......@@ -118,25 +118,25 @@ void getShapeSupport(const TriangleP* triangle, const Vec3f& dir, Vec3f& support
}
}
inline void getShapeSupport(const Box* box, const Vec3f& dir, Vec3f& support, int&)
inline void getShapeSupport(const Box* box, const Vec3f& dir, Vec3f& support, int&, MinkowskiDiff::ShapeData*)
{
const FCL_REAL inflate = (dir.array() == 0).any() ? 1.00000001 : 1.;
support.noalias() = (dir.array() > 0).select(inflate * box->halfSide, -inflate * box->halfSide);
}
inline void getShapeSupport(const Sphere*, const Vec3f& /*dir*/, Vec3f& support, int&)
inline void getShapeSupport(const Sphere*, const Vec3f& /*dir*/, Vec3f& support, int&, MinkowskiDiff::ShapeData*)
{
support.setZero();
}
inline void getShapeSupport(const Capsule* capsule, const Vec3f& dir, Vec3f& support, int&)
inline void getShapeSupport(const Capsule* capsule, const Vec3f& dir, Vec3f& support, int&, MinkowskiDiff::ShapeData*)
{
support.head<2>().setZero();
if (dir[2] > 0) support[2] = capsule->halfLength;
else support[2] = - capsule->halfLength;
}
void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support, int&)
void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support, int&, MinkowskiDiff::ShapeData*)
{
// The cone radius is, for -h < z < h, (h - z) * r / (2*h)
static const FCL_REAL inflate = 1.00001;
......@@ -174,7 +174,7 @@ void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support, int&)
}
}
void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, Vec3f& support, int&)
void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, Vec3f& support, int&, MinkowskiDiff::ShapeData*)
{
// The inflation makes the object look strictly convex to GJK and EPA. This
// helps solving particular cases (e.g. a cylinder with itself at the same
......@@ -196,8 +196,13 @@ void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, Vec3f& support,
< sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
}
void getShapeSupport(const ConvexBase* convex, const Vec3f& dir, Vec3f& support, int& hint)
struct SmallConvex : ShapeBase{};
struct LargeConvex : ShapeBase{};
void getShapeSupportLog(const ConvexBase* convex, const Vec3f& dir, Vec3f& support, int& hint, MinkowskiDiff::ShapeData* data)
{
assert(data != NULL);
const Vec3f* pts = convex->points;
const ConvexBase::Neighbors* nn = convex->neighbors;
......@@ -205,16 +210,30 @@ void getShapeSupport(const ConvexBase* convex, const Vec3f& dir, Vec3f& support,
hint = 0;
FCL_REAL maxdot = pts[hint].dot(dir);
FCL_REAL dot;
bool found = true;
std::vector<int8_t>& visited = data->visited;
visited.assign(convex->num_points, false);
visited[hint] = true;
// when the first face is orthogonal to dir, all the dot products will be
// equal. Yet, the neighbors must be visited.
bool found = true, loose_check = true;
while (found)
{
const ConvexBase::Neighbors& n = nn[hint];
found = false;
for (int in = 0; in < n.count(); ++in) {
dot = pts[n[in]].dot(dir);
const int ip = n[in];
if (visited[ip]) continue;
visited[ip] = true;
dot = pts[ip].dot(dir);
bool better = false;
if (dot > maxdot) {
better = true;
loose_check = false;
} else if (loose_check && dot == maxdot)
better = true;
if (better) {
maxdot = dot;
hint = n[in];
hint = ip;
found = true;
}
}
......@@ -223,11 +242,49 @@ void getShapeSupport(const ConvexBase* convex, const Vec3f& dir, Vec3f& support,
support = pts[hint];
}
void getShapeSupportLinear(const ConvexBase* convex, const Vec3f& dir, Vec3f& support, int& hint, MinkowskiDiff::ShapeData*)
{
const Vec3f* pts = convex->points;
hint = 0;
FCL_REAL maxdot = pts[0].dot(dir);
for (int i = 1; i < convex->num_points; ++i) {
FCL_REAL dot = pts[i].dot(dir);
if (dot > maxdot) {
maxdot = dot;
hint = i;
}
}
support = pts[hint];
}
void getShapeSupport(const ConvexBase* convex, const Vec3f& dir, Vec3f& support, int& hint, MinkowskiDiff::ShapeData*)
{
// TODO add benchmark to set a proper value for switching between linear and
// logarithmic.
if (convex->num_points > 32) {
MinkowskiDiff::ShapeData data;
getShapeSupportLog(convex, dir, support, hint, &data);
}
else
getShapeSupportLinear(convex, dir, support, hint, NULL);
}
inline void getShapeSupport(const SmallConvex* convex, const Vec3f& dir, Vec3f& support, int& hint, MinkowskiDiff::ShapeData* data)
{
getShapeSupportLinear(reinterpret_cast<const ConvexBase*>(convex), dir, support, hint, data);
}
inline void getShapeSupport(const LargeConvex* convex, const Vec3f& dir, Vec3f& support, int& hint, MinkowskiDiff::ShapeData* data)
{
getShapeSupportLog(reinterpret_cast<const ConvexBase*>(convex), dir, support, hint, data);
}
#define CALL_GET_SHAPE_SUPPORT(ShapeType) \
getShapeSupport (static_cast<const ShapeType*>(shape), \
(shape_traits<ShapeType>::NeedNormalizedDir && !dirIsNormalized) \
? dir.normalized() : dir, \
support, hint)
support, hint, NULL)
Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized, int& hint)
{
......@@ -271,13 +328,13 @@ template <typename Shape0, typename Shape1, bool TransformIsIdentity>
void getSupportTpl (const Shape0* s0, const Shape1* s1,
const Matrix3f& oR1, const Vec3f& ot1,
const Vec3f& dir, Vec3f& support0, Vec3f& support1,
support_func_guess_t& hint)
support_func_guess_t& hint, MinkowskiDiff::ShapeData data[2])
{
getShapeSupport (s0, dir, support0, hint[0]);
getShapeSupport (s0, dir, support0, hint[0], &(data[0]));
if (TransformIsIdentity)
getShapeSupport (s1, - dir, support1, hint[1]);
getShapeSupport (s1, - dir, support1, hint[1], &(data[1]));
else {
getShapeSupport (s1, - oR1.transpose() * dir, support1, hint[1]);
getShapeSupport (s1, - oR1.transpose() * dir, support1, hint[1], &(data[1]));
support1 = oR1 * support1 + ot1;
}
}
......@@ -285,7 +342,7 @@ void getSupportTpl (const Shape0* s0, const Shape1* s1,
template <typename Shape0, typename Shape1, bool TransformIsIdentity>
void getSupportFuncTpl (const MinkowskiDiff& md,
const Vec3f& dir, bool dirIsNormalized, Vec3f& support0, Vec3f& support1,
support_func_guess_t& hint)
support_func_guess_t& hint, MinkowskiDiff::ShapeData data[2])
{
enum { NeedNormalizedDir =
bool ( (bool)shape_traits<Shape0>::NeedNormalizedDir
......@@ -304,12 +361,12 @@ void getSupportFuncTpl (const MinkowskiDiff& md,
static_cast <const Shape1*>(md.shapes[1]),
md.oR1, md.ot1,
(NeedNormalizedDir && !dirIsNormalized) ? dir.normalized() : dir,
support0, support1, hint);
support0, support1, hint, data);
}
template <typename Shape0>
MinkowskiDiff::GetSupportFunction makeGetSupportFunction1 (const ShapeBase* s1, bool identity,
Eigen::Array<FCL_REAL, 1, 2>& inflation)
Eigen::Array<FCL_REAL, 1, 2>& inflation, int linear_log_convex_threshold)
{
inflation[1] = 0;
switch(s1->getNodeType())
......@@ -335,41 +392,49 @@ MinkowskiDiff::GetSupportFunction makeGetSupportFunction1 (const ShapeBase* s1,
if (identity) return getSupportFuncTpl<Shape0, Cylinder, true >;
else return getSupportFuncTpl<Shape0, Cylinder, false>;
case GEOM_CONVEX:
if (identity) return getSupportFuncTpl<Shape0, ConvexBase, true >;
else return getSupportFuncTpl<Shape0, ConvexBase, false>;
if (static_cast<const ConvexBase*>(s1)->num_points > linear_log_convex_threshold) {
if (identity) return getSupportFuncTpl<Shape0, LargeConvex, true >;
else return getSupportFuncTpl<Shape0, LargeConvex, false>;
} else {
if (identity) return getSupportFuncTpl<Shape0, SmallConvex, true >;
else return getSupportFuncTpl<Shape0, SmallConvex, false>;
}
default:
throw std::logic_error ("Unsupported geometric shape");
}
}
MinkowskiDiff::GetSupportFunction makeGetSupportFunction0 (const ShapeBase* s0, const ShapeBase* s1, bool identity,
Eigen::Array<FCL_REAL, 1, 2>& inflation)
Eigen::Array<FCL_REAL, 1, 2>& inflation, int linear_log_convex_threshold)
{
inflation[0] = 0;
switch(s0->getNodeType())
{
case GEOM_TRIANGLE:
return makeGetSupportFunction1<TriangleP> (s1, identity, inflation);
return makeGetSupportFunction1<TriangleP> (s1, identity, inflation, linear_log_convex_threshold);
break;
case GEOM_BOX:
return makeGetSupportFunction1<Box> (s1, identity, inflation);
return makeGetSupportFunction1<Box> (s1, identity, inflation, linear_log_convex_threshold);
break;
case GEOM_SPHERE:
inflation[0] = static_cast<const Sphere*>(s0)->radius;
return makeGetSupportFunction1<Sphere> (s1, identity, inflation);
return makeGetSupportFunction1<Sphere> (s1, identity, inflation, linear_log_convex_threshold);
break;
case GEOM_CAPSULE:
inflation[0] = static_cast<const Capsule*>(s0)->radius;
return makeGetSupportFunction1<Capsule> (s1, identity, inflation);
return makeGetSupportFunction1<Capsule> (s1, identity, inflation, linear_log_convex_threshold);
break;
case GEOM_CONE:
return makeGetSupportFunction1<Cone> (s1, identity, inflation);
return makeGetSupportFunction1<Cone> (s1, identity, inflation, linear_log_convex_threshold);
break;
case GEOM_CYLINDER:
return makeGetSupportFunction1<Cylinder> (s1, identity, inflation);
return makeGetSupportFunction1<Cylinder> (s1, identity, inflation, linear_log_convex_threshold);
break;
case GEOM_CONVEX:
return makeGetSupportFunction1<ConvexBase> (s1, identity, inflation);
if (static_cast<const ConvexBase*>(s0)->num_points > linear_log_convex_threshold)
return makeGetSupportFunction1<LargeConvex> (s1, identity, inflation, linear_log_convex_threshold);
else
return makeGetSupportFunction1<SmallConvex> (s1, identity, inflation, linear_log_convex_threshold);
break;
default:
throw std::logic_error ("Unsupported geometric shape");
......@@ -387,7 +452,7 @@ void MinkowskiDiff::set (const ShapeBase* shape0, const ShapeBase* shape1,
bool identity = (oR1.isIdentity() && ot1.isZero());
getSupportFunc = makeGetSupportFunction0 (shape0, shape1, identity, inflation);
getSupportFunc = makeGetSupportFunction0 (shape0, shape1, identity, inflation, linear_log_convex_threshold);
}
void MinkowskiDiff::set (const ShapeBase* shape0, const ShapeBase* shape1)
......@@ -398,7 +463,7 @@ void MinkowskiDiff::set (const ShapeBase* shape0, const ShapeBase* shape1)
oR1.setIdentity();
ot1.setZero();
getSupportFunc = makeGetSupportFunction0 (shape0, shape1, true, inflation);
getSupportFunc = makeGetSupportFunction0 (shape0, shape1, true, inflation, linear_log_convex_threshold);
}
void GJK::initialize()
......
Markdown is supported
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