Skip to content

GitLab

Commit 34cf662b authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

Add return value sqrDistLowerBound to leafTesting method 

  - in class CollisionTraversalNodeBase and derived classes.
parent 8d9badbc
Hide whitespace changes
Inline Side-by-side
include/fcl/traversal/traversal_node_base.h
View file @ 34cf662b
......@@ -97,16 +97,19 @@ public:
virtual ~CollisionTraversalNodeBase();
/// @brief BV test between b1 and b2
virtual bool BVTesting(int b1, int b2) const;
virtual bool BVTesting(int b1, int b2) const = 0;
/// 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;
virtual bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const = 0;
/// @brief Leaf test between node b1 and b2, if they are both leafs
virtual void leafTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const;
virtual void leafTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
{
throw std::runtime_error ("Not implemented");
}
/// @brief Check whether the traversal can stop
virtual bool canStop() const;
......@@ -139,7 +142,7 @@ public:
virtual FCL_REAL BVTesting(int b1, int b2) const;
/// @brief Leaf test between node b1 and b2, if they are both leafs
virtual void leafTesting(int b1, int b2) const;
virtual void leafTesting(int b1, int b2) const = 0;
/// @brief Check whether the traversal can stop
virtual bool canStop(FCL_REAL c) const;
......
include/fcl/traversal/traversal_node_bvhs.h
View file @ 34cf662b
......@@ -167,7 +167,7 @@ public:
}
/// @brief Intersection testing between leaves (two triangles)
void leafTesting(int b1, int b2) const
void leafTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
{
if(this->enable_statistics) this->num_leaf_tests++;
......@@ -267,11 +267,12 @@ public:
bool BVTesting(int b1, int b2) const;
void leafTesting(int b1, int b2) const;
void leafTesting(int b1, int b2, FCL_REAL&) const;
bool BVTesting(int b1, int b2, const Matrix3f& Rc, const Vec3f& Tc) const;
void leafTesting(int b1, int b2, const Matrix3f& Rc, const Vec3f& Tc) const;
void leafTesting(int b1, int b2, const Matrix3f& Rc, const Vec3f& Tc,
FCL_REAL& sqrDistLowerBound) const;
Matrix3f R;
Vec3f T;
......@@ -284,11 +285,12 @@ public:
bool BVTesting(int b1, int b2) const;
void leafTesting(int b1, int b2) const;
void leafTesting(int b1, int b2, FCL_REAL&) const;
bool BVTesting(int b1, int b2, const Matrix3f& Rc, const Vec3f& Tc) const;
void leafTesting(int b1, int b2, const Matrix3f& Rc, const Vec3f& Tc) const;
void leafTesting(int b1, int b2, const Matrix3f& Rc, const Vec3f& Tc,
FCL_REAL& sqrDistLowerBound) const;
Matrix3f R;
Vec3f T;
......@@ -301,7 +303,7 @@ public:
bool BVTesting(int b1, int b2) const;
void leafTesting(int b1, int b2) const;
void leafTesting(int b1, int b2, FCL_REAL&) const;
Matrix3f R;
Vec3f T;
......@@ -316,7 +318,7 @@ public:
bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const;
void leafTesting(int b1, int b2) const;
void leafTesting(int b1, int b2, FCL_REAL&) const;
Matrix3f R;
Vec3f T;
......@@ -359,7 +361,7 @@ public:
}
/// @brief Intersection testing between leaves (two triangles)
void leafTesting(int b1, int b2) const
void leafTesting(int b1, int b2, FCL_REAL&) const
{
if(this->enable_statistics) this->num_leaf_tests++;
......
include/fcl/traversal/traversal_node_octree.h
View file @ 34cf662b
......@@ -1041,7 +1041,7 @@ public:
return false;
}
void leafTesting(int, int) const
void leafTesting(int, int, FCL_REAL&) const
{
otsolver->OcTreeIntersect(model1, model2, tf1, tf2, request, *result);
}
......@@ -1102,7 +1102,7 @@ public:
return false;
}
void leafTesting(int, int) const
void leafTesting(int, int, FCL_REAL&) const
{
otsolver->OcTreeShapeIntersect(model2, *model1, tf2, tf1, request, *result);
}
......@@ -1133,7 +1133,7 @@ public:
return false;
}
void leafTesting(int, int) const
void leafTesting(int, int, FCL_REAL&) const
{
otsolver->OcTreeShapeIntersect(model1, *model2, tf1, tf2, request, *result);
}
......@@ -1222,7 +1222,7 @@ public:
return false;
}
void leafTesting(int, int) const
void leafTesting(int, int, FCL_REAL&) const
{
otsolver->OcTreeMeshIntersect(model2, model1, tf2, tf1, request, *result);
}
......@@ -1253,7 +1253,7 @@ public:
return false;
}
void leafTesting(int, int) const
void leafTesting(int, int, FCL_REAL&) const
{
otsolver->OcTreeMeshIntersect(model1, model2, tf1, tf2, request, *result);
}
......
include/fcl/traversal/traversal_node_shapes.h
View file @ 34cf662b
......@@ -77,7 +77,7 @@ public:
}
/// @brief Intersection testing between leaves (two shapes)
void leafTesting(int, int) const
void leafTesting(int, int, FCL_REAL&) const
{
abort ();
if(model1->isOccupied() && model2->isOccupied())
......
src/traversal/traversal_node_base.cpp
View file @ 34cf662b
......@@ -85,20 +85,6 @@ CollisionTraversalNodeBase::~CollisionTraversalNodeBase()
{
}
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, int, FCL_REAL&) const
{
}
bool CollisionTraversalNodeBase::canStop() const
{
return false;
......@@ -114,10 +100,6 @@ FCL_REAL DistanceTraversalNodeBase::BVTesting(int b1, int b2) const
return std::numeric_limits<FCL_REAL>::max();
}
void DistanceTraversalNodeBase::leafTesting(int b1, int b2) const
{
}
bool DistanceTraversalNodeBase::canStop(FCL_REAL c) const
{
return false;
......
src/traversal/traversal_node_bvhs.cpp
View file @ 34cf662b
......@@ -44,17 +44,13 @@ namespace fcl
namespace details
{
template<typename BV>
static inline void meshCollisionOrientedNodeLeafTesting(int b1, int b2,
const BVHModel<BV>* model1, const BVHModel<BV>* model2,
Vec3f* vertices1, Vec3f* vertices2,
Triangle* tri_indices1, Triangle* tri_indices2,
const Matrix3f& R, const Vec3f& T,
const Transform3f& tf1, const Transform3f& tf2,
bool enable_statistics,
FCL_REAL cost_density,
int& num_leaf_tests,
const CollisionRequest& request,
CollisionResult& result)
static inline void meshCollisionOrientedNodeLeafTesting
(int b1, int b2, const BVHModel<BV>* model1, const BVHModel<BV>* model2,
Vec3f* vertices1, Vec3f* vertices2, Triangle* tri_indices1,
Triangle* tri_indices2, const Matrix3f& R, const Vec3f& T,
const Transform3f& tf1, const Transform3f& tf2, bool enable_statistics,
FCL_REAL cost_density, int& num_leaf_tests, const CollisionRequest& request,
CollisionResult& result, FCL_REAL& sqrDistLowerBound)
{
if(enable_statistics) num_leaf_tests++;
......@@ -80,11 +76,23 @@ static inline void meshCollisionOrientedNodeLeafTesting(int b1, int b2,
if(!request.enable_contact) // only interested in collision or not
{
if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3, R, T))
{
is_intersect = true;
if(result.numContacts() < request.num_max_contacts)
result.addContact(Contact(model1, model2, primitive_id1, primitive_id2));
if (request.enable_distance_lower_bound) {
Vec3f P, Q;
sqrDistLowerBound = TriangleDistance::sqrTriDistance
(p1, p2, p3, q1, q2, q3, R, T, P, Q);
if (sqrDistLowerBound == 0) {
is_intersect = true;
if(result.numContacts() < request.num_max_contacts)
result.addContact(Contact(model1, model2, primitive_id1,
primitive_id2));
}
} else {
if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3, R, T)) {
is_intersect = true;
if(result.numContacts() < request.num_max_contacts)
result.addContact(Contact(model1, model2, primitive_id1,
primitive_id2));
}
}
}
else // need compute the contact information
......@@ -191,15 +199,14 @@ bool MeshCollisionTraversalNodeOBB::BVTesting(int b1, int b2) const
return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
}
void MeshCollisionTraversalNodeOBB::leafTesting(int b1, int b2) const
void MeshCollisionTraversalNodeOBB::leafTesting
(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
{
details::meshCollisionOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2,
tri_indices1, tri_indices2,
R, T,
tf1, tf2,
enable_statistics, cost_density,
num_leaf_tests,
request, *result);
details::meshCollisionOrientedNodeLeafTesting
(b1, b2, model1, model2, vertices1, vertices2,
tri_indices1, tri_indices2, R, T, tf1, tf2,
enable_statistics, cost_density, num_leaf_tests, request, *result,
sqrDistLowerBound);
}
......@@ -209,16 +216,15 @@ bool MeshCollisionTraversalNodeOBB::BVTesting(int b1, int b2, const Matrix3f& Rc
return obbDisjoint(Rc, Tc, model1->getBV(b1).bv.extent, model2->getBV(b2).bv.extent);
}
void MeshCollisionTraversalNodeOBB::leafTesting(int b1, int b2, const Matrix3f& Rc, const Vec3f& Tc) const
{
details::meshCollisionOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2,
tri_indices1, tri_indices2,
R, T,
tf1, tf2,
enable_statistics, cost_density,
num_leaf_tests,
request, *result);
}
void MeshCollisionTraversalNodeOBB::leafTesting
(int b1, int b2, const Matrix3f& Rc, const Vec3f& Tc,
FCL_REAL& sqrDistLowerBound) const
{
details::meshCollisionOrientedNodeLeafTesting
(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2,
R, T, tf1, tf2, enable_statistics, cost_density, num_leaf_tests, request,
*result, sqrDistLowerBound);
}
......@@ -235,15 +241,13 @@ bool MeshCollisionTraversalNodeRSS::BVTesting(int b1, int b2) const
return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
}
void MeshCollisionTraversalNodeRSS::leafTesting(int b1, int b2) const
void MeshCollisionTraversalNodeRSS::leafTesting
(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
{
details::meshCollisionOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2,
tri_indices1, tri_indices2,
R, T,
tf1, tf2,
enable_statistics, cost_density,
num_leaf_tests,
request, *result);
details::meshCollisionOrientedNodeLeafTesting
(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2,
R, T, tf1, tf2, enable_statistics, cost_density, num_leaf_tests,
request, *result, sqrDistLowerBound);
}
......@@ -262,15 +266,13 @@ bool MeshCollisionTraversalNodekIOS::BVTesting(int b1, int b2) const
return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
}
void MeshCollisionTraversalNodekIOS::leafTesting(int b1, int b2) const
void MeshCollisionTraversalNodekIOS::leafTesting
(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
{
details::meshCollisionOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2,
tri_indices1, tri_indices2,
R, T,
tf1, tf2,
enable_statistics, cost_density,
num_leaf_tests,
request, *result);
details::meshCollisionOrientedNodeLeafTesting
(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2,
R, T, tf1, tf2, enable_statistics, cost_density, num_leaf_tests,
request, *result, sqrDistLowerBound);
}
......@@ -296,15 +298,13 @@ bool MeshCollisionTraversalNodeOBBRSS::BVTesting(int b1, int b2) const
sqrDistLowerBound);
}
void MeshCollisionTraversalNodeOBBRSS::leafTesting(int b1, int b2) const
void MeshCollisionTraversalNodeOBBRSS::leafTesting
(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
{
details::meshCollisionOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2,
tri_indices1, tri_indices2,
R, T,
tf1, tf2,
enable_statistics, cost_density,
num_leaf_tests,
request,*result);
details::meshCollisionOrientedNodeLeafTesting
(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2,
R, T, tf1, tf2, enable_statistics, cost_density, num_leaf_tests,
request,*result, sqrDistLowerBound);
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment