Skip to content

GitLab

Commit 358e5fca authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Remove duplication of MeshShapeCollisionTraversalNode

parent 5e219fd2
Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
View file @ 358e5fca
......@@ -130,6 +130,7 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/fcl/math/vec_3f.h
include/hpp/fcl/math/tools.h
include/hpp/fcl/math/transform.h
include/hpp/fcl/traversal/details/traversal.h
include/hpp/fcl/traversal/traversal_node_shapes.h
include/hpp/fcl/traversal/traversal_node_setup.h
include/hpp/fcl/traversal/traversal_recurse.h
......
include/hpp/fcl/traversal/details/traversal.h 0 → 100644
View file @ 358e5fca
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2019, LAAS CNRS
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/** \author Joseph Mirabel */
#ifndef HPP_FCL_TRAVERSAL_DETAILS_TRAVERSAL_H
#define HPP_FCL_TRAVERSAL_DETAILS_TRAVERSAL_H
namespace hpp
{
namespace fcl
{
enum {
RelativeTransformationIsIdentity = 1
};
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
}
} // namespace hpp
#endif
include/hpp/fcl/traversal/traversal_node_bvh_shape.h
View file @ 358e5fca
......@@ -43,6 +43,7 @@
#include <hpp/fcl/shape/geometric_shapes.h>
#include <hpp/fcl/shape/geometric_shapes_utility.h>
#include <hpp/fcl/traversal/traversal_node_base.h>
#include <hpp/fcl/traversal/details/traversal.h>
#include <hpp/fcl/BVH/BVH_model.h>
......@@ -85,24 +86,6 @@ public:
return model1->getBV(b).rightChild();
}
/// @brief BV culling test in one BVTT node
bool BVTesting(int b1, int /*b2*/) const
{
if(this->enable_statistics) num_bv_tests++;
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, request, sqrDistLowerBound);
}
const BVHModel<BV>* model1;
const S* model2;
BV model2_bv;
......@@ -181,10 +164,16 @@ public:
/// @brief Traversal node for collision between mesh and shape
template<typename BV, typename S, typename NarrowPhaseSolver>
template<typename BV, typename S, typename NarrowPhaseSolver,
int _Options = RelativeTransformationIsIdentity>
class MeshShapeCollisionTraversalNode : public BVHShapeCollisionTraversalNode<BV, S>
{
public:
enum {
Options = _Options,
RTIsIdentity = _Options & RelativeTransformationIsIdentity
};
MeshShapeCollisionTraversalNode(const CollisionRequest& request) :
BVHShapeCollisionTraversalNode<BV, S> (request)
{
......@@ -194,8 +183,37 @@ public:
nsolver = NULL;
}
/// @brief BV culling test in one BVTT node
bool BVTesting(int b1, int /*b2*/) const
{
if(this->enable_statistics) this->num_bv_tests++;
bool res;
if (RTIsIdentity)
res = !this->model1->getBV(b1).bv.overlap(this->model2_bv);
else
res = !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv);
assert (!res || sqrDistLowerBound > 0);
return res;
}
/// test between BV b1 and shape
/// \param b1 BV 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) this->num_bv_tests++;
if (RTIsIdentity)
return !this->model1->getBV(b1).bv.overlap(this->model2_bv, this->request, sqrDistLowerBound);
else
return !overlap(this->tf1.getRotation(), this->tf1.getTranslation(),
this->model2_bv, this->model1->getBV(b1).bv,
this->request, sqrDistLowerBound);
}
/// @brief Intersection testing between leaves (one triangle and one shape)
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->model1->getBV(b1);
......@@ -210,18 +228,29 @@ public:
FCL_REAL distance;
Vec3f normal;
Vec3f c1, c2;
Vec3f c1, c2; // closest point
bool collision;
if (RTIsIdentity) {
collision =
nsolver->shapeTriangleInteraction(*(this->model2), this->tf2, p1, p2, p3,
Transform3f (), distance, c1, c2,
normal);
} else {
collision =
nsolver->shapeTriangleInteraction(*(this->model2), this->tf2, p1, p2, p3,
this->tf1 , distance, c1, c2, normal);
}
if(nsolver->shapeTriangleInteraction(*(this->model2), this->tf2, p1, p2, p3,
Transform3f (), distance, c1, c2,
normal))
{
if(collision) {
if(this->request.num_max_contacts > this->result->numContacts())
this->result->addContact(Contact(this->model1, this->model2,
primitive_id, Contact::NONE,
c1, -normal, -distance));
assert (this->result->isCollision ());
return;
}
sqrDistLowerBound = distance * distance;
assert (distance > 0);
if (this->request.security_margin > 0) {
if (distance <= this->request.security_margin) {
......@@ -231,6 +260,7 @@ public:
-distance));
}
}
assert (!this->result->isCollision () || sqrDistLowerBound > 0);
}
/// @brief Whether the traversal process can stop early
......@@ -297,127 +327,48 @@ static inline void meshShapeCollisionOrientedNodeLeafTesting
/// @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>
class MeshShapeCollisionTraversalNodeOBB : public MeshShapeCollisionTraversalNode<OBB, S, NarrowPhaseSolver, 0>
{
public:
MeshShapeCollisionTraversalNodeOBB(const CollisionRequest& request) :
MeshShapeCollisionTraversalNode<OBB, S, NarrowPhaseSolver>
MeshShapeCollisionTraversalNode<OBB, S, NarrowPhaseSolver, 0>
(request)
{
}
bool BVTesting(int b1, int /*b2*/) 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);
}
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,
this->request, sqrDistLowerBound);
}
void leafTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
{
details::meshShapeCollisionOrientedNodeLeafTesting
(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
this->tf1, this->tf2, this->nsolver, this->enable_statistics,
this->num_leaf_tests, this->request, *(this->result), sqrDistLowerBound);
}
};
template<typename S, typename NarrowPhaseSolver>
class MeshShapeCollisionTraversalNodeRSS : public MeshShapeCollisionTraversalNode<RSS, S, NarrowPhaseSolver>
class MeshShapeCollisionTraversalNodeRSS : public MeshShapeCollisionTraversalNode<RSS, S, NarrowPhaseSolver, 0>
{
public:
MeshShapeCollisionTraversalNodeRSS (const CollisionRequest& request):
MeshShapeCollisionTraversalNode<RSS, S, NarrowPhaseSolver>
MeshShapeCollisionTraversalNode<RSS, S, NarrowPhaseSolver, 0>
(request)
{
}
bool BVTesting(int b1, int /*b2*/) 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);
}
void leafTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
{
details::meshShapeCollisionOrientedNodeLeafTesting
(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
this->tf1, this->tf2, this->nsolver, this->enable_statistics,
this->num_leaf_tests, this->request, *(this->result), sqrDistLowerBound);
}
};
template<typename S, typename NarrowPhaseSolver>
class MeshShapeCollisionTraversalNodekIOS : public MeshShapeCollisionTraversalNode<kIOS, S, NarrowPhaseSolver>
class MeshShapeCollisionTraversalNodekIOS : public MeshShapeCollisionTraversalNode<kIOS, S, NarrowPhaseSolver, 0>
{
public:
MeshShapeCollisionTraversalNodekIOS(const CollisionRequest& request):
MeshShapeCollisionTraversalNode<kIOS, S, NarrowPhaseSolver>
MeshShapeCollisionTraversalNode<kIOS, S, NarrowPhaseSolver, 0>
(request)
{
}
bool BVTesting(int b1, int /*b2*/) 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);
}
void leafTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
{
details::meshShapeCollisionOrientedNodeLeafTesting
(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
this->tf1, this->tf2, this->nsolver, this->enable_statistics,
this->num_leaf_tests, this->request, *(this->result), sqrDistLowerBound);
}
};
template<typename S, typename NarrowPhaseSolver>
class MeshShapeCollisionTraversalNodeOBBRSS : public MeshShapeCollisionTraversalNode<OBBRSS, S, NarrowPhaseSolver>
class MeshShapeCollisionTraversalNodeOBBRSS : public MeshShapeCollisionTraversalNode<OBBRSS, S, NarrowPhaseSolver, 0>
{
public:
MeshShapeCollisionTraversalNodeOBBRSS
(const CollisionRequest& request) :
MeshShapeCollisionTraversalNode
<OBBRSS, S, NarrowPhaseSolver>(request)
{
}
bool BVTesting(int b1, int /*b2*/) 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);
}
bool BVTesting(int b1, int /*b2*/, FCL_REAL& sqrDistLowerBound) const
{
if(this->enable_statistics) this->num_bv_tests++;
bool res (!overlap(this->tf1.getRotation(), this->tf1.getTranslation(),
this->model2_bv, this->model1->getBV(b1).bv,
this->request, sqrDistLowerBound));
assert (!res || sqrDistLowerBound > 0);
return res;
}
void leafTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
MeshShapeCollisionTraversalNodeOBBRSS (const CollisionRequest& request) :
MeshShapeCollisionTraversalNode <OBBRSS, S, NarrowPhaseSolver, 0>
(request)
{
details::meshShapeCollisionOrientedNodeLeafTesting
(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
this->tf1, this->tf2, this->nsolver, this->enable_statistics,
this->num_leaf_tests, this->request, *(this->result), sqrDistLowerBound);
assert (this->result->isCollision () || sqrDistLowerBound > 0);
}
};
......
include/hpp/fcl/traversal/traversal_node_bvhs.h
View file @ 358e5fca
......@@ -47,6 +47,7 @@
#include <hpp/fcl/intersect.h>
#include <hpp/fcl/shape/geometric_shapes.h>
#include <hpp/fcl/narrowphase/narrowphase.h>
#include <hpp/fcl/traversal/details/traversal.h>
#include <boost/shared_array.hpp>
#include <boost/shared_ptr.hpp>
......@@ -59,10 +60,6 @@ namespace hpp
{
namespace fcl
{
enum {
RelativeTransformationIsIdentity = 1
};
/// @brief Traversal node for collision between BVH models
template<typename BV>
class BVHCollisionTraversalNode : public CollisionTraversalNodeBase
......@@ -140,28 +137,6 @@ 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, int _Options = RelativeTransformationIsIdentity>
class MeshCollisionTraversalNode : public BVHCollisionTraversalNode<BV>
......@@ -288,7 +263,6 @@ public:
details::RelativeTransformation<!bool(RTIsIdentity)> RT;
};
/// @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 ;
......
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