Skip to content
Snippets Groups Projects
Commit a9498986 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Remove duplication of ShapeMeshCollisionTraversalNode

parent ba466e33
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
include/hpp/fcl/traversal/traversal_node_bvh_shape.h
+ 71
− 166
View file @ a9498986
......@@ -135,24 +135,6 @@ public:
return model2->getBV(b).rightChild();
}
/// 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;
......@@ -277,56 +259,6 @@ public:
const NarrowPhaseSolver* nsolver;
};
/// @cond IGNORE
namespace details
{
template<typename BV, typename S, typename NarrowPhaseSolver>
static inline void meshShapeCollisionOrientedNodeLeafTesting
(int b1, int /*b2*/, const BVHModel<BV>* model1, const S& model2,
Vec3f* vertices, Triangle* tri_indices, const Transform3f& tf1,
const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
bool enable_statistics, int& num_leaf_tests,
const CollisionRequest& request, CollisionResult& result,
FCL_REAL& sqrDistLowerBound)
{
if(enable_statistics) num_leaf_tests++;
const BVNode<BV>& node = model1->getBV(b1);
int primitive_id = node.primitiveId();
const Triangle& tri_id = tri_indices[primitive_id];
const Vec3f& P1 = vertices[tri_id[0]];
const Vec3f& P2 = vertices[tri_id[1]];
const Vec3f& P3 = vertices[tri_id[2]];
FCL_REAL distance;
Vec3f normal;
Vec3f p1, p2; // closest points
if(nsolver->shapeTriangleInteraction(model2, tf2, P1, P2, P3, tf1,
distance, p1, p2, normal))
{
if(request.num_max_contacts > result.numContacts())
result.addContact(Contact(model1, &model2, primitive_id, Contact::NONE,
p1, -normal, -distance));
assert (result.isCollision ());
return;
}
sqrDistLowerBound = distance * distance;
assert (distance > 0);
if (request.security_margin == 0) return;
if (distance <= request.security_margin) {
result.addContact(Contact(model1, &model2, primitive_id, Contact::NONE,
.5*(p1+p2), (p2-p1).normalized (), -distance));
}
}
}
/// @endcond
/// @brief Traversal node for mesh and shape, when mesh BVH is one of the oriented node (OBB, RSS, OBBRSS, kIOS)
template<typename S, typename NarrowPhaseSolver>
class MeshShapeCollisionTraversalNodeOBB : public MeshShapeCollisionTraversalNode<OBB, S, NarrowPhaseSolver, 0>
......@@ -375,10 +307,16 @@ public:
/// @brief Traversal node for collision between shape and mesh
template<typename S, typename BV, typename NarrowPhaseSolver>
template<typename S, typename BV, typename NarrowPhaseSolver,
int _Options = RelativeTransformationIsIdentity>
class ShapeMeshCollisionTraversalNode : public ShapeBVHCollisionTraversalNode<S, BV>
{
public:
enum {
Options = _Options,
RTIsIdentity = _Options & RelativeTransformationIsIdentity
};
ShapeMeshCollisionTraversalNode() : ShapeBVHCollisionTraversalNode<S, BV>()
{
vertices = NULL;
......@@ -387,8 +325,37 @@ public:
nsolver = NULL;
}
/// BV test between b1 and b2
/// \param b2 Bounding volumes to test,
bool BVTesting(int /*b1*/, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
if (RTIsIdentity)
return !this->model2->getBV(b2).bv.overlap(this->model1_bv);
else
return !overlap(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv);
}
/// BV test between b1 and b2
/// \param 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++;
bool res;
if (RTIsIdentity)
res = !this->model2->getBV(b2).bv.overlap(this->model1_bv, sqrDistLowerBound);
else
res = !overlap(this->tf2.getRotation(), this->tf2.getTranslation(),
this->model1_bv, this->model2->getBV(b2).bv,
sqrDistLowerBound);
assert (!res || sqrDistLowerBound > 0);
return res;
}
/// @brief Intersection testing between leaves (one shape and one triangle)
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++;
const BVNode<BV>& node = this->model2->getBV(b2);
......@@ -397,33 +364,47 @@ public:
const Triangle& tri_id = tri_indices[primitive_id];
const Vec3f& P1 = vertices[tri_id[0]];
const Vec3f& P2 = vertices[tri_id[1]];
const Vec3f& P3 = vertices[tri_id[2]];
const Vec3f& p1 = vertices[tri_id[0]];
const Vec3f& p2 = vertices[tri_id[1]];
const Vec3f& p3 = vertices[tri_id[2]];
FCL_REAL distance;
Vec3f normal;
Vec3f p1, p2; // closest points
Vec3f c1, c2; // closest points
if(nsolver->shapeTriangleInteraction(*(this->model1), this->tf1, P1, P2, P3,
Transform3f (), p1, p2, distance,
normal))
{
bool collision;
if (RTIsIdentity) {
static const Transform3f Id;
collision =
nsolver->shapeTriangleInteraction(*(this->model1), this->tf1, p1, p2, p3,
Id , c1, c2, distance, normal);
} else {
collision =
nsolver->shapeTriangleInteraction(*(this->model1), this->tf1, p1, p2, p3,
this->tf2, c1, c2, distance, normal);
}
if (collision) {
if(this->request.num_max_contacts > this->result->numContacts())
{
this->result->addContact
(Contact(this->model1, this->model2, primitive_id,
Contact::NONE, p1, -normal, -distance));
this->result->addContact (Contact(this->model1 , this->model2,
Contact::NONE, primitive_id,
c1, normal, -distance));
assert (this->result->isCollision ());
return;
}
}
sqrDistLowerBound = distance * distance;
assert (distance > 0);
if (this->request.security_margin == 0) return;
if (distance <= this->request.security_margin)
if ( this->request.security_margin == 0
&& distance <= this->request.security_margin)
{
this->result.addContact
(Contact(this->model1, this->model2, primitive_id, Contact::NONE,
.5 * (p1+p2), (p2-p1).normalized (), -distance));
this->result.addContact (Contact(this->model1 , this->model2,
Contact::NONE, primitive_id,
.5 * (c1+c2), (c2-c1).normalized (),
-distance));
}
assert (!this->result->isCollision () || sqrDistLowerBound > 0);
}
/// @brief Whether the traversal process can stop early
......@@ -440,118 +421,42 @@ public:
/// @brief Traversal node for shape and mesh, when mesh BVH is one of the oriented node (OBB, RSS, OBBRSS, kIOS)
template<typename S, typename NarrowPhaseSolver>
class ShapeMeshCollisionTraversalNodeOBB : public ShapeMeshCollisionTraversalNode<S, OBB, NarrowPhaseSolver>
class ShapeMeshCollisionTraversalNodeOBB : public ShapeMeshCollisionTraversalNode<S, OBB, NarrowPhaseSolver, 0>
{
public:
ShapeMeshCollisionTraversalNodeOBB() : ShapeMeshCollisionTraversalNode<S, OBB, NarrowPhaseSolver>()
{
}
bool BVTesting(int b1, int b2) 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);
}
void leafTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
{
details::meshShapeCollisionOrientedNodeLeafTesting
(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices,
this->tf2, this->tf1, this->nsolver, this->enable_statistics,
this->num_leaf_tests, this->request,*(this->request), sqrDistLowerBound);
// may need to change the order in pairs
}
};
template<typename S, typename NarrowPhaseSolver>
class ShapeMeshCollisionTraversalNodeRSS : public ShapeMeshCollisionTraversalNode<S, RSS, NarrowPhaseSolver>
class ShapeMeshCollisionTraversalNodeRSS : public ShapeMeshCollisionTraversalNode<S, RSS, NarrowPhaseSolver, 0>
{
public:
ShapeMeshCollisionTraversalNodeRSS() : ShapeMeshCollisionTraversalNode<S, RSS, NarrowPhaseSolver>()
{
}
bool BVTesting(int b1, int b2) 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);
}
void leafTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
{
details::meshShapeCollisionOrientedNodeLeafTesting
(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices,
this->tf2, this->tf1, this->nsolver, this->enable_statistics,
this->num_leaf_tests, this->request, *(this->request));
// may need to change the order in pairs
}
};
template<typename S, typename NarrowPhaseSolver>
class ShapeMeshCollisionTraversalNodekIOS : public ShapeMeshCollisionTraversalNode<S, kIOS, NarrowPhaseSolver>
class ShapeMeshCollisionTraversalNodekIOS : public ShapeMeshCollisionTraversalNode<S, kIOS, NarrowPhaseSolver, 0>
{
public:
ShapeMeshCollisionTraversalNodekIOS() : ShapeMeshCollisionTraversalNode<S, kIOS, NarrowPhaseSolver>()
{
}
bool BVTesting(int b1, int b2) 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);
}
void leafTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
{
details::meshShapeCollisionOrientedNodeLeafTesting
(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices,
this->tf2, this->tf1, this->nsolver, this->enable_statistics,
this->num_leaf_tests, this->request,*(this->request), sqrDistLowerBound);
// may need to change the order in pairs
}
};
template<typename S, typename NarrowPhaseSolver>
class ShapeMeshCollisionTraversalNodeOBBRSS : public ShapeMeshCollisionTraversalNode<S, OBBRSS, NarrowPhaseSolver>
class ShapeMeshCollisionTraversalNodeOBBRSS : public ShapeMeshCollisionTraversalNode<S, OBBRSS, NarrowPhaseSolver, 0>
{
public:
ShapeMeshCollisionTraversalNodeOBBRSS() : ShapeMeshCollisionTraversalNode<S, OBBRSS, NarrowPhaseSolver>()
{
}
bool BVTesting(int b1, int b2) 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);
}
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, FCL_REAL& sqrDistLowerBound) const
{
details::meshShapeCollisionOrientedNodeLeafTesting
(b2, b1, *(this->model2), this->model1, this->vertices,
this->tri_indices, this->tf2, this->tf1, this->nsolver,
this->enable_statistics, this->num_leaf_tests,
this->request, *(this->request), sqrDistLowerBound);
// may need to change the order in pairs
}
};
/// @brief Traversal node for distance computation between BVH and shape
......
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