diff --git a/include/fcl/BV/AABB.h b/include/fcl/BV/AABB.h
index a29cda41cf724c79ff4694ba0f83c76046b6b479..ae379878769db4bc0a92a1f26f8b814d5227249f 100644
--- a/include/fcl/BV/AABB.h
+++ b/include/fcl/BV/AABB.h
@@ -38,7 +38,7 @@
#ifndef FCL_AABB_H
#define FCL_AABB_H
-
+#include <stdexcept>
#include "fcl/math/vec_3f.h"
namespace fcl
@@ -93,6 +93,12 @@ public:
return true;
}
+ /// Not implemented
+ inline bool overlap(const AABB&, FCL_REAL&) const
+ {
+ throw std::runtime_error ("Not implemented");
+ }
+
/// @brief Check whether the AABB contains another AABB
inline bool contain(const AABB& other) const
{
diff --git a/include/fcl/BV/BV_node.h b/include/fcl/BV/BV_node.h
index a39ea548cd3b89d78414e5b318a9bdd3e21b201a..fa2fa13a959f2b1bd659e2ddb86052e92d70ff24 100644
--- a/include/fcl/BV/BV_node.h
+++ b/include/fcl/BV/BV_node.h
@@ -89,6 +89,11 @@ struct BVNode : public BVNodeBase
{
return bv.overlap(other.bv);
}
+ /// @brief Check whether two BVNode collide
+ bool overlap(const BVNode& other, FCL_REAL& sqrDistLowerBound) const
+ {
+ return bv.overlap(other.bv, sqrDistLowerBound);
+ }
/// @brief Compute the distance between two BVNode. P1 and P2, if not NULL and the underlying BV supports distance, return the nearest points.
FCL_REAL distance(const BVNode& other, Vec3f* P1 = NULL, Vec3f* P2 = NULL) const
diff --git a/include/fcl/BV/OBB.h b/include/fcl/BV/OBB.h
index 9290fec8387a47f6eae01a8036bed6bdf6a72d85..ac035eda6175d8623f196a958644d009b2eeb57d 100644
--- a/include/fcl/BV/OBB.h
+++ b/include/fcl/BV/OBB.h
@@ -59,9 +59,16 @@ public:
/// @brief Half dimensions of OBB
Vec3f extent;
- /// @brief Check collision between two OBB, return true if collision happens.
+ /// Check collision between two OBB
+ /// \return true if collision happens.
bool overlap(const OBB& other) const;
+ /// Check collision between two OBB
+ /// \return true if collision happens.
+ /// \retval sqrDistLowerBound squared lower bound on distance between boxes if
+ /// they do not overlap.
+ bool overlap(const OBB& other, FCL_REAL& sqrDistLowerBound) const;
+
/// @brief Check collision between two OBB and return the overlap part. For OBB, the overlap_part return value is NOT used as the overlap part of two obbs usually is not an obb.
bool overlap(const OBB& other, OBB& overlap_part) const
@@ -133,9 +140,16 @@ OBB translate(const OBB& bv, const Vec3f& t);
/// @brief Check collision between two obbs, b1 is in configuration (R0, T0) and b2 is in identity.
bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2);
+/// @brief Check collision between two obbs, b1 is in configuration (R0, T0) and b2 is in identity.
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1,
+ const OBB& b2, FCL_REAL& sqrDistLowerBound);
+
-/// @brief Check collision between two boxes: the first box is in configuration (R, T) and its half dimension is set by a;
-/// the second box is in identity configuration and its half dimension is set by b.
+/// Check collision between two boxes
+/// \param B, T orientation and position of first box,
+/// \param a half dimensions of first box,
+/// \param b half dimensions of second box.
+/// The second box is in identity configuration.
bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b);
}
diff --git a/include/fcl/BV/OBBRSS.h b/include/fcl/BV/OBBRSS.h
index bb77f1312f38d7d9c6f8716b94ff53535598b727..aaf172b0666f3f0b7cc9f8ca42a9673417039d4d 100644
--- a/include/fcl/BV/OBBRSS.h
+++ b/include/fcl/BV/OBBRSS.h
@@ -63,6 +63,14 @@ public:
return obb.overlap(other.obb);
}
+ /// Check collision between two OBBRSS
+ /// \retval sqrDistLowerBound squared lower bound on distance between
+ /// objects if they do not overlap.
+ bool overlap(const OBBRSS& other, FCL_REAL& sqrDistLowerBound) const
+ {
+ return obb.overlap(other.obb, sqrDistLowerBound);
+ }
+
/// @brief Check collision between two OBBRSS and return the overlap part.
bool overlap(const OBBRSS& other, OBBRSS& overlap_part) const
{
@@ -148,6 +156,13 @@ OBBRSS translate(const OBBRSS& bv, const Vec3f& t);
/// @brief Check collision between two OBBRSS, b1 is in configuration (R0, T0) and b2 is in indentity
bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2);
+/// Check collision between two OBBRSS
+/// \param b1 first OBBRSS in configuration (R0, T0)
+/// \param b2 second OBBRSS in identity position
+/// \retval squared lower bound on the distance if OBBRSS do not overlap.
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2,
+ FCL_REAL& sqrDistLowerBound);
+
/// @brief Computate distance between two OBBRSS, b1 is in configuation (R0, T0) and b2 is in indentity; P and Q, is not NULL, returns the nearest points
FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2, Vec3f* P = NULL, Vec3f* Q = NULL);
diff --git a/include/fcl/BV/RSS.h b/include/fcl/BV/RSS.h
index 91fc442ae356614fe7139660f3e2d1970443e7bc..e25a546ad92ea5677982a0e5d21b3f7a9c154b56 100644
--- a/include/fcl/BV/RSS.h
+++ b/include/fcl/BV/RSS.h
@@ -38,7 +38,7 @@
#ifndef FCL_RSS_H
#define FCL_RSS_H
-
+#include <stdexcept>
#include "fcl/math/vec_3f.h"
#include "fcl/math/matrix_3f.h"
#include <boost/math/constants/constants.hpp>
@@ -66,6 +66,13 @@ public:
/// @brief Check collision between two RSS
bool overlap(const RSS& other) const;
+ /// Not implemented
+ bool overlap(const RSS& other, FCL_REAL& sqrDistLowerBound) const
+ {
+ throw std::runtime_error ("Not implemented.");
+ return false;
+ }
+
/// @brief Check collision between two RSS and return the overlap part.
/// For RSS, we return nothing, as the overlap part of two RSSs usually is not a RSS.
bool overlap(const RSS& other, RSS& overlap_part) const
diff --git a/include/fcl/BV/kDOP.h b/include/fcl/BV/kDOP.h
index add3ba20cd394baa08244287face4e565da8afa2..d33f5a8ac56c3df7e5bb3aadb691ef363db19310 100644
--- a/include/fcl/BV/kDOP.h
+++ b/include/fcl/BV/kDOP.h
@@ -38,7 +38,7 @@
#ifndef FCL_KDOP_H
#define FCL_KDOP_H
-
+#include <stdexcept>
#include "fcl/math/vec_3f.h"
namespace fcl
@@ -96,6 +96,12 @@ public:
/// @brief Check whether two KDOPs are overlapped
bool overlap(const KDOP<N>& other) const;
+ /// Not implemented
+ bool overlap(const KDOP<N>& other, FCL_REAL&) const
+ {
+ throw std::runtime_error ("Not implemented");
+ }
+
//// @brief Check whether one point is inside the KDOP
bool inside(const Vec3f& p) const;
diff --git a/include/fcl/BV/kIOS.h b/include/fcl/BV/kIOS.h
index 8e52424d8f2470503eb320868d9efea47c522101..eab7c94059e49ca8df642c695cf35c8429bf29a7 100644
--- a/include/fcl/BV/kIOS.h
+++ b/include/fcl/BV/kIOS.h
@@ -93,6 +93,9 @@ public:
/// @brief Check collision between two kIOS
bool overlap(const kIOS& other) const;
+ /// @brief Check collision between two kIOS
+ bool overlap(const kIOS& other, FCL_REAL& sqrDistLowerBound) const;
+
/// @brief Check collision between two kIOS and return the overlap part.
/// For kIOS, we return nothing, as the overlappart of two kIOS usually is not an kIOS
/// @todo Not efficient. It first checks the sphere collisions and then use OBB for further culling.
diff --git a/include/fcl/collision_data.h b/include/fcl/collision_data.h
index eee6acf4ebe89fcbd44f0127d249a98816801a9a..dcfe25f60915b5b9cfcbb33970b7014fac860257 100644
--- a/include/fcl/collision_data.h
+++ b/include/fcl/collision_data.h
@@ -205,16 +205,18 @@ struct CollisionRequest
CollisionRequest(size_t num_max_contacts_ = 1,
bool enable_contact_ = false,
- bool enable_distance_lower_bound = false,
+ bool enable_distance_lower_bound_ = false,
size_t num_max_cost_sources_ = 1,
bool enable_cost_ = false,
bool use_approximate_cost_ = true,
- GJKSolverType gjk_solver_type_ = GST_LIBCCD) : num_max_contacts(num_max_contacts_),
- enable_contact(enable_contact_),
- num_max_cost_sources(num_max_cost_sources_),
- enable_cost(enable_cost_),
- use_approximate_cost(use_approximate_cost_),
- gjk_solver_type(gjk_solver_type_)
+ GJKSolverType gjk_solver_type_ = GST_LIBCCD) :
+ num_max_contacts(num_max_contacts_),
+ enable_contact(enable_contact_),
+ enable_distance_lower_bound (enable_distance_lower_bound_),
+ num_max_cost_sources(num_max_cost_sources_),
+ enable_cost(enable_cost_),
+ use_approximate_cost(use_approximate_cost_),
+ gjk_solver_type(gjk_solver_type_)
{
enable_cached_gjk_guess = false;
cached_gjk_guess = Vec3f(1, 0, 0);
diff --git a/include/fcl/collision_node.h b/include/fcl/collision_node.h
index 7c92543e434e9d88a818a8f355d5e08fc8db5d27..bc52dbb9a22ebae81b2f62e597ef5637e68ee607 100644
--- a/include/fcl/collision_node.h
+++ b/include/fcl/collision_node.h
@@ -50,8 +50,15 @@ namespace fcl
{
-/// @brief collision on collision traversal node; can use front list to accelerate
-void collide(CollisionTraversalNodeBase* node, BVHFrontList* front_list = NULL);
+/// collision on collision traversal node
+///
+/// \param node node containing both objects to test,
+/// \retval squared lower bound to the distance between the objects if they
+/// do not collide.
+/// \param front_list list of nodes visited by the query, can be used to
+/// accelerate computation
+ void collide(CollisionTraversalNodeBase* node, FCL_REAL& sqrDistLowerBound,
+ BVHFrontList* front_list = NULL);
/// @brief self collision on collision traversal node; can use front list to accelerate
void selfCollide(CollisionTraversalNodeBase* node, BVHFrontList* front_list = NULL);
diff --git a/include/fcl/traversal/traversal_node_base.h b/include/fcl/traversal/traversal_node_base.h
index f6668131e076e8291b3ed5112a3420c69fa333f2..6e8559ffbc7f51cbefdd2bd50b1d8f37855b2e17 100644
--- a/include/fcl/traversal/traversal_node_base.h
+++ b/include/fcl/traversal/traversal_node_base.h
@@ -90,13 +90,21 @@ public:
class CollisionTraversalNodeBase : public TraversalNodeBase
{
public:
- CollisionTraversalNodeBase() : result(NULL), enable_statistics(false) {}
+ CollisionTraversalNodeBase(bool enable_distance_lower_bound_ = false) :
+ result(NULL), enable_statistics(false),
+ enable_distance_lower_bound (enable_distance_lower_bound_){}
virtual ~CollisionTraversalNodeBase();
/// @brief BV test between b1 and b2
virtual bool BVTesting(int b1, int b2) const;
+ /// BV test between b1 and b2
+ /// \param b1, b2 Bounding volumes to test,
+ /// \retval sqrDistLowerBound square of a lower bound of the minimal
+ /// distance between bounding volumes.
+ virtual bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const;
+
/// @brief Leaf test between node b1 and b2, if they are both leafs
virtual void leafTesting(int b1, int b2) const;
@@ -114,6 +122,9 @@ public:
/// @brief Whether stores statistics
bool enable_statistics;
+
+ /// Whether to compute a lower bound on distance between bounding volumes
+ bool enable_distance_lower_bound;
};
/// @brief Node structure encoding the information required for distance traversal.
diff --git a/include/fcl/traversal/traversal_node_bvh_shape.h b/include/fcl/traversal/traversal_node_bvh_shape.h
index 5b4a86cf1338747eaaa0f35cf1714d6e81a6c0c1..f1f766eee5fc2a60d94b049e508067fdfb2cbf19 100644
--- a/include/fcl/traversal/traversal_node_bvh_shape.h
+++ b/include/fcl/traversal/traversal_node_bvh_shape.h
@@ -54,7 +54,8 @@ template<typename BV, typename S>
class BVHShapeCollisionTraversalNode : public CollisionTraversalNodeBase
{
public:
- BVHShapeCollisionTraversalNode() : CollisionTraversalNodeBase()
+ BVHShapeCollisionTraversalNode(bool enable_distance_lower_bound) :
+ CollisionTraversalNodeBase (enable_distance_lower_bound)
{
model1 = NULL;
model2 = NULL;
@@ -89,6 +90,17 @@ public:
return !model1->getBV(b1).bv.overlap(model2_bv);
}
+ /// BV test between b1 and b2
+ /// \param b1, b2 Bounding volumes to test,
+ /// \retval sqrDistLowerBound square of a lower bound of the minimal
+ /// distance between bounding volumes.
+ /// @brief BV culling test in one BVTT node
+ bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
+ {
+ if(this->enable_statistics) num_bv_tests++;
+ return !model1->getBV(b1).bv.overlap(model2_bv, sqrDistLowerBound);
+ }
+
const BVHModel<BV>* model1;
const S* model2;
BV model2_bv;
@@ -137,13 +149,24 @@ public:
return model2->getBV(b).rightChild();
}
- /// @brief BV culling test in one BVTT node
+ /// BV test between b1 and b2
+ /// \param b1, b2 Bounding volumes to test,
bool BVTesting(int b1, int b2) const
{
if(this->enable_statistics) num_bv_tests++;
return !model2->getBV(b2).bv.overlap(model1_bv);
}
+ /// BV test between b1 and b2
+ /// \param b1, b2 Bounding volumes to test,
+ /// \retval sqrDistLowerBound square of a lower bound of the minimal
+ /// distance between bounding volumes.
+ bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
+ {
+ if(this->enable_statistics) num_bv_tests++;
+ return !model2->getBV(b2).bv.overlap(model1_bv, sqrDistLowerBound);
+ }
+
const S* model1;
const BVHModel<BV>* model2;
BV model1_bv;
@@ -159,7 +182,8 @@ template<typename BV, typename S, typename NarrowPhaseSolver>
class MeshShapeCollisionTraversalNode : public BVHShapeCollisionTraversalNode<BV, S>
{
public:
- MeshShapeCollisionTraversalNode() : BVHShapeCollisionTraversalNode<BV, S>()
+ MeshShapeCollisionTraversalNode(bool enable_distance_lower_bound = false) :
+ BVHShapeCollisionTraversalNode<BV, S> (enable_distance_lower_bound)
{
vertices = NULL;
tri_indices = NULL;
@@ -330,7 +354,9 @@ template<typename S, typename NarrowPhaseSolver>
class MeshShapeCollisionTraversalNodeOBB : public MeshShapeCollisionTraversalNode<OBB, S, NarrowPhaseSolver>
{
public:
- MeshShapeCollisionTraversalNodeOBB() : MeshShapeCollisionTraversalNode<OBB, S, NarrowPhaseSolver>()
+ MeshShapeCollisionTraversalNodeOBB(bool enable_distance_lower_bound = false) :
+ MeshShapeCollisionTraversalNode<OBB, S, NarrowPhaseSolver>
+ (enable_distance_lower_bound)
{
}
@@ -352,7 +378,9 @@ template<typename S, typename NarrowPhaseSolver>
class MeshShapeCollisionTraversalNodeRSS : public MeshShapeCollisionTraversalNode<RSS, S, NarrowPhaseSolver>
{
public:
- MeshShapeCollisionTraversalNodeRSS() : MeshShapeCollisionTraversalNode<RSS, S, NarrowPhaseSolver>()
+ MeshShapeCollisionTraversalNodeRSS (bool enable_distance_lower_bound = false):
+ MeshShapeCollisionTraversalNode<RSS, S, NarrowPhaseSolver>
+ (enable_distance_lower_bound)
{
}
@@ -374,7 +402,9 @@ template<typename S, typename NarrowPhaseSolver>
class MeshShapeCollisionTraversalNodekIOS : public MeshShapeCollisionTraversalNode<kIOS, S, NarrowPhaseSolver>
{
public:
- MeshShapeCollisionTraversalNodekIOS() : MeshShapeCollisionTraversalNode<kIOS, S, NarrowPhaseSolver>()
+ MeshShapeCollisionTraversalNodekIOS(bool enable_distance_lower_bound = false):
+ MeshShapeCollisionTraversalNode<kIOS, S, NarrowPhaseSolver>
+ (enable_distance_lower_bound)
{
}
@@ -396,7 +426,10 @@ template<typename S, typename NarrowPhaseSolver>
class MeshShapeCollisionTraversalNodeOBBRSS : public MeshShapeCollisionTraversalNode<OBBRSS, S, NarrowPhaseSolver>
{
public:
- MeshShapeCollisionTraversalNodeOBBRSS() : MeshShapeCollisionTraversalNode<OBBRSS, S, NarrowPhaseSolver>()
+ MeshShapeCollisionTraversalNodeOBBRSS
+ (bool enable_distance_lower_bound = false) :
+ MeshShapeCollisionTraversalNode
+ <OBBRSS, S, NarrowPhaseSolver>(enable_distance_lower_bound)
{
}
@@ -406,6 +439,14 @@ public:
return !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv);
}
+ bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
+ {
+ if(this->enable_statistics) this->num_bv_tests++;
+ return !overlap(this->tf1.getRotation(), this->tf1.getTranslation(),
+ this->model2_bv, this->model1->getBV(b1).bv,
+ sqrDistLowerBound);
+ }
+
void leafTesting(int b1, int b2) const
{
details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
@@ -594,6 +635,14 @@ public:
return !overlap(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv);
}
+ bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
+ {
+ if(this->enable_statistics) this->num_bv_tests++;
+ return !overlap(this->tf2.getRotation(), this->tf2.getTranslation(),
+ this->model1_bv, this->model2->getBV(b2).bv,
+ sqrDistLowerBound);
+ }
+
void leafTesting(int b1, int b2) const
{
details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices,
diff --git a/include/fcl/traversal/traversal_node_bvhs.h b/include/fcl/traversal/traversal_node_bvhs.h
index ac0508243ed718af3c0d265908111338a5a837ca..eea425a509934a96c942f0ff3b2fe11873e968a4 100644
--- a/include/fcl/traversal/traversal_node_bvhs.h
+++ b/include/fcl/traversal/traversal_node_bvhs.h
@@ -129,6 +129,16 @@ public:
return !model1->getBV(b1).overlap(model2->getBV(b2));
}
+ /// BV test between b1 and b2
+ /// \param b1, b2 Bounding volumes to test,
+ /// \retval sqrDistLowerBound square of a lower bound of the minimal
+ /// distance between bounding volumes.
+ bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
+ {
+ if(enable_statistics) num_bv_tests++;
+ return !model1->getBV(b1).overlap(model2->getBV(b2), sqrDistLowerBound);
+ }
+
/// @brief The first BVH model
const BVHModel<BV>* model1;
/// @brief The second BVH model
@@ -302,6 +312,8 @@ public:
bool BVTesting(int b1, int b2) const;
+ bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const;
+
void leafTesting(int b1, int b2) const;
Matrix3f R;
@@ -659,6 +671,8 @@ public:
FCL_REAL BVTesting(int b1, int b2) const;
+ FCL_REAL BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const;
+
void leafTesting(int b1, int b2) const;
Matrix3f R;
@@ -980,6 +994,8 @@ public:
FCL_REAL BVTesting(int b1, int b2) const;
+ FCL_REAL BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const;
+
void leafTesting(int b1, int b2) const;
bool canStop(FCL_REAL c) const;
diff --git a/include/fcl/traversal/traversal_node_shapes.h b/include/fcl/traversal/traversal_node_shapes.h
index 01f98d149a1f319bd425cfdf1de2ee87f4a8858f..a67fe4d4d86be3efb1f158f7d87463a542455412 100644
--- a/include/fcl/traversal/traversal_node_shapes.h
+++ b/include/fcl/traversal/traversal_node_shapes.h
@@ -70,6 +70,12 @@ public:
return false;
}
+ /// @brief BV culling test in one BVTT node
+ bool BVTesting(int, int, FCL_REAL&) const
+ {
+ throw std::runtime_error ("Not implemented");
+ }
+
/// @brief Intersection testing between leaves (two shapes)
void leafTesting(int, int) const
{
diff --git a/include/fcl/traversal/traversal_recurse.h b/include/fcl/traversal/traversal_recurse.h
index 90c6644208adb2da5c6817d253391619457f08f7..0225c632958b9531107ab778ac7fa354ee97e9cc 100644
--- a/include/fcl/traversal/traversal_recurse.h
+++ b/include/fcl/traversal/traversal_recurse.h
@@ -47,8 +47,12 @@
namespace fcl
{
-/// @brief Recurse function for collision
-void collisionRecurse(CollisionTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list);
+/// Recurse function for collision
+/// \param node collision node,
+/// \param b1, b2 ids of bounding volume nodes for object 1 and object 2
+/// \retval sqrDistLowerBound squared lower bound on distance between objects.
+void collisionRecurse(CollisionTraversalNodeBase* node, int b1, int b2,
+ BVHFrontList* front_list, FCL_REAL& sqrDistLowerBound);
/// @brief Recurse function for collision, specialized for OBB type
void collisionRecurse(MeshCollisionTraversalNodeOBB* node, int b1, int b2, const Matrix3f& R, const Vec3f& T, BVHFrontList* front_list);
@@ -66,7 +70,9 @@ void distanceRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFrontLi
void distanceQueueRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list, int qsize);
/// @brief Recurse function for front list propagation
-void propagateBVHFrontListCollisionRecurse(CollisionTraversalNodeBase* node, BVHFrontList* front_list);
+void propagateBVHFrontListCollisionRecurse
+ (CollisionTraversalNodeBase* node, BVHFrontList* front_list,
+ FCL_REAL& sqrDistLowerBound);
}
diff --git a/src/BV/OBB.cpp b/src/BV/OBB.cpp
index f54fe602b076d6654a988f9c59a994b834177881..6bb9b02d4c51d20100c4180d2f30cc614b45c5bf 100644
--- a/src/BV/OBB.cpp
+++ b/src/BV/OBB.cpp
@@ -513,6 +513,24 @@ bool OBB::overlap(const OBB& other) const
return !obbDisjoint(R, T, extent, other.extent);
}
+ bool OBB::overlap(const OBB& other, FCL_REAL& sqrDistLowerBound) const
+ {
+ /// compute what transform [R,T] that takes us from cs1 to cs2.
+ /// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)]
+ /// First compute the rotation part, then translation part
+ Vec3f t = other.To - To; // T2 - T1
+ Vec3f T(t.dot(axis[0]), t.dot(axis[1]), t.dot(axis[2])); // R1'(T2-T1)
+ Matrix3f R(axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]),
+ axis[0].dot(other.axis[2]),
+ axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]),
+ axis[1].dot(other.axis[2]),
+ axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]),
+ axis[2].dot(other.axis[2]));
+
+ return !obbDisjointAndLowerBoundDistance
+ (R, T, extent, other.extent, sqrDistLowerBound);
+}
+
bool OBB::contain(const Vec3f& p) const
{
@@ -584,6 +602,24 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2)
return !obbDisjoint(R, T, b1.extent, b2.extent);
}
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2,
+ FCL_REAL& sqrDistLowerBound)
+{
+ Matrix3f R0b2(R0.dotX(b2.axis[0]), R0.dotX(b2.axis[1]), R0.dotX(b2.axis[2]),
+ R0.dotY(b2.axis[0]), R0.dotY(b2.axis[1]), R0.dotY(b2.axis[2]),
+ R0.dotZ(b2.axis[0]), R0.dotZ(b2.axis[1]), R0.dotZ(b2.axis[2]));
+
+ Matrix3f R(R0b2.transposeDotX(b1.axis[0]), R0b2.transposeDotY(b1.axis[0]), R0b2.transposeDotZ(b1.axis[0]),
+ R0b2.transposeDotX(b1.axis[1]), R0b2.transposeDotY(b1.axis[1]), R0b2.transposeDotZ(b1.axis[1]),
+ R0b2.transposeDotX(b1.axis[2]), R0b2.transposeDotY(b1.axis[2]), R0b2.transposeDotZ(b1.axis[2]));
+
+ Vec3f Ttemp = R0 * b2.To + T0 - b1.To;
+ Vec3f T(Ttemp.dot(b1.axis[0]), Ttemp.dot(b1.axis[1]), Ttemp.dot(b1.axis[2]));
+
+ return !obbDisjointAndLowerBoundDistance (R, T, b1.extent, b2.extent,
+ sqrDistLowerBound);
+}
+
OBB translate(const OBB& bv, const Vec3f& t)
{
OBB res(bv);
diff --git a/src/BV/OBBRSS.cpp b/src/BV/OBBRSS.cpp
index ecc447c283cd45c9c4ff7f7ba1454bddaa00ff11..9d257d2c148d38ebe2e3202debc4c3a44665daa2 100644
--- a/src/BV/OBBRSS.cpp
+++ b/src/BV/OBBRSS.cpp
@@ -45,6 +45,12 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS
return overlap(R0, T0, b1.obb, b2.obb);
}
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2,
+ FCL_REAL& sqrDistLowerBound)
+{
+ return overlap(R0, T0, b1.obb, b2.obb, sqrDistLowerBound);
+}
+
FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2, Vec3f* P, Vec3f* Q)
{
return distance(R0, T0, b1.rss, b2.rss, P, Q);
diff --git a/src/BV/kIOS.cpp b/src/BV/kIOS.cpp
index 3b922eb46017cdb62f0da7736fbb47ba9fc2f607..50422c5a35a071a479bd78b9d4700473c9d4aebb 100644
--- a/src/BV/kIOS.cpp
+++ b/src/BV/kIOS.cpp
@@ -63,6 +63,11 @@ bool kIOS::overlap(const kIOS& other) const
return true;
}
+ bool kIOS::overlap(const kIOS& other, FCL_REAL& sqrDistLowerBound) const
+ {
+ throw std::runtime_error ("Not implemented yet.");
+ }
+
bool kIOS::contain(const Vec3f& p) const
{
diff --git a/src/collision_func_matrix.cpp b/src/collision_func_matrix.cpp
index 40ca30366359f71f5e34c4fb32bc8d97623f5ed3..6e2e2be6c37f2343dc83d9dfcbb97d5a3d16f0d9 100644
--- a/src/collision_func_matrix.cpp
+++ b/src/collision_func_matrix.cpp
@@ -83,7 +83,7 @@ std::size_t OcTreeShapeCollide(const CollisionGeometry* o1, const Transform3f& t
return result.numContacts();
}
-template<typename NarrowPhaseSolver>
+etemplate<typename NarrowPhaseSolver>
std::size_t OcTreeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
@@ -234,7 +234,9 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf
}
initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
- collide(&node);
+ FCL_REAL sqrDistance = 0;
+ collide(&node, sqrDistance);
+ result.distance_lower_bound = sqrt (sqrDistance);
if(request.enable_cached_gjk_guess)
result.cached_gjk_guess = nsolver->getCachedGuess();
@@ -263,7 +265,9 @@ struct BVHShapeCollider
const T_SH* obj2 = static_cast<const T_SH*>(o2);
initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, no_cost_request, result);
- fcl::collide(&node);
+ FCL_REAL sqrDistance;
+ fcl::collide(&node, sqrDistance);
+ result.distance_lower_bound = sqrt (sqrDistance);
delete obj1_tmp;
@@ -287,7 +291,9 @@ struct BVHShapeCollider
const T_SH* obj2 = static_cast<const T_SH*>(o2);
initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, request, result);
- fcl::collide(&node);
+ FCL_REAL sqrDistance;
+ fcl::collide(&node, sqrDistance);
+ result.distance_lower_bound = sqrt (sqrDistance);
delete obj1_tmp;
}
@@ -316,7 +322,9 @@ std::size_t orientedBVHShapeCollide(const CollisionGeometry* o1, const Transform
const T_SH* obj2 = static_cast<const T_SH*>(o2);
initialize(node, *obj1, tf1, *obj2, tf2, nsolver, no_cost_request, result);
- fcl::collide(&node);
+ FCL_REAL sqrDistance;
+ fcl::collide(&node, sqrDistance);
+ result.distance_lower_bound = sqrt (sqrDistance);
Box box;
Transform3f box_tf;
@@ -331,12 +339,15 @@ std::size_t orientedBVHShapeCollide(const CollisionGeometry* o1, const Transform
}
else
{
- OrientMeshShapeCollisionTraveralNode node;
+ OrientMeshShapeCollisionTraveralNode node
+ (request.enable_distance_lower_bound);
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
const T_SH* obj2 = static_cast<const T_SH*>(o2);
initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
- fcl::collide(&node);
+ FCL_REAL sqrDistance = 0;
+ fcl::collide(&node, sqrDistance);
+ result.distance_lower_bound = sqrt (sqrDistance);
}
return result.numContacts();
@@ -407,7 +418,9 @@ std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, cons
Transform3f tf2_tmp = tf2;
initialize(node, *obj1_tmp, tf1_tmp, *obj2_tmp, tf2_tmp, request, result);
- collide(&node);
+ FCL_REAL sqrDistance;
+ fcl::collide(&node, sqrDistance);
+ result.distance_lower_bound = sqrt (sqrDistance);
delete obj1_tmp;
delete obj2_tmp;
@@ -427,7 +440,9 @@ std::size_t orientedMeshCollide(const CollisionGeometry* o1, const Transform3f&
const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>* >(o2);
initialize(node, *obj1, tf1, *obj2, tf2, request, result);
- collide(&node);
+ FCL_REAL sqrDistance = 0;
+ collide(&node, sqrDistance);
+ result.distance_lower_bound = sqrt (sqrDistance);
return result.numContacts();
}
diff --git a/src/collision_node.cpp b/src/collision_node.cpp
index 3d7f0c38b2958a24477d8cf99df2c0addb44aa12..26cbdd978c50857eb7cca5857372d4acee00f1d4 100644
--- a/src/collision_node.cpp
+++ b/src/collision_node.cpp
@@ -42,23 +42,25 @@
namespace fcl
{
-void collide(CollisionTraversalNodeBase* node, BVHFrontList* front_list)
+void collide(CollisionTraversalNodeBase* node, FCL_REAL& sqrDistLowerBound,
+ BVHFrontList* front_list)
{
if(front_list && front_list->size() > 0)
{
- propagateBVHFrontListCollisionRecurse(node, front_list);
+ propagateBVHFrontListCollisionRecurse(node, front_list, sqrDistLowerBound);
}
else
{
- collisionRecurse(node, 0, 0, front_list);
+ collisionRecurse(node, 0, 0, front_list, sqrDistLowerBound);
}
}
void collide2(MeshCollisionTraversalNodeOBB* node, BVHFrontList* front_list)
{
+ FCL_REAL sqrDistLowerBound = 0;
if(front_list && front_list->size() > 0)
{
- propagateBVHFrontListCollisionRecurse(node, front_list);
+ propagateBVHFrontListCollisionRecurse(node, front_list, sqrDistLowerBound);
}
else
{
@@ -76,9 +78,10 @@ void collide2(MeshCollisionTraversalNodeOBB* node, BVHFrontList* front_list)
void collide2(MeshCollisionTraversalNodeRSS* node, BVHFrontList* front_list)
{
+ FCL_REAL sqrDistLowerBound = 0;
if(front_list && front_list->size() > 0)
{
- propagateBVHFrontListCollisionRecurse(node, front_list);
+ propagateBVHFrontListCollisionRecurse(node, front_list, sqrDistLowerBound);
}
else
{
@@ -91,9 +94,10 @@ void collide2(MeshCollisionTraversalNodeRSS* node, BVHFrontList* front_list)
void selfCollide(CollisionTraversalNodeBase* node, BVHFrontList* front_list)
{
+ FCL_REAL sqrDistLowerBound = 0;
if(front_list && front_list->size() > 0)
{
- propagateBVHFrontListCollisionRecurse(node, front_list);
+ propagateBVHFrontListCollisionRecurse(node, front_list, sqrDistLowerBound);
}
else
{
diff --git a/src/continuous_collision.cpp b/src/continuous_collision.cpp
index 80818714109f5eacc178669406e0ef6307032d5e..1768eca99dcdbe3cc956460e7c0e21acdc03b463 100644
--- a/src/continuous_collision.cpp
+++ b/src/continuous_collision.cpp
@@ -113,7 +113,8 @@ FCL_REAL continuousCollideBVHPolynomial(const CollisionGeometry* o1_, const Tran
if(!initialize<BV>(node, *o1, tf1, *o2, tf2, c_request))
return -1.0;
- collide(&node);
+ FCL_REAL unused;
+ collide(&node, unused);
result.is_collide = (node.pairs.size() > 0);
result.time_of_contact = node.time_of_contact;
diff --git a/src/traversal/traversal_node_base.cpp b/src/traversal/traversal_node_base.cpp
index 0ba6c2e084ae09dc8aeb30bc7b01c1507b60166d..66172aeed096a4c775ad36cbe32ec7c98cc4654a 100644
--- a/src/traversal/traversal_node_base.cpp
+++ b/src/traversal/traversal_node_base.cpp
@@ -90,6 +90,11 @@ bool CollisionTraversalNodeBase::BVTesting(int b1, int b2) const
return true;
}
+bool CollisionTraversalNodeBase::BVTesting(int b1, int b2, FCL_REAL&) const
+{
+ throw std::runtime_error ("Not implemented yet");
+}
+
void CollisionTraversalNodeBase::leafTesting(int b1, int b2) const
{
}
diff --git a/src/traversal/traversal_node_bvhs.cpp b/src/traversal/traversal_node_bvhs.cpp
index 11bb544a7ee4a15a1c7abf629d9106c66b9276e0..a07d99d66f33d39431a2d8abbc8e130de353e9cf 100644
--- a/src/traversal/traversal_node_bvhs.cpp
+++ b/src/traversal/traversal_node_bvhs.cpp
@@ -281,6 +281,14 @@ bool MeshCollisionTraversalNodeOBBRSS::BVTesting(int b1, int b2) const
return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
}
+ bool MeshCollisionTraversalNodeOBBRSS::BVTesting
+ (int b1, int b2, FCL_REAL& sqrDistLowerBound) const
+ {
+ if(enable_statistics) num_bv_tests++;
+ return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv,
+ sqrDistLowerBound);
+ }
+
void MeshCollisionTraversalNodeOBBRSS::leafTesting(int b1, int b2) const
{
details::meshCollisionOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2,
diff --git a/src/traversal/traversal_recurse.cpp b/src/traversal/traversal_recurse.cpp
index 24ac4c0d7189dfed70baa43f79a6c11a2fa1fa09..e2c731c42e0e25cbdc0cf03ed12d337d231a56f0 100644
--- a/src/traversal/traversal_recurse.cpp
+++ b/src/traversal/traversal_recurse.cpp
@@ -40,50 +40,62 @@
namespace fcl
{
-void collisionRecurse(CollisionTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list)
+void collisionRecurse(CollisionTraversalNodeBase* node, int b1, int b2,
+ BVHFrontList* front_list, FCL_REAL& sqrDistLowerBound)
{
+ FCL_REAL sqrDistLowerBound1, sqrDistLowerBound2 = 0;
bool l1 = node->isFirstNodeLeaf(b1);
bool l2 = node->isSecondNodeLeaf(b2);
-
if(l1 && l2)
{
updateFrontList(front_list, b1, b2);
- if(node->BVTesting(b1, b2)) return;
+ if (node->enable_distance_lower_bound) {
+ if(node->BVTesting(b1, b2, sqrDistLowerBound)) return;
+ } else {
+ if(node->BVTesting(b1, b2)) return;
+ }
node->leafTesting(b1, b2);
return;
}
- if(node->BVTesting(b1, b2))
- {
- updateFrontList(front_list, b1, b2);
- return;
+ if (node->enable_distance_lower_bound) {
+ if(node->BVTesting(b1, b2, sqrDistLowerBound)) {
+ updateFrontList(front_list, b1, b2);
+ return;
+ }
+ } else {
+ if(node->BVTesting(b1, b2)) {
+ updateFrontList(front_list, b1, b2);
+ return;
+ }
}
-
if(node->firstOverSecond(b1, b2))
{
int c1 = node->getFirstLeftChild(b1);
int c2 = node->getFirstRightChild(b1);
- collisionRecurse(node, c1, b2, front_list);
+ collisionRecurse(node, c1, b2, front_list, sqrDistLowerBound1);
// early stop is disabled is front_list is used
if(node->canStop() && !front_list) return;
- collisionRecurse(node, c2, b2, front_list);
+ collisionRecurse(node, c2, b2, front_list, sqrDistLowerBound2);
+ sqrDistLowerBound = std::min (sqrDistLowerBound1, sqrDistLowerBound2);
}
else
{
int c1 = node->getSecondLeftChild(b2);
int c2 = node->getSecondRightChild(b2);
- collisionRecurse(node, b1, c1, front_list);
+ collisionRecurse(node, b1, c1, front_list, sqrDistLowerBound1);
// early stop is disabled is front_list is used
if(node->canStop() && !front_list) return;
- collisionRecurse(node, b1, c2, front_list);
+ collisionRecurse(node, b1, c2, front_list, sqrDistLowerBound2);
+ sqrDistLowerBound = std::min (sqrDistLowerBound1, sqrDistLowerBound2);
}
}
@@ -177,6 +189,7 @@ void collisionRecurse(MeshCollisionTraversalNodeRSS* node, int b1, int b2, const
*/
void selfCollisionRecurse(CollisionTraversalNodeBase* node, int b, BVHFrontList* front_list)
{
+ FCL_REAL sqrDistLowerBound;
bool l = node->isFirstNodeLeaf(b);
if(l) return;
@@ -190,7 +203,7 @@ void selfCollisionRecurse(CollisionTraversalNodeBase* node, int b, BVHFrontList*
selfCollisionRecurse(node, c2, front_list);
if(node->canStop() && !front_list) return;
- collisionRecurse(node, c1, c2, front_list);
+ collisionRecurse(node, c1, c2, front_list, sqrDistLowerBound);
}
void distanceRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list)
@@ -389,8 +402,11 @@ void distanceQueueRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFr
}
}
-void propagateBVHFrontListCollisionRecurse(CollisionTraversalNodeBase* node, BVHFrontList* front_list)
+void propagateBVHFrontListCollisionRecurse
+(CollisionTraversalNodeBase* node, BVHFrontList* front_list,
+ FCL_REAL& sqrDistLowerBound)
{
+ FCL_REAL sqrDistLowerBound1, sqrDistLowerBound2 = 0;
BVHFrontList::iterator front_iter;
BVHFrontList append;
for(front_iter = front_list->begin(); front_iter != front_list->end(); ++front_iter)
@@ -403,30 +419,52 @@ void propagateBVHFrontListCollisionRecurse(CollisionTraversalNodeBase* node, BVH
if(l1 & l2)
{
front_iter->valid = false; // the front node is no longer valid, in collideRecurse will add again.
- collisionRecurse(node, b1, b2, &append);
+ collisionRecurse(node, b1, b2, &append, sqrDistLowerBound);
}
else
{
- if(!node->BVTesting(b1, b2))
- {
- front_iter->valid = false;
-
- if(node->firstOverSecond(b1, b2))
- {
- int c1 = node->getFirstLeftChild(b1);
- int c2 = node->getFirstRightChild(b2);
-
- collisionRecurse(node, c1, b2, front_list);
- collisionRecurse(node, c2, b2, front_list);
- }
- else
- {
- int c1 = node->getSecondLeftChild(b2);
- int c2 = node->getSecondRightChild(b2);
-
- collisionRecurse(node, b1, c1, front_list);
- collisionRecurse(node, b1, c2, front_list);
- }
+ if (node->enable_distance_lower_bound) {
+ if(!node->BVTesting(b1, b2, sqrDistLowerBound)) {
+ front_iter->valid = false;
+ if(node->firstOverSecond(b1, b2)) {
+ int c1 = node->getFirstLeftChild(b1);
+ int c2 = node->getFirstRightChild(b2);
+
+ collisionRecurse(node, c1, b2, front_list, sqrDistLowerBound1);
+ collisionRecurse(node, c2, b2, front_list, sqrDistLowerBound2);
+ sqrDistLowerBound = std::min (sqrDistLowerBound1,
+ sqrDistLowerBound2);
+ } else {
+ int c1 = node->getSecondLeftChild(b2);
+ int c2 = node->getSecondRightChild(b2);
+
+ collisionRecurse(node, b1, c1, front_list, sqrDistLowerBound1);
+ collisionRecurse(node, b1, c2, front_list, sqrDistLowerBound2);
+ sqrDistLowerBound = std::min (sqrDistLowerBound1,
+ sqrDistLowerBound2);
+ }
+ }
+ } else {
+ if(!node->BVTesting(b1, b2)) {
+ front_iter->valid = false;
+ if(node->firstOverSecond(b1, b2)) {
+ int c1 = node->getFirstLeftChild(b1);
+ int c2 = node->getFirstRightChild(b2);
+
+ collisionRecurse(node, c1, b2, front_list, sqrDistLowerBound1);
+ collisionRecurse(node, c2, b2, front_list, sqrDistLowerBound2);
+ sqrDistLowerBound = std::min (sqrDistLowerBound1,
+ sqrDistLowerBound2);
+ } else {
+ int c1 = node->getSecondLeftChild(b2);
+ int c2 = node->getSecondRightChild(b2);
+
+ collisionRecurse(node, b1, c1, front_list, sqrDistLowerBound1);
+ collisionRecurse(node, b1, c2, front_list, sqrDistLowerBound2);
+ sqrDistLowerBound = std::min (sqrDistLowerBound1,
+ sqrDistLowerBound2);
+ }
+ }
}
}
}
diff --git a/test/test_fcl_collision.cpp b/test/test_fcl_collision.cpp
index 74fdd850c6f527a9fad79abb4bdbb9408c36c819..d70fe25e897f3fc47936857337545230718c4c8a 100644
--- a/test/test_fcl_collision.cpp
+++ b/test/test_fcl_collision.cpp
@@ -839,7 +839,8 @@ bool collide_Test2(const Transform3f& tf,
node.enable_statistics = verbose;
- collide(&node);
+ FCL_REAL sqrDistLowerBound = 0;
+ collide(&node, sqrDistLowerBound);
if(local_result.numContacts() > 0)
@@ -898,7 +899,8 @@ bool collide_Test(const Transform3f& tf,
node.enable_statistics = verbose;
- collide(&node);
+ FCL_REAL sqrDistLowerBound = 0;
+ collide(&node, sqrDistLowerBound);
if(local_result.numContacts() > 0)
@@ -955,7 +957,8 @@ bool collide_Test_Oriented(const Transform3f& tf,
node.enable_statistics = verbose;
- collide(&node);
+ FCL_REAL sqrDistLowerBound = 0;
+ collide(&node, sqrDistLowerBound);
if(local_result.numContacts() > 0)
{
diff --git a/test/test_fcl_distance.cpp b/test/test_fcl_distance.cpp
index 23aecc6c668471fb84a7d4c01b8628234bb3aa9a..181a659286099b0698839e37432219c8dd910dd5 100644
--- a/test/test_fcl_distance.cpp
+++ b/test/test_fcl_distance.cpp
@@ -418,7 +418,8 @@ bool collide_Test_OBB(const Transform3f& tf,
node.enable_statistics = verbose;
- collide(&node);
+ FCL_REAL sqrDistLowerBound;
+ collide(&node, sqrDistLowerBound);
if(local_result.numContacts() > 0)
return true;
diff --git a/test/test_fcl_frontlist.cpp b/test/test_fcl_frontlist.cpp
index fc4d61db7231e8e3e916daae88c4082b29e7bbd0..45ad58227b60ba5c212cc4eae786a4fd45cc07e4 100644
--- a/test/test_fcl_frontlist.cpp
+++ b/test/test_fcl_frontlist.cpp
@@ -235,7 +235,8 @@ bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2,
node.enable_statistics = verbose;
- collide(&node, &front_list);
+ FCL_REAL sqrDistLowerBound = 0;
+ collide(&node, sqrDistLowerBound, &front_list);
if(verbose) std::cout << "front list size " << front_list.size() << std::endl;
@@ -255,7 +256,8 @@ bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2,
m2.endReplaceModel(true, refit_bottomup);
local_result.clear();
- collide(&node, &front_list);
+ sqrDistLowerBound = 0;
+ collide(&node, sqrDistLowerBound, &front_list);
if(local_result.numContacts() > 0)
return true;
@@ -298,7 +300,8 @@ bool collide_front_list_Test_Oriented(const Transform3f& tf1, const Transform3f&
node.enable_statistics = verbose;
- collide(&node, &front_list);
+ FCL_REAL sqrDistLowerBound = 0;
+ collide(&node, sqrDistLowerBound, &front_list);
if(verbose) std::cout << "front list size " << front_list.size() << std::endl;
@@ -309,7 +312,8 @@ bool collide_front_list_Test_Oriented(const Transform3f& tf1, const Transform3f&
std::cout << "initialize error" << std::endl;
local_result.clear();
- collide(&node, &front_list);
+ sqrDistLowerBound = 0;
+ collide(&node, sqrDistLowerBound, &front_list);
if(local_result.numContacts() > 0)
return true;
@@ -347,7 +351,8 @@ bool collide_Test(const Transform3f& tf,
node.enable_statistics = verbose;
- collide(&node);
+ FCL_REAL sqrDistLowerBound = 0;
+ collide(&node, sqrDistLowerBound);
if(local_result.numContacts() > 0)