diff --git a/include/hpp/fcl/internal/traversal_node_bvhs.h b/include/hpp/fcl/internal/traversal_node_bvhs.h
index ed251b832357a59cc260229c818f2130101286b7..39f3abff8479bef76e5b145f797a36df3e2f4481 100644
--- a/include/hpp/fcl/internal/traversal_node_bvhs.h
+++ b/include/hpp/fcl/internal/traversal_node_bvhs.h
@@ -283,6 +283,10 @@ namespace details
{
return b1.distance(b2);
}
+ static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode<BV>& b1, const BVNode<BV>& b2)
+ {
+ return distance(R, T, b1.bv, b2.bv);
+ }
};
template<> struct DistanceTraversalBVDistanceLowerBound_impl<OBB>
@@ -298,6 +302,43 @@ namespace details
}
return sqrt (sqrDistLowerBound);
}
+ static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode<OBB>& b1, const BVNode<OBB>& b2)
+ {
+ FCL_REAL sqrDistLowerBound;
+ CollisionRequest request (DISTANCE_LOWER_BOUND, 0);
+ // request.break_distance = ?
+ if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) {
+ // TODO A penetration upper bound should be computed.
+ return -1;
+ }
+ return sqrt (sqrDistLowerBound);
+ }
+ };
+
+ template<> struct DistanceTraversalBVDistanceLowerBound_impl<AABB>
+ {
+ static FCL_REAL run(const BVNode<AABB>& b1, const BVNode<AABB>& 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);
+ }
+ static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode<AABB>& b1, const BVNode<AABB>& b2)
+ {
+ FCL_REAL sqrDistLowerBound;
+ CollisionRequest request (DISTANCE_LOWER_BOUND, 0);
+ // request.break_distance = ?
+ if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) {
+ // TODO A penetration upper bound should be computed.
+ return -1;
+ }
+ return sqrt (sqrDistLowerBound);
+ }
};
} // namespace details
@@ -369,14 +410,6 @@ public:
return model2->getBV(b).rightChild();
}
- /// @brief BV culling test in one BVTT node
- FCL_REAL BVDistanceLowerBound(int b1, int b2) const
- {
- if(enable_statistics) num_bv_tests++;
- return details::DistanceTraversalBVDistanceLowerBound_impl<BV>
- ::run (model1->getBV(b1), model2->getBV(b2));
- }
-
/// @brief The first BVH model
const BVHModel<BV>* model1;
/// @brief The second BVH model
@@ -390,10 +423,24 @@ public:
/// @brief Traversal node for distance computation between two meshes
-template<typename BV>
+template<typename BV, int _Options = RelativeTransformationIsIdentity>
class MeshDistanceTraversalNode : public BVHDistanceTraversalNode<BV>
{
public:
+ enum {
+ Options = _Options,
+ RTIsIdentity = _Options & RelativeTransformationIsIdentity
+ };
+
+ using BVHDistanceTraversalNode<BV>::enable_statistics;
+ using BVHDistanceTraversalNode<BV>::request;
+ using BVHDistanceTraversalNode<BV>::result;
+ using BVHDistanceTraversalNode<BV>::tf1;
+ using BVHDistanceTraversalNode<BV>::model1;
+ using BVHDistanceTraversalNode<BV>::model2;
+ using BVHDistanceTraversalNode<BV>::num_bv_tests;
+ using BVHDistanceTraversalNode<BV>::num_leaf_tests;
+
MeshDistanceTraversalNode() : BVHDistanceTraversalNode<BV>()
{
vertices1 = NULL;
@@ -405,6 +452,28 @@ public:
abs_err = this->request.abs_err;
}
+ void preprocess()
+ {
+ if(!RTIsIdentity) preprocessOrientedNode();
+ }
+
+ void postprocess()
+ {
+ if(!RTIsIdentity) postprocessOrientedNode();
+ }
+
+ /// @brief BV culling test in one BVTT node
+ FCL_REAL BVDistanceLowerBound(int b1, int b2) const
+ {
+ if(enable_statistics) num_bv_tests++;
+ if (RTIsIdentity)
+ return details::DistanceTraversalBVDistanceLowerBound_impl<BV>
+ ::run (model1->getBV(b1), model2->getBV(b2));
+ else
+ return details::DistanceTraversalBVDistanceLowerBound_impl<BV>
+ ::run (RT._R(), RT._T(), model1->getBV(b1), model2->getBV(b2));
+ }
+
/// @brief Distance testing between leaves (two triangles)
void leafComputeDistance(int b1, int b2) const
{
@@ -430,8 +499,15 @@ public:
// nearest point pair
Vec3f P1, P2, normal;
- FCL_REAL d = sqrt (TriangleDistance::sqrTriDistance
- (t11, t12, t13, t21, t22, t23, P1, P2));
+ FCL_REAL d2;
+ if (RTIsIdentity)
+ d2 = TriangleDistance::sqrTriDistance (t11, t12, t13, t21, t22, t23,
+ P1, P2);
+ else
+ d2 = TriangleDistance::sqrTriDistance (t11, t12, t13, t21, t22, t23,
+ RT._R(), RT._T(),
+ P1, P2);
+ FCL_REAL d = sqrt(d2);
this->result->update(d, this->model1, this->model2, primitive_id1,
primitive_id2, P1, P2, normal);
@@ -454,62 +530,52 @@ public:
/// @brief relative and absolute error, default value is 0.01 for both terms
FCL_REAL rel_err;
FCL_REAL abs_err;
-};
-
-/// @brief Traversal node for distance computation between two meshes if their underlying BVH node is oriented node (RSS, OBBRSS, kIOS)
-class MeshDistanceTraversalNodeRSS : public MeshDistanceTraversalNode<RSS>
-{
-public:
- MeshDistanceTraversalNodeRSS();
-
- void preprocess();
-
- void postprocess();
-
- FCL_REAL BVDistanceLowerBound(int b1, int b2) const;
-
- void leafComputeDistance(int b1, int b2) const;
-
- Matrix3f R;
- Vec3f T;
-};
+ details::RelativeTransformation<!bool(RTIsIdentity)> RT;
-class MeshDistanceTraversalNodekIOS : public MeshDistanceTraversalNode<kIOS>
-{
-public:
- MeshDistanceTraversalNodekIOS();
-
- void preprocess();
-
- void postprocess();
-
- FCL_REAL BVDistanceLowerBound(int b1, int b2) const;
-
- void leafComputeDistance(int b1, int b2) const;
-
- Matrix3f R;
- Vec3f T;
+private:
+ void preprocessOrientedNode()
+ {
+ const int init_tri_id1 = 0, init_tri_id2 = 0;
+ const Triangle& init_tri1 = tri_indices1[init_tri_id1];
+ const Triangle& init_tri2 = tri_indices2[init_tri_id2];
+
+ Vec3f init_tri1_points[3];
+ Vec3f init_tri2_points[3];
+
+ init_tri1_points[0] = vertices1[init_tri1[0]];
+ init_tri1_points[1] = vertices1[init_tri1[1]];
+ init_tri1_points[2] = vertices1[init_tri1[2]];
+
+ init_tri2_points[0] = vertices2[init_tri2[0]];
+ init_tri2_points[1] = vertices2[init_tri2[1]];
+ init_tri2_points[2] = vertices2[init_tri2[2]];
+
+ Vec3f p1, p2, normal;
+ FCL_REAL distance = sqrt (TriangleDistance::sqrTriDistance
+ (init_tri1_points[0], init_tri1_points[1],
+ init_tri1_points[2], init_tri2_points[0],
+ init_tri2_points[1], init_tri2_points[2],
+ RT._R(), RT._T(), p1, p2));
+
+ result->update(distance, model1, model2, init_tri_id1, init_tri_id2, p1, p2,
+ normal);
+ }
+ void postprocessOrientedNode()
+ {
+ /// the points obtained by triDistance are not in world space: both are in object1's local coordinate system, so we need to convert them into the world space.
+ if(request.enable_nearest_points && (result->o1 == model1) && (result->o2 == model2))
+ {
+ result->nearest_points[0] = tf1.transform(result->nearest_points[0]);
+ result->nearest_points[1] = tf1.transform(result->nearest_points[1]);
+ }
+ }
};
-class MeshDistanceTraversalNodeOBBRSS : public MeshDistanceTraversalNode<OBBRSS>
-{
-public:
- MeshDistanceTraversalNodeOBBRSS();
-
- void preprocess();
-
- void postprocess();
-
- FCL_REAL BVDistanceLowerBound(int b1, int b2) const;
-
- FCL_REAL BVDistanceLowerBound(int b1, int b2, FCL_REAL& sqrDistLowerBound) const;
-
- void leafComputeDistance(int b1, int b2) const;
-
- Matrix3f R;
- Vec3f T;
-};
+/// @brief Traversal node for distance computation between two meshes if their underlying BVH node is oriented node (RSS, OBBRSS, kIOS)
+typedef MeshDistanceTraversalNode<RSS , 0> MeshDistanceTraversalNodeRSS;
+typedef MeshDistanceTraversalNode<kIOS , 0> MeshDistanceTraversalNodekIOS;
+typedef MeshDistanceTraversalNode<OBBRSS, 0> MeshDistanceTraversalNodeOBBRSS;
/// @}
diff --git a/include/hpp/fcl/internal/traversal_node_setup.h b/include/hpp/fcl/internal/traversal_node_setup.h
index 047067b93eb17f787d99bfb9cb14fc86a638fbc2..cb52eb800a7545e97f3889a996174f7c4c2359c3 100644
--- a/include/hpp/fcl/internal/traversal_node_setup.h
+++ b/include/hpp/fcl/internal/traversal_node_setup.h
@@ -40,6 +40,7 @@
/// @cond INTERNAL
+#include <hpp/fcl/internal/tools.h>
#include <hpp/fcl/internal/traversal_node_bvhs.h>
#include <hpp/fcl/internal/traversal_node_shapes.h>
#include <hpp/fcl/internal/traversal_node_bvh_shape.h>
@@ -513,7 +514,7 @@ bool initialize(ShapeDistanceTraversalNode<S1, S2>& node,
/// @brief Initialize traversal node for distance computation between two meshes, given the current transforms
template<typename BV>
-bool initialize(MeshDistanceTraversalNode<BV>& node,
+bool initialize(MeshDistanceTraversalNode<BV, RelativeTransformationIsIdentity>& node,
BVHModel<BV>& model1, Transform3f& tf1,
BVHModel<BV>& model2, Transform3f& tf2,
const DistanceRequest& request,
@@ -574,27 +575,37 @@ bool initialize(MeshDistanceTraversalNode<BV>& node,
return true;
}
-
-/// @brief Initialize traversal node for distance computation between two meshes, specialized for RSS type
-bool initialize(MeshDistanceTraversalNodeRSS& node,
- const BVHModel<RSS>& model1, const Transform3f& tf1,
- const BVHModel<RSS>& model2, const Transform3f& tf2,
+/// @brief Initialize traversal node for distance computation between two meshes
+template<typename BV>
+bool initialize(MeshDistanceTraversalNode<BV, 0>& node,
+ const BVHModel<BV>& model1, const Transform3f& tf1,
+ const BVHModel<BV>& model2, const Transform3f& tf2,
const DistanceRequest& request,
- DistanceResult& result);
+ DistanceResult& result)
+{
+ if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
+ return false;
-/// @brief Initialize traversal node for distance computation between two meshes, specialized for kIOS type
-bool initialize(MeshDistanceTraversalNodekIOS& node,
- const BVHModel<kIOS>& model1, const Transform3f& tf1,
- const BVHModel<kIOS>& model2, const Transform3f& tf2,
- const DistanceRequest& request,
- DistanceResult& result);
+ node.request = request;
+ node.result = &result;
-/// @brief Initialize traversal node for distance computation between two meshes, specialized for OBBRSS type
-bool initialize(MeshDistanceTraversalNodeOBBRSS& node,
- const BVHModel<OBBRSS>& model1, const Transform3f& tf1,
- const BVHModel<OBBRSS>& model2, const Transform3f& tf2,
- const DistanceRequest& request,
- DistanceResult& result);
+ node.model1 = &model1;
+ node.tf1 = tf1;
+ node.model2 = &model2;
+ node.tf2 = tf2;
+
+ node.vertices1 = model1.vertices;
+ node.vertices2 = model2.vertices;
+
+ node.tri_indices1 = model1.tri_indices;
+ node.tri_indices2 = model2.tri_indices;
+
+ relativeTransform(tf1.getRotation(), tf1.getTranslation(),
+ tf2.getRotation(), tf2.getTranslation(),
+ node.RT.R, node.RT.T);
+
+ return true;
+}
/// @brief Initialize traversal node for distance computation between one mesh and one shape, given the current transforms
template<typename BV, typename S>
diff --git a/src/traversal/traversal_node_bvhs.cpp b/src/traversal/traversal_node_bvhs.cpp
deleted file mode 100644
index cdaeddbecf728bc79ff218f7b12114786eb23f4d..0000000000000000000000000000000000000000
--- a/src/traversal/traversal_node_bvhs.cpp
+++ /dev/null
@@ -1,226 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011-2014, Willow Garage, Inc.
- * Copyright (c) 2014-2015, Open Source Robotics Foundation
- * 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 Jia Pan */
-
-
-#include <hpp/fcl/internal/traversal_node_bvhs.h>
-
-namespace hpp
-{
-namespace fcl
-{
-
-namespace details
-{
-template<typename BV>
-static inline void meshDistanceOrientedNodeleafComputeDistance(int b1, int b2,
- const BVHModel<BV>* model1, const BVHModel<BV>* model2,
- Vec3f* vertices1, Vec3f* vertices2,
- Triangle* tri_indices1, Triangle* tri_indices2,
- const Matrix3f& R, const Vec3f& T,
- bool enable_statistics,
- int& num_leaf_tests,
- const DistanceRequest&,
- DistanceResult& result)
-{
- if(enable_statistics) num_leaf_tests++;
-
- const BVNode<BV>& node1 = model1->getBV(b1);
- const BVNode<BV>& node2 = model2->getBV(b2);
-
- int primitive_id1 = node1.primitiveId();
- int primitive_id2 = node2.primitiveId();
-
- const Triangle& tri_id1 = tri_indices1[primitive_id1];
- const Triangle& tri_id2 = tri_indices2[primitive_id2];
-
- const Vec3f& t11 = vertices1[tri_id1[0]];
- const Vec3f& t12 = vertices1[tri_id1[1]];
- const Vec3f& t13 = vertices1[tri_id1[2]];
-
- const Vec3f& t21 = vertices2[tri_id2[0]];
- const Vec3f& t22 = vertices2[tri_id2[1]];
- const Vec3f& t23 = vertices2[tri_id2[2]];
-
- // nearest point pair
- Vec3f P1, P2, normal;
-
- FCL_REAL d = sqrt (TriangleDistance::sqrTriDistance
- (t11, t12, t13, t21, t22, t23, R, T, P1, P2));
-
- result.update(d, model1, model2, primitive_id1, primitive_id2, P1, P2,
- normal);
-}
-} // namespace details
-
-namespace details
-{
-
-template<typename BV>
-static inline void distancePreprocessOrientedNode(const BVHModel<BV>* model1, const BVHModel<BV>* model2,
- const Vec3f* vertices1, Vec3f* vertices2,
- Triangle* tri_indices1, Triangle* tri_indices2,
- int init_tri_id1, int init_tri_id2,
- const Matrix3f& R, const Vec3f& T,
- const DistanceRequest&,
- DistanceResult& result)
-{
- const Triangle& init_tri1 = tri_indices1[init_tri_id1];
- const Triangle& init_tri2 = tri_indices2[init_tri_id2];
-
- Vec3f init_tri1_points[3];
- Vec3f init_tri2_points[3];
-
- init_tri1_points[0] = vertices1[init_tri1[0]];
- init_tri1_points[1] = vertices1[init_tri1[1]];
- init_tri1_points[2] = vertices1[init_tri1[2]];
-
- init_tri2_points[0] = vertices2[init_tri2[0]];
- init_tri2_points[1] = vertices2[init_tri2[1]];
- init_tri2_points[2] = vertices2[init_tri2[2]];
-
- Vec3f p1, p2, normal;
- FCL_REAL distance = sqrt (TriangleDistance::sqrTriDistance
- (init_tri1_points[0], init_tri1_points[1],
- init_tri1_points[2], init_tri2_points[0],
- init_tri2_points[1], init_tri2_points[2],
- R, T, p1, p2));
-
- result.update(distance, model1, model2, init_tri_id1, init_tri_id2, p1, p2,
- normal);
-}
-
-template<typename BV>
-static inline void distancePostprocessOrientedNode(const BVHModel<BV>* model1, const BVHModel<BV>* model2,
- const Transform3f& tf1, const DistanceRequest& request, DistanceResult& result)
-{
- /// the points obtained by triDistance are not in world space: both are in object1's local coordinate system, so we need to convert them into the world space.
- if(request.enable_nearest_points && (result.o1 == model1) && (result.o2 == model2))
- {
- result.nearest_points[0] = tf1.transform(result.nearest_points[0]).eval();
- result.nearest_points[1] = tf1.transform(result.nearest_points[1]).eval();
- }
-}
-
-}
-
-MeshDistanceTraversalNodeRSS::MeshDistanceTraversalNodeRSS() : MeshDistanceTraversalNode<RSS>()
-{
- R.setIdentity();
-}
-
-void MeshDistanceTraversalNodeRSS::preprocess()
-{
- details::distancePreprocessOrientedNode(model1, model2, vertices1, vertices2, tri_indices1, tri_indices2, 0, 0, R, T, request, *result);
-}
-
-void MeshDistanceTraversalNodeRSS::postprocess()
-{
- details::distancePostprocessOrientedNode(model1, model2, tf1, request, *result);
-}
-
-FCL_REAL MeshDistanceTraversalNodeRSS::BVDistanceLowerBound(int b1, int b2) const
-{
- if(enable_statistics) num_bv_tests++;
- return distance(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
-}
-
-void MeshDistanceTraversalNodeRSS::leafComputeDistance(int b1, int b2) const
-{
- details::meshDistanceOrientedNodeleafComputeDistance(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2,
- R, T, enable_statistics, num_leaf_tests,
- request, *result);
-}
-
-MeshDistanceTraversalNodekIOS::MeshDistanceTraversalNodekIOS() : MeshDistanceTraversalNode<kIOS>()
-{
- R.setIdentity();
-}
-
-void MeshDistanceTraversalNodekIOS::preprocess()
-{
- details::distancePreprocessOrientedNode(model1, model2, vertices1, vertices2, tri_indices1, tri_indices2, 0, 0, R, T, request, *result);
-}
-
-void MeshDistanceTraversalNodekIOS::postprocess()
-{
- details::distancePostprocessOrientedNode(model1, model2, tf1, request, *result);
-}
-
-FCL_REAL MeshDistanceTraversalNodekIOS::BVDistanceLowerBound(int b1, int b2) const
-{
- if(enable_statistics) num_bv_tests++;
- return distance(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
-}
-
-void MeshDistanceTraversalNodekIOS::leafComputeDistance(int b1, int b2) const
-{
- details::meshDistanceOrientedNodeleafComputeDistance(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2,
- R, T, enable_statistics, num_leaf_tests,
- request, *result);
-}
-
-MeshDistanceTraversalNodeOBBRSS::MeshDistanceTraversalNodeOBBRSS() : MeshDistanceTraversalNode<OBBRSS>()
-{
- R.setIdentity();
-}
-
-void MeshDistanceTraversalNodeOBBRSS::preprocess()
-{
- details::distancePreprocessOrientedNode(model1, model2, vertices1, vertices2, tri_indices1, tri_indices2, 0, 0, R, T, request, *result);
-}
-
-void MeshDistanceTraversalNodeOBBRSS::postprocess()
-{
- details::distancePostprocessOrientedNode(model1, model2, tf1, request, *result);
-}
-
-FCL_REAL MeshDistanceTraversalNodeOBBRSS::BVDistanceLowerBound(int b1, int b2) const
-{
- if(enable_statistics) num_bv_tests++;
- return distance(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
-}
-
-void MeshDistanceTraversalNodeOBBRSS::leafComputeDistance(int b1, int b2) const
-{
- details::meshDistanceOrientedNodeleafComputeDistance(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2,
- R, T, enable_statistics, num_leaf_tests,
- request, *result);
-}
-
-}
-
-} // namespace hpp
diff --git a/src/traversal/traversal_node_setup.cpp b/src/traversal/traversal_node_setup.cpp
deleted file mode 100644
index b0d13e489e719ee648d1e17f6a62236bcbbee824..0000000000000000000000000000000000000000
--- a/src/traversal/traversal_node_setup.cpp
+++ /dev/null
@@ -1,117 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011-2014, Willow Garage, Inc.
- * Copyright (c) 2014-2015, Open Source Robotics Foundation
- * 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 Jia Pan */
-
-
-#include <hpp/fcl/internal/traversal_node_setup.h>
-#include <hpp/fcl/internal/tools.h>
-namespace hpp
-{
-namespace fcl
-{
-
-namespace details
-{
-template<typename BV, typename OrientedNode>
-static inline bool setupMeshDistanceOrientedNode(OrientedNode& node,
- const BVHModel<BV>& model1, const Transform3f& tf1,
- const BVHModel<BV>& model2, const Transform3f& tf2,
- const DistanceRequest& request,
- DistanceResult& result)
-{
- if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
- return false;
-
- node.request = request;
- node.result = &result;
-
- node.model1 = &model1;
- node.tf1 = tf1;
- node.model2 = &model2;
- node.tf2 = tf2;
-
- node.vertices1 = model1.vertices;
- node.vertices2 = model2.vertices;
-
- node.tri_indices1 = model1.tri_indices;
- node.tri_indices2 = model2.tri_indices;
-
- relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation(), node.R, node.T);
-
- return true;
-}
-
-
-}
-
-bool initialize(MeshDistanceTraversalNodeRSS& node,
- const BVHModel<RSS>& model1, const Transform3f& tf1,
- const BVHModel<RSS>& model2, const Transform3f& tf2,
- const DistanceRequest& request,
- DistanceResult& result)
-{
- return details::setupMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, request, result);
-}
-
-
-bool initialize(MeshDistanceTraversalNodekIOS& node,
- const BVHModel<kIOS>& model1, const Transform3f& tf1,
- const BVHModel<kIOS>& model2, const Transform3f& tf2,
- const DistanceRequest& request,
- DistanceResult& result)
-{
- return details::setupMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, request, result);
-}
-
-bool initialize(MeshDistanceTraversalNodeOBBRSS& node,
- const BVHModel<OBBRSS>& model1, const Transform3f& tf1,
- const BVHModel<OBBRSS>& model2, const Transform3f& tf2,
- const DistanceRequest& request,
- DistanceResult& result)
-{
- return details::setupMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, request, result);
-}
-
-namespace details
-{
-
-
-}
-
-
-}
-
-} // namespace hpp