Unverified Commit dffbd8fb authored by Joseph Mirabel's avatar Joseph Mirabel Committed by GitHub
Browse files

Merge pull request #97 from LucileRemigy/refactoring

New folder Internal for internal header
parents 92261deb 5ef010b2
......@@ -38,10 +38,11 @@
#include <hpp/fcl/collision_func_matrix.h>
#include "traversal/traversal_node_setup.h"
#include <hpp/fcl/internal/traversal_node_setup.h>
#include <../src/collision_node.h>
#include <hpp/fcl/narrowphase/narrowphase.h>
#include "distance_func_matrix.h"
#include <../src/distance_func_matrix.h>
#include <../src/traits_traversal.h>
namespace hpp
{
......@@ -49,51 +50,17 @@ namespace fcl
{
#ifdef HPP_FCL_HAVE_OCTOMAP
template<typename T_SH>
std::size_t ShapeOcTreeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const GJKSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
if(request.isSatisfied(result)) return result.numContacts();
ShapeOcTreeCollisionTraversalNode<T_SH> node (request);
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, result);
collide(&node, request, result);
return result.numContacts();
}
template<typename T_SH>
std::size_t OcTreeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
template<typename TypeA, typename TypeB>
std::size_t Collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const GJKSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
if(request.isSatisfied(result)) return result.numContacts();
OcTreeShapeCollisionTraversalNode<T_SH> node (request);
const OcTree* obj1 = static_cast<const OcTree*>(o1);
const T_SH* obj2 = static_cast<const T_SH*>(o2);
OcTreeSolver otsolver(nsolver);
initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, result);
collide(&node, request, result);
return result.numContacts();
}
std::size_t OcTreeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const GJKSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
if(request.isSatisfied(result)) return result.numContacts();
OcTreeCollisionTraversalNode node (request);
const OcTree* obj1 = static_cast<const OcTree*>(o1);
const OcTree* obj2 = static_cast<const OcTree*>(o2);
typename TraversalTraitsCollision<TypeA, TypeB>::CollisionTraversal_t node (request);
const TypeA* obj1 = dynamic_cast<const TypeA*>(o1);
const TypeB* obj2 = dynamic_cast<const TypeB*>(o2);
OcTreeSolver otsolver(nsolver);
initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, result);
......@@ -101,41 +68,6 @@ std::size_t OcTreeCollide(const CollisionGeometry* o1, const Transform3f& tf1, c
return result.numContacts();
}
template<typename T_BVH>
std::size_t OcTreeBVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const GJKSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
if(request.isSatisfied(result)) return result.numContacts();
OcTreeMeshCollisionTraversalNode<T_BVH> node (request);
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, result);
collide(&node, request, result);
return result.numContacts();
}
template<typename T_BVH>
std::size_t BVHOcTreeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const GJKSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
if(request.isSatisfied(result)) return result.numContacts();
MeshOcTreeCollisionTraversalNode<T_BVH> node (request);
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, result);
collide(&node, request, result);
return result.numContacts();
}
#endif
template<typename T_SH1, typename T_SH2>
......@@ -178,6 +110,28 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf
return 0;
}
namespace details
{
template<typename OrientMeshShapeCollisionTraveralNode, typename T_BVH, typename T_SH>
std::size_t orientedBVHShapeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const GJKSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
if(request.isSatisfied(result)) return result.numContacts();
OrientMeshShapeCollisionTraveralNode node (request);
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
const T_SH* obj2 = static_cast<const T_SH*>(o2);
initialize(node, *obj1, tf1, *obj2, tf2, nsolver, result);
fcl::collide(&node, request, result);
return result.numContacts();
}
}
template<typename T_BVH, typename T_SH>
struct BVHShapeCollider
{
......@@ -201,28 +155,6 @@ struct BVHShapeCollider
}
};
namespace details
{
template<typename OrientMeshShapeCollisionTraveralNode, typename T_BVH, typename T_SH>
std::size_t orientedBVHShapeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const GJKSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
if(request.isSatisfied(result)) return result.numContacts();
OrientMeshShapeCollisionTraveralNode node (request);
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
const T_SH* obj2 = static_cast<const T_SH*>(o2);
initialize(node, *obj1, tf1, *obj2, tf2, nsolver, result);
fcl::collide(&node, request, result);
return result.numContacts();
}
}
template<typename T_SH>
struct BVHShapeCollider<OBB, T_SH>
{
......@@ -270,6 +202,24 @@ struct BVHShapeCollider<OBBRSS, T_SH>
}
};
namespace details
{
template<typename OrientedMeshCollisionTraversalNode, typename T_BVH>
std::size_t orientedMeshCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const CollisionRequest& request, CollisionResult& result)
{
if(request.isSatisfied(result)) return result.numContacts();
OrientedMeshCollisionTraversalNode node (request);
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>* >(o2);
initialize(node, *obj1, tf1, *obj2, tf2, result);
collide(&node, request, result);
return result.numContacts();
}
}
template<typename T_BVH>
std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const CollisionRequest& request, CollisionResult& result)
......@@ -293,25 +243,6 @@ std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, cons
return result.numContacts();
}
namespace details
{
template<typename OrientedMeshCollisionTraversalNode, typename T_BVH>
std::size_t orientedMeshCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const CollisionRequest& request, CollisionResult& result)
{
if(request.isSatisfied(result)) return result.numContacts();
OrientedMeshCollisionTraversalNode node (request);
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>* >(o2);
initialize(node, *obj1, tf1, *obj2, tf2, result);
collide(&node, request, result);
return result.numContacts();
}
}
template<>
std::size_t BVHCollide<OBB>(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const CollisionRequest& request, CollisionResult& result)
{
......@@ -503,43 +434,43 @@ CollisionFunctionMatrix::CollisionFunctionMatrix()
collision_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHCollide<OBBRSS>;
#ifdef HPP_FCL_HAVE_OCTOMAP
collision_matrix[GEOM_OCTREE][GEOM_BOX] = &OcTreeShapeCollide<Box>;
collision_matrix[GEOM_OCTREE][GEOM_SPHERE] = &OcTreeShapeCollide<Sphere>;
collision_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &OcTreeShapeCollide<Capsule>;
collision_matrix[GEOM_OCTREE][GEOM_CONE] = &OcTreeShapeCollide<Cone>;
collision_matrix[GEOM_OCTREE][GEOM_CYLINDER] = &OcTreeShapeCollide<Cylinder>;
collision_matrix[GEOM_OCTREE][GEOM_CONVEX] = &OcTreeShapeCollide<ConvexBase>;
collision_matrix[GEOM_OCTREE][GEOM_PLANE] = &OcTreeShapeCollide<Plane>;
collision_matrix[GEOM_OCTREE][GEOM_HALFSPACE] = &OcTreeShapeCollide<Halfspace>;
collision_matrix[GEOM_BOX][GEOM_OCTREE] = &ShapeOcTreeCollide<Box>;
collision_matrix[GEOM_SPHERE][GEOM_OCTREE] = &ShapeOcTreeCollide<Sphere>;
collision_matrix[GEOM_CAPSULE][GEOM_OCTREE] = &ShapeOcTreeCollide<Capsule>;
collision_matrix[GEOM_CONE][GEOM_OCTREE] = &ShapeOcTreeCollide<Cone>;
collision_matrix[GEOM_CYLINDER][GEOM_OCTREE] = &ShapeOcTreeCollide<Cylinder>;
collision_matrix[GEOM_CONVEX][GEOM_OCTREE] = &ShapeOcTreeCollide<ConvexBase>;
collision_matrix[GEOM_PLANE][GEOM_OCTREE] = &ShapeOcTreeCollide<Plane>;
collision_matrix[GEOM_HALFSPACE][GEOM_OCTREE] = &ShapeOcTreeCollide<Halfspace>;
collision_matrix[GEOM_OCTREE][GEOM_OCTREE] = &OcTreeCollide;
collision_matrix[GEOM_OCTREE][BV_AABB] = &OcTreeBVHCollide<AABB>;
collision_matrix[GEOM_OCTREE][BV_OBB] = &OcTreeBVHCollide<OBB>;
collision_matrix[GEOM_OCTREE][BV_RSS] = &OcTreeBVHCollide<RSS>;
collision_matrix[GEOM_OCTREE][BV_OBBRSS] = &OcTreeBVHCollide<OBBRSS>;
collision_matrix[GEOM_OCTREE][BV_kIOS] = &OcTreeBVHCollide<kIOS>;
collision_matrix[GEOM_OCTREE][BV_KDOP16] = &OcTreeBVHCollide<KDOP<16> >;
collision_matrix[GEOM_OCTREE][BV_KDOP18] = &OcTreeBVHCollide<KDOP<18> >;
collision_matrix[GEOM_OCTREE][BV_KDOP24] = &OcTreeBVHCollide<KDOP<24> >;
collision_matrix[BV_AABB][GEOM_OCTREE] = &BVHOcTreeCollide<AABB>;
collision_matrix[BV_OBB][GEOM_OCTREE] = &BVHOcTreeCollide<OBB>;
collision_matrix[BV_RSS][GEOM_OCTREE] = &BVHOcTreeCollide<RSS>;
collision_matrix[BV_OBBRSS][GEOM_OCTREE] = &BVHOcTreeCollide<OBBRSS>;
collision_matrix[BV_kIOS][GEOM_OCTREE] = &BVHOcTreeCollide<kIOS>;
collision_matrix[BV_KDOP16][GEOM_OCTREE] = &BVHOcTreeCollide<KDOP<16> >;
collision_matrix[BV_KDOP18][GEOM_OCTREE] = &BVHOcTreeCollide<KDOP<18> >;
collision_matrix[BV_KDOP24][GEOM_OCTREE] = &BVHOcTreeCollide<KDOP<24> >;
collision_matrix[GEOM_OCTREE][GEOM_BOX] = &Collide<OcTree, Box>;
collision_matrix[GEOM_OCTREE][GEOM_SPHERE] = &Collide<OcTree, Sphere>;
collision_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &Collide<OcTree, Capsule>;
collision_matrix[GEOM_OCTREE][GEOM_CONE] = &Collide<OcTree, Cone>;
collision_matrix[GEOM_OCTREE][GEOM_CYLINDER] = &Collide<OcTree, Cylinder>;
collision_matrix[GEOM_OCTREE][GEOM_CONVEX] = &Collide<OcTree, ConvexBase>;
collision_matrix[GEOM_OCTREE][GEOM_PLANE] = &Collide<OcTree, Plane>;
collision_matrix[GEOM_OCTREE][GEOM_HALFSPACE] = &Collide<OcTree, Halfspace>;
collision_matrix[GEOM_BOX][GEOM_OCTREE] = &Collide<Box, OcTree>;
collision_matrix[GEOM_SPHERE][GEOM_OCTREE] = &Collide<Sphere, OcTree>;
collision_matrix[GEOM_CAPSULE][GEOM_OCTREE] = &Collide<Capsule, OcTree>;
collision_matrix[GEOM_CONE][GEOM_OCTREE] = &Collide<Cone, OcTree>;
collision_matrix[GEOM_CYLINDER][GEOM_OCTREE] = &Collide<Cylinder, OcTree>;
collision_matrix[GEOM_CONVEX][GEOM_OCTREE] = &Collide<ConvexBase, OcTree>;
collision_matrix[GEOM_PLANE][GEOM_OCTREE] = &Collide<Plane, OcTree>;
collision_matrix[GEOM_HALFSPACE][GEOM_OCTREE] = &Collide<Halfspace, OcTree>;
collision_matrix[GEOM_OCTREE][GEOM_OCTREE] = &Collide<OcTree, OcTree>;
collision_matrix[GEOM_OCTREE][BV_AABB ] = &Collide<OcTree, BVHModel<AABB > >;
collision_matrix[GEOM_OCTREE][BV_OBB ] = &Collide<OcTree, BVHModel<OBB > >;
collision_matrix[GEOM_OCTREE][BV_RSS ] = &Collide<OcTree, BVHModel<RSS > >;
collision_matrix[GEOM_OCTREE][BV_OBBRSS] = &Collide<OcTree, BVHModel<OBBRSS > >;
collision_matrix[GEOM_OCTREE][BV_kIOS ] = &Collide<OcTree, BVHModel<kIOS > >;
collision_matrix[GEOM_OCTREE][BV_KDOP16] = &Collide<OcTree, BVHModel<KDOP<16> > >;
collision_matrix[GEOM_OCTREE][BV_KDOP18] = &Collide<OcTree, BVHModel<KDOP<18> > >;
collision_matrix[GEOM_OCTREE][BV_KDOP24] = &Collide<OcTree, BVHModel<KDOP<24> > >;
collision_matrix[BV_AABB ][GEOM_OCTREE] = &Collide<BVHModel<AABB >, OcTree>;
collision_matrix[BV_OBB ][GEOM_OCTREE] = &Collide<BVHModel<OBB >, OcTree>;
collision_matrix[BV_RSS ][GEOM_OCTREE] = &Collide<BVHModel<RSS >, OcTree>;
collision_matrix[BV_OBBRSS][GEOM_OCTREE] = &Collide<BVHModel<OBBRSS >, OcTree>;
collision_matrix[BV_kIOS ][GEOM_OCTREE] = &Collide<BVHModel<kIOS >, OcTree>;
collision_matrix[BV_KDOP16][GEOM_OCTREE] = &Collide<BVHModel<KDOP<16> >, OcTree>;
collision_matrix[BV_KDOP18][GEOM_OCTREE] = &Collide<BVHModel<KDOP<18> >, OcTree>;
collision_matrix[BV_KDOP24][GEOM_OCTREE] = &Collide<BVHModel<KDOP<24> >, OcTree>;
#endif
}
//template struct CollisionFunctionMatrix;
......
......@@ -37,7 +37,7 @@
#include <../src/collision_node.h>
#include "traversal/traversal_recurse.h"
#include <hpp/fcl/internal/traversal_recurse.h>
namespace hpp
{
......
......@@ -41,8 +41,8 @@
/// @cond INTERNAL
#include <hpp/fcl/BVH/BVH_front.h>
#include "traversal/traversal_node_base.h"
#include "traversal/traversal_node_bvhs.h"
#include <hpp/fcl/internal/traversal_node_base.h>
#include <hpp/fcl/internal/traversal_node_bvhs.h>
/// @brief collision and distance function on traversal nodes. these functions provide a higher level abstraction for collision functions provided in collision_func_matrix
namespace hpp
......
......@@ -40,8 +40,8 @@
#include <limits>
#include <hpp/fcl/math/transform.h>
#include <hpp/fcl/shape/geometric_shapes.h>
#include "distance_func_matrix.h"
#include "../src/narrowphase/details.h"
#include <../src/distance_func_matrix.h>
#include "narrowphase/details.h"
namespace hpp
{
......
......@@ -40,8 +40,8 @@
#include <limits>
#include <hpp/fcl/math/transform.h>
#include <hpp/fcl/shape/geometric_shapes.h>
#include "distance_func_matrix.h"
#include "../src/narrowphase/details.h"
#include <../src/distance_func_matrix.h>
#include "narrowphase/details.h"
namespace hpp
{
......
......@@ -40,8 +40,8 @@
#include <limits>
#include <hpp/fcl/math/transform.h>
#include <hpp/fcl/shape/geometric_shapes.h>
#include "distance_func_matrix.h"
#include "../src/narrowphase/details.h"
#include <../src/distance_func_matrix.h>
#include "narrowphase/details.h"
namespace hpp
{
......
......@@ -10,7 +10,7 @@
#include <limits>
#include <hpp/fcl/math/transform.h>
#include <hpp/fcl/shape/geometric_shapes.h>
#include "distance_func_matrix.h"
#include <../src/distance_func_matrix.h>
// Note that partial specialization of template functions is not allowed.
// Therefore, two implementations with the default narrow phase solvers are
......
......@@ -40,8 +40,8 @@
#include <limits>
#include <hpp/fcl/math/transform.h>
#include <hpp/fcl/shape/geometric_shapes.h>
#include "distance_func_matrix.h"
#include "../src/narrowphase/details.h"
#include <../src/distance_func_matrix.h>
#include "narrowphase/details.h"
namespace hpp
{
......
......@@ -40,8 +40,8 @@
#include <limits>
#include <hpp/fcl/math/transform.h>
#include <hpp/fcl/shape/geometric_shapes.h>
#include "distance_func_matrix.h"
#include "../src/narrowphase/details.h"
#include <../src/distance_func_matrix.h>
#include "narrowphase/details.h"
namespace hpp
{
......
......@@ -40,8 +40,8 @@
#include <limits>
#include <hpp/fcl/math/transform.h>
#include <hpp/fcl/shape/geometric_shapes.h>
#include "distance_func_matrix.h"
#include "../src/narrowphase/details.h"
#include <../src/distance_func_matrix.h>
#include "narrowphase/details.h"
namespace hpp
{
......
......@@ -40,8 +40,8 @@
#include <limits>
#include <hpp/fcl/math/transform.h>
#include <hpp/fcl/shape/geometric_shapes.h>
#include "distance_func_matrix.h"
#include "../src/narrowphase/details.h"
#include <../src/distance_func_matrix.h>
#include "narrowphase/details.h"
namespace hpp
{
......
......@@ -40,8 +40,8 @@
#include <limits>
#include <hpp/fcl/math/transform.h>
#include <hpp/fcl/shape/geometric_shapes.h>
#include "distance_func_matrix.h"
#include "../src/narrowphase/details.h"
#include <../src/distance_func_matrix.h>
#include "narrowphase/details.h"
namespace hpp
{
......
......@@ -40,8 +40,8 @@
#include <limits>
#include <hpp/fcl/math/transform.h>
#include <hpp/fcl/shape/geometric_shapes.h>
#include "distance_func_matrix.h"
#include "../src/narrowphase/details.h"
#include <../src/distance_func_matrix.h>
#include "narrowphase/details.h"
namespace hpp
{
......
......@@ -38,7 +38,8 @@
#include <hpp/fcl/distance_func_matrix.h>
#include <../src/collision_node.h>
#include "traversal/traversal_node_setup.h"
#include <hpp/fcl/internal/traversal_node_setup.h>
#include <../src/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>;