diff --git a/CMakeLists.txt b/CMakeLists.txt
index 33b6335a344d944fd2b64d1853c62c12e36ede37..7af219281a1e2a138dc696ee4234fb1de6db3619 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/BV/AABB.h b/include/hpp/fcl/BV/AABB.h
index b875b3c869016b12708968af98169204eecf7a6a..0b93c48a0ab6829d21abf18557526aa2f1d442b5 100644
--- a/include/hpp/fcl/BV/AABB.h
+++ b/include/hpp/fcl/BV/AABB.h
@@ -262,6 +262,14 @@ static inline AABB rotate(const AABB& aabb, const Matrix3f& t)
return res;
}
+/// @brief Check collision between two aabbs, b1 is in configuration (R0, T0) and b2 is in identity.
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, const AABB& b2);
+
+/// @brief Check collision between two aabbs, b1 is in configuration (R0, T0) and b2 is in identity.
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1,
+ const AABB& b2, const CollisionRequest& request,
+ FCL_REAL& sqrDistLowerBound);
+
}
} // namespace hpp
diff --git a/include/hpp/fcl/BV/RSS.h b/include/hpp/fcl/BV/RSS.h
index 3b33b28023ef9893ad70fae4e01d2ee3b77d3273..6822e97e42abb8ab1186e1e78a0fd3ed74bba667 100644
--- a/include/hpp/fcl/BV/RSS.h
+++ b/include/hpp/fcl/BV/RSS.h
@@ -155,6 +155,10 @@ FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS&
/// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and b2 is in identity.
bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2);
+/// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and b2 is in identity.
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2,
+ const CollisionRequest& request,
+ FCL_REAL& sqrDistLowerBound);
}
diff --git a/include/hpp/fcl/BV/kDOP.h b/include/hpp/fcl/BV/kDOP.h
index 72be7f89fa9dfaf9d0a0c2652f53ec5cbf553812..da5901624e1f4f8f8e5d380b024daaf36d7c8f26 100644
--- a/include/hpp/fcl/BV/kDOP.h
+++ b/include/hpp/fcl/BV/kDOP.h
@@ -39,7 +39,7 @@
#define HPP_FCL_KDOP_H
#include <stdexcept>
-#include <hpp/fcl/math/vec_3f.h>
+#include <hpp/fcl/math/matrix_3f.h>
namespace hpp
{
@@ -176,6 +176,20 @@ public:
};
+template<size_t N>
+bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/,
+ const KDOP<N>& /*b1*/, const KDOP<N>& /*b2*/)
+{
+ throw std::logic_error ("not implemented");
+}
+
+template<size_t N>
+bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/,
+ const KDOP<N>& /*b1*/, const KDOP<N>& /*b2*/,
+ const CollisionRequest& /*request*/, FCL_REAL& /*sqrDistLowerBound*/)
+{
+ throw std::logic_error ("not implemented");
+}
/// @brief translate the KDOP BV
template<size_t N>
diff --git a/include/hpp/fcl/BV/kIOS.h b/include/hpp/fcl/BV/kIOS.h
index 64550f3885322617f8512ff827227e2c1bed2572..a7228168d3d3ffcbf45defdeb6acf6d850037fb7 100644
--- a/include/hpp/fcl/BV/kIOS.h
+++ b/include/hpp/fcl/BV/kIOS.h
@@ -157,6 +157,12 @@ kIOS translate(const kIOS& bv, const Vec3f& t);
/// @todo Not efficient
bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2);
+/// @brief Check collision between two kIOSs, b1 is in configuration (R0, T0) and b2 is in identity.
+/// @todo Not efficient
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2,
+ const CollisionRequest& request,
+ FCL_REAL& sqrDistLowerBound);
+
/// @brief Approximate distance between two kIOS bounding volumes
/// @todo P and Q is not returned, need implementation
FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2, Vec3f* P = NULL, Vec3f* Q = NULL);
diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h
index 1c0d831e5f1d4da24f9bedf54e6a0521d5eae283..a85f7fae3131b7333d88401a4168af161bdaf42d 100644
--- a/include/hpp/fcl/narrowphase/narrowphase.h
+++ b/include/hpp/fcl/narrowphase/narrowphase.h
@@ -101,6 +101,7 @@ namespace fcl
}
//// @brief intersection checking between one shape and a triangle with transformation
+ /// \return true if the shape are colliding.
template<typename S>
bool shapeTriangleInteraction
(const S& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2,
diff --git a/include/hpp/fcl/traversal/details/traversal.h b/include/hpp/fcl/traversal/details/traversal.h
new file mode 100644
index 0000000000000000000000000000000000000000..2979a601f169303b8f3e6cf5b0a010c18d0e43fa
--- /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_base.h b/include/hpp/fcl/traversal/traversal_node_base.h
index 81e1d61c8e6f6419c34787a4c6a92def8b5d4699..918505de5e55d4f0678e8dad64603c96378559ba 100644
--- a/include/hpp/fcl/traversal/traversal_node_base.h
+++ b/include/hpp/fcl/traversal/traversal_node_base.h
@@ -137,6 +137,8 @@ public:
virtual ~DistanceTraversalNodeBase();
/// @brief BV test between b1 and b2
+ /// \return a lower bound of the distance between the two BV.
+ /// \note except for OBB, this method returns the distance.
virtual FCL_REAL BVTesting(int b1, int b2) const;
/// @brief Leaf test between node b1 and b2, if they are both leafs
diff --git a/include/hpp/fcl/traversal/traversal_node_bvh_shape.h b/include/hpp/fcl/traversal/traversal_node_bvh_shape.h
index 738d63c58dd1d3e0c31c140443d10c2c5a9670d2..27ec3f9afed15285cdb311fff127bab9028e824e 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;
@@ -152,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;
@@ -181,10 +146,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 +165,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++;
+ if (RTIsIdentity)
+ return !this->model1->getBV(b1).bv.overlap(this->model2_bv);
+ else
+ return !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv);
+ }
+
+ /// 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++;
+ bool res;
+ if (RTIsIdentity)
+ res = !this->model1->getBV(b1).bv.overlap(this->model2_bv, this->request, sqrDistLowerBound);
+ else
+ 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;
+ }
+
/// @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,27 +210,41 @@ public:
FCL_REAL distance;
Vec3f normal;
- Vec3f c1, c2;
+ Vec3f c1, c2; // closest point
+
+ bool collision;
+ if (RTIsIdentity) {
+ static const Transform3f Id;
+ collision =
+ nsolver->shapeTriangleInteraction(*(this->model2), this->tf2, p1, p2, p3,
+ Id , distance, c2, c1, normal);
+ } else {
+ collision =
+ nsolver->shapeTriangleInteraction(*(this->model2), this->tf2, p1, p2, p3,
+ this->tf1, distance, c2, c1, 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));
- return;
+ assert (this->result->isCollision ());
+ return;
+ }
}
+ sqrDistLowerBound = distance * distance;
assert (distance > 0);
- if (this->request.security_margin > 0) {
- if (distance <= this->request.security_margin) {
- this->result->addContact(Contact(this->model1, this->model2,
- primitive_id, Contact::NONE,
- .5 * (c1+c2), (c2-c1).normalized (),
- -distance));
- }
+ if ( this->request.security_margin > 0
+ && distance <= this->request.security_margin)
+ {
+ this->result->addContact(Contact(this->model1, this->model2,
+ primitive_id, Contact::NONE,
+ .5 * (c1+c2), (c2-c1).normalized (),
+ -distance));
}
+ assert (!this->result->isCollision () || sqrDistLowerBound > 0);
}
/// @brief Whether the traversal process can stop early
@@ -241,191 +255,68 @@ public:
Vec3f* vertices;
Triangle* tri_indices;
-
+
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>
+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);
}
-
};
/// @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;
@@ -434,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);
@@ -444,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
+
+ 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(nsolver->shapeTriangleInteraction(*(this->model1), this->tf1, P1, P2, P3,
- Transform3f (), p1, p2, 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
@@ -487,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
diff --git a/include/hpp/fcl/traversal/traversal_node_bvhs.h b/include/hpp/fcl/traversal/traversal_node_bvhs.h
index 972955e0f6f74ef0d69ba6509f77c82ccdf24228..985aa90d3f7c9e84fcec17d0d121d4b657f6379b 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>
@@ -124,24 +125,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 +137,16 @@ public:
mutable FCL_REAL query_time_seconds;
};
-
/// @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 +156,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 +259,41 @@ 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;
-};
-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;
+ details::RelativeTransformation<!bool(RTIsIdentity)> RT;
};
-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;
-};
+/// @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;
-class MeshCollisionTraversalNodeOBBRSS : public MeshCollisionTraversalNode<OBBRSS>
+namespace details
{
-public:
- MeshCollisionTraversalNodeOBBRSS (const CollisionRequest& request);
-
- bool BVTesting(int b1, int b2) const;
-
- bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const;
+ template<typename BV> struct DistanceTraversalBVTesting_impl
+ {
+ static FCL_REAL run(const BVNode<BV>& b1, const BVNode<BV>& b2)
+ {
+ return b1.distance(b2);
+ }
+ };
- Matrix3f R;
- Vec3f T;
-};
+ template<> struct DistanceTraversalBVTesting_impl<OBB>
+ {
+ static FCL_REAL run(const BVNode<OBB>& b1, const BVNode<OBB>& b2)
+ {
+ FCL_REAL sqrDistLowerBound;
+ CollisionRequest request (DISTANCE_LOWER_BOUND, 0);
+ // request.break_distance = ?
+ if (b1.overlap(b2, request, sqrDistLowerBound)) {
+ // TODO A penetration upper bound should be computed.
+ return -1;
+ }
+ return sqrt (sqrDistLowerBound);
+ }
+ };
+} // namespace details
/// @brief Traversal node for distance computation between BVH models
template<typename BV>
@@ -367,7 +364,8 @@ public:
FCL_REAL BVTesting(int b1, int b2) const
{
if(enable_statistics) num_bv_tests++;
- return model1->getBV(b1).distance(model2->getBV(b2));
+ return details::DistanceTraversalBVTesting_impl<BV>
+ ::run (model1->getBV(b1), model2->getBV(b2));
}
/// @brief The first BVH model
diff --git a/src/BV/AABB.cpp b/src/BV/AABB.cpp
index e65bee4bb899c521a70ec1b4bce45b6ea1a389af..782e0bc16b778e3a31ad3b92acdd95a1671679a6 100644
--- a/src/BV/AABB.cpp
+++ b/src/BV/AABB.cpp
@@ -128,6 +128,20 @@ FCL_REAL AABB::distance(const AABB& other) const
return std::sqrt(result);
}
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, const AABB& b2)
+{
+ AABB bb1 (translate (rotate (b1, R0), T0));
+ return bb1.overlap (b2);
+}
+
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1,
+ const AABB& b2, const CollisionRequest& request,
+ FCL_REAL& sqrDistLowerBound)
+{
+ AABB bb1 (translate (rotate (b1, R0), T0));
+ return bb1.overlap (b2, request, sqrDistLowerBound);
+}
+
}
} // namespace hpp
diff --git a/src/BV/RSS.cpp b/src/BV/RSS.cpp
index 7ef7cc7b75e9f979fff6ff1d5c7491fc8b5354ff..0f719bcebdad1f71a2bcbfe2b78005dd164842a9 100644
--- a/src/BV/RSS.cpp
+++ b/src/BV/RSS.cpp
@@ -862,6 +862,25 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2)
return (dist <= (b1.r + b2.r));
}
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2,
+ const CollisionRequest& /*request*/,
+ FCL_REAL& sqrDistLowerBound)
+{
+ // ROb2 = R0 . b2
+ // where b2 = [ b2.axis [0] | b2.axis [1] | b2.axis [2] ]
+
+ // (1 0 0)^T R0b2^T axis [0] = (1 0 0)^T b2^T R0^T axis [0]
+ // R = b2^T RO^T b1
+ Vec3f Ttemp (R0 * b2.Tr + T0 - b1.Tr);
+ Vec3f T(b1.axes.transpose() * Ttemp);
+ Matrix3f R(b1.axes.transpose() * R0 * b2.axes);
+
+ FCL_REAL dist = rectDistance(R, T, b1.l, b2.l) - b1.r - b2.r;
+ if (dist <= 0) return true;
+ sqrDistLowerBound = dist * dist;
+ return false;
+}
+
bool RSS::contain(const Vec3f& p) const
{
Vec3f local_p = p - Tr;
diff --git a/src/BV/kIOS.cpp b/src/BV/kIOS.cpp
index e6c267a8b6df9e55e801d807cf0856c3246a022b..010966e3b0580c7fce54e34774d1ebb38fa9c51c 100644
--- a/src/BV/kIOS.cpp
+++ b/src/BV/kIOS.cpp
@@ -61,17 +61,27 @@ bool kIOS::overlap(const kIOS& other) const
}
return obb.overlap(other.obb);
-
- return true;
}
- bool kIOS::overlap(const kIOS& other, const CollisionRequest&,
- FCL_REAL& sqrDistLowerBound) const
+bool kIOS::overlap(const kIOS& other, const CollisionRequest& request,
+ FCL_REAL& sqrDistLowerBound) const
+{
+ for(unsigned int i = 0; i < num_spheres; ++i)
{
- sqrDistLowerBound = sqrt (-1);
- return overlap (other);
+ for(unsigned int j = 0; j < other.num_spheres; ++j)
+ {
+ FCL_REAL o_dist = (spheres[i].o - other.spheres[j].o).squaredNorm();
+ FCL_REAL sum_r = spheres[i].r + other.spheres[j].r;
+ if(o_dist > sum_r * sum_r) {
+ o_dist = sqrt(o_dist) - sum_r;
+ sqrDistLowerBound = o_dist*o_dist;
+ return false;
+ }
+ }
}
+ return obb.overlap(other.obb, request, sqrDistLowerBound);
+}
bool kIOS::contain(const Vec3f& p) const
{
@@ -192,6 +202,23 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2
return b1.overlap(b2_temp);
}
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2,
+ const CollisionRequest& request,
+ FCL_REAL& sqrDistLowerBound)
+{
+ kIOS b2_temp = b2;
+ for(unsigned int i = 0; i < b2_temp.num_spheres; ++i)
+ {
+ b2_temp.spheres[i].o = R0 * b2_temp.spheres[i].o + T0;
+ }
+
+
+ b2_temp.obb.To = R0 * b2_temp.obb.To + T0;
+ b2_temp.obb.axes.applyOnTheLeft(R0);
+
+ return b1.overlap(b2_temp, request, sqrDistLowerBound);
+}
+
FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2, Vec3f* P, Vec3f* Q)
{
kIOS b2_temp = b2;
diff --git a/src/distance_func_matrix.cpp b/src/distance_func_matrix.cpp
index c83148c07e5bc412692f9eef213e3f9391d78c3b..423d8d62e52306a70bf19559e5a5020479ee8c30 100644
--- a/src/distance_func_matrix.cpp
+++ b/src/distance_func_matrix.cpp
@@ -379,6 +379,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
distance_matrix[BV_AABB][GEOM_CONVEX] = &BVHShapeDistancer<AABB, Convex, NarrowPhaseSolver>::distance;
distance_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeDistancer<AABB, Plane, NarrowPhaseSolver>::distance;
distance_matrix[BV_AABB][GEOM_HALFSPACE] = &BVHShapeDistancer<AABB, Halfspace, NarrowPhaseSolver>::distance;
+ */
distance_matrix[BV_OBB][GEOM_BOX] = &BVHShapeDistancer<OBB, Box, NarrowPhaseSolver>::distance;
distance_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeDistancer<OBB, Sphere, NarrowPhaseSolver>::distance;
@@ -388,7 +389,6 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
distance_matrix[BV_OBB][GEOM_CONVEX] = &BVHShapeDistancer<OBB, Convex, NarrowPhaseSolver>::distance;
distance_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeDistancer<OBB, Plane, NarrowPhaseSolver>::distance;
distance_matrix[BV_OBB][GEOM_HALFSPACE] = &BVHShapeDistancer<OBB, Halfspace, NarrowPhaseSolver>::distance;
- */
distance_matrix[BV_RSS][GEOM_BOX] = &BVHShapeDistancer<RSS, Box, NarrowPhaseSolver>::distance;
distance_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeDistancer<RSS, Sphere, NarrowPhaseSolver>::distance;
@@ -448,6 +448,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
distance_matrix[BV_OBBRSS][GEOM_HALFSPACE] = &BVHShapeDistancer<OBBRSS, Halfspace, NarrowPhaseSolver>::distance;
distance_matrix[BV_AABB][BV_AABB] = &BVHDistance<AABB, NarrowPhaseSolver>;
+ distance_matrix[BV_OBB][BV_OBB] = &BVHDistance<OBB, NarrowPhaseSolver>;
distance_matrix[BV_RSS][BV_RSS] = &BVHDistance<RSS, NarrowPhaseSolver>;
distance_matrix[BV_kIOS][BV_kIOS] = &BVHDistance<kIOS, NarrowPhaseSolver>;
distance_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHDistance<OBBRSS, NarrowPhaseSolver>;
diff --git a/src/traversal/traversal_node_bvhs.cpp b/src/traversal/traversal_node_bvhs.cpp
index d241780e58bd83bf0a4ecc8fe6832dbdb5f8480a..5fe97fa5a0eaf8b892e77971b24ca2854e8b02d2 100644
--- a/src/traversal/traversal_node_bvhs.cpp
+++ b/src/traversal/traversal_node_bvhs.cpp
@@ -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
{
diff --git a/src/traversal/traversal_node_setup.cpp b/src/traversal/traversal_node_setup.cpp
index 321ca128f8f2b6028b7b14981f5b59c1288b8169..7275286062ab008bbba9dcb3abbb7f5d3053bb87 100644
--- a/src/traversal/traversal_node_setup.cpp
+++ b/src/traversal/traversal_node_setup.cpp
@@ -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;
}
diff --git a/test/benchmark.cpp b/test/benchmark.cpp
index 4ecf04e29dfc3c4a64df5690c8eb90ced11efd4f..28b718c5df33032ccd153c1b949ac7583ec518d4 100644
--- a/test/benchmark.cpp
+++ b/test/benchmark.cpp
@@ -127,6 +127,7 @@ double collide (const std::vector<Transform3f>& tf,
for (std::size_t i = 0; i < tf.size(); ++i) {
bool success (initialize(node, m1, tf[i], m2, pose2, local_result));
+ (void)success;
assert (success);
CollisionResult result;
diff --git a/test/test_fcl_distance.cpp b/test/test_fcl_distance.cpp
index 192a9508120dabf0dd6eacd146477c1b484246ed..35eb58408d061fbafbed6f6f8ff60ad9ffebfae8 100644
--- a/test/test_fcl_distance.cpp
+++ b/test/test_fcl_distance.cpp
@@ -198,7 +198,36 @@ BOOST_AUTO_TEST_CASE(mesh_distance)
BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+ distance_Test<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose);
+ BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
+ BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+
+ distance_Test<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose);
+
+ BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
+ BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+
+ distance_Test<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose);
+
+ BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
+ BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+
+ distance_Test<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose);
+
+ BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
+ BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+
+ distance_Test<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose);
+
+ BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
+ BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+
+ distance_Test<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose);
+
+ BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
+ BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+
distance_Test<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose);
BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
diff --git a/test/test_fcl_utility.cpp b/test/test_fcl_utility.cpp
index b60b015d9d4a07c12d606490dff2ac3760290135..9a22a9dffeac0c854e64a551ed151c1529a71e98 100644
--- a/test/test_fcl_utility.cpp
+++ b/test/test_fcl_utility.cpp
@@ -68,8 +68,8 @@ double Timer::getElapsedTimeInMicroSec()
if(!stopped)
gettimeofday(&endCount, NULL);
- startTimeInMicroSec = (startCount.tv_sec * 1000000.0) + startCount.tv_usec;
- endTimeInMicroSec = (endCount.tv_sec * 1000000.0) + endCount.tv_usec;
+ startTimeInMicroSec = ((double)startCount.tv_sec * 1000000.0) + (double)startCount.tv_usec;
+ endTimeInMicroSec = ((double)endCount.tv_sec * 1000000.0) + (double)endCount.tv_usec;
#endif
return endTimeInMicroSec - startTimeInMicroSec;
@@ -333,8 +333,8 @@ void generateRandomTransforms(FCL_REAL extents[6], FCL_REAL delta_trans[3], FCL_
}
void generateRandomTransform_ccd(FCL_REAL extents[6], std::vector<Transform3f>& transforms, std::vector<Transform3f>& transforms2, FCL_REAL delta_trans[3], FCL_REAL delta_rot, std::size_t n,
- const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
- const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2)
+ const std::vector<Vec3f>& /*vertices1*/, const std::vector<Triangle>& /*riangles1*/,
+ const std::vector<Vec3f>& /*vertices2*/, const std::vector<Triangle>& /*riangles2*/)
{
transforms.resize(n);
transforms2.resize(n);