diff --git a/CMakeLists.txt b/CMakeLists.txt
index 68bd2278fc7de457da864fa703c1afa6d251f3ae..735b0be017a6c8511968e17ffcbc2a9636acd4c7 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -237,9 +237,7 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/fcl/narrowphase/gjk.h
include/hpp/fcl/narrowphase/narrowphase_defaults.h
include/hpp/fcl/narrowphase/minkowski_difference.h
- include/hpp/fcl/narrowphase/minkowski_difference.hxx
include/hpp/fcl/narrowphase/support_functions.h
- include/hpp/fcl/narrowphase/support_functions.hxx
include/hpp/fcl/shape/convex.h
include/hpp/fcl/shape/details/convex.hxx
include/hpp/fcl/shape/geometric_shape_to_BVH_model.h
diff --git a/include/hpp/fcl/narrowphase/minkowski_difference.h b/include/hpp/fcl/narrowphase/minkowski_difference.h
index 881a7a4a08f1c3ef0d13f2298d9016d252914fc2..f6c0f93f4c72a4fb3deadb5392ef30d8d97f5241 100644
--- a/include/hpp/fcl/narrowphase/minkowski_difference.h
+++ b/include/hpp/fcl/narrowphase/minkowski_difference.h
@@ -185,6 +185,4 @@ struct HPP_FCL_DLLAPI MinkowskiDiff {
} // namespace fcl
} // namespace hpp
-#include "hpp/fcl/narrowphase/minkowski_difference.hxx"
-
#endif // HPP_FCL_MINKOWSKI_DIFFERENCE_H
diff --git a/include/hpp/fcl/narrowphase/minkowski_difference.hxx b/include/hpp/fcl/narrowphase/minkowski_difference.hxx
deleted file mode 100644
index b0c369a9d1d99155b84b91780ce4c77eb862eb40..0000000000000000000000000000000000000000
--- a/include/hpp/fcl/narrowphase/minkowski_difference.hxx
+++ /dev/null
@@ -1,308 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011-2014, Willow Garage, Inc.
- * Copyright (c) 2014-2015, Open Source Robotics Foundation
- * Copyright (c) 2021-2024, INRIA
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Open Source Robotics Foundation nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/** \authors Jia Pan, Florent Lamiraux, Josef Mirabel, Louis Montaut */
-
-#ifndef HPP_FCL_MINKOWSKI_DIFFERENCE_HXX
-#define HPP_FCL_MINKOWSKI_DIFFERENCE_HXX
-
-#include "hpp/fcl/shape/geometric_shapes_traits.h"
-
-namespace hpp {
-namespace fcl {
-
-namespace details {
-
-// ============================================================================
-template <typename Shape0, typename Shape1, bool TransformIsIdentity,
- int _SupportOptions>
-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,
- ShapeSupportData data[2]) {
- assert(dir.norm() > Eigen::NumTraits<FCL_REAL>::epsilon());
- getShapeSupport<_SupportOptions>(s0, dir, support0, hint[0], &(data[0]));
-
- if (TransformIsIdentity) {
- getShapeSupport<_SupportOptions>(s1, -dir, support1, hint[1], &(data[1]));
- } else {
- getShapeSupport<_SupportOptions>(s1, -oR1.transpose() * dir, support1,
- hint[1], &(data[1]));
- support1 = oR1 * support1 + ot1;
- }
-}
-
-// ============================================================================
-template <typename Shape0, typename Shape1, bool TransformIsIdentity,
- int _SupportOptions>
-void getSupportFuncTpl(const MinkowskiDiff& md, const Vec3f& dir,
- Vec3f& support0, Vec3f& support1,
- support_func_guess_t& hint, ShapeSupportData data[2]) {
- getSupportTpl<Shape0, Shape1, TransformIsIdentity, _SupportOptions>(
- static_cast<const Shape0*>(md.shapes[0]),
- static_cast<const Shape1*>(md.shapes[1]), md.oR1, md.ot1, dir, support0,
- support1, hint, data);
-}
-
-// ============================================================================
-template <typename Shape0, int _SupportOptions>
-MinkowskiDiff::GetSupportFunction makeGetSupportFunction1(
- const ShapeBase* s1, bool identity,
- Eigen::Array<FCL_REAL, 1, 2>& swept_sphere_radius,
- ShapeSupportData data[2]) {
- if (_SupportOptions == SupportOptions::WithSweptSphere) {
- // No need to store the information of swept sphere radius
- swept_sphere_radius[1] = 0;
- } else {
- // We store the information of swept sphere radius.
- // GJK and EPA will use this information to correct the solution they find.
- swept_sphere_radius[1] = s1->getSweptSphereRadius();
- }
-
- switch (s1->getNodeType()) {
- case GEOM_TRIANGLE:
- if (identity)
- return getSupportFuncTpl<Shape0, TriangleP, true, _SupportOptions>;
- else
- return getSupportFuncTpl<Shape0, TriangleP, false, _SupportOptions>;
- case GEOM_BOX:
- if (identity)
- return getSupportFuncTpl<Shape0, Box, true, _SupportOptions>;
- else
- return getSupportFuncTpl<Shape0, Box, false, _SupportOptions>;
- case GEOM_SPHERE:
- if (_SupportOptions == SupportOptions::NoSweptSphere) {
- // Sphere can be considered a swept-sphere point.
- swept_sphere_radius[1] += static_cast<const Sphere*>(s1)->radius;
- }
- if (identity)
- return getSupportFuncTpl<Shape0, Sphere, true, _SupportOptions>;
- else
- return getSupportFuncTpl<Shape0, Sphere, false, _SupportOptions>;
- case GEOM_ELLIPSOID:
- if (identity)
- return getSupportFuncTpl<Shape0, Ellipsoid, true, _SupportOptions>;
- else
- return getSupportFuncTpl<Shape0, Ellipsoid, false, _SupportOptions>;
- case GEOM_CAPSULE:
- if (_SupportOptions == SupportOptions::NoSweptSphere) {
- // Sphere can be considered as a swept-sphere segment.
- swept_sphere_radius[1] += static_cast<const Capsule*>(s1)->radius;
- }
- if (identity)
- return getSupportFuncTpl<Shape0, Capsule, true, _SupportOptions>;
- else
- return getSupportFuncTpl<Shape0, Capsule, false, _SupportOptions>;
- case GEOM_CONE:
- if (identity)
- return getSupportFuncTpl<Shape0, Cone, true, _SupportOptions>;
- else
- return getSupportFuncTpl<Shape0, Cone, false, _SupportOptions>;
- case GEOM_CYLINDER:
- if (identity)
- return getSupportFuncTpl<Shape0, Cylinder, true, _SupportOptions>;
- else
- return getSupportFuncTpl<Shape0, Cylinder, false, _SupportOptions>;
- case GEOM_CONVEX: {
- const ConvexBase* convex1 = static_cast<const ConvexBase*>(s1);
- if (static_cast<size_t>(convex1->num_points) >
- ConvexBase::num_vertices_large_convex_threshold) {
- data[1].visited.assign(convex1->num_points, false);
- if (identity)
- return getSupportFuncTpl<Shape0, LargeConvex, true, _SupportOptions>;
- else
- return getSupportFuncTpl<Shape0, LargeConvex, false, _SupportOptions>;
- } else {
- if (identity)
- return getSupportFuncTpl<Shape0, SmallConvex, true, _SupportOptions>;
- else
- return getSupportFuncTpl<Shape0, SmallConvex, false, _SupportOptions>;
- }
- }
- default:
- HPP_FCL_THROW_PRETTY("Unsupported geometric shape.", std::logic_error);
- }
-}
-
-// ============================================================================
-template <int _SupportOptions>
-MinkowskiDiff::GetSupportFunction makeGetSupportFunction0(
- const ShapeBase* s0, const ShapeBase* s1, bool identity,
- Eigen::Array<FCL_REAL, 1, 2>& swept_sphere_radius,
- ShapeSupportData data[2]) {
- if (_SupportOptions == SupportOptions::WithSweptSphere) {
- // No need to store the information of swept sphere radius
- swept_sphere_radius[0] = 0;
- } else {
- // We store the information of swept sphere radius.
- // GJK and EPA will use this information to correct the solution they find.
- swept_sphere_radius[0] = s0->getSweptSphereRadius();
- }
-
- switch (s0->getNodeType()) {
- case GEOM_TRIANGLE:
- return makeGetSupportFunction1<TriangleP, _SupportOptions>(
- s1, identity, swept_sphere_radius, data);
- break;
- case GEOM_BOX:
- return makeGetSupportFunction1<Box, _SupportOptions>(
- s1, identity, swept_sphere_radius, data);
- break;
- case GEOM_SPHERE:
- if (_SupportOptions == SupportOptions::NoSweptSphere) {
- // Sphere can always be considered as a swept-sphere point.
- swept_sphere_radius[0] += static_cast<const Sphere*>(s0)->radius;
- }
- return makeGetSupportFunction1<Sphere, _SupportOptions>(
- s1, identity, swept_sphere_radius, data);
- break;
- case GEOM_ELLIPSOID:
- return makeGetSupportFunction1<Ellipsoid, _SupportOptions>(
- s1, identity, swept_sphere_radius, data);
- break;
- case GEOM_CAPSULE:
- if (_SupportOptions == SupportOptions::NoSweptSphere) {
- // Capsule can always be considered as a swept-sphere segment.
- swept_sphere_radius[0] += static_cast<const Capsule*>(s0)->radius;
- }
- return makeGetSupportFunction1<Capsule, _SupportOptions>(
- s1, identity, swept_sphere_radius, data);
- break;
- case GEOM_CONE:
- return makeGetSupportFunction1<Cone, _SupportOptions>(
- s1, identity, swept_sphere_radius, data);
- break;
- case GEOM_CYLINDER:
- return makeGetSupportFunction1<Cylinder, _SupportOptions>(
- s1, identity, swept_sphere_radius, data);
- break;
- case GEOM_CONVEX: {
- const ConvexBase* convex0 = static_cast<const ConvexBase*>(s0);
- if (static_cast<size_t>(convex0->num_points) >
- ConvexBase::num_vertices_large_convex_threshold) {
- data[0].visited.assign(convex0->num_points, false);
- return makeGetSupportFunction1<LargeConvex, _SupportOptions>(
- s1, identity, swept_sphere_radius, data);
- } else
- return makeGetSupportFunction1<SmallConvex, _SupportOptions>(
- s1, identity, swept_sphere_radius, data);
- break;
- }
- default:
- HPP_FCL_THROW_PRETTY("Unsupported geometric shape", std::logic_error);
- }
-}
-
-// ============================================================================
-inline bool getNormalizeSupportDirection(const ShapeBase* shape) {
- switch (shape->getNodeType()) {
- case GEOM_TRIANGLE:
- return (bool)shape_traits<TriangleP>::NeedNesterovNormalizeHeuristic;
- break;
- case GEOM_BOX:
- return (bool)shape_traits<Box>::NeedNesterovNormalizeHeuristic;
- break;
- case GEOM_SPHERE:
- return (bool)shape_traits<Sphere>::NeedNesterovNormalizeHeuristic;
- break;
- case GEOM_ELLIPSOID:
- return (bool)shape_traits<Ellipsoid>::NeedNesterovNormalizeHeuristic;
- break;
- case GEOM_CAPSULE:
- return (bool)shape_traits<Capsule>::NeedNesterovNormalizeHeuristic;
- break;
- case GEOM_CONE:
- return (bool)shape_traits<Cone>::NeedNesterovNormalizeHeuristic;
- break;
- case GEOM_CYLINDER:
- return (bool)shape_traits<Cylinder>::NeedNesterovNormalizeHeuristic;
- break;
- case GEOM_CONVEX:
- return (bool)shape_traits<ConvexBase>::NeedNesterovNormalizeHeuristic;
- break;
- default:
- HPP_FCL_THROW_PRETTY("Unsupported geometric shape", std::logic_error);
- }
-}
-
-// ============================================================================
-inline void getNormalizeSupportDirectionFromShapes(
- const ShapeBase* shape0, const ShapeBase* shape1,
- bool& normalize_support_direction) {
- normalize_support_direction = getNormalizeSupportDirection(shape0) &&
- getNormalizeSupportDirection(shape1);
-}
-
-// ============================================================================
-template <int _SupportOptions>
-void MinkowskiDiff::set(const ShapeBase* shape0, const ShapeBase* shape1,
- const Transform3f& tf0, const Transform3f& tf1) {
- shapes[0] = shape0;
- shapes[1] = shape1;
- getNormalizeSupportDirectionFromShapes(shape0, shape1,
- normalize_support_direction);
-
- oR1.noalias() = tf0.getRotation().transpose() * tf1.getRotation();
- ot1.noalias() = tf0.getRotation().transpose() *
- (tf1.getTranslation() - tf0.getTranslation());
-
- bool identity = (oR1.isIdentity() && ot1.isZero());
-
- getSupportFunc = makeGetSupportFunction0<_SupportOptions>(
- shape0, shape1, identity, swept_sphere_radius, data);
-}
-
-// ============================================================================
-template <int _SupportOptions>
-void MinkowskiDiff::set(const ShapeBase* shape0, const ShapeBase* shape1) {
- shapes[0] = shape0;
- shapes[1] = shape1;
- getNormalizeSupportDirectionFromShapes(shape0, shape1,
- normalize_support_direction);
-
- oR1.setIdentity();
- ot1.setZero();
-
- getSupportFunc = makeGetSupportFunction0<_SupportOptions>(
- shape0, shape1, true, swept_sphere_radius, data);
-}
-
-} // namespace details
-
-} // namespace fcl
-} // namespace hpp
-
-#endif // HPP_FCL_MINKOWSKI_DIFFERENCE_HXX
diff --git a/include/hpp/fcl/narrowphase/support_functions.h b/include/hpp/fcl/narrowphase/support_functions.h
index fc9b5ea64021de577bef02c5ec6319a91b8c3922..a38fd5d50b10591286646fe4e6d796f2e3d12387 100644
--- a/include/hpp/fcl/narrowphase/support_functions.h
+++ b/include/hpp/fcl/narrowphase/support_functions.h
@@ -75,7 +75,8 @@ enum SupportOptions {
/// sphere radii. Please see `MinkowskiDiff::set(const ShapeBase*, const
/// ShapeBase*)` for more details.
template <int _SupportOptions = SupportOptions::NoSweptSphere>
-Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, int& hint);
+HPP_FCL_DLLAPI Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir,
+ int& hint);
/// @brief Templated version of @ref getSupport.
template <typename ShapeType,
@@ -196,9 +197,9 @@ void getShapeSupport(const LargeConvex* convex, const Vec3f& dir,
/// v.dot(dir) <= tol` is tol distant from the support plane.
///
template <int _SupportOptions = SupportOptions::NoSweptSphere>
-void getSupportSet(const ShapeBase* shape, const Vec3f& dir,
- const Transform3f& ctfi, FCL_REAL tol,
- SupportSet& support_set, const int hint);
+HPP_FCL_DLLAPI void getSupportSet(const ShapeBase* shape, const Vec3f& dir,
+ const Transform3f& ctfi, FCL_REAL tol,
+ SupportSet& support_set, const int hint);
/// @brief Templated support set function, i.e. templated version of @ref
/// getSupportSet.
@@ -286,6 +287,4 @@ void getShapeSupportSet(const LargeConvex* convex, const Vec3f& dir,
} // namespace fcl
} // namespace hpp
-#include "hpp/fcl/narrowphase/support_functions.hxx"
-
#endif // HPP_FCL_SUPPORT_FUNCTIONS_H
diff --git a/include/hpp/fcl/narrowphase/support_functions.hxx b/include/hpp/fcl/narrowphase/support_functions.hxx
deleted file mode 100644
index 055e821874a4bc2bdd4c7298cf21123e5f9f78f8..0000000000000000000000000000000000000000
--- a/include/hpp/fcl/narrowphase/support_functions.hxx
+++ /dev/null
@@ -1,577 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011-2014, Willow Garage, Inc.
- * Copyright (c) 2014-2015, Open Source Robotics Foundation
- * Copyright (c) 2021-2024, INRIA
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Open Source Robotics Foundation nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/** \authors Jia Pan, Florent Lamiraux, Josef Mirabel, Louis Montaut */
-
-#ifndef HPP_FCL_SUPPORT_FUNCTIONS_HXX
-#define HPP_FCL_SUPPORT_FUNCTIONS_HXX
-
-namespace hpp {
-namespace fcl {
-
-namespace details {
-
-// ============================================================================
-#define CALL_GET_SHAPE_SUPPORT(ShapeType) \
- getShapeSupport<_SupportOptions>(static_cast<const ShapeType*>(shape), dir, \
- support, hint, nullptr)
-template <int _SupportOptions>
-Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, int& hint) {
- Vec3f support;
- switch (shape->getNodeType()) {
- case GEOM_TRIANGLE:
- CALL_GET_SHAPE_SUPPORT(TriangleP);
- break;
- case GEOM_BOX:
- CALL_GET_SHAPE_SUPPORT(Box);
- break;
- case GEOM_SPHERE:
- CALL_GET_SHAPE_SUPPORT(Sphere);
- break;
- case GEOM_ELLIPSOID:
- CALL_GET_SHAPE_SUPPORT(Ellipsoid);
- break;
- case GEOM_CAPSULE:
- CALL_GET_SHAPE_SUPPORT(Capsule);
- break;
- case GEOM_CONE:
- CALL_GET_SHAPE_SUPPORT(Cone);
- break;
- case GEOM_CYLINDER:
- CALL_GET_SHAPE_SUPPORT(Cylinder);
- break;
- case GEOM_CONVEX:
- CALL_GET_SHAPE_SUPPORT(ConvexBase);
- break;
- case GEOM_PLANE:
- case GEOM_HALFSPACE:
- default:
- support.setZero();
- ; // nothing
- }
-
- return support;
-}
-#undef CALL_GET_SHAPE_SUPPORT
-
-// ============================================================================
-template <typename ShapeType, int _SupportOptions>
-Vec3f getSupportTpl(const ShapeBase* shape_, const Vec3f& dir, int& hint) {
- const ShapeType* shape = static_cast<const ShapeType*>(shape_);
- Vec3f support;
- getShapeSupport(shape, dir, support, hint, nullptr);
- return support;
-}
-
-// ============================================================================
-template <typename ShapeType, int _SupportOptions>
-void getShapeSupportTpl(const ShapeBase* shape_, const Vec3f& dir,
- Vec3f& support, int& hint, ShapeSupportData* data) {
- const ShapeType* shape = static_cast<const ShapeType*>(shape_);
- return getShapeSupport(shape, dir, support, hint, data);
-}
-
-// ============================================================================
-template <int _SupportOptions>
-void getShapeSupport(const TriangleP* triangle, const Vec3f& dir,
- Vec3f& support, int& /*unused*/,
- ShapeSupportData* /*data*/) {
- FCL_REAL dota = dir.dot(triangle->a);
- FCL_REAL dotb = dir.dot(triangle->b);
- FCL_REAL dotc = dir.dot(triangle->c);
- if (dota > dotb) {
- if (dotc > dota) {
- support = triangle->c;
- } else {
- support = triangle->a;
- }
- } else {
- if (dotc > dotb) {
- support = triangle->c;
- } else {
- support = triangle->b;
- }
- }
-
- if (_SupportOptions == SupportOptions::WithSweptSphere) {
- support += triangle->getSweptSphereRadius() * dir.normalized();
- }
-}
-
-// ============================================================================
-template <int _SupportOptions>
-inline void getShapeSupport(const Box* box, const Vec3f& dir, Vec3f& support,
- int& /*unused*/, ShapeSupportData* /*unused*/) {
- // The inflate value is simply to make the specialized functions with box
- // have a preferred side for edge cases.
- static const FCL_REAL inflate = (dir.array() == 0).any() ? 1 + 1e-10 : 1.;
- static const FCL_REAL dummy_precision =
- Eigen::NumTraits<FCL_REAL>::dummy_precision();
- Vec3f support1 = (dir.array() > dummy_precision).select(box->halfSide, 0);
- Vec3f support2 =
- (dir.array() < -dummy_precision).select(-inflate * box->halfSide, 0);
- support.noalias() = support1 + support2;
-
- if (_SupportOptions == SupportOptions::WithSweptSphere) {
- support += box->getSweptSphereRadius() * dir.normalized();
- }
-}
-
-// ============================================================================
-template <int _SupportOptions>
-inline void getShapeSupport(const Sphere* sphere, const Vec3f& dir,
- Vec3f& support, int& /*unused*/,
- ShapeSupportData* /*unused*/) {
- if (_SupportOptions == SupportOptions::WithSweptSphere) {
- support.noalias() =
- (sphere->radius + sphere->getSweptSphereRadius()) * dir.normalized();
- } else {
- support.setZero();
- }
-
- HPP_FCL_UNUSED_VARIABLE(sphere);
- HPP_FCL_UNUSED_VARIABLE(dir);
-}
-
-// ============================================================================
-template <int _SupportOptions>
-inline void getShapeSupport(const Ellipsoid* ellipsoid, const Vec3f& dir,
- Vec3f& support, int& /*unused*/,
- ShapeSupportData* /*unused*/) {
- FCL_REAL a2 = ellipsoid->radii[0] * ellipsoid->radii[0];
- FCL_REAL b2 = ellipsoid->radii[1] * ellipsoid->radii[1];
- FCL_REAL c2 = ellipsoid->radii[2] * ellipsoid->radii[2];
-
- Vec3f v(a2 * dir[0], b2 * dir[1], c2 * dir[2]);
-
- FCL_REAL d = std::sqrt(v.dot(dir));
-
- support = v / d;
-
- if (_SupportOptions == SupportOptions::WithSweptSphere) {
- support += ellipsoid->getSweptSphereRadius() * dir.normalized();
- }
-}
-
-// ============================================================================
-template <int _SupportOptions>
-inline void getShapeSupport(const Capsule* capsule, const Vec3f& dir,
- Vec3f& support, int& /*unused*/,
- ShapeSupportData* /*unused*/) {
- static const FCL_REAL dummy_precision =
- Eigen::NumTraits<FCL_REAL>::dummy_precision();
- support.setZero();
- if (dir[2] > dummy_precision) {
- support[2] = capsule->halfLength;
- } else if (dir[2] < -dummy_precision) {
- support[2] = -capsule->halfLength;
- }
-
- if (_SupportOptions == SupportOptions::WithSweptSphere) {
- support +=
- (capsule->radius + capsule->getSweptSphereRadius()) * dir.normalized();
- }
-}
-
-// ============================================================================
-template <int _SupportOptions>
-void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support,
- int& /*unused*/, ShapeSupportData* /*unused*/) {
- static const FCL_REAL dummy_precision =
- Eigen::NumTraits<FCL_REAL>::dummy_precision();
-
- // The cone radius is, for -h < z < h, (h - z) * r / (2*h)
- // The inflate value is simply to make the specialized functions with cone
- // have a preferred side for edge cases.
- static const FCL_REAL inflate = 1 + 1e-10;
- FCL_REAL h = cone->halfLength;
- FCL_REAL r = cone->radius;
-
- if (dir.head<2>().isZero(dummy_precision)) {
- support.head<2>().setZero();
- if (dir[2] > dummy_precision) {
- support[2] = h;
- } else {
- support[2] = -inflate * h;
- }
- } else {
- FCL_REAL zdist = dir[0] * dir[0] + dir[1] * dir[1];
- FCL_REAL len = zdist + dir[2] * dir[2];
- zdist = std::sqrt(zdist);
-
- if (dir[2] <= 0) {
- FCL_REAL rad = r / zdist;
- support.head<2>() = rad * dir.head<2>();
- support[2] = -h;
- } else {
- len = std::sqrt(len);
- FCL_REAL sin_a = r / std::sqrt(r * r + 4 * h * h);
-
- if (dir[2] > len * sin_a)
- support << 0, 0, h;
- else {
- FCL_REAL rad = r / zdist;
- support.head<2>() = rad * dir.head<2>();
- support[2] = -h;
- }
- }
- }
-
- if (_SupportOptions == SupportOptions::WithSweptSphere) {
- support += cone->getSweptSphereRadius() * dir.normalized();
- }
-}
-
-// ============================================================================
-template <int _SupportOptions>
-void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, Vec3f& support,
- int& /*unused*/, ShapeSupportData* /*unused*/) {
- static const FCL_REAL dummy_precision =
- Eigen::NumTraits<FCL_REAL>::dummy_precision();
-
- // The inflate value is simply to make the specialized functions with cylinder
- // have a preferred side for edge cases.
- static const FCL_REAL inflate = 1 + 1e-10;
- FCL_REAL half_h = cylinder->halfLength;
- FCL_REAL r = cylinder->radius;
-
- const bool dir_is_aligned_with_z = dir.head<2>().isZero(dummy_precision);
- if (dir_is_aligned_with_z) half_h *= inflate;
-
- if (dir[2] > dummy_precision) {
- support[2] = half_h;
- } else if (dir[2] < -dummy_precision) {
- support[2] = -half_h;
- } else {
- support[2] = 0;
- r *= inflate;
- }
-
- if (dir_is_aligned_with_z) {
- support.head<2>().setZero();
- } else {
- support.head<2>() = dir.head<2>().normalized() * r;
- }
-
- assert(fabs(support[0] * dir[1] - support[1] * dir[0]) <
- sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
-
- if (_SupportOptions == SupportOptions::WithSweptSphere) {
- support += cylinder->getSweptSphereRadius() * dir.normalized();
- }
-}
-
-// ============================================================================
-template <int _SupportOptions>
-void getShapeSupportLog(const ConvexBase* convex, const Vec3f& dir,
- Vec3f& support, int& hint, ShapeSupportData* data) {
- assert(data != nullptr && "data is null.");
- assert(convex->neighbors != nullptr && "Convex has no neighbors.");
-
- // Use warm start if current support direction is distant from last support
- // direction.
- const double use_warm_start_threshold = 0.9;
- Vec3f dir_normalized = dir.normalized();
- if (!data->last_dir.isZero() && !convex->support_warm_starts.points.empty() &&
- data->last_dir.dot(dir_normalized) < use_warm_start_threshold) {
- // Change hint if last dir is too far from current dir.
- FCL_REAL maxdot = convex->support_warm_starts.points[0].dot(dir);
- hint = convex->support_warm_starts.indices[0];
- for (size_t i = 1; i < convex->support_warm_starts.points.size(); ++i) {
- FCL_REAL dot = convex->support_warm_starts.points[i].dot(dir);
- if (dot > maxdot) {
- maxdot = dot;
- hint = convex->support_warm_starts.indices[i];
- }
- }
- }
- data->last_dir = dir_normalized;
-
- const std::vector<Vec3f>& pts = *(convex->points);
- const std::vector<ConvexBase::Neighbors>& nn = *(convex->neighbors);
-
- if (hint < 0 || hint >= (int)convex->num_points) {
- hint = 0;
- }
- FCL_REAL maxdot = pts[static_cast<size_t>(hint)].dot(dir);
- std::vector<int8_t>& visited = data->visited;
- assert(data->visited.size() == convex->num_points);
- std::fill(visited.begin(), visited.end(), false);
- visited[static_cast<std::size_t>(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;
- bool loose_check = true;
- while (found) {
- const ConvexBase::Neighbors& n = nn[static_cast<size_t>(hint)];
- found = false;
- for (int in = 0; in < n.count(); ++in) {
- const unsigned int ip = n[in];
- if (visited[ip]) continue;
- visited[ip] = true;
- const FCL_REAL 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 = static_cast<int>(ip);
- found = true;
- }
- }
- }
-
- support = pts[static_cast<size_t>(hint)];
-
- if (_SupportOptions == SupportOptions::WithSweptSphere) {
- support += convex->getSweptSphereRadius() * dir.normalized();
- }
-}
-
-// ============================================================================
-template <int _SupportOptions>
-void getShapeSupportLinear(const ConvexBase* convex, const Vec3f& dir,
- Vec3f& support, int& hint, ShapeSupportData*) {
- const std::vector<Vec3f>& pts = *(convex->points);
-
- hint = 0;
- FCL_REAL maxdot = pts[0].dot(dir);
- for (int i = 1; i < (int)convex->num_points; ++i) {
- FCL_REAL dot = pts[static_cast<size_t>(i)].dot(dir);
- if (dot > maxdot) {
- maxdot = dot;
- hint = i;
- }
- }
-
- support = pts[static_cast<size_t>(hint)];
-
- if (_SupportOptions == SupportOptions::WithSweptSphere) {
- support += convex->getSweptSphereRadius() * dir.normalized();
- }
-}
-
-// ============================================================================
-template <int _SupportOptions>
-void getShapeSupport(const ConvexBase* convex, const Vec3f& dir, Vec3f& support,
- int& hint, ShapeSupportData* /*unused*/) {
- // TODO add benchmark to set a proper value for switching between linear and
- // logarithmic.
- if (convex->num_points > ConvexBase::num_vertices_large_convex_threshold &&
- convex->neighbors != nullptr) {
- ShapeSupportData data;
- data.visited.assign(convex->num_points, false);
- getShapeSupportLog<_SupportOptions>(convex, dir, support, hint, &data);
- } else
- getShapeSupportLinear<_SupportOptions>(convex, dir, support, hint, nullptr);
-}
-
-// ============================================================================
-template <int _SupportOptions>
-inline void getShapeSupport(const SmallConvex* convex, const Vec3f& dir,
- Vec3f& support, int& hint, ShapeSupportData* data) {
- getShapeSupportLinear<_SupportOptions>(
- reinterpret_cast<const ConvexBase*>(convex), dir, support, hint, data);
-}
-
-// ============================================================================
-template <int _SupportOptions>
-inline void getShapeSupport(const LargeConvex* convex, const Vec3f& dir,
- Vec3f& support, int& hint, ShapeSupportData* data) {
- getShapeSupportLog<_SupportOptions>(
- reinterpret_cast<const ConvexBase*>(convex), dir, support, hint, data);
-}
-
-// ============================================================================
-#define CALL_GET_SHAPE_SUPPORT_SET(ShapeType) \
- getShapeSupportSet<_SupportOptions>(static_cast<const ShapeType*>(shape), \
- dir, ctfi, support_set, hint, nullptr)
-template <int _SupportOptions>
-void getSupportSet(const ShapeBase* shape, const Vec3f& dir,
- const Transform3f& ctfi, SupportSet& support_set,
- const int hint) {
- switch (shape->getNodeType()) {
- case GEOM_TRIANGLE:
- CALL_GET_SHAPE_SUPPORT_SET(TriangleP);
- break;
- case GEOM_BOX:
- CALL_GET_SHAPE_SUPPORT_SET(Box);
- break;
- case GEOM_SPHERE:
- CALL_GET_SHAPE_SUPPORT_SET(Sphere);
- break;
- case GEOM_ELLIPSOID:
- CALL_GET_SHAPE_SUPPORT_SET(Ellipsoid);
- break;
- case GEOM_CAPSULE:
- CALL_GET_SHAPE_SUPPORT_SET(Capsule);
- break;
- case GEOM_CONE:
- CALL_GET_SHAPE_SUPPORT_SET(Cone);
- break;
- case GEOM_CYLINDER:
- CALL_GET_SHAPE_SUPPORT_SET(Cylinder);
- break;
- case GEOM_CONVEX:
- CALL_GET_SHAPE_SUPPORT_SET(ConvexBase);
- break;
- case GEOM_PLANE:
- case GEOM_HALFSPACE:
- default:; // nothing
- }
-}
-#undef CALL_GET_SHAPE_SUPPORT
-
-// ============================================================================
-template <typename ShapeType, int _SupportOptions>
-void getSupportSetTpl(const ShapeBase* shape_, const Vec3f& dir,
- const Transform3f& ctfi, SupportSet& support_set,
- const int hint) {
- const ShapeType* shape = static_cast<const ShapeType*>(shape_);
- getShapeSupportSet<_SupportOptions>(shape, dir, ctfi, support_set, hint,
- nullptr);
-}
-
-// ============================================================================
-template <typename ShapeType, int _SupportOptions>
-void getShapeSupportSetTpl(const ShapeBase* shape_, const Vec3f& dir,
- const Transform3f& ctfi, SupportSet& support_set,
- const int hint, ShapeSupportData* data) {
- const ShapeType* shape = static_cast<const ShapeType*>(shape_);
- getShapeSupportSet<_SupportOptions>(shape, dir, ctfi, support_set, hint,
- data);
-}
-
-// ============================================================================
-template <int _SupportOptions>
-void getShapeSupportSet(const TriangleP* triangle, const Vec3f& dir,
- const Transform3f& ctfi, SupportSet& support_set,
- const int /*unused*/, ShapeSupportData* /*unused*/) {}
-
-// ============================================================================
-template <int _SupportOptions>
-void getShapeSupportSet(const Box* box, const Vec3f& dir,
- const Transform3f& ctfi, SupportSet& support_set,
- const int /*unused*/, ShapeSupportData* /*unused*/) {}
-
-// ============================================================================
-template <int _SupportOptions>
-void getShapeSupportSet(const Sphere* sphere, const Vec3f& dir,
- const Transform3f& ctfi, SupportSet& support_set,
- const int /*unused*/, ShapeSupportData* /*unused*/) {}
-
-// ============================================================================
-template <int _SupportOptions>
-void getShapeSupportSet(const Ellipsoid* ellipsoid, const Vec3f& dir,
- const Transform3f& ctfi, SupportSet& support_set,
- const int /*unused*/, ShapeSupportData* /*unused*/) {}
-
-// ============================================================================
-template <int _SupportOptions>
-void getShapeSupportSet(const Capsule* capsule, const Vec3f& dir,
- const Transform3f& ctfi, SupportSet& support_set,
- const int /*unused*/, ShapeSupportData* /*unused*/) {}
-
-// ============================================================================
-template <int _SupportOptions>
-void getShapeSupportSet(const Cone* cone, const Vec3f& dir,
- const Transform3f& ctfi, SupportSet& support_set,
- const int /*unused*/, ShapeSupportData* /*unused*/) {}
-
-// ============================================================================
-template <int _SupportOptions>
-void getShapeSupportSet(const Cylinder* cylinder, const Vec3f& dir,
- const Transform3f& ctfi, SupportSet& support_set,
- const int /*unused*/, ShapeSupportData* /*unused*/) {}
-
-// ============================================================================
-template <int _SupportOptions>
-void getShapeSupportSetLog(const ConvexBase* convex, const Vec3f& dir,
- const Transform3f& ctfi, SupportSet& support_set,
- const int hint, ShapeSupportData* data) {}
-
-// ============================================================================
-template <int _SupportOptions>
-void getShapeSupportSetLinear(const ConvexBase* convex, const Vec3f& dir,
- const Transform3f& ctfi, SupportSet& support_set,
- const int hint, ShapeSupportData* /*unused*/) {}
-
-// ============================================================================
-template <int _SupportOptions>
-void getShapeSupportSet(const ConvexBase* convex, const Vec3f& dir,
- const Transform3f& ctfi, SupportSet& support_set,
- const int hint, ShapeSupportData* /*unused*/) {
- if (convex->num_points > ConvexBase::num_vertices_large_convex_threshold &&
- convex->neighbors != nullptr) {
- ShapeSupportData data;
- data.visited.assign(convex->num_points, false);
- getShapeSupportSetLog<_SupportOptions>(convex, dir, ctfi, support_set, hint,
- &data);
- } else {
- getShapeSupportSetLinear<_SupportOptions>(convex, dir, ctfi, support_set,
- hint, nullptr);
- }
-}
-
-// ============================================================================
-template <int _SupportOptions>
-void getShapeSupportSet(const SmallConvex* convex, const Vec3f& dir,
- const Transform3f& ctfi, SupportSet& support_set,
- const int hint, ShapeSupportData* data) {
- getShapeSupportSetLinear<_SupportOptions>(
- reinterpret_cast<const ConvexBase*>(convex), dir, ctfi, support_set, hint,
- data);
-}
-
-// ============================================================================
-template <int _SupportOptions>
-void getShapeSupportSet(const LargeConvex* convex, const Vec3f& dir,
- const Transform3f& ctfi, SupportSet& support_set,
- const int hint, ShapeSupportData* data) {
- getShapeSupportSetLog<_SupportOptions>(
- reinterpret_cast<const ConvexBase*>(convex), dir, ctfi, support_set, hint,
- data);
-}
-
-} // namespace details
-
-} // namespace fcl
-} // namespace hpp
-
-#endif // HPP_FCL_SUPPORT_FUNCTIONS_HXX
diff --git a/src/narrowphase/minkowski_difference.cpp b/src/narrowphase/minkowski_difference.cpp
index 80685bee32742636bac6a7a662cb371e0df2e19b..481ec1f574d9cafad10b49fba70bd0bab43ddec4 100644
--- a/src/narrowphase/minkowski_difference.cpp
+++ b/src/narrowphase/minkowski_difference.cpp
@@ -1,22 +1,329 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
+ * Copyright (c) 2021-2024, INRIA
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \authors Jia Pan, Florent Lamiraux, Josef Mirabel, Louis Montaut */
+
#include "hpp/fcl/narrowphase/minkowski_difference.h"
+#include "hpp/fcl/shape/geometric_shapes_traits.h"
namespace hpp {
namespace fcl {
namespace details {
-// Explicit instantiation
+// ============================================================================
+template <typename Shape0, typename Shape1, bool TransformIsIdentity,
+ int _SupportOptions>
+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,
+ ShapeSupportData data[2]) {
+ assert(dir.norm() > Eigen::NumTraits<FCL_REAL>::epsilon());
+ getShapeSupport<_SupportOptions>(s0, dir, support0, hint[0], &(data[0]));
+
+ if (TransformIsIdentity) {
+ getShapeSupport<_SupportOptions>(s1, -dir, support1, hint[1], &(data[1]));
+ } else {
+ getShapeSupport<_SupportOptions>(s1, -oR1.transpose() * dir, support1,
+ hint[1], &(data[1]));
+ support1 = oR1 * support1 + ot1;
+ }
+}
+
+// ============================================================================
+template <typename Shape0, typename Shape1, bool TransformIsIdentity,
+ int _SupportOptions>
+void getSupportFuncTpl(const MinkowskiDiff& md, const Vec3f& dir,
+ Vec3f& support0, Vec3f& support1,
+ support_func_guess_t& hint, ShapeSupportData data[2]) {
+ getSupportTpl<Shape0, Shape1, TransformIsIdentity, _SupportOptions>(
+ static_cast<const Shape0*>(md.shapes[0]),
+ static_cast<const Shape1*>(md.shapes[1]), md.oR1, md.ot1, dir, support0,
+ support1, hint, data);
+}
+
+// ============================================================================
+template <typename Shape0, int _SupportOptions>
+MinkowskiDiff::GetSupportFunction makeGetSupportFunction1(
+ const ShapeBase* s1, bool identity,
+ Eigen::Array<FCL_REAL, 1, 2>& swept_sphere_radius,
+ ShapeSupportData data[2]) {
+ if (_SupportOptions == SupportOptions::WithSweptSphere) {
+ // No need to store the information of swept sphere radius
+ swept_sphere_radius[1] = 0;
+ } else {
+ // We store the information of swept sphere radius.
+ // GJK and EPA will use this information to correct the solution they find.
+ swept_sphere_radius[1] = s1->getSweptSphereRadius();
+ }
+
+ switch (s1->getNodeType()) {
+ case GEOM_TRIANGLE:
+ if (identity)
+ return getSupportFuncTpl<Shape0, TriangleP, true, _SupportOptions>;
+ else
+ return getSupportFuncTpl<Shape0, TriangleP, false, _SupportOptions>;
+ case GEOM_BOX:
+ if (identity)
+ return getSupportFuncTpl<Shape0, Box, true, _SupportOptions>;
+ else
+ return getSupportFuncTpl<Shape0, Box, false, _SupportOptions>;
+ case GEOM_SPHERE:
+ if (_SupportOptions == SupportOptions::NoSweptSphere) {
+ // Sphere can be considered a swept-sphere point.
+ swept_sphere_radius[1] += static_cast<const Sphere*>(s1)->radius;
+ }
+ if (identity)
+ return getSupportFuncTpl<Shape0, Sphere, true, _SupportOptions>;
+ else
+ return getSupportFuncTpl<Shape0, Sphere, false, _SupportOptions>;
+ case GEOM_ELLIPSOID:
+ if (identity)
+ return getSupportFuncTpl<Shape0, Ellipsoid, true, _SupportOptions>;
+ else
+ return getSupportFuncTpl<Shape0, Ellipsoid, false, _SupportOptions>;
+ case GEOM_CAPSULE:
+ if (_SupportOptions == SupportOptions::NoSweptSphere) {
+ // Sphere can be considered as a swept-sphere segment.
+ swept_sphere_radius[1] += static_cast<const Capsule*>(s1)->radius;
+ }
+ if (identity)
+ return getSupportFuncTpl<Shape0, Capsule, true, _SupportOptions>;
+ else
+ return getSupportFuncTpl<Shape0, Capsule, false, _SupportOptions>;
+ case GEOM_CONE:
+ if (identity)
+ return getSupportFuncTpl<Shape0, Cone, true, _SupportOptions>;
+ else
+ return getSupportFuncTpl<Shape0, Cone, false, _SupportOptions>;
+ case GEOM_CYLINDER:
+ if (identity)
+ return getSupportFuncTpl<Shape0, Cylinder, true, _SupportOptions>;
+ else
+ return getSupportFuncTpl<Shape0, Cylinder, false, _SupportOptions>;
+ case GEOM_CONVEX: {
+ const ConvexBase* convex1 = static_cast<const ConvexBase*>(s1);
+ if (static_cast<size_t>(convex1->num_points) >
+ ConvexBase::num_vertices_large_convex_threshold) {
+ data[1].visited.assign(convex1->num_points, false);
+ if (identity)
+ return getSupportFuncTpl<Shape0, LargeConvex, true, _SupportOptions>;
+ else
+ return getSupportFuncTpl<Shape0, LargeConvex, false, _SupportOptions>;
+ } else {
+ if (identity)
+ return getSupportFuncTpl<Shape0, SmallConvex, true, _SupportOptions>;
+ else
+ return getSupportFuncTpl<Shape0, SmallConvex, false, _SupportOptions>;
+ }
+ }
+ default:
+ HPP_FCL_THROW_PRETTY("Unsupported geometric shape.", std::logic_error);
+ }
+}
+
+// ============================================================================
+template <int _SupportOptions>
+MinkowskiDiff::GetSupportFunction makeGetSupportFunction0(
+ const ShapeBase* s0, const ShapeBase* s1, bool identity,
+ Eigen::Array<FCL_REAL, 1, 2>& swept_sphere_radius,
+ ShapeSupportData data[2]) {
+ if (_SupportOptions == SupportOptions::WithSweptSphere) {
+ // No need to store the information of swept sphere radius
+ swept_sphere_radius[0] = 0;
+ } else {
+ // We store the information of swept sphere radius.
+ // GJK and EPA will use this information to correct the solution they find.
+ swept_sphere_radius[0] = s0->getSweptSphereRadius();
+ }
+
+ switch (s0->getNodeType()) {
+ case GEOM_TRIANGLE:
+ return makeGetSupportFunction1<TriangleP, _SupportOptions>(
+ s1, identity, swept_sphere_radius, data);
+ break;
+ case GEOM_BOX:
+ return makeGetSupportFunction1<Box, _SupportOptions>(
+ s1, identity, swept_sphere_radius, data);
+ break;
+ case GEOM_SPHERE:
+ if (_SupportOptions == SupportOptions::NoSweptSphere) {
+ // Sphere can always be considered as a swept-sphere point.
+ swept_sphere_radius[0] += static_cast<const Sphere*>(s0)->radius;
+ }
+ return makeGetSupportFunction1<Sphere, _SupportOptions>(
+ s1, identity, swept_sphere_radius, data);
+ break;
+ case GEOM_ELLIPSOID:
+ return makeGetSupportFunction1<Ellipsoid, _SupportOptions>(
+ s1, identity, swept_sphere_radius, data);
+ break;
+ case GEOM_CAPSULE:
+ if (_SupportOptions == SupportOptions::NoSweptSphere) {
+ // Capsule can always be considered as a swept-sphere segment.
+ swept_sphere_radius[0] += static_cast<const Capsule*>(s0)->radius;
+ }
+ return makeGetSupportFunction1<Capsule, _SupportOptions>(
+ s1, identity, swept_sphere_radius, data);
+ break;
+ case GEOM_CONE:
+ return makeGetSupportFunction1<Cone, _SupportOptions>(
+ s1, identity, swept_sphere_radius, data);
+ break;
+ case GEOM_CYLINDER:
+ return makeGetSupportFunction1<Cylinder, _SupportOptions>(
+ s1, identity, swept_sphere_radius, data);
+ break;
+ case GEOM_CONVEX: {
+ const ConvexBase* convex0 = static_cast<const ConvexBase*>(s0);
+ if (static_cast<size_t>(convex0->num_points) >
+ ConvexBase::num_vertices_large_convex_threshold) {
+ data[0].visited.assign(convex0->num_points, false);
+ return makeGetSupportFunction1<LargeConvex, _SupportOptions>(
+ s1, identity, swept_sphere_radius, data);
+ } else
+ return makeGetSupportFunction1<SmallConvex, _SupportOptions>(
+ s1, identity, swept_sphere_radius, data);
+ break;
+ }
+ default:
+ HPP_FCL_THROW_PRETTY("Unsupported geometric shape", std::logic_error);
+ }
+}
+
+// ============================================================================
+bool getNormalizeSupportDirection(const ShapeBase* shape) {
+ switch (shape->getNodeType()) {
+ case GEOM_TRIANGLE:
+ return (bool)shape_traits<TriangleP>::NeedNesterovNormalizeHeuristic;
+ break;
+ case GEOM_BOX:
+ return (bool)shape_traits<Box>::NeedNesterovNormalizeHeuristic;
+ break;
+ case GEOM_SPHERE:
+ return (bool)shape_traits<Sphere>::NeedNesterovNormalizeHeuristic;
+ break;
+ case GEOM_ELLIPSOID:
+ return (bool)shape_traits<Ellipsoid>::NeedNesterovNormalizeHeuristic;
+ break;
+ case GEOM_CAPSULE:
+ return (bool)shape_traits<Capsule>::NeedNesterovNormalizeHeuristic;
+ break;
+ case GEOM_CONE:
+ return (bool)shape_traits<Cone>::NeedNesterovNormalizeHeuristic;
+ break;
+ case GEOM_CYLINDER:
+ return (bool)shape_traits<Cylinder>::NeedNesterovNormalizeHeuristic;
+ break;
+ case GEOM_CONVEX:
+ return (bool)shape_traits<ConvexBase>::NeedNesterovNormalizeHeuristic;
+ break;
+ default:
+ HPP_FCL_THROW_PRETTY("Unsupported geometric shape", std::logic_error);
+ }
+}
+
+// ============================================================================
+void getNormalizeSupportDirectionFromShapes(const ShapeBase* shape0,
+ const ShapeBase* shape1,
+ bool& normalize_support_direction) {
+ normalize_support_direction = getNormalizeSupportDirection(shape0) &&
+ getNormalizeSupportDirection(shape1);
+}
+
+// ============================================================================
+template <int _SupportOptions>
+void MinkowskiDiff::set(const ShapeBase* shape0, const ShapeBase* shape1,
+ const Transform3f& tf0, const Transform3f& tf1) {
+ shapes[0] = shape0;
+ shapes[1] = shape1;
+ getNormalizeSupportDirectionFromShapes(shape0, shape1,
+ normalize_support_direction);
+
+ oR1.noalias() = tf0.getRotation().transpose() * tf1.getRotation();
+ ot1.noalias() = tf0.getRotation().transpose() *
+ (tf1.getTranslation() - tf0.getTranslation());
+
+ bool identity = (oR1.isIdentity() && ot1.isZero());
+
+ getSupportFunc = makeGetSupportFunction0<_SupportOptions>(
+ shape0, shape1, identity, swept_sphere_radius, data);
+}
+// clang-format off
+template <>
+void HPP_FCL_DLLAPI MinkowskiDiff::set<SupportOptions::NoSweptSphere>(const ShapeBase*, const ShapeBase*);
+
+template <>
+void HPP_FCL_DLLAPI MinkowskiDiff::set<SupportOptions::WithSweptSphere>(const ShapeBase*, const ShapeBase*);
+// clang-format on
+
+// ============================================================================
+template <int _SupportOptions>
+void MinkowskiDiff::set(const ShapeBase* shape0, const ShapeBase* shape1) {
+ shapes[0] = shape0;
+ shapes[1] = shape1;
+ getNormalizeSupportDirectionFromShapes(shape0, shape1,
+ normalize_support_direction);
+
+ oR1.setIdentity();
+ ot1.setZero();
+
+ getSupportFunc = makeGetSupportFunction0<_SupportOptions>(
+ shape0, shape1, true, swept_sphere_radius, data);
+}
+// clang-format off
+template <>
+void HPP_FCL_DLLAPI MinkowskiDiff::set<SupportOptions::NoSweptSphere>(const ShapeBase*, const ShapeBase*, const Transform3f&, const Transform3f&);
+
+template <>
+void HPP_FCL_DLLAPI MinkowskiDiff::set<SupportOptions::WithSweptSphere>(const ShapeBase*, const ShapeBase*, const Transform3f&, const Transform3f&);
+// clang-format on
+
+// ============================================================================
// clang-format off
-template void HPP_FCL_DLLAPI MinkowskiDiff::set<SupportOptions::NoSweptSphere>(const ShapeBase*, const ShapeBase*);
-template void HPP_FCL_DLLAPI MinkowskiDiff::set<SupportOptions::WithSweptSphere>(const ShapeBase*, const ShapeBase*);
+template <>
+Vec3f HPP_FCL_DLLAPI MinkowskiDiff::support0<SupportOptions::NoSweptSphere>(const Vec3f&, int&) const;
-template void HPP_FCL_DLLAPI MinkowskiDiff::set<SupportOptions::NoSweptSphere>(const ShapeBase*, const ShapeBase*, const Transform3f&, const Transform3f&);
-template void HPP_FCL_DLLAPI MinkowskiDiff::set<SupportOptions::WithSweptSphere>(const ShapeBase*, const ShapeBase*, const Transform3f&, const Transform3f&);
+template <>
+Vec3f HPP_FCL_DLLAPI MinkowskiDiff::support0<SupportOptions::WithSweptSphere>(const Vec3f&, int&) const;
-template Vec3f HPP_FCL_DLLAPI MinkowskiDiff::support0<SupportOptions::NoSweptSphere>(const Vec3f&, int&) const;
-template Vec3f HPP_FCL_DLLAPI MinkowskiDiff::support0<SupportOptions::WithSweptSphere>(const Vec3f&, int&) const;
+template <>
+Vec3f HPP_FCL_DLLAPI MinkowskiDiff::support1<SupportOptions::NoSweptSphere>(const Vec3f&, int&) const;
-template Vec3f HPP_FCL_DLLAPI MinkowskiDiff::support1<SupportOptions::NoSweptSphere>(const Vec3f&, int&) const;
-template Vec3f HPP_FCL_DLLAPI MinkowskiDiff::support1<SupportOptions::WithSweptSphere>(const Vec3f&, int&) const;
+template <>
+Vec3f HPP_FCL_DLLAPI MinkowskiDiff::support1<SupportOptions::WithSweptSphere>(const Vec3f&, int&) const;
// clang-format on
} // namespace details
diff --git a/src/narrowphase/support_functions.cpp b/src/narrowphase/support_functions.cpp
index 1cd6e9a99c94e2b6bf2d2715782bbb1417978bce..0a234d886fbe10dd203e3b4bbc7faf1426b99f51 100644
--- a/src/narrowphase/support_functions.cpp
+++ b/src/narrowphase/support_functions.cpp
@@ -1,15 +1,584 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
+ * Copyright (c) 2021-2024, INRIA
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \authors Jia Pan, Florent Lamiraux, Josef Mirabel, Louis Montaut */
+
#include "hpp/fcl/narrowphase/support_functions.h"
namespace hpp {
namespace fcl {
namespace details {
+// ============================================================================
+#define CALL_GET_SHAPE_SUPPORT(ShapeType) \
+ getShapeSupport<_SupportOptions>(static_cast<const ShapeType*>(shape), dir, \
+ support, hint, nullptr)
+template <int _SupportOptions>
+Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, int& hint) {
+ Vec3f support;
+ switch (shape->getNodeType()) {
+ case GEOM_TRIANGLE:
+ CALL_GET_SHAPE_SUPPORT(TriangleP);
+ break;
+ case GEOM_BOX:
+ CALL_GET_SHAPE_SUPPORT(Box);
+ break;
+ case GEOM_SPHERE:
+ CALL_GET_SHAPE_SUPPORT(Sphere);
+ break;
+ case GEOM_ELLIPSOID:
+ CALL_GET_SHAPE_SUPPORT(Ellipsoid);
+ break;
+ case GEOM_CAPSULE:
+ CALL_GET_SHAPE_SUPPORT(Capsule);
+ break;
+ case GEOM_CONE:
+ CALL_GET_SHAPE_SUPPORT(Cone);
+ break;
+ case GEOM_CYLINDER:
+ CALL_GET_SHAPE_SUPPORT(Cylinder);
+ break;
+ case GEOM_CONVEX:
+ CALL_GET_SHAPE_SUPPORT(ConvexBase);
+ break;
+ case GEOM_PLANE:
+ case GEOM_HALFSPACE:
+ default:
+ support.setZero();
+ ; // nothing
+ }
+
+ return support;
+}
+#undef CALL_GET_SHAPE_SUPPORT
+
// Explicit instantiation
// clang-format off
template Vec3f HPP_FCL_DLLAPI getSupport<SupportOptions::NoSweptSphere>(const ShapeBase*, const Vec3f&, int&);
template Vec3f HPP_FCL_DLLAPI getSupport<SupportOptions::WithSweptSphere>(const ShapeBase*, const Vec3f&, int&);
// clang-format on
+// ============================================================================
+template <typename ShapeType, int _SupportOptions>
+Vec3f getSupportTpl(const ShapeBase* shape_, const Vec3f& dir, int& hint) {
+ const ShapeType* shape = static_cast<const ShapeType*>(shape_);
+ Vec3f support;
+ getShapeSupport(shape, dir, support, hint, nullptr);
+ return support;
+}
+
+// ============================================================================
+template <typename ShapeType, int _SupportOptions>
+void getShapeSupportTpl(const ShapeBase* shape_, const Vec3f& dir,
+ Vec3f& support, int& hint, ShapeSupportData* data) {
+ const ShapeType* shape = static_cast<const ShapeType*>(shape_);
+ return getShapeSupport(shape, dir, support, hint, data);
+}
+
+// ============================================================================
+template <int _SupportOptions>
+void getShapeSupport(const TriangleP* triangle, const Vec3f& dir,
+ Vec3f& support, int& /*unused*/,
+ ShapeSupportData* /*data*/) {
+ FCL_REAL dota = dir.dot(triangle->a);
+ FCL_REAL dotb = dir.dot(triangle->b);
+ FCL_REAL dotc = dir.dot(triangle->c);
+ if (dota > dotb) {
+ if (dotc > dota) {
+ support = triangle->c;
+ } else {
+ support = triangle->a;
+ }
+ } else {
+ if (dotc > dotb) {
+ support = triangle->c;
+ } else {
+ support = triangle->b;
+ }
+ }
+
+ if (_SupportOptions == SupportOptions::WithSweptSphere) {
+ support += triangle->getSweptSphereRadius() * dir.normalized();
+ }
+}
+
+// ============================================================================
+template <int _SupportOptions>
+inline void getShapeSupport(const Box* box, const Vec3f& dir, Vec3f& support,
+ int& /*unused*/, ShapeSupportData* /*unused*/) {
+ // The inflate value is simply to make the specialized functions with box
+ // have a preferred side for edge cases.
+ static const FCL_REAL inflate = (dir.array() == 0).any() ? 1 + 1e-10 : 1.;
+ static const FCL_REAL dummy_precision =
+ Eigen::NumTraits<FCL_REAL>::dummy_precision();
+ Vec3f support1 = (dir.array() > dummy_precision).select(box->halfSide, 0);
+ Vec3f support2 =
+ (dir.array() < -dummy_precision).select(-inflate * box->halfSide, 0);
+ support.noalias() = support1 + support2;
+
+ if (_SupportOptions == SupportOptions::WithSweptSphere) {
+ support += box->getSweptSphereRadius() * dir.normalized();
+ }
+}
+
+// ============================================================================
+template <int _SupportOptions>
+inline void getShapeSupport(const Sphere* sphere, const Vec3f& dir,
+ Vec3f& support, int& /*unused*/,
+ ShapeSupportData* /*unused*/) {
+ if (_SupportOptions == SupportOptions::WithSweptSphere) {
+ support.noalias() =
+ (sphere->radius + sphere->getSweptSphereRadius()) * dir.normalized();
+ } else {
+ support.setZero();
+ }
+
+ HPP_FCL_UNUSED_VARIABLE(sphere);
+ HPP_FCL_UNUSED_VARIABLE(dir);
+}
+
+// ============================================================================
+template <int _SupportOptions>
+inline void getShapeSupport(const Ellipsoid* ellipsoid, const Vec3f& dir,
+ Vec3f& support, int& /*unused*/,
+ ShapeSupportData* /*unused*/) {
+ FCL_REAL a2 = ellipsoid->radii[0] * ellipsoid->radii[0];
+ FCL_REAL b2 = ellipsoid->radii[1] * ellipsoid->radii[1];
+ FCL_REAL c2 = ellipsoid->radii[2] * ellipsoid->radii[2];
+
+ Vec3f v(a2 * dir[0], b2 * dir[1], c2 * dir[2]);
+
+ FCL_REAL d = std::sqrt(v.dot(dir));
+
+ support = v / d;
+
+ if (_SupportOptions == SupportOptions::WithSweptSphere) {
+ support += ellipsoid->getSweptSphereRadius() * dir.normalized();
+ }
+}
+
+// ============================================================================
+template <int _SupportOptions>
+inline void getShapeSupport(const Capsule* capsule, const Vec3f& dir,
+ Vec3f& support, int& /*unused*/,
+ ShapeSupportData* /*unused*/) {
+ static const FCL_REAL dummy_precision =
+ Eigen::NumTraits<FCL_REAL>::dummy_precision();
+ support.setZero();
+ if (dir[2] > dummy_precision) {
+ support[2] = capsule->halfLength;
+ } else if (dir[2] < -dummy_precision) {
+ support[2] = -capsule->halfLength;
+ }
+
+ if (_SupportOptions == SupportOptions::WithSweptSphere) {
+ support +=
+ (capsule->radius + capsule->getSweptSphereRadius()) * dir.normalized();
+ }
+}
+
+// ============================================================================
+template <int _SupportOptions>
+void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support,
+ int& /*unused*/, ShapeSupportData* /*unused*/) {
+ static const FCL_REAL dummy_precision =
+ Eigen::NumTraits<FCL_REAL>::dummy_precision();
+
+ // The cone radius is, for -h < z < h, (h - z) * r / (2*h)
+ // The inflate value is simply to make the specialized functions with cone
+ // have a preferred side for edge cases.
+ static const FCL_REAL inflate = 1 + 1e-10;
+ FCL_REAL h = cone->halfLength;
+ FCL_REAL r = cone->radius;
+
+ if (dir.head<2>().isZero(dummy_precision)) {
+ support.head<2>().setZero();
+ if (dir[2] > dummy_precision) {
+ support[2] = h;
+ } else {
+ support[2] = -inflate * h;
+ }
+ } else {
+ FCL_REAL zdist = dir[0] * dir[0] + dir[1] * dir[1];
+ FCL_REAL len = zdist + dir[2] * dir[2];
+ zdist = std::sqrt(zdist);
+
+ if (dir[2] <= 0) {
+ FCL_REAL rad = r / zdist;
+ support.head<2>() = rad * dir.head<2>();
+ support[2] = -h;
+ } else {
+ len = std::sqrt(len);
+ FCL_REAL sin_a = r / std::sqrt(r * r + 4 * h * h);
+
+ if (dir[2] > len * sin_a)
+ support << 0, 0, h;
+ else {
+ FCL_REAL rad = r / zdist;
+ support.head<2>() = rad * dir.head<2>();
+ support[2] = -h;
+ }
+ }
+ }
+
+ if (_SupportOptions == SupportOptions::WithSweptSphere) {
+ support += cone->getSweptSphereRadius() * dir.normalized();
+ }
+}
+
+// ============================================================================
+template <int _SupportOptions>
+void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, Vec3f& support,
+ int& /*unused*/, ShapeSupportData* /*unused*/) {
+ static const FCL_REAL dummy_precision =
+ Eigen::NumTraits<FCL_REAL>::dummy_precision();
+
+ // The inflate value is simply to make the specialized functions with cylinder
+ // have a preferred side for edge cases.
+ static const FCL_REAL inflate = 1 + 1e-10;
+ FCL_REAL half_h = cylinder->halfLength;
+ FCL_REAL r = cylinder->radius;
+
+ const bool dir_is_aligned_with_z = dir.head<2>().isZero(dummy_precision);
+ if (dir_is_aligned_with_z) half_h *= inflate;
+
+ if (dir[2] > dummy_precision) {
+ support[2] = half_h;
+ } else if (dir[2] < -dummy_precision) {
+ support[2] = -half_h;
+ } else {
+ support[2] = 0;
+ r *= inflate;
+ }
+
+ if (dir_is_aligned_with_z) {
+ support.head<2>().setZero();
+ } else {
+ support.head<2>() = dir.head<2>().normalized() * r;
+ }
+
+ assert(fabs(support[0] * dir[1] - support[1] * dir[0]) <
+ sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
+
+ if (_SupportOptions == SupportOptions::WithSweptSphere) {
+ support += cylinder->getSweptSphereRadius() * dir.normalized();
+ }
+}
+
+// ============================================================================
+template <int _SupportOptions>
+void getShapeSupportLog(const ConvexBase* convex, const Vec3f& dir,
+ Vec3f& support, int& hint, ShapeSupportData* data) {
+ assert(data != nullptr && "data is null.");
+ assert(convex->neighbors != nullptr && "Convex has no neighbors.");
+
+ // Use warm start if current support direction is distant from last support
+ // direction.
+ const double use_warm_start_threshold = 0.9;
+ Vec3f dir_normalized = dir.normalized();
+ if (!data->last_dir.isZero() && !convex->support_warm_starts.points.empty() &&
+ data->last_dir.dot(dir_normalized) < use_warm_start_threshold) {
+ // Change hint if last dir is too far from current dir.
+ FCL_REAL maxdot = convex->support_warm_starts.points[0].dot(dir);
+ hint = convex->support_warm_starts.indices[0];
+ for (size_t i = 1; i < convex->support_warm_starts.points.size(); ++i) {
+ FCL_REAL dot = convex->support_warm_starts.points[i].dot(dir);
+ if (dot > maxdot) {
+ maxdot = dot;
+ hint = convex->support_warm_starts.indices[i];
+ }
+ }
+ }
+ data->last_dir = dir_normalized;
+
+ const std::vector<Vec3f>& pts = *(convex->points);
+ const std::vector<ConvexBase::Neighbors>& nn = *(convex->neighbors);
+
+ if (hint < 0 || hint >= (int)convex->num_points) {
+ hint = 0;
+ }
+ FCL_REAL maxdot = pts[static_cast<size_t>(hint)].dot(dir);
+ std::vector<int8_t>& visited = data->visited;
+ assert(data->visited.size() == convex->num_points);
+ std::fill(visited.begin(), visited.end(), false);
+ visited[static_cast<std::size_t>(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;
+ bool loose_check = true;
+ while (found) {
+ const ConvexBase::Neighbors& n = nn[static_cast<size_t>(hint)];
+ found = false;
+ for (int in = 0; in < n.count(); ++in) {
+ const unsigned int ip = n[in];
+ if (visited[ip]) continue;
+ visited[ip] = true;
+ const FCL_REAL 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 = static_cast<int>(ip);
+ found = true;
+ }
+ }
+ }
+
+ support = pts[static_cast<size_t>(hint)];
+
+ if (_SupportOptions == SupportOptions::WithSweptSphere) {
+ support += convex->getSweptSphereRadius() * dir.normalized();
+ }
+}
+
+// ============================================================================
+template <int _SupportOptions>
+void getShapeSupportLinear(const ConvexBase* convex, const Vec3f& dir,
+ Vec3f& support, int& hint, ShapeSupportData*) {
+ const std::vector<Vec3f>& pts = *(convex->points);
+
+ hint = 0;
+ FCL_REAL maxdot = pts[0].dot(dir);
+ for (int i = 1; i < (int)convex->num_points; ++i) {
+ FCL_REAL dot = pts[static_cast<size_t>(i)].dot(dir);
+ if (dot > maxdot) {
+ maxdot = dot;
+ hint = i;
+ }
+ }
+
+ support = pts[static_cast<size_t>(hint)];
+
+ if (_SupportOptions == SupportOptions::WithSweptSphere) {
+ support += convex->getSweptSphereRadius() * dir.normalized();
+ }
+}
+
+// ============================================================================
+template <int _SupportOptions>
+void getShapeSupport(const ConvexBase* convex, const Vec3f& dir, Vec3f& support,
+ int& hint, ShapeSupportData* /*unused*/) {
+ // TODO add benchmark to set a proper value for switching between linear and
+ // logarithmic.
+ if (convex->num_points > ConvexBase::num_vertices_large_convex_threshold &&
+ convex->neighbors != nullptr) {
+ ShapeSupportData data;
+ data.visited.assign(convex->num_points, false);
+ getShapeSupportLog<_SupportOptions>(convex, dir, support, hint, &data);
+ } else
+ getShapeSupportLinear<_SupportOptions>(convex, dir, support, hint, nullptr);
+}
+
+// ============================================================================
+template <int _SupportOptions>
+inline void getShapeSupport(const SmallConvex* convex, const Vec3f& dir,
+ Vec3f& support, int& hint, ShapeSupportData* data) {
+ getShapeSupportLinear<_SupportOptions>(
+ reinterpret_cast<const ConvexBase*>(convex), dir, support, hint, data);
+}
+
+// ============================================================================
+template <int _SupportOptions>
+inline void getShapeSupport(const LargeConvex* convex, const Vec3f& dir,
+ Vec3f& support, int& hint, ShapeSupportData* data) {
+ getShapeSupportLog<_SupportOptions>(
+ reinterpret_cast<const ConvexBase*>(convex), dir, support, hint, data);
+}
+
+// ============================================================================
+#define CALL_GET_SHAPE_SUPPORT_SET(ShapeType) \
+ getShapeSupportSet<_SupportOptions>(static_cast<const ShapeType*>(shape), \
+ dir, ctfi, support_set, hint, nullptr)
+template <int _SupportOptions>
+void getSupportSet(const ShapeBase* shape, const Vec3f& dir,
+ const Transform3f& ctfi, FCL_REAL tol,
+ SupportSet& support_set, const int hint) {
+ switch (shape->getNodeType()) {
+ case GEOM_TRIANGLE:
+ CALL_GET_SHAPE_SUPPORT_SET(TriangleP);
+ break;
+ case GEOM_BOX:
+ CALL_GET_SHAPE_SUPPORT_SET(Box);
+ break;
+ case GEOM_SPHERE:
+ CALL_GET_SHAPE_SUPPORT_SET(Sphere);
+ break;
+ case GEOM_ELLIPSOID:
+ CALL_GET_SHAPE_SUPPORT_SET(Ellipsoid);
+ break;
+ case GEOM_CAPSULE:
+ CALL_GET_SHAPE_SUPPORT_SET(Capsule);
+ break;
+ case GEOM_CONE:
+ CALL_GET_SHAPE_SUPPORT_SET(Cone);
+ break;
+ case GEOM_CYLINDER:
+ CALL_GET_SHAPE_SUPPORT_SET(Cylinder);
+ break;
+ case GEOM_CONVEX:
+ CALL_GET_SHAPE_SUPPORT_SET(ConvexBase);
+ break;
+ case GEOM_PLANE:
+ case GEOM_HALFSPACE:
+ default:; // nothing
+ }
+}
+#undef CALL_GET_SHAPE_SUPPORT
+
+// Explicit instantiation
+// clang-format off
+template HPP_FCL_DLLAPI void getSupportSet<SupportOptions::NoSweptSphere>(const ShapeBase*, const Vec3f&, const Transform3f&, FCL_REAL tol, SupportSet&, const int);
+template HPP_FCL_DLLAPI void getSupportSet<SupportOptions::WithSweptSphere>(const ShapeBase*, const Vec3f&, const Transform3f&, FCL_REAL tol, SupportSet&, const int);
+// clang-format on
+
+// ============================================================================
+template <typename ShapeType, int _SupportOptions>
+void getSupportSetTpl(const ShapeBase* shape_, const Vec3f& dir,
+ const Transform3f& ctfi, SupportSet& support_set,
+ const int hint) {
+ const ShapeType* shape = static_cast<const ShapeType*>(shape_);
+ getShapeSupportSet<_SupportOptions>(shape, dir, ctfi, support_set, hint,
+ nullptr);
+}
+
+// ============================================================================
+template <typename ShapeType, int _SupportOptions>
+void getShapeSupportSetTpl(const ShapeBase* shape_, const Vec3f& dir,
+ const Transform3f& ctfi, SupportSet& support_set,
+ const int hint, ShapeSupportData* data) {
+ const ShapeType* shape = static_cast<const ShapeType*>(shape_);
+ getShapeSupportSet<_SupportOptions>(shape, dir, ctfi, support_set, hint,
+ data);
+}
+
+// ============================================================================
+template <int _SupportOptions>
+void getShapeSupportSet(const TriangleP* triangle, const Vec3f& dir,
+ const Transform3f& ctfi, SupportSet& support_set,
+ const int /*unused*/, ShapeSupportData* /*unused*/) {}
+
+// ============================================================================
+template <int _SupportOptions>
+void getShapeSupportSet(const Box* box, const Vec3f& dir,
+ const Transform3f& ctfi, SupportSet& support_set,
+ const int /*unused*/, ShapeSupportData* /*unused*/) {}
+
+// ============================================================================
+template <int _SupportOptions>
+void getShapeSupportSet(const Sphere* sphere, const Vec3f& dir,
+ const Transform3f& ctfi, SupportSet& support_set,
+ const int /*unused*/, ShapeSupportData* /*unused*/) {}
+
+// ============================================================================
+template <int _SupportOptions>
+void getShapeSupportSet(const Ellipsoid* ellipsoid, const Vec3f& dir,
+ const Transform3f& ctfi, SupportSet& support_set,
+ const int /*unused*/, ShapeSupportData* /*unused*/) {}
+
+// ============================================================================
+template <int _SupportOptions>
+void getShapeSupportSet(const Capsule* capsule, const Vec3f& dir,
+ const Transform3f& ctfi, SupportSet& support_set,
+ const int /*unused*/, ShapeSupportData* /*unused*/) {}
+
+// ============================================================================
+template <int _SupportOptions>
+void getShapeSupportSet(const Cone* cone, const Vec3f& dir,
+ const Transform3f& ctfi, SupportSet& support_set,
+ const int /*unused*/, ShapeSupportData* /*unused*/) {}
+
+// ============================================================================
+template <int _SupportOptions>
+void getShapeSupportSet(const Cylinder* cylinder, const Vec3f& dir,
+ const Transform3f& ctfi, SupportSet& support_set,
+ const int /*unused*/, ShapeSupportData* /*unused*/) {}
+
+// ============================================================================
+template <int _SupportOptions>
+void getShapeSupportSetLog(const ConvexBase* convex, const Vec3f& dir,
+ const Transform3f& ctfi, SupportSet& support_set,
+ const int hint, ShapeSupportData* data) {}
+
+// ============================================================================
+template <int _SupportOptions>
+void getShapeSupportSetLinear(const ConvexBase* convex, const Vec3f& dir,
+ const Transform3f& ctfi, SupportSet& support_set,
+ const int hint, ShapeSupportData* /*unused*/) {}
+
+// ============================================================================
+template <int _SupportOptions>
+void getShapeSupportSet(const ConvexBase* convex, const Vec3f& dir,
+ const Transform3f& ctfi, SupportSet& support_set,
+ const int hint, ShapeSupportData* /*unused*/) {
+ if (convex->num_points > ConvexBase::num_vertices_large_convex_threshold &&
+ convex->neighbors != nullptr) {
+ ShapeSupportData data;
+ data.visited.assign(convex->num_points, false);
+ getShapeSupportSetLog<_SupportOptions>(convex, dir, ctfi, support_set, hint,
+ &data);
+ } else {
+ getShapeSupportSetLinear<_SupportOptions>(convex, dir, ctfi, support_set,
+ hint, nullptr);
+ }
+}
+
+// ============================================================================
+template <int _SupportOptions>
+void getShapeSupportSet(const SmallConvex* convex, const Vec3f& dir,
+ const Transform3f& ctfi, SupportSet& support_set,
+ const int hint, ShapeSupportData* data) {
+ getShapeSupportSetLinear<_SupportOptions>(
+ reinterpret_cast<const ConvexBase*>(convex), dir, ctfi, support_set, hint,
+ data);
+}
+
+// ============================================================================
+template <int _SupportOptions>
+void getShapeSupportSet(const LargeConvex* convex, const Vec3f& dir,
+ const Transform3f& ctfi, SupportSet& support_set,
+ const int hint, ShapeSupportData* data) {
+ getShapeSupportSetLog<_SupportOptions>(
+ reinterpret_cast<const ConvexBase*>(convex), dir, ctfi, support_set, hint,
+ data);
+}
+
} // namespace details
} // namespace fcl
} // namespace hpp