Commit 5e219fd2 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Merge classes MeshCollisionTraversalNode[bvtype] into MeshCollisionTraversalNode

parent afa59832
......@@ -59,6 +59,10 @@ namespace hpp
{
namespace fcl
{
enum {
RelativeTransformationIsIdentity = 1
};
/// @brief Traversal node for collision between BVH models
template<typename BV>
class BVHCollisionTraversalNode : public CollisionTraversalNodeBase
......@@ -124,24 +128,6 @@ public:
{
return model2->getBV(b).rightChild();
}
/// @brief BV culling test in one BVTT node
bool BVTesting(int b1, int b2) const
{
if(enable_statistics) num_bv_tests++;
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), request,
sqrDistLowerBound);
}
/// @brief The first BVH model
const BVHModel<BV>* model1;
......@@ -154,12 +140,38 @@ public:
mutable FCL_REAL query_time_seconds;
};
namespace details
{
template <bool enabled>
struct RelativeTransformation
{
RelativeTransformation () : R (Matrix3f::Identity()) {}
const Matrix3f& _R () const { return R; }
const Vec3f & _T () const { return T; }
Matrix3f R;
Vec3f T;
};
template <>
struct RelativeTransformation <false>
{
static const Matrix3f& _R () { throw std::logic_error ("should never reach this point"); }
static const Vec3f & _T () { throw std::logic_error ("should never reach this point"); }
};
} // namespace details
/// @brief Traversal node for collision between two meshes
template<typename BV>
template<typename BV, int _Options = RelativeTransformationIsIdentity>
class MeshCollisionTraversalNode : public BVHCollisionTraversalNode<BV>
{
public:
enum {
Options = _Options,
RTIsIdentity = _Options & RelativeTransformationIsIdentity
};
MeshCollisionTraversalNode(const CollisionRequest& request) :
BVHCollisionTraversalNode<BV> (request)
{
......@@ -169,6 +181,36 @@ public:
tri_indices2 = NULL;
}
/// @brief BV culling test in one BVTT node
bool BVTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
if (RTIsIdentity)
return !this->model1->getBV(b1).overlap(this->model2->getBV(b2));
else
return !overlap(RT._R(), RT._T(),
this->model1->getBV(b1).bv, this->model2->getBV(b2).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) this->num_bv_tests++;
if (RTIsIdentity)
return !this->model1->getBV(b1).overlap(this->model2->getBV(b2),
this->request, sqrDistLowerBound);
else {
bool res = !overlap(RT._R(), RT._T(),
this->model1->getBV(b1).bv, this->model2->getBV(b2).bv,
this->request, sqrDistLowerBound);
assert (!res || sqrDistLowerBound > 0);
return res;
}
}
/// Intersection testing between leaves (two triangles)
///
/// \param b1, b2 id of primitive in bounding volume hierarchy
......@@ -242,61 +284,16 @@ public:
Triangle* tri_indices1;
Triangle* tri_indices2;
};
/// @brief Traversal node for collision between two meshes if their underlying BVH node is oriented node (OBB, RSS, OBBRSS, kIOS)
class MeshCollisionTraversalNodeOBB : public MeshCollisionTraversalNode<OBB>
{
public:
MeshCollisionTraversalNodeOBB (const CollisionRequest& request);
bool BVTesting(int b1, int b2) const;
bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const;
Matrix3f R;
Vec3f T;
details::RelativeTransformation<!bool(RTIsIdentity)> RT;
};
class MeshCollisionTraversalNodeRSS : public MeshCollisionTraversalNode<RSS>
{
public:
MeshCollisionTraversalNodeRSS (const CollisionRequest& request);
bool BVTesting(int b1, int b2) const;
bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const;
Matrix3f R;
Vec3f T;
};
class MeshCollisionTraversalNodekIOS : public MeshCollisionTraversalNode<kIOS>
{
public:
MeshCollisionTraversalNodekIOS (const CollisionRequest& request);
bool BVTesting(int b1, int b2) const;
bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const;
Matrix3f R;
Vec3f T;
};
class MeshCollisionTraversalNodeOBBRSS : public MeshCollisionTraversalNode<OBBRSS>
{
public:
MeshCollisionTraversalNodeOBBRSS (const CollisionRequest& request);
bool BVTesting(int b1, int b2) const;
bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const;
Matrix3f R;
Vec3f T;
};
/// @brief Traversal node for collision between two meshes if their underlying BVH node is oriented node (OBB, RSS, OBBRSS, kIOS)
typedef MeshCollisionTraversalNode<OBB , 0> MeshCollisionTraversalNodeOBB ;
typedef MeshCollisionTraversalNode<RSS , 0> MeshCollisionTraversalNodeRSS ;
typedef MeshCollisionTraversalNode<kIOS , 0> MeshCollisionTraversalNodekIOS ;
typedef MeshCollisionTraversalNode<OBBRSS, 0> MeshCollisionTraversalNodeOBBRSS;
namespace details
{
......
......@@ -86,92 +86,6 @@ static inline void meshDistanceOrientedNodeLeafTesting(int b1, int b2,
}
} // namespace details
MeshCollisionTraversalNodeOBB::MeshCollisionTraversalNodeOBB
(const CollisionRequest& request) :
MeshCollisionTraversalNode<OBB> (request)
{
R.setIdentity();
}
bool MeshCollisionTraversalNodeOBB::BVTesting(int b1, int b2) const
{
if(enable_statistics) num_bv_tests++;
return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
}
bool MeshCollisionTraversalNodeOBB::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,
request, sqrDistLowerBound);
}
MeshCollisionTraversalNodeRSS::MeshCollisionTraversalNodeRSS
(const CollisionRequest& request) :
MeshCollisionTraversalNode<RSS> (request)
{
R.setIdentity();
}
bool MeshCollisionTraversalNodeRSS::BVTesting(int b1, int b2) const
{
if(enable_statistics) num_bv_tests++;
return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
}
bool MeshCollisionTraversalNodeRSS::BVTesting(int b1, int b2,
FCL_REAL& sqrDistLowerBound) const
{
if(enable_statistics) num_bv_tests++;
sqrDistLowerBound = 0;
return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
}
MeshCollisionTraversalNodekIOS::MeshCollisionTraversalNodekIOS
(const CollisionRequest& request) :
MeshCollisionTraversalNode<kIOS>(request)
{
R.setIdentity();
}
bool MeshCollisionTraversalNodekIOS::BVTesting(int b1, int b2) const
{
if(enable_statistics) num_bv_tests++;
return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
}
bool MeshCollisionTraversalNodekIOS::BVTesting
(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
{
if(enable_statistics) num_bv_tests++;
sqrDistLowerBound = 0;
return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
}
MeshCollisionTraversalNodeOBBRSS::MeshCollisionTraversalNodeOBBRSS
(const CollisionRequest& request) :
MeshCollisionTraversalNode<OBBRSS> (request)
{
R.setIdentity();
}
bool MeshCollisionTraversalNodeOBBRSS::BVTesting(int b1, int b2) const
{
if(enable_statistics) num_bv_tests++;
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++;
bool res (!overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv,
request, sqrDistLowerBound));
assert (!res || sqrDistLowerBound > 0);
return res;
}
namespace details
{
......
......@@ -68,7 +68,7 @@ static inline bool setupMeshCollisionOrientedNode(OrientedNode& node,
node.result = &result;
relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation(), node.R, node.T);
relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation(), node.RT.R, node.RT.T);
return true;
}
......
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