Commit 2646e08f authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

Adding parameter to track distance lower bound between BVH (preliminary).

parent 16a9f62f
......@@ -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
{
......
......@@ -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
......
......@@ -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);
}
......
......@@ -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);
......
......@@ -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
......
......@@ -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;
......
......@@ -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.
......
......@@ -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);
......
......@@ -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);
......
......@@ -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.
......
......@@ -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,
......
......@@ -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;
......
......@@ -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
{
......
......@@ -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);
}
......
......@@ -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);
......
......@@ -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);
......
......@@ -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
{
......
......@@ -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;