Setting default parameters for the narrowphase

CHANGELOG.md

@@ -10,6 +10,8 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 
 - CMake: fix assimp finder
 - Don't define GCC7 Boost serialization hack when `HPP_FCL_SKIP_EIGEN_BOOST_SERIALIZATION` is defined ([#530](https://github.com/humanoid-path-planner/hpp-fcl/pull/530))
+- Default parameters for narrowphase algorithms (GJK and EPA).
+- Fixed assertion checks that were sometimes failing in GJK simplex projection and BVH `collide`.
 
 ## [2.4.1] - 2024-01-23
 

CMakeLists.txt

@@ -222,6 +222,7 @@ SET(${PROJECT_NAME}_HEADERS
   include/hpp/fcl/broadphase/detail/spatial_hash.h
   include/hpp/fcl/narrowphase/narrowphase.h
   include/hpp/fcl/narrowphase/gjk.h
+  include/hpp/fcl/narrowphase/narrowphase_defaults.h
   include/hpp/fcl/shape/convex.h
   include/hpp/fcl/shape/details/convex.hxx
   include/hpp/fcl/shape/geometric_shape_to_BVH_model.h

include/hpp/fcl/collision_data.h

@@ -47,6 +47,7 @@
 #include <hpp/fcl/config.hh>
 #include <hpp/fcl/data_types.h>
 #include <hpp/fcl/timings.h>
+#include <hpp/fcl/narrowphase/narrowphase_defaults.h>
 
 namespace hpp {
 namespace fcl {
@@ -224,14 +225,14 @@ struct HPP_FCL_DLLAPI QueryRequest {
         gjk_variant(GJKVariant::DefaultGJK),
         gjk_convergence_criterion(GJKConvergenceCriterion::VDB),
         gjk_convergence_criterion_type(GJKConvergenceCriterionType::Relative),
-        gjk_tolerance(1e-6),
-        gjk_max_iterations(128),
+        gjk_tolerance(GJK_DEFAULT_TOLERANCE),
+        gjk_max_iterations(GJK_DEFAULT_MAX_ITERATIONS),
         cached_gjk_guess(1, 0, 0),
         cached_support_func_guess(support_func_guess_t::Zero()),
-        epa_max_face_num(128),
-        epa_max_vertex_num(64),
-        epa_max_iterations(255),
-        epa_tolerance(1e-6),
+        epa_max_face_num(EPA_DEFAULT_MAX_FACES),
+        epa_max_vertex_num(EPA_DEFAULT_MAX_VERTICES),
+        epa_max_iterations(EPA_DEFAULT_MAX_ITERATIONS),
+        epa_tolerance(EPA_DEFAULT_TOLERANCE),
         enable_timings(false),
         collision_distance_threshold(
             Eigen::NumTraits<FCL_REAL>::dummy_precision()) {}

include/hpp/fcl/narrowphase/narrowphase.h

@@ -45,6 +45,7 @@
 
 #include <hpp/fcl/narrowphase/gjk.h>
 #include <hpp/fcl/collision_data.h>
+#include <hpp/fcl/narrowphase/narrowphase_defaults.h>
 
 namespace hpp {
 namespace fcl {
@@ -362,12 +363,12 @@ struct HPP_FCL_DLLAPI GJKSolver {
   HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
   /// @brief Default constructor for GJK algorithm
   GJKSolver() {
-    gjk_max_iterations = 128;
-    gjk_tolerance = 1e-6;
-    epa_max_face_num = 128;
-    epa_max_vertex_num = 64;
-    epa_max_iterations = 255;
-    epa_tolerance = 1e-6;
+    gjk_max_iterations = GJK_DEFAULT_MAX_ITERATIONS;
+    gjk_tolerance = GJK_DEFAULT_TOLERANCE;
+    epa_max_face_num = EPA_DEFAULT_MAX_FACES;
+    epa_max_vertex_num = EPA_DEFAULT_MAX_VERTICES;
+    epa_max_iterations = EPA_DEFAULT_MAX_ITERATIONS;
+    epa_tolerance = EPA_DEFAULT_TOLERANCE;
     enable_cached_guess = false;  // TODO: use gjk_initial_guess instead
     cached_guess = Vec3f(1, 0, 0);
     support_func_cached_guess = support_func_guess_t::Zero();
@@ -395,6 +396,8 @@ struct HPP_FCL_DLLAPI GJKSolver {
   /// \param[in] request DistanceRequest input
   ///
   void set(const DistanceRequest& request) {
+    // ---------------------
+    // GJK settings
     gjk_initial_guess = request.gjk_initial_guess;
     // TODO: use gjk_initial_guess instead
     enable_cached_guess = request.enable_cached_gjk_guess;
@@ -409,11 +412,30 @@ struct HPP_FCL_DLLAPI GJKSolver {
       support_func_cached_guess = request.cached_support_func_guess;
     }
 
+    // ---------------------
     // EPA settings
     epa_max_face_num = request.epa_max_face_num;
     epa_max_vertex_num = request.epa_max_vertex_num;
     epa_max_iterations = request.epa_max_iterations;
     epa_tolerance = request.epa_tolerance;
+
+    // Only in debug mode, to warn the user
+#ifndef NDEBUG
+    if (gjk_tolerance < GJK_MINIMUM_TOLERANCE) {
+      std::cout << "WARNING - GJK: using a tolerance (";
+      std::cout << gjk_tolerance;
+      std::cout << ") which is lower than the recommended lowest tolerance (";
+      std::cout << GJK_DEFAULT_TOLERANCE;
+      std::cout << "). Selecting this tolerance might trigger assertions.\n";
+    }
+    if (epa_tolerance < EPA_MINIMUM_TOLERANCE) {
+      std::cout << "WARNING - EPA: using a tolerance (";
+      std::cout << epa_tolerance;
+      std::cout << ") which is lower than the recommended lowest tolerance (";
+      std::cout << EPA_MINIMUM_TOLERANCE;
+      std::cout << "). Selecting this tolerance might trigger assertions.\n";
+    }
+#endif
   }
 
   /// @brief Constructor from a CollisionRequest
@@ -433,6 +455,8 @@ struct HPP_FCL_DLLAPI GJKSolver {
   /// \param[in] request CollisionRequest input
   ///
   void set(const CollisionRequest& request) {
+    // ---------------------
+    // GJK settings
     gjk_initial_guess = request.gjk_initial_guess;
     // TODO: use gjk_initial_guess instead
     enable_cached_guess = request.enable_cached_gjk_guess;
@@ -452,6 +476,7 @@ struct HPP_FCL_DLLAPI GJKSolver {
     distance_upper_bound = (std::max)(
         0., (std::max)(request.distance_upper_bound, request.security_margin));
 
+    // ---------------------
     // EPA settings
     epa_max_face_num = request.epa_max_face_num;
     epa_max_vertex_num = request.epa_max_vertex_num;
@@ -527,7 +552,7 @@ struct HPP_FCL_DLLAPI GJKSolver {
   /// @brief smart guess for the support function
   mutable support_func_guess_t support_func_cached_guess;
 
-  /// @brief Distance above which the GJK solver stoppes its computations and
+  /// @brief Distance above which the GJK solver stops its computations and
   /// processes to an early stopping.
   ///        The two witness points are incorrect, but with the guaranty that
   ///        the two shapes have a distance greather than distance_upper_bound.

include/hpp/fcl/narrowphase/narrowphase_defaults.h

@@ -0,0 +1,58 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 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.
+ */
+
+/// This file defines different macros used to characterize the default behavior
+/// of the narrowphase algorithms GJK and EPA.
+
+#ifndef HPP_FCL_NARROWPHASE_DEFAULTS
+#define HPP_FCL_NARROWPHASE_DEFAULTS
+
+/// GJK
+#define GJK_DEFAULT_MAX_ITERATIONS 128
+#define GJK_DEFAULT_TOLERANCE 1e-6
+/// Note: if the considered shapes are on the order of the meter, and the
+/// convergence criterion of GJK is the default VDB criterion,
+/// setting a tolerance of 1e-6 on the GJK algorithm makes it precise up to
+/// the micro-meter.
+/// The same is true for EPA.
+#define GJK_MINIMUM_TOLERANCE 1e-6
+
+/// EPA
+#define EPA_DEFAULT_MAX_ITERATIONS 255
+#define EPA_DEFAULT_TOLERANCE 1e-6
+#define EPA_MINIMUM_TOLERANCE 1e-6
+#define EPA_DEFAULT_MAX_FACES 128
+#define EPA_DEFAULT_MAX_VERTICES 64
+
+#endif  // HPP_FCL_NARROWPHASE_DEFAULTS

src/collision_node.cpp

@@ -53,13 +53,16 @@ void collide(CollisionTraversalNodeBase* node, const CollisionRequest& request,
     else
       collisionNonRecurse(node, front_list, sqrDistLowerBound);
 
+    const FCL_REAL dummy_precision =
+        std::sqrt(Eigen::NumTraits<FCL_REAL>::epsilon());
+    HPP_FCL_UNUSED_VARIABLE(dummy_precision);
     if (!std::isnan(sqrDistLowerBound)) {
       if (sqrDistLowerBound == 0) {
-        assert(result.distance_lower_bound <= 0);
+        assert(result.distance_lower_bound <= dummy_precision);
       } else {
         assert(result.distance_lower_bound * result.distance_lower_bound -
                    sqrDistLowerBound <
-               1e-8);
+               dummy_precision);
       }
     }
   }

src/narrowphase/gjk.cpp

@@ -36,11 +36,12 @@
 
 /** \author Jia Pan */
 
-#include "hpp/fcl/shape/geometric_shapes.h"
+#include <hpp/fcl/shape/geometric_shapes.h>
 #include <hpp/fcl/narrowphase/gjk.h>
 #include <hpp/fcl/internal/intersect.h>
 #include <hpp/fcl/internal/tools.h>
 #include <hpp/fcl/shape/geometric_shapes_traits.h>
+#include <hpp/fcl/narrowphase/narrowphase_defaults.h>
 
 namespace hpp {
 namespace fcl {
@@ -331,12 +332,12 @@ void getSupportFuncTpl(const MinkowskiDiff& md, const Vec3f& dir,
 #ifndef NDEBUG
   // Need normalized direction and direction is normalized
   assert(!NeedNormalizedDir || !dirIsNormalized ||
-         fabs(dir.squaredNorm() - 1) < 1e-6);
+         fabs(dir.squaredNorm() - 1) < GJK_MINIMUM_TOLERANCE);
   // Need normalized direction but direction is not normalized.
   assert(!NeedNormalizedDir || dirIsNormalized ||
-         fabs(dir.normalized().squaredNorm() - 1) < 1e-6);
+         fabs(dir.normalized().squaredNorm() - 1) < GJK_MINIMUM_TOLERANCE);
   // Don't need normalized direction. Check that dir is not zero.
-  assert(NeedNormalizedDir || dir.cwiseAbs().maxCoeff() >= 1e-6);
+  assert(NeedNormalizedDir || dir.norm() >= GJK_MINIMUM_TOLERANCE);
 #endif
   getSupportTpl<Shape0, Shape1, TransformIsIdentity>(
       static_cast<const Shape0*>(md.shapes[0]),
@@ -1094,7 +1095,10 @@ bool GJK::projectTetrahedraOrigin(const Simplex& current, Simplex& next) {
   const Vec3f a_cross_b = A.cross(B);
   const Vec3f a_cross_c = A.cross(C);
 
-  const FCL_REAL dummy_precision = Eigen::NumTraits<FCL_REAL>::epsilon();
+  // dummy_precision is 1e-14 if FCL_REAL is double
+  //                    1e-5  if FCL_REAL is float
+  static const FCL_REAL dummy_precision(
+      100 * std::numeric_limits<FCL_REAL>::epsilon());
   HPP_FCL_UNUSED_VARIABLE(dummy_precision);
 
 #define REGION_INSIDE()               \
@@ -1286,8 +1290,8 @@ bool GJK::projectTetrahedraOrigin(const Simplex& current, Simplex& next) {
             }       // end of (ACD ^ AD).AO >= 0
           } else {  // not (ACD ^ AC).AO >= 0 / !a10.a11.a2.a12.!a6
             assert(!(da * ca_da + dc * da_aa - dd * ca_aa <=
-                     dummy_precision));  // Not (ACD ^ AD).AO >= 0 /
-                                         // !a10.a11.a2.a12.!a6.!a7
+                     -dummy_precision));  // Not (ACD ^ AD).AO >= 0 /
+                                          // !a10.a11.a2.a12.!a6.!a7
             if (ca * ba_ca + cb * ca_aa - cc * ba_aa <=
                 0) {  // if (ABC ^ AC).AO >= 0 / !a10.a11.a2.a12.!a6.!a7.a5
               // Region AC

test/CMakeLists.txt

@@ -34,6 +34,7 @@ add_fcl_test(security_margin security_margin.cpp)
 add_fcl_test(geometric_shapes geometric_shapes.cpp)
 add_fcl_test(shape_inflation shape_inflation.cpp)
 #add_fcl_test(shape_mesh_consistency shape_mesh_consistency.cpp)
+add_fcl_test(gjk_asserts gjk_asserts.cpp)
 add_fcl_test(frontlist frontlist.cpp)
 SET_TESTS_PROPERTIES(frontlist PROPERTIES TIMEOUT 7200)
 
@@ -48,6 +49,7 @@ add_fcl_test(obb obb.cpp)
 add_fcl_test(convex convex.cpp)
 
 add_fcl_test(bvh_models bvh_models.cpp)
+add_fcl_test(collision_node_asserts collision_node_asserts.cpp)
 add_fcl_test(hfields hfields.cpp)
 
 add_fcl_test(profiling profiling.cpp)

test/collision_node_asserts.cpp

@@ -0,0 +1,58 @@
+#define BOOST_TEST_MODULE FCL_COLLISION_NODE_ASSERT
+
+#define _USE_MATH_DEFINES
+#include <cmath>
+#include <boost/test/included/unit_test.hpp>
+#include <hpp/fcl/BVH/BVH_model.h>
+#include <hpp/fcl/collision.h>
+
+using namespace hpp::fcl;
+
+inline double DegToRad(const double& deg) {
+  static double degToRad = M_PI / 180.;
+  return deg * degToRad;
+}
+std::vector<Vec3f> dirs{Vec3f::UnitZ(),  -Vec3f::UnitZ(), Vec3f::UnitY(),
+                        -Vec3f::UnitY(), Vec3f::UnitX(),  -Vec3f::UnitX()};
+
+BOOST_AUTO_TEST_CASE(TestTriangles) {
+  std::vector<Vec3f> triVertices{Vec3f(1, 0, 0), Vec3f(1, 1, 0),
+                                 Vec3f(0, 1, 0)};
+  std::vector<Triangle> triangle{{0, 1, 2}};
+
+  BVHModel<OBBRSS> tri1{};
+  BVHModel<OBBRSS> tri2{};
+
+  tri1.beginModel();
+  tri1.addSubModel(triVertices, triangle);
+  tri1.endModel();
+
+  tri2.beginModel();
+  tri2.addSubModel(triVertices, triangle);
+  tri2.endModel();
+
+  CollisionRequest request(CONTACT | DISTANCE_LOWER_BOUND, 1);
+
+  ComputeCollision compute(&tri1, &tri2);
+
+  Transform3f tri1Tf{};
+  Transform3f tri2Tf{};
+
+  /// check some angles for two triangles
+  for (int i = 0; i < 360; i += 30) {
+    for (int j = 0; j < 180; j += 30) {
+      for (int k = 0; k < 180; k += 30) {
+        tri1Tf.setQuatRotation(
+            Eigen::AngleAxis<double>(0., Vec3f::UnitZ()) *
+            Eigen::AngleAxis<double>(DegToRad(k), Vec3f::UnitY()));
+        tri2Tf.setQuatRotation(
+            Eigen::AngleAxis<double>(DegToRad(i), Vec3f::UnitZ()) *
+            Eigen::AngleAxis<double>(DegToRad(j), Vec3f::UnitY()));
+        CollisionResult result;
+
+        /// assertion: src/collision_node.cpp:58
+        BOOST_CHECK_NO_THROW(compute(tri2Tf, tri1Tf, request, result));
+      }
+    }
+  }
+}