From 358e5fca05def769192b17f0341af69de64d80ba Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Thu, 13 Jun 2019 17:43:20 +0200
Subject: [PATCH] Remove duplication of MeshShapeCollisionTraversalNode
---
CMakeLists.txt | 1 +
include/hpp/fcl/traversal/details/traversal.h | 76 ++++++++
.../fcl/traversal/traversal_node_bvh_shape.h | 179 +++++++-----------
.../hpp/fcl/traversal/traversal_node_bvhs.h | 28 +--
4 files changed, 143 insertions(+), 141 deletions(-)
create mode 100644 include/hpp/fcl/traversal/details/traversal.h
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 33b6335a..7af21928 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -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
diff --git a/include/hpp/fcl/traversal/details/traversal.h b/include/hpp/fcl/traversal/details/traversal.h
new file mode 100644
index 00000000..2979a601
--- /dev/null
+++ b/include/hpp/fcl/traversal/details/traversal.h
@@ -0,0 +1,76 @@
+/*
+ * 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
diff --git a/include/hpp/fcl/traversal/traversal_node_bvh_shape.h b/include/hpp/fcl/traversal/traversal_node_bvh_shape.h
index 738d63c5..b919ac08 100644
--- a/include/hpp/fcl/traversal/traversal_node_bvh_shape.h
+++ b/include/hpp/fcl/traversal/traversal_node_bvh_shape.h
@@ -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);
}
-
};
diff --git a/include/hpp/fcl/traversal/traversal_node_bvhs.h b/include/hpp/fcl/traversal/traversal_node_bvhs.h
index 71dafc1f..985aa90d 100644
--- a/include/hpp/fcl/traversal/traversal_node_bvhs.h
+++ b/include/hpp/fcl/traversal/traversal_node_bvhs.h
@@ -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 ;
--
GitLab