BVH: add throw if distance_lower_bound conditions are not met

lmontaut · Feb 9, 2024 · 1e8e52a · 1e8e52a
1 parent 4de669a
commit 1e8e52a
Showing 1 changed file with 19 additions and 11 deletions.
diff --git a/src/collision_node.cpp b/src/collision_node.cpp
@@ -41,6 +41,24 @@
 namespace hpp {
 namespace fcl {
 
+void checkResultLowerBound(const CollisionResult& result,
+                           FCL_REAL sqrDistLowerBound) {
+  const FCL_REAL dummy_precision =
+      std::sqrt(Eigen::NumTraits<FCL_REAL>::epsilon());
+  if (sqrDistLowerBound == 0) {
+    if (result.distance_lower_bound > dummy_precision)
+      HPP_FCL_THROW_PRETTY("Distance lower bound should not be positive.",
+                           std::logic_error);
+  } else {
+    if (result.distance_lower_bound * result.distance_lower_bound -
+            sqrDistLowerBound >
+        dummy_precision)
+      HPP_FCL_THROW_PRETTY(
+          "Distance lower bound and sqrDistLowerBound should coincide.",
+          std::logic_error);
+  }
+}
+
 void collide(CollisionTraversalNodeBase* node, const CollisionRequest& request,
              CollisionResult& result, BVHFrontList* front_list,
              bool recursive) {
@@ -52,18 +70,8 @@ void collide(CollisionTraversalNodeBase* node, const CollisionRequest& request,
       collisionRecurse(node, 0, 0, front_list, sqrDistLowerBound);
     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 <= dummy_precision);
-      } else {
-        assert(result.distance_lower_bound * result.distance_lower_bound -
-                   sqrDistLowerBound <
-               dummy_precision);
-      }
+      checkResultLowerBound(result, sqrDistLowerBound);
     }
   }
 }