diff --git a/src/collision_func_matrix.cpp b/src/collision_func_matrix.cpp
index 73d6a9317c6378689858446c79178f35e2d0e60c..11c100eeb12cf832736301a1d5c0611d89f18683 100644
--- a/src/collision_func_matrix.cpp
+++ b/src/collision_func_matrix.cpp
@@ -42,6 +42,7 @@
#include <../src/collision_node.h>
#include <hpp/fcl/narrowphase/narrowphase.h>
#include "distance_func_matrix.h"
+#include "traits_traversal.h"
namespace hpp
{
@@ -57,7 +58,7 @@ std::size_t Collide(const CollisionGeometry* o1, const Transform3f& tf1, const C
{
if(request.isSatisfied(result)) return result.numContacts();
- typename TraversalTraits<TypeA, TypeB>::CollisionTraversal_t node (request);
+ typename TraversalTraitsCollision<TypeA, TypeB>::CollisionTraversal_t node (request);
const TypeA* obj1 = static_cast<const TypeA*>(o1);
const TypeB* obj2 = static_cast<const TypeB*>(o2);
OcTreeSolver otsolver(nsolver);
@@ -67,7 +68,6 @@ std::size_t Collide(const CollisionGeometry* o1, const Transform3f& tf1, const C
return result.numContacts();
-
#endif
template<typename T_SH1, typename T_SH2>
diff --git a/src/distance_func_matrix.cpp b/src/distance_func_matrix.cpp
index c4b4e33c67dad5a0e66322ca369365f09d44d26b..3d0220d2fd1428562793ead4119dc480c46b6538 100644
--- a/src/distance_func_matrix.cpp
+++ b/src/distance_func_matrix.cpp
@@ -39,6 +39,7 @@
#include <../src/collision_node.h>
#include "traversal/traversal_node_setup.h"
+#include "traits_traversal.h"
namespace hpp
{
@@ -46,30 +47,15 @@ namespace fcl
{
#ifdef HPP_FCL_HAVE_OCTOMAP
-template<typename T_SH>
-FCL_REAL ShapeOcTreeDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver* nsolver,
- const DistanceRequest& request, DistanceResult& result)
-{
- if(request.isSatisfied(result)) return result.min_distance;
- ShapeOcTreeDistanceTraversalNode<T_SH> node;
- const T_SH* obj1 = static_cast<const T_SH*>(o1);
- const OcTree* obj2 = static_cast<const OcTree*>(o2);
- OcTreeSolver otsolver(nsolver);
-
- initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
- distance(&node);
-
- return result.min_distance;
-}
-template<typename T_SH>
-FCL_REAL OcTreeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver* nsolver,
+template<typename TypeA, typename TypeB>
+FCL_REAL Distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver* nsolver,
const DistanceRequest& request, DistanceResult& result)
{
if(request.isSatisfied(result)) return result.min_distance;
- OcTreeShapeDistanceTraversalNode<T_SH> node;
- const OcTree* obj1 = static_cast<const OcTree*>(o1);
- const T_SH* obj2 = static_cast<const T_SH*>(o2);
+ typename TraversalTraitsDistance<TypeA, TypeB>::CollisionTraversal_t node;
+ const TypeA* obj1 = static_cast<const TypeA*>(o1);
+ const TypeB* obj2 = static_cast<const TypeB*>(o2);
OcTreeSolver otsolver(nsolver);
initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
@@ -78,53 +64,6 @@ FCL_REAL OcTreeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1
return result.min_distance;
}
-FCL_REAL OcTreeDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver* nsolver,
- const DistanceRequest& request, DistanceResult& result)
-{
- if(request.isSatisfied(result)) return result.min_distance;
- OcTreeDistanceTraversalNode node;
- const OcTree* obj1 = static_cast<const OcTree*>(o1);
- const OcTree* obj2 = static_cast<const OcTree*>(o2);
- OcTreeSolver otsolver(nsolver);
-
- initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
- distance(&node);
-
- return result.min_distance;
-}
-
-template<typename T_BVH>
-FCL_REAL BVHOcTreeDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver* nsolver,
- const DistanceRequest& request, DistanceResult& result)
-{
- if(request.isSatisfied(result)) return result.min_distance;
- MeshOcTreeDistanceTraversalNode<T_BVH> node;
- const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>*>(o1);
- const OcTree* obj2 = static_cast<const OcTree*>(o2);
- OcTreeSolver otsolver(nsolver);
-
- initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
- distance(&node);
-
- return result.min_distance;
-}
-
-template<typename T_BVH>
-FCL_REAL OcTreeBVHDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver* nsolver,
- const DistanceRequest& request, DistanceResult& result)
-{
- if(request.isSatisfied(result)) return result.min_distance;
- OcTreeMeshDistanceTraversalNode<T_BVH> node;
- const OcTree* obj1 = static_cast<const OcTree*>(o1);
- const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>*>(o2);
- OcTreeSolver otsolver(nsolver);
-
- initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
- distance(&node);
-
- return result.min_distance;
-}
-
#endif
template<typename T_SH1, typename T_SH2>
@@ -451,43 +390,43 @@ DistanceFunctionMatrix::DistanceFunctionMatrix()
distance_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHDistance<OBBRSS>;
#ifdef HPP_FCL_HAVE_OCTOMAP
- distance_matrix[GEOM_OCTREE][GEOM_BOX] = &OcTreeShapeDistance<Box>;
- distance_matrix[GEOM_OCTREE][GEOM_SPHERE] = &OcTreeShapeDistance<Sphere>;
- distance_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &OcTreeShapeDistance<Capsule>;
- distance_matrix[GEOM_OCTREE][GEOM_CONE] = &OcTreeShapeDistance<Cone>;
- distance_matrix[GEOM_OCTREE][GEOM_CYLINDER] = &OcTreeShapeDistance<Cylinder>;
- distance_matrix[GEOM_OCTREE][GEOM_CONVEX] = &OcTreeShapeDistance<ConvexBase>;
- distance_matrix[GEOM_OCTREE][GEOM_PLANE] = &OcTreeShapeDistance<Plane>;
- distance_matrix[GEOM_OCTREE][GEOM_HALFSPACE] = &OcTreeShapeDistance<Halfspace>;
-
- distance_matrix[GEOM_BOX][GEOM_OCTREE] = &ShapeOcTreeDistance<Box>;
- distance_matrix[GEOM_SPHERE][GEOM_OCTREE] = &ShapeOcTreeDistance<Sphere>;
- distance_matrix[GEOM_CAPSULE][GEOM_OCTREE] = &ShapeOcTreeDistance<Capsule>;
- distance_matrix[GEOM_CONE][GEOM_OCTREE] = &ShapeOcTreeDistance<Cone>;
- distance_matrix[GEOM_CYLINDER][GEOM_OCTREE] = &ShapeOcTreeDistance<Cylinder>;
- distance_matrix[GEOM_CONVEX][GEOM_OCTREE] = &ShapeOcTreeDistance<ConvexBase>;
- distance_matrix[GEOM_PLANE][GEOM_OCTREE] = &ShapeOcTreeDistance<Plane>;
- distance_matrix[GEOM_HALFSPACE][GEOM_OCTREE] = &ShapeOcTreeDistance<Halfspace>;
-
- distance_matrix[GEOM_OCTREE][GEOM_OCTREE] = &OcTreeDistance;
-
- distance_matrix[GEOM_OCTREE][BV_AABB] = &OcTreeBVHDistance<AABB>;
- distance_matrix[GEOM_OCTREE][BV_OBB] = &OcTreeBVHDistance<OBB>;
- distance_matrix[GEOM_OCTREE][BV_RSS] = &OcTreeBVHDistance<RSS>;
- distance_matrix[GEOM_OCTREE][BV_OBBRSS] = &OcTreeBVHDistance<OBBRSS>;
- distance_matrix[GEOM_OCTREE][BV_kIOS] = &OcTreeBVHDistance<kIOS>;
- distance_matrix[GEOM_OCTREE][BV_KDOP16] = &OcTreeBVHDistance<KDOP<16> >;
- distance_matrix[GEOM_OCTREE][BV_KDOP18] = &OcTreeBVHDistance<KDOP<18> >;
- distance_matrix[GEOM_OCTREE][BV_KDOP24] = &OcTreeBVHDistance<KDOP<24> >;
-
- distance_matrix[BV_AABB][GEOM_OCTREE] = &BVHOcTreeDistance<AABB>;
- distance_matrix[BV_OBB][GEOM_OCTREE] = &BVHOcTreeDistance<OBB>;
- distance_matrix[BV_RSS][GEOM_OCTREE] = &BVHOcTreeDistance<RSS>;
- distance_matrix[BV_OBBRSS][GEOM_OCTREE] = &BVHOcTreeDistance<OBBRSS>;
- distance_matrix[BV_kIOS][GEOM_OCTREE] = &BVHOcTreeDistance<kIOS>;
- distance_matrix[BV_KDOP16][GEOM_OCTREE] = &BVHOcTreeDistance<KDOP<16> >;
- distance_matrix[BV_KDOP18][GEOM_OCTREE] = &BVHOcTreeDistance<KDOP<18> >;
- distance_matrix[BV_KDOP24][GEOM_OCTREE] = &BVHOcTreeDistance<KDOP<24> >;
+ distance_matrix[GEOM_OCTREE][GEOM_BOX] = &Distance<Box>;
+ distance_matrix[GEOM_OCTREE][GEOM_SPHERE] = &Distance<Sphere>;
+ distance_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &Distance<Capsule>;
+ distance_matrix[GEOM_OCTREE][GEOM_CONE] = &Distance<Cone>;
+ distance_matrix[GEOM_OCTREE][GEOM_CYLINDER] = &Distance<Cylinder>;
+ distance_matrix[GEOM_OCTREE][GEOM_CONVEX] = &Distance<ConvexBase>;
+ distance_matrix[GEOM_OCTREE][GEOM_PLANE] = &Distance<Plane>;
+ distance_matrix[GEOM_OCTREE][GEOM_HALFSPACE] = &Distance<Halfspace>;
+
+ distance_matrix[GEOM_BOX][GEOM_OCTREE] = &Distance<Box>;
+ distance_matrix[GEOM_SPHERE][GEOM_OCTREE] = &Distance<Sphere>;
+ distance_matrix[GEOM_CAPSULE][GEOM_OCTREE] = &Distance<Capsule>;
+ distance_matrix[GEOM_CONE][GEOM_OCTREE] = &Distance<Cone>;
+ distance_matrix[GEOM_CYLINDER][GEOM_OCTREE] = &Distance<Cylinder>;
+ distance_matrix[GEOM_CONVEX][GEOM_OCTREE] = &Distance<ConvexBase>;
+ distance_matrix[GEOM_PLANE][GEOM_OCTREE] = &Distance<Plane>;
+ distance_matrix[GEOM_HALFSPACE][GEOM_OCTREE] = &Distance<Halfspace>;
+
+ distance_matrix[GEOM_OCTREE][GEOM_OCTREE] = &Distance;
+
+ distance_matrix[GEOM_OCTREE][BV_AABB] = &Distance<AABB>;
+ distance_matrix[GEOM_OCTREE][BV_OBB] = &Distance<OBB>;
+ distance_matrix[GEOM_OCTREE][BV_RSS] = &Distance<RSS>;
+ distance_matrix[GEOM_OCTREE][BV_OBBRSS] = &Distance<OBBRSS>;
+ distance_matrix[GEOM_OCTREE][BV_kIOS] = &Distance<kIOS>;
+ distance_matrix[GEOM_OCTREE][BV_KDOP16] = &Distance<KDOP<16> >;
+ distance_matrix[GEOM_OCTREE][BV_KDOP18] = &Distance<KDOP<18> >;
+ distance_matrix[GEOM_OCTREE][BV_KDOP24] = &Distance<KDOP<24> >;
+
+ distance_matrix[BV_AABB][GEOM_OCTREE] = &Distance<AABB>;
+ distance_matrix[BV_OBB][GEOM_OCTREE] = &Distance<OBB>;
+ distance_matrix[BV_RSS][GEOM_OCTREE] = &Distance<RSS>;
+ distance_matrix[BV_OBBRSS][GEOM_OCTREE] = &Distance<OBBRSS>;
+ distance_matrix[BV_kIOS][GEOM_OCTREE] = &Distance<kIOS>;
+ distance_matrix[BV_KDOP16][GEOM_OCTREE] = &Distance<KDOP<16> >;
+ distance_matrix[BV_KDOP18][GEOM_OCTREE] = &Distance<KDOP<18> >;
+ distance_matrix[BV_KDOP24][GEOM_OCTREE] = &Distance<KDOP<24> >;
#endif
diff --git a/src/traits_traversal.h b/src/traits_traversal.h
index f67db3642d0312251096ba47004b9a24b2327f49..c11834295de7e8aa1bcb39eb04a3d7b6c9889cda 100644
--- a/src/traits_traversal.h
+++ b/src/traits_traversal.h
@@ -1,109 +1,92 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2014, CNRS-LAAS
- * 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 Willow Garage, Inc. 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 Florent Lamiraux */
-
-
#include <hpp/fcl/collision_func_matrix.h>
-
-#include "traversal/traversal_node_setup.h"
-#include <../src/collision_node.h>
#include <hpp/fcl/narrowphase/narrowphase.h>
+#include <../src/collision_node.h>
+#include "traversal/traversal_node_setup.h"
#include "distance_func_matrix.h"
+
namespace hpp
{
namespace fcl
{
+#ifdef HPP_FCL_HAVE_OCTOMAP
+
+// TraversalTraitsCollision for collision_func_matrix.cpp
+
template <typename TypeA, typename TypeB>
-struct TraversalTraits
+struct TraversalTraitsCollision
{
};
template <typename T_SH>
-struct TraversalTraits <T_SH, OcTree>
+struct TraversalTraitsCollision <T_SH, OcTree>
{
typedef ShapeOcTreeCollisionTraversalNode<T_SH> CollisionTraversal_t;
};
template <typename T_SH>
-struct TraversalTraits <OcTree, T_SH>
+struct TraversalTraitsCollision <OcTree, T_SH>
{
typedef OcTreeShapeCollisionTraversalNode<T_SH> CollisionTraversal_t;
};
template <>
-struct TraversalTraits <OcTree, OcTree>
+struct TraversalTraitsCollision <OcTree, OcTree>
{
typedef OcTreeCollisionTraversalNode<> CollisionTraversal_t;
};
template <typename T_BVH>
-struct TraversalTraits <OcTree, BVHModel<T_BVH>>
+struct TraversalTraitsCollision <OcTree, BVHModel<T_BVH>>
{
typedef OcTreeMeshCollisionTraversalNode<T_BVH> CollisionTraversal_t;
};
template <typename T_BVH>
-struct TraversalTraits <BVHModel<T_BVH>, OcTree>
+struct TraversalTraitsCollision <BVHModel<T_BVH>, OcTree>
{
typedef MeshOcTreeCollisionTraversalNode<T_BVH> CollisionTraversal_t;
};
-template <typename OrientedMeshCollisionTraversalNode, typename T_BVH, typename T_SH>
-struct TraversalTraits <BVHModel<T_BVH>, T_SH>
+// TraversalTraitsDistance for distance_func_matrix.cpp
+
+template <typename TypeA, typename TypeB>
+struct TraversalTraitsDistance
{
- typedef OrientMeshShapeCollisionTraveralNode<> CollisionTraversal_t;
};
-template <typename OrientedMeshCollisionTraversalNode, typename T_BVH>
-struct TraversalTraits <BVHModel<T_BVH>, BVHModel<T_BVH>>
+template <typename T_SH>
+struct TraversalTraitsDistance <T_SH, OcTree>
{
- typedef OrientedMeshCollisionTraversalNode<> CollisionTraversal_t;
+ typedef ShapeOcTreeDistanceTraversalNode<T_SH> CollisionTraversal_t;
};
+template <typename T_SH>
+struct TraversalTraitsDistance <OcTree, T_SH>
+{
+ typedef OcTreeShapeDistanceTraversalNode<T_SH> CollisionTraversal_t;
+};
+template <>
+struct TraversalTraitsDistance <OcTree, OcTree>
+{
+ typedef OcTreeDistanceTraversalNode CollisionTraversal_t;
+};
+template <typename T_BVH>
+struct TraversalTraitsDistance <OcTree, BVHModel<T_BVH>>
+{
+ typedef OcTreeMeshDistanceTraversalNode<T_BVH> CollisionTraversal_t;
+};
+template <typename T_BVH>
+struct TraversalTraitsDistance <BVHModel<T_BVH>, OcTree>
+{
+ typedef MeshOcTreeDistanceTraversalNode<T_BVH> CollisionTraversal_t;
+};
-
-
-
-
-
-
+#endif
}