From 229f20780fa2562e65aef90a4285a3972f66b0b0 Mon Sep 17 00:00:00 2001
From: jpan <jpan@253336fb-580f-4252-a368-f3cef5a2a82b>
Date: Sat, 21 Jul 2012 01:46:15 +0000
Subject: [PATCH] add octomap in shape; all collision_managers can handle
octomap now, and DynamicAABB manager has special optimization for octomap.
Need more test.
git-svn-id: https://kforge.ros.org/fcl/fcl_ros@135 253336fb-580f-4252-a368-f3cef5a2a82b
---
trunk/fcl/include/fcl/BV.h | 67 +++++
trunk/fcl/include/fcl/broad_phase_collision.h | 63 +++-
trunk/fcl/include/fcl/collision_func_matrix.h | 2 +-
trunk/fcl/include/fcl/collision_object.h | 3 +
trunk/fcl/include/fcl/distance_func_matrix.h | 2 +-
trunk/fcl/include/fcl/geometric_shapes.h | 17 +-
trunk/fcl/include/fcl/octree.h | 39 ++-
trunk/fcl/include/fcl/simple_setup.h | 200 +++++++++++++
trunk/fcl/include/fcl/traversal_node_octree.h | 250 ++++++++--------
trunk/fcl/src/broad_phase_collision.cpp | 276 +++++++++++++++++-
trunk/fcl/src/collision_func_matrix.cpp | 156 +++++++++-
trunk/fcl/src/distance_func_matrix.cpp | 107 ++++++-
trunk/fcl/src/geometric_shapes_utility.cpp | 4 -
trunk/fcl/src/octomap_extension.cpp | 35 ---
14 files changed, 1015 insertions(+), 206 deletions(-)
diff --git a/trunk/fcl/include/fcl/BV.h b/trunk/fcl/include/fcl/BV.h
index 4ae915f2..9b84c478 100644
--- a/trunk/fcl/include/fcl/BV.h
+++ b/trunk/fcl/include/fcl/BV.h
@@ -44,11 +44,78 @@
#include "fcl/BV/RSS.h"
#include "fcl/BV/OBBRSS.h"
#include "fcl/BV/kIOS.h"
+#include "fcl/transform.h"
/** \brief Main namespace */
namespace fcl
{
+
+template<typename BV1, typename BV2>
+class Converter
+{
+private:
+ static void convert(const BV1& bv1, const SimpleTransform& tf1, BV2& bv2)
+ {
+ // should only use the specialized version, so it is private.
+ }
+};
+
+template<typename BV1>
+class Converter<BV1, AABB>
+{
+public:
+ static void convert(const BV1& bv1, const SimpleTransform& tf1, AABB& bv2)
+ {
+ const Vec3f& center = bv1.center();
+ FCL_REAL r = Vec3f(bv1.width(), bv1.height(), bv1.depth()).length() * 0.5;
+ Vec3f delta(r, r, r);
+ Vec3f center2 = tf1.transform(center);
+ bv2.min_ = center2 - delta;
+ bv2.max_ = center2 + delta;
+ }
+};
+
+
+template<>
+class Converter<AABB, AABB>
+{
+public:
+ static void convert(const AABB& bv1, const SimpleTransform& tf1, AABB& bv2)
+ {
+ const Vec3f& center = bv1.center();
+ FCL_REAL r = (bv1.max_ - bv1.min_).length() * 0.5;
+ Vec3f center2 = tf1.transform(center);
+ Vec3f delta(r, r, r);
+ bv2.min_ = center2 - delta;
+ bv2.max_ = center2 + delta;
+ }
+};
+
+template<>
+class Converter<AABB, OBB>
+{
+public:
+ static void convert(const AABB& bv1, const SimpleTransform& tf1, OBB& bv2)
+ {
+ bv2.extent = (bv1.max_ - bv1.min_) * 0.5;
+ bv2.To = tf1.transform(bv1.center());
+ const Matrix3f& R = tf1.getRotation();
+ bv2.axis[0] = R.getColumn(0);
+ bv2.axis[1] = R.getColumn(1);
+ bv2.axis[2] = R.getColumn(2);
+ }
+};
+
+
+
+
+template<typename BV1, typename BV2>
+static inline void convertBV(const BV1& bv1, const SimpleTransform& tf1, BV2& bv2)
+{
+ Converter<BV1, BV2>::convert(bv1, tf1, bv2);
+}
+
}
#endif
diff --git a/trunk/fcl/include/fcl/broad_phase_collision.h b/trunk/fcl/include/fcl/broad_phase_collision.h
index 1402f5d1..ae3c4a4a 100644
--- a/trunk/fcl/include/fcl/broad_phase_collision.h
+++ b/trunk/fcl/include/fcl/broad_phase_collision.h
@@ -45,6 +45,7 @@
#include "fcl/interval_tree.h"
#include "fcl/hierarchy_tree.h"
#include "fcl/hash.h"
+#include "fcl/octree.h"
#include <vector>
#include <list>
#include <iostream>
@@ -1271,7 +1272,6 @@ protected:
bool setup_;
};
-
class DynamicAABBTreeCollisionManager : public BroadPhaseCollisionManager
{
public:
@@ -1335,7 +1335,17 @@ public:
void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
{
if(size() == 0) return;
- collisionRecurse(dtree.getRoot(), obj, cdata, callback);
+ switch(obj->getCollisionGeometry()->getNodeType())
+ {
+ case GEOM_OCTREE:
+ {
+ const OcTree* octree = static_cast<const OcTree*>(obj->getCollisionGeometry());
+ collisionRecurse(dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback);
+ }
+ break;
+ default:
+ collisionRecurse(dtree.getRoot(), obj, cdata, callback);
+ }
}
/** \brief perform distance computation between one object and all the objects belonging to the manager */
@@ -1343,7 +1353,17 @@ public:
{
if(size() == 0) return;
FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
- distanceRecurse(dtree.getRoot(), obj, cdata, callback, min_dist);
+ switch(obj->getCollisionGeometry()->getNodeType())
+ {
+ case GEOM_OCTREE:
+ {
+ const OcTree* octree = static_cast<const OcTree*>(obj->getCollisionGeometry());
+ distanceRecurse(dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback, min_dist);
+ }
+ break;
+ default:
+ distanceRecurse(dtree.getRoot(), obj, cdata, callback, min_dist);
+ }
}
/** \brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision) */
@@ -1412,6 +1432,12 @@ private:
bool selfDistanceRecurse(DynamicAABBNode* root, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
void update_(CollisionObject* updated_obj);
+
+ /** \brief special manager-obj collision for octree */
+ bool collisionRecurse(DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const SimpleTransform& tf2, void* cdata, CollisionCallBack callback) const;
+
+ bool distanceRecurse(DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const SimpleTransform& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
+
};
@@ -1479,7 +1505,17 @@ public:
void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
{
if(size() == 0) return;
- collisionRecurse(dtree.getNodes(), dtree.getRoot(), obj, cdata, callback);
+ switch(obj->getCollisionGeometry()->getNodeType())
+ {
+ case GEOM_OCTREE:
+ {
+ const OcTree* octree = static_cast<const OcTree*>(obj->getCollisionGeometry());
+ collisionRecurse(dtree.getNodes(), dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback);
+ }
+ break;
+ default:
+ collisionRecurse(dtree.getNodes(), dtree.getRoot(), obj, cdata, callback);
+ }
}
/** \brief perform distance computation between one object and all the objects belonging to the manager */
@@ -1487,7 +1523,17 @@ public:
{
if(size() == 0) return;
FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
- distanceRecurse(dtree.getNodes(), dtree.getRoot(), obj, cdata, callback, min_dist);
+ switch(obj->getCollisionGeometry()->getNodeType())
+ {
+ case GEOM_OCTREE:
+ {
+ const OcTree* octree = static_cast<const OcTree*>(obj->getCollisionGeometry());
+ distanceRecurse(dtree.getNodes(), dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback, min_dist);
+ }
+ break;
+ default:
+ distanceRecurse(dtree.getNodes(), dtree.getRoot(), obj, cdata, callback, min_dist);
+ }
}
/** \brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision) */
@@ -1556,6 +1602,13 @@ private:
bool selfDistanceRecurse(DynamicAABBNode* nodes, size_t root, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
void update_(CollisionObject* updated_obj);
+
+ /** \brief special manager-obj collision for octree */
+ bool collisionRecurse(DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const SimpleTransform& tf2, void* cdata, CollisionCallBack callback) const;
+
+ bool distanceRecurse(DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const SimpleTransform& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
+
+
};
}
diff --git a/trunk/fcl/include/fcl/collision_func_matrix.h b/trunk/fcl/include/fcl/collision_func_matrix.h
index c46c6e08..ab18a847 100644
--- a/trunk/fcl/include/fcl/collision_func_matrix.h
+++ b/trunk/fcl/include/fcl/collision_func_matrix.h
@@ -50,7 +50,7 @@ struct CollisionFunctionMatrix
{
typedef int (*CollisionFunc)(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts);
- CollisionFunc collision_matrix[NODE_COUNT-1][NODE_COUNT-1];
+ CollisionFunc collision_matrix[NODE_COUNT][NODE_COUNT];
CollisionFunctionMatrix();
};
diff --git a/trunk/fcl/include/fcl/collision_object.h b/trunk/fcl/include/fcl/collision_object.h
index 49e8ef70..9ce444e6 100644
--- a/trunk/fcl/include/fcl/collision_object.h
+++ b/trunk/fcl/include/fcl/collision_object.h
@@ -89,17 +89,20 @@ class CollisionObject
public:
CollisionObject(const boost::shared_ptr<CollisionGeometry> &cgeom_) : cgeom(cgeom_)
{
+ cgeom->computeLocalAABB();
computeAABB();
}
CollisionObject(const boost::shared_ptr<CollisionGeometry> &cgeom_, const SimpleTransform& tf) : cgeom(cgeom_), t(tf)
{
+ cgeom->computeLocalAABB();
computeAABB();
}
CollisionObject(const boost::shared_ptr<CollisionGeometry> &cgeom_, const Matrix3f& R, const Vec3f& T):
cgeom(cgeom_), t(SimpleTransform(R, T))
{
+ cgeom->computeLocalAABB();
computeAABB();
}
diff --git a/trunk/fcl/include/fcl/distance_func_matrix.h b/trunk/fcl/include/fcl/distance_func_matrix.h
index ea697304..622f5a90 100644
--- a/trunk/fcl/include/fcl/distance_func_matrix.h
+++ b/trunk/fcl/include/fcl/distance_func_matrix.h
@@ -48,7 +48,7 @@ struct DistanceFunctionMatrix
{
typedef FCL_REAL (*DistanceFunc)(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver);
- DistanceFunc distance_matrix[NODE_COUNT-1][NODE_COUNT-1];
+ DistanceFunc distance_matrix[NODE_COUNT][NODE_COUNT];
DistanceFunctionMatrix();
};
diff --git a/trunk/fcl/include/fcl/geometric_shapes.h b/trunk/fcl/include/fcl/geometric_shapes.h
index 3dfa447c..c4288c01 100644
--- a/trunk/fcl/include/fcl/geometric_shapes.h
+++ b/trunk/fcl/include/fcl/geometric_shapes.h
@@ -62,7 +62,6 @@ class Triangle2 : public ShapeBase
public:
Triangle2(const Vec3f& a_, const Vec3f& b_, const Vec3f& c_) : ShapeBase(), a(a_), b(b_), c(c_)
{
- computeLocalAABB();
}
void computeLocalAABB();
@@ -78,14 +77,14 @@ class Box : public ShapeBase
public:
Box(FCL_REAL x, FCL_REAL y, FCL_REAL z) : ShapeBase(), side(x, y, z)
{
- computeLocalAABB();
}
Box(const Vec3f& side_) : ShapeBase(), side(side_)
{
- computeLocalAABB();
}
+ Box() {}
+
/** box side length */
Vec3f side;
@@ -102,7 +101,6 @@ class Sphere : public ShapeBase
public:
Sphere(FCL_REAL radius_) : ShapeBase(), radius(radius_)
{
- computeLocalAABB();
}
/** \brief Radius of the sphere */
@@ -121,7 +119,6 @@ class Capsule : public ShapeBase
public:
Capsule(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_), lz(lz_)
{
- computeLocalAABB();
}
/** \brief Radius of capsule */
@@ -143,7 +140,6 @@ class Cone : public ShapeBase
public:
Cone(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_), lz(lz_)
{
- computeLocalAABB();
}
@@ -166,7 +162,6 @@ class Cylinder : public ShapeBase
public:
Cylinder(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_), lz(lz_)
{
- computeLocalAABB();
}
@@ -211,8 +206,6 @@ public:
center = sum * (FCL_REAL)(1.0 / num_points);
fillEdges();
-
- computeLocalAABB();
}
/** Copy constructor */
@@ -225,8 +218,6 @@ public:
polygons = other.polygons;
edges = new Edge[other.num_edges];
memcpy(edges, other.edges, sizeof(Edge) * num_edges);
-
- computeLocalAABB();
}
~Convex()
@@ -277,16 +268,12 @@ public:
Plane(const Vec3f& n_, FCL_REAL d_) : ShapeBase(), n(n_), d(d_)
{
unitNormalTest();
-
- computeLocalAABB();
}
/** \brief Construct a plane with normal direction and offset */
Plane(FCL_REAL a, FCL_REAL b, FCL_REAL c, FCL_REAL d_) : n(a, b, c), d(d_)
{
unitNormalTest();
-
- computeLocalAABB();
}
/** \brief Compute AABB */
diff --git a/trunk/fcl/include/fcl/octree.h b/trunk/fcl/include/fcl/octree.h
index 7dec38c3..e598dbe2 100644
--- a/trunk/fcl/include/fcl/octree.h
+++ b/trunk/fcl/include/fcl/octree.h
@@ -75,7 +75,7 @@ public:
return tree->getRoot();
}
- inline bool isNodeOccupied(OcTreeNode* node) const
+ inline bool isNodeOccupied(const OcTreeNode* node) const
{
return tree->isNodeOccupied(node);
}
@@ -116,6 +116,43 @@ public:
}
};
+static inline void computeChildBV(const AABB& root_bv, unsigned int i, AABB& child_bv)
+{
+ if(i&1)
+ {
+ child_bv.min_[0] = (root_bv.min_[0] + root_bv.max_[0]) * 0.5;
+ child_bv.max_[0] = root_bv.max_[0];
+ }
+ else
+ {
+ child_bv.min_[0] = root_bv.min_[0];
+ child_bv.max_[0] = (root_bv.min_[0] + root_bv.max_[0]) * 0.5;
+ }
+
+ if(i&2)
+ {
+ child_bv.min_[1] = (root_bv.min_[1] + root_bv.max_[1]) * 0.5;
+ child_bv.max_[1] = root_bv.max_[1];
+ }
+ else
+ {
+ child_bv.min_[1] = root_bv.min_[1];
+ child_bv.max_[1] = (root_bv.min_[1] + root_bv.max_[1]) * 0.5;
+ }
+
+ if(i&4)
+ {
+ child_bv.min_[2] = (root_bv.min_[2] + root_bv.max_[2]) * 0.5;
+ child_bv.max_[2] = root_bv.max_[2];
+ }
+ else
+ {
+ child_bv.min_[2] = root_bv.min_[2];
+ child_bv.max_[2] = (root_bv.min_[2] + root_bv.max_[2]) * 0.5;
+ }
+}
+
+
}
diff --git a/trunk/fcl/include/fcl/simple_setup.h b/trunk/fcl/include/fcl/simple_setup.h
index 1acb7e96..ba3ed19d 100644
--- a/trunk/fcl/include/fcl/simple_setup.h
+++ b/trunk/fcl/include/fcl/simple_setup.h
@@ -41,12 +41,212 @@
#include "fcl/traversal_node_bvhs.h"
#include "fcl/traversal_node_shapes.h"
#include "fcl/traversal_node_bvh_shape.h"
+#include "fcl/traversal_node_octree.h"
#include "fcl/BVH_utility.h"
/** \brief Main namespace */
namespace fcl
{
+template<typename NarrowPhaseSolver>
+bool initialize(OcTreeCollisionTraversalNode<NarrowPhaseSolver>& node,
+ const OcTree& model1, const SimpleTransform& tf1,
+ const OcTree& model2, const SimpleTransform& tf2,
+ const OcTreeSolver<NarrowPhaseSolver>* otsolver,
+ int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
+{
+ node.enable_contact = enable_contact;
+ node.num_max_contacts = num_max_contacts;
+ node.exhaustive = exhaustive;
+
+ node.model1 = &model1;
+ node.model2 = &model2;
+
+ node.otsolver = otsolver;
+
+ node.tf1 = tf1;
+ node.tf2 = tf2;
+
+ return true;
+}
+
+template<typename NarrowPhaseSolver>
+bool initialize(OcTreeDistanceTraversalNode<NarrowPhaseSolver>& node,
+ const OcTree& model1, const SimpleTransform& tf1,
+ const OcTree& model2, const SimpleTransform& tf2,
+ const OcTreeSolver<NarrowPhaseSolver>* otsolver)
+{
+ node.model1 = &model1;
+ node.model2 = &model2;
+
+ node.otsolver = otsolver;
+
+ node.tf1 = tf1;
+ node.tf2 = tf2;
+
+ return true;
+}
+
+template<typename S, typename NarrowPhaseSolver>
+bool initialize(ShapeOcTreeCollisionTraversalNode<S, NarrowPhaseSolver>& node,
+ const S& model1, const SimpleTransform& tf1,
+ const OcTree& model2, const SimpleTransform& tf2,
+ const OcTreeSolver<NarrowPhaseSolver>* otsolver,
+ int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
+{
+ node.enable_contact = enable_contact;
+ node.num_max_contacts = num_max_contacts;
+ node.exhaustive = exhaustive;
+
+ node.model1 = &model1;
+ node.model2 = &model2;
+
+ node.otsolver = otsolver;
+
+ node.tf1 = tf1;
+ node.tf2 = tf2;
+
+ return true;
+}
+
+template<typename S, typename NarrowPhaseSolver>
+bool initialize(OcTreeShapeCollisionTraversalNode<S, NarrowPhaseSolver>& node,
+ const OcTree& model1, const SimpleTransform& tf1,
+ const S& model2, const SimpleTransform& tf2,
+ const OcTreeSolver<NarrowPhaseSolver>* otsolver,
+ int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
+{
+ node.enable_contact = enable_contact;
+ node.num_max_contacts = num_max_contacts;
+ node.exhaustive = exhaustive;
+
+ node.model1 = &model1;
+ node.model2 = &model2;
+
+ node.otsolver = otsolver;
+
+ node.tf1 = tf1;
+ node.tf2 = tf2;
+
+ return true;
+}
+
+template<typename S, typename NarrowPhaseSolver>
+bool initialize(ShapeOcTreeDistanceTraversalNode<S, NarrowPhaseSolver>& node,
+ const S& model1, const SimpleTransform& tf1,
+ const OcTree& model2, const SimpleTransform& tf2,
+ const OcTreeSolver<NarrowPhaseSolver>* otsolver)
+{
+ node.model1 = &model1;
+ node.model2 = &model2;
+
+ node.otsolver = otsolver;
+
+ node.tf1 = tf1;
+ node.tf2 = tf2;
+
+ return true;
+}
+
+
+template<typename S, typename NarrowPhaseSolver>
+bool initialize(OcTreeShapeDistanceTraversalNode<S, NarrowPhaseSolver>& node,
+ const OcTree& model1, const SimpleTransform& tf1,
+ const S& model2, const SimpleTransform& tf2,
+ const OcTreeSolver<NarrowPhaseSolver>* otsolver)
+{
+ node.model1 = &model1;
+ node.model2 = &model2;
+
+ node.otsolver = otsolver;
+
+ node.tf1 = tf1;
+ node.tf2 = tf2;
+
+ return true;
+}
+
+template<typename BV, typename NarrowPhaseSolver>
+bool initialize(MeshOcTreeCollisionTraversalNode<BV, NarrowPhaseSolver>& node,
+ const BVHModel<BV>& model1, const SimpleTransform& tf1,
+ const OcTree& model2, const SimpleTransform& tf2,
+ const OcTreeSolver<NarrowPhaseSolver>* otsolver,
+ int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
+{
+ node.enable_contact = enable_contact;
+ node.num_max_contacts = num_max_contacts;
+ node.exhaustive = exhaustive;
+
+ node.model1 = &model1;
+ node.model2 = &model2;
+
+ node.otsolver = otsolver;
+
+ node.tf1 = tf1;
+ node.tf2 = tf2;
+
+ return true;
+}
+
+template<typename BV, typename NarrowPhaseSolver>
+bool initialize(OcTreeMeshCollisionTraversalNode<BV, NarrowPhaseSolver>& node,
+ const OcTree& model1, const SimpleTransform& tf1,
+ const BVHModel<BV>& model2, const SimpleTransform& tf2,
+ const OcTreeSolver<NarrowPhaseSolver>* otsolver,
+ int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
+{
+ node.enable_contact = enable_contact;
+ node.num_max_contacts = num_max_contacts;
+ node.exhaustive = exhaustive;
+
+ node.model1 = &model1;
+ node.model2 = &model2;
+
+ node.otsolver = otsolver;
+
+ node.tf1 = tf1;
+ node.tf2 = tf2;
+
+ return true;
+}
+
+template<typename BV, typename NarrowPhaseSolver>
+bool initialize(MeshOcTreeDistanceTraversalNode<BV, NarrowPhaseSolver>& node,
+ const BVHModel<BV>& model1, const SimpleTransform& tf1,
+ const OcTree& model2, const SimpleTransform& tf2,
+ const OcTreeSolver<NarrowPhaseSolver>* otsolver)
+{
+ node.model1 = &model1;
+ node.model2 = &model2;
+
+ node.otsolver = otsolver;
+
+ node.tf1 = tf1;
+ node.tf2 = tf2;
+
+ return true;
+}
+
+
+template<typename BV, typename NarrowPhaseSolver>
+bool initialize(OcTreeMeshDistanceTraversalNode<BV, NarrowPhaseSolver>& node,
+ const OcTree& model1, const SimpleTransform& tf1,
+ const BVHModel<BV>& model2, const SimpleTransform& tf2,
+ const OcTreeSolver<NarrowPhaseSolver>* otsolver)
+{
+ node.model1 = &model1;
+ node.model2 = &model2;
+
+ node.otsolver = otsolver;
+
+ node.tf1 = tf1;
+ node.tf2 = tf2;
+
+ return true;
+}
+
+
+
/** \brief Initialize traversal node for collision between two geometric shapes */
template<typename S1, typename S2, typename NarrowPhaseSolver>
bool initialize(ShapeCollisionTraversalNode<S1, S2, NarrowPhaseSolver>& node,
diff --git a/trunk/fcl/include/fcl/traversal_node_octree.h b/trunk/fcl/include/fcl/traversal_node_octree.h
index 739e4dce..41368bc5 100644
--- a/trunk/fcl/include/fcl/traversal_node_octree.h
+++ b/trunk/fcl/include/fcl/traversal_node_octree.h
@@ -49,8 +49,8 @@ namespace fcl
struct OcTreeCollisionPair
{
/** \brief The pointer to the octree nodes */
- OcTree::OcTreeNode* node1;
- OcTree::OcTreeNode* node2;
+ const OcTree::OcTreeNode* node1;
+ const OcTree::OcTreeNode* node2;
/** \brief contact_point */
Vec3f contact_point;
@@ -61,10 +61,10 @@ struct OcTreeCollisionPair
/** \brief penetration depth between two octree cells */
FCL_REAL penetration_depth;
- OcTreeCollisionPair(OcTree::OcTreeNode* node1_, OcTree::OcTreeNode* node2_)
+ OcTreeCollisionPair(const OcTree::OcTreeNode* node1_, const OcTree::OcTreeNode* node2_)
: node1(node1_), node2(node2_) {}
- OcTreeCollisionPair(OcTree::OcTreeNode* node1_, OcTree::OcTreeNode* node2_,
+ OcTreeCollisionPair(const OcTree::OcTreeNode* node1_, const OcTree::OcTreeNode* node2_,
const Vec3f& contact_point_, const Vec3f& normal_, FCL_REAL penetration_depth_)
: node1(node1_), node2(node2_), contact_point(contact_point_), normal(normal_), penetration_depth(penetration_depth_) {}
};
@@ -72,7 +72,7 @@ struct OcTreeCollisionPair
struct OcTreeMeshCollisionPair
{
/** \brief The pointer to the octree node */
- OcTree::OcTreeNode* node;
+ const OcTree::OcTreeNode* node;
/** \brief The index of BVH primitive */
int id;
@@ -86,10 +86,10 @@ struct OcTreeMeshCollisionPair
/** \brief penetration depth between two octree cells */
FCL_REAL penetration_depth;
- OcTreeMeshCollisionPair(OcTree::OcTreeNode* node_, int id_)
+ OcTreeMeshCollisionPair(const OcTree::OcTreeNode* node_, int id_)
: node(node_), id(id_) {}
- OcTreeMeshCollisionPair(OcTree::OcTreeNode* node_, int id_,
+ OcTreeMeshCollisionPair(const OcTree::OcTreeNode* node_, int id_,
const Vec3f& contact_point_, const Vec3f& normal_, FCL_REAL penetration_depth_)
: node(node_), id(id_), contact_point(contact_point_), normal(normal_), penetration_depth(penetration_depth_) {}
};
@@ -98,7 +98,7 @@ struct OcTreeMeshCollisionPair
struct OcTreeShapeCollisionPair
{
/** \brief The pointer to the octree node */
- OcTree::OcTreeNode* node;
+ const OcTree::OcTreeNode* node;
/** \brief contact_point */
Vec3f contact_point;
@@ -109,8 +109,8 @@ struct OcTreeShapeCollisionPair
/** \brief penetration depth between two octree cells */
FCL_REAL penetration_depth;
- OcTreeShapeCollisionPair(OcTree::OcTreeNode* node_) : node(node_) {}
- OcTreeShapeCollisionPair(OcTree::OcTreeNode* node_,
+ OcTreeShapeCollisionPair(const OcTree::OcTreeNode* node_) : node(node_) {}
+ OcTreeShapeCollisionPair(const OcTree::OcTreeNode* node_,
const Vec3f& contact_point_, const Vec3f& normal_, FCL_REAL penetration_depth_)
: node(node_), contact_point(contact_point_), normal(normal_), penetration_depth(penetration_depth_) {}
};
@@ -120,24 +120,32 @@ template<typename NarrowPhaseSolver>
class OcTreeSolver
{
private:
- NarrowPhaseSolver* solver;
+ const NarrowPhaseSolver* solver;
+
+ mutable int num_max_contacts;
+ mutable bool enable_contact;
+ mutable bool exhaustive;
public:
- OcTreeSolver(NarrowPhaseSolver* solver_) : solver(solver_) {}
+ OcTreeSolver(const NarrowPhaseSolver* solver_) : solver(solver_) {}
- void OcTreeIntersect(OcTree* tree1, OcTree* tree2,
+ void OcTreeIntersect(const OcTree* tree1, const OcTree* tree2,
const SimpleTransform& tf1, const SimpleTransform& tf2,
std::vector<OcTreeCollisionPair>& pairs,
- int num_max_contacts, bool enable_contact, bool exhaustive)
+ int num_max_contacts_, bool enable_contact_, bool exhaustive_) const
{
+ num_max_contacts = num_max_contacts_;
+ enable_contact = enable_contact_;
+ exhaustive = exhaustive_;
+
OcTreeIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(),
tree2, tree2->getRoot(), tree2->getRootBV(),
- tf1, tf2, pairs, num_max_contacts, enable_contact, exhaustive);
+ tf1, tf2, pairs);
}
- FCL_REAL OcTreeDistance(OcTree* tree1, OcTree* tree2,
- const SimpleTransform& tf1, const SimpleTransform& tf2)
+ FCL_REAL OcTreeDistance(const OcTree* tree1, const OcTree* tree2,
+ const SimpleTransform& tf1, const SimpleTransform& tf2) const
{
FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
@@ -149,25 +157,28 @@ public:
}
template<typename BV>
- void OcTreeMeshIntersect(OcTree* tree1, BVHModel<BV>* tree2,
+ void OcTreeMeshIntersect(const OcTree* tree1, const BVHModel<BV>* tree2,
const SimpleTransform& tf1, const SimpleTransform& tf2,
std::vector<OcTreeMeshCollisionPair>& pairs,
- int num_max_contacts, bool enable_contact, bool exhaustive)
+ int num_max_contacts_, bool enable_contact_, bool exhaustive_) const
{
+ num_max_contacts = num_max_contacts_;
+ enable_contact = enable_contact_;
+ exhaustive = exhaustive_;
+
OcTreeMeshIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(),
- tree2, 0, tree2->vertices, tree2->tri_indices,
- tf1, tf2,
- pairs, num_max_contacts, enable_contact, exhaustive);
+ tree2, 0,
+ tf1, tf2, pairs);
}
template<typename BV>
- FCL_REAL OcTreeMeshDistance(OcTree* tree1, BVHModel<BV>* tree2,
- const SimpleTransform& tf1, const SimpleTransform& tf2)
+ FCL_REAL OcTreeMeshDistance(const OcTree* tree1, const BVHModel<BV>* tree2,
+ const SimpleTransform& tf1, const SimpleTransform& tf2) const
{
FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
OcTreeMeshDistanceRecurse(tree1, tree1->getRoot(), tree1->getRootBV(),
- tree2, 0, tree2->vertices, tree2->tri_indices,
+ tree2, 0,
tf1, tf2, min_dist);
return min_dist;
@@ -175,26 +186,29 @@ public:
template<typename BV>
- void MeshOcTreeIntersect(BVHModel<BV>* tree1, OcTree* tree2,
+ void MeshOcTreeIntersect(const BVHModel<BV>* tree1, const OcTree* tree2,
const SimpleTransform& tf1, const SimpleTransform& tf2,
std::vector<OcTreeMeshCollisionPair>& pairs,
- int num_max_contacts, bool enable_contact, bool exhaustive)
+ int num_max_contacts_, bool enable_contact_, bool exhaustive_) const
{
+ num_max_contacts = num_max_contacts_;
+ enable_contact = enable_contact_;
+ exhaustive = exhaustive_;
+
OcTreeMeshIntersectRecurse(tree2, tree2->getRoot(), tree2->getRootBV(),
- tree1, 0, tree1->vertices, tree1->tri_indices,
- tf2, tf1,
- pairs, num_max_contacts, enable_contact, exhaustive);
+ tree1, 0,
+ tf2, tf1, pairs);
}
template<typename BV>
- FCL_REAL MeshOcTreeDistance(BVHModel<BV>* tree1, OcTree* tree2,
- const SimpleTransform& tf1, const SimpleTransform& tf2)
+ FCL_REAL MeshOcTreeDistance(const BVHModel<BV>* tree1, const OcTree* tree2,
+ const SimpleTransform& tf1, const SimpleTransform& tf2) const
{
FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
- OcTreeMeshDistanceRecurse(tree1, 0, tree1->vertices, tree1->tri_indices,
+ OcTreeMeshDistanceRecurse(tree1, 0,
tree2, tree2->getRoot(), tree2->getRootBV(),
tf1, tf2, min_dist);
@@ -202,11 +216,15 @@ public:
}
template<typename S>
- void OcTreeShapeIntersect(OcTree* tree, const S& s,
+ void OcTreeShapeIntersect(const OcTree* tree, const S& s,
const SimpleTransform& tf1, const SimpleTransform& tf2,
std::vector<OcTreeShapeCollisionPair>& pairs,
- int num_max_contacts, bool enable_contact, bool exhaustive)
+ int num_max_contacts_, bool enable_contact_, bool exhaustive_) const
{
+ num_max_contacts = num_max_contacts_;
+ enable_contact = enable_contact_;
+ exhaustive = exhaustive_;
+
AABB bv2;
computeBV<AABB>(s, SimpleTransform(), bv2);
Box box2;
@@ -214,17 +232,20 @@ public:
constructBox(bv2, tf2, box2, box2_tf);
OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(),
s, box2, box2_tf,
- tf1, tf2,
- pairs, num_max_contacts, enable_contact, exhaustive);
+ tf1, tf2, pairs);
}
template<typename S>
- void ShapeOcTreeIntersect(const S& s, OcTree* tree,
+ void ShapeOcTreeIntersect(const S& s, const OcTree* tree,
const SimpleTransform& tf1, const SimpleTransform& tf2,
std::vector<OcTreeShapeCollisionPair>& pairs,
- int num_max_contacts, bool enable_contact, bool exhaustive)
+ int num_max_contacts_, bool enable_contact_, bool exhaustive_) const
{
+ num_max_contacts = num_max_contacts_;
+ enable_contact = enable_contact_;
+ exhaustive = exhaustive_;
+
AABB bv1;
computeBV<AABB>(s, SimpleTransform(), bv1);
Box box1;
@@ -232,13 +253,12 @@ public:
constructBox(bv1, tf1, box1, box1_tf);
OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(),
s, box1, box1_tf,
- tf2, tf1,
- pairs, num_max_contacts, enable_contact, exhaustive);
+ tf2, tf1, pairs);
}
template<typename S>
- FCL_REAL OcTreeShapeDistance(OcTree* tree, const S& s,
- const SimpleTransform& tf1, const SimpleTransform& tf2)
+ FCL_REAL OcTreeShapeDistance(const OcTree* tree, const S& s,
+ const SimpleTransform& tf1, const SimpleTransform& tf2) const
{
AABB bv2;
computeBV<AABB>(s, SimpleTransform(), bv2);
@@ -255,8 +275,8 @@ public:
}
template<typename S>
- FCL_REAL ShapeOcTreeDistance(const S& s, OcTree* tree,
- const SimpleTransform& tf1, const SimpleTransform& tf2)
+ FCL_REAL ShapeOcTreeDistance(const S& s, const OcTree* tree,
+ const SimpleTransform& tf1, const SimpleTransform& tf2) const
{
AABB bv1;
computeBV<AABB>(s, SimpleTransform(), bv1);
@@ -275,47 +295,11 @@ public:
private:
- static inline void computeChildBV(const AABB& root_bv, unsigned int i, AABB& child_bv)
- {
- if(i&1)
- {
- child_bv.min_[0] = (root_bv.min_[0] + root_bv.max_[0]) * 0.5;
- child_bv.max_[0] = root_bv.max_[0];
- }
- else
- {
- child_bv.min_[0] = root_bv.min_[0];
- child_bv.max_[0] = (root_bv.min_[0] + root_bv.max_[0]) * 0.5;
- }
-
- if(i&2)
- {
- child_bv.min_[1] = (root_bv.min_[1] + root_bv.max_[1]) * 0.5;
- child_bv.max_[1] = root_bv.max_[1];
- }
- else
- {
- child_bv.min_[1] = root_bv.min_[1];
- child_bv.max_[1] = (root_bv.min_[1] + root_bv.max_[1]) * 0.5;
- }
-
- if(i&4)
- {
- child_bv.min_[2] = (root_bv.min_[2] + root_bv.max_[2]) * 0.5;
- child_bv.max_[2] = root_bv.max_[2];
- }
- else
- {
- child_bv.min_[2] = root_bv.min_[2];
- child_bv.max_[2] = (root_bv.min_[2] + root_bv.max_[2]) * 0.5;
- }
- }
-
template<typename S>
- bool OcTreeShapeDistanceRecurse(OcTree* tree1, OcTree::OcTreeNode* root1, const AABB& bv1,
+ bool OcTreeShapeDistanceRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1,
const S& s, const Box& box2, const SimpleTransform& box2_tf,
const SimpleTransform& tf1, const SimpleTransform& tf2,
- FCL_REAL& min_dist)
+ FCL_REAL& min_dist) const
{
if(!root1->hasChildren())
{
@@ -341,7 +325,7 @@ private:
{
if(root1->childExists(i))
{
- OcTree::OcTreeNode* child = root1->getChild(i);
+ const OcTree::OcTreeNode* child = root1->getChild(i);
AABB child_bv;
computeChildBV(bv1, i, child_bv);
@@ -362,11 +346,10 @@ private:
}
template<typename S>
- bool OcTreeShapeIntersectRecurse(OcTree* tree1, OcTree::OcTreeNode* root1, const AABB& bv1,
+ bool OcTreeShapeIntersectRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1,
const S& s, const Box& box2, const SimpleTransform& box2_tf,
const SimpleTransform& tf1, const SimpleTransform& tf2,
- std::vector<OcTreeShapeCollisionPair>& pairs,
- int num_max_contacts, bool enable_contact, bool exhaustive)
+ std::vector<OcTreeShapeCollisionPair>& pairs) const
{
if(!root1->hasChildren())
{
@@ -409,11 +392,11 @@ private:
{
if(root1->childExists(i))
{
- OcTree::OcTreeNode* child = root1->getChild(i);
+ const OcTree::OcTreeNode* child = root1->getChild(i);
AABB child_bv;
computeChildBV(bv1, i, child_bv);
- if(OcTreeShapeIntersectRecurse(tree1, child, child_bv, s, box2, box2_tf, tf1, tf2, pairs, num_max_contacts, enable_contact, exhaustive))
+ if(OcTreeShapeIntersectRecurse(tree1, child, child_bv, s, box2, box2_tf, tf1, tf2, pairs))
return true;
}
}
@@ -422,9 +405,9 @@ private:
}
template<typename BV>
- bool OcTreeMeshDistanceRecurse(OcTree* tree1, OcTree::OcTreeNode* root1, const AABB& bv1,
- BVHModel<BV>* tree2, int root2, Vec3f* vertices2, Triangle* tri_indices2,
- const SimpleTransform& tf1, const SimpleTransform& tf2, FCL_REAL& min_dist)
+ bool OcTreeMeshDistanceRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1,
+ const BVHModel<BV>* tree2, int root2,
+ const SimpleTransform& tf1, const SimpleTransform& tf2, FCL_REAL& min_dist) const
{
if(!root1->hasChildren() && tree2->getBV(root2).isLeaf())
{
@@ -435,13 +418,13 @@ private:
constructBox(bv1, tf1, box, box_tf);
int primitive_id = tree2->getBV(root2).primitiveId();
- const Triangle& tri_id = tri_indices2[primitive_id];
- const Vec3f& p1 = vertices2[tri_id[0]];
- const Vec3f& p2 = vertices2[tri_id[1]];
- const Vec3f& p3 = vertices2[tri_id[2]];
+ const Triangle& tri_id = tree2->tri_indices[primitive_id];
+ const Vec3f& p1 = tree2->vertices[tri_id[0]];
+ const Vec3f& p2 = tree2->vertices[tri_id[1]];
+ const Vec3f& p3 = tree2->vertices[tri_id[2]];
FCL_REAL dist;
- solver->shapeDistance(box, box_tf, p1, p2, p3, tf2.getRotation(), tf2.getTranslation(), &dist);
+ solver->shapeTriangleDistance(box, box_tf, p1, p2, p3, tf2.getRotation(), tf2.getTranslation(), &dist);
if(dist < min_dist) min_dist = dist;
return (min_dist <= 0);
}
@@ -457,7 +440,7 @@ private:
{
if(root1->childExists(i))
{
- OcTree::OcTreeNode* child = root1->getChild(i);
+ const OcTree::OcTreeNode* child = root1->getChild(i);
AABB child_bv;
computeChildBV(bv1, i, child_bv);
@@ -470,7 +453,7 @@ private:
solver->shapeDistance(box1, box1_tf, box2, box2_tf, &dist);
if(dist < min_dist)
{
- if(OcTreeMeshDistanceRecurse(tree1, child, child_bv, tree2, root2, vertices2, tri_indices2, tf1, tf2, min_dist))
+ if(OcTreeMeshDistanceRecurse(tree1, child, child_bv, tree2, root2, tf1, tf2, min_dist))
return true;
}
}
@@ -489,7 +472,7 @@ private:
solver->shapeDistance(box1, box1_tf, box2, box2_tf, &dist);
if(dist < min_dist)
{
- if(OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, vertices2, tri_indices2, tf1, tf2, min_dist))
+ if(OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1, tf2, min_dist))
return true;
}
@@ -498,7 +481,7 @@ private:
solver->shapeDistance(box1, box1_tf, box2, box2_tf, &dist);
if(dist < min_dist)
{
- if(OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, vertices2, tri_indices2, tf1, tf2, min_dist))
+ if(OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1, tf2, min_dist))
return true;
}
}
@@ -508,11 +491,10 @@ private:
template<typename BV>
- bool OcTreeMeshIntersectRecurse(OcTree* tree1, OcTree::OcTreeNode* root1, const AABB& bv1,
- BVHModel<BV>* tree2, int root2, Vec3f* vertices2, Triangle* tri_indices2,
+ bool OcTreeMeshIntersectRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1,
+ const BVHModel<BV>* tree2, int root2,
const SimpleTransform& tf1, const SimpleTransform& tf2,
- std::vector<OcTreeMeshCollisionPair>& pairs,
- int num_max_contacts, bool enable_contact, bool exhaustive)
+ std::vector<OcTreeMeshCollisionPair>& pairs) const
{
if(!root1->hasChildren() && tree2->getBV(root2).isLeaf())
{
@@ -523,10 +505,10 @@ private:
constructBox(bv1, tf1, box, box_tf);
int primitive_id = tree2->getBV(root2).primitiveId();
- const Triangle& tri_id = tri_indices2[primitive_id];
- const Vec3f& p1 = vertices2[tri_id[0]];
- const Vec3f& p2 = vertices2[tri_id[1]];
- const Vec3f& p3 = vertices2[tri_id[2]];
+ const Triangle& tri_id = tree2->tri_indices[primitive_id];
+ const Vec3f& p1 = tree2->vertices[tri_id[0]];
+ const Vec3f& p2 = tree2->vertices[tri_id[1]];
+ const Vec3f& p3 = tree2->vertices[tri_id[2]];
if(!enable_contact)
@@ -564,21 +546,21 @@ private:
{
if(root1->childExists(i))
{
- OcTree::OcTreeNode* child = root1->getChild(i);
+ const OcTree::OcTreeNode* child = root1->getChild(i);
AABB child_bv;
computeChildBV(bv1, i, child_bv);
- if(OcTreeMeshIntersectRecurse(tree1, child, child_bv, tree2, root2, vertices2, tri_indices2, tf1, tf2, pairs, num_max_contacts, enable_contact, exhaustive))
+ if(OcTreeMeshIntersectRecurse(tree1, child, child_bv, tree2, root2, tf1, tf2, pairs))
return true;
}
}
}
else
{
- if(OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, tree2->getBV(root2).leftChild(), vertices2, tri_indices2, tf1, tf2, pairs, num_max_contacts, enable_contact, exhaustive))
+ if(OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, tree2->getBV(root2).leftChild(), tf1, tf2, pairs))
return true;
- if(OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, tree2->getBV(root2).rightChild(), vertices2, tri_indices2, tf1, tf2, pairs, num_max_contacts, enable_contact, exhaustive))
+ if(OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, tree2->getBV(root2).rightChild(), tf1, tf2, pairs))
return true;
}
@@ -586,10 +568,10 @@ private:
return false;
}
- bool OcTreeDistanceRecurse(OcTree* tree1, OcTree::OcTreeNode* root1, const AABB& bv1,
- OcTree* tree2, OcTree::OcTreeNode* root2, const AABB& bv2,
+ bool OcTreeDistanceRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1,
+ const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& bv2,
const SimpleTransform& tf1, const SimpleTransform& tf2,
- FCL_REAL& min_dist)
+ FCL_REAL& min_dist) const
{
if(!root1->hasChildren() && !root2->hasChildren())
{
@@ -618,7 +600,7 @@ private:
{
if(root1->childExists(i))
{
- OcTree::OcTreeNode* child = root1->getChild(i);
+ const OcTree::OcTreeNode* child = root1->getChild(i);
AABB child_bv;
computeChildBV(bv1, i, child_bv);
@@ -643,7 +625,7 @@ private:
{
if(root2->childExists(i))
{
- OcTree::OcTreeNode* child = root2->getChild(i);
+ const OcTree::OcTreeNode* child = root2->getChild(i);
AABB child_bv;
computeChildBV(bv2, i, child_bv);
@@ -667,11 +649,10 @@ private:
}
- bool OcTreeIntersectRecurse(OcTree* tree1, OcTree::OcTreeNode* root1, const AABB& bv1,
- OcTree* tree2, OcTree::OcTreeNode* root2, const AABB& bv2,
+ bool OcTreeIntersectRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1,
+ const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& bv2,
const SimpleTransform& tf1, const SimpleTransform& tf2,
- std::vector<OcTreeCollisionPair>& pairs,
- int num_max_contacts, bool enable_contact, bool exhaustive)
+ std::vector<OcTreeCollisionPair>& pairs) const
{
if(!root1->hasChildren() && !root2->hasChildren())
{
@@ -710,14 +691,13 @@ private:
{
if(root1->childExists(i))
{
- OcTree::OcTreeNode* child = root1->getChild(i);
+ const OcTree::OcTreeNode* child = root1->getChild(i);
AABB child_bv;
computeChildBV(bv1, i, child_bv);
if(OcTreeIntersectRecurse(tree1, child, child_bv,
tree2, root2, bv2,
- tf1, tf2, pairs,
- num_max_contacts, enable_contact, exhaustive))
+ tf1, tf2, pairs))
return true;
}
}
@@ -728,14 +708,13 @@ private:
{
if(root2->childExists(i))
{
- OcTree::OcTreeNode* child = root2->getChild(i);
+ const OcTree::OcTreeNode* child = root2->getChild(i);
AABB child_bv;
computeChildBV(bv2, i, child_bv);
if(OcTreeIntersectRecurse(tree1, root1, bv1,
tree2, child, child_bv,
- tf1, tf2, pairs,
- num_max_contacts, enable_contact, exhaustive))
+ tf1, tf2, pairs))
return true;
}
}
@@ -802,6 +781,7 @@ public:
otsolver = NULL;
}
+
FCL_REAL BVTesting(int, int) const
{
return -1;
@@ -809,7 +789,7 @@ public:
void leafTesting(int, int) const
{
- min_distance = otsolver->OcTreeDistance(model1, tf1, model2, tf2);
+ min_distance = otsolver->OcTreeDistance(model1, model2, tf1, tf2);
}
const OcTree* model1;
@@ -843,7 +823,7 @@ public:
void leafTesting(int, int) const
{
- otsolver->OcTreeShapeIntersect(model2, model1, tf2, tf1, pairs, num_max_contacts, enable_contact, exhaustive);
+ otsolver->OcTreeShapeIntersect(model2, *model1, tf2, tf1, pairs, num_max_contacts, enable_contact, exhaustive);
}
const S* model1;
@@ -883,7 +863,7 @@ public:
void leafTesting(int, int) const
{
- otsolver->OcTreeShapeIntersect(model1, model2, tf1, tf2, pairs, num_max_contacts, enable_contact, exhaustive);
+ otsolver->OcTreeShapeIntersect(model1, *model2, tf1, tf2, pairs, num_max_contacts, enable_contact, exhaustive);
}
const OcTree* model1;
@@ -919,7 +899,7 @@ public:
void leafTesting(int, int) const
{
- min_distance = otsolver->OcTreeShapeDistance(model2, tf2, model1, tf1);
+ min_distance = otsolver->OcTreeShapeDistance(model2, *model1, tf2, tf1);
}
const S* model1;
@@ -949,7 +929,7 @@ public:
void leafTesting(int, int) const
{
- min_distance = otsolver->OcTreeShapeDistance(model1, tf1, model2, tf2);
+ min_distance = otsolver->OcTreeShapeDistance(model1, *model2, tf1, tf2);
}
const OcTree* model1;
@@ -1061,7 +1041,7 @@ public:
void leafTesting(int, int) const
{
- min_distance = otsolver->OcTreeMeshDistance(model2, tf2, model1, tf1);
+ min_distance = otsolver->OcTreeMeshDistance(model2, model1, tf2, tf1);
}
const BVHModel<BV>* model1;
@@ -1092,7 +1072,7 @@ public:
void leafTesting(int, int) const
{
- min_distance = otsolver->OcTreeMeshDistance(model1, tf1, model2, tf2);
+ min_distance = otsolver->OcTreeMeshDistance(model1, model2, tf1, tf2);
}
const OcTree* model1;
diff --git a/trunk/fcl/src/broad_phase_collision.cpp b/trunk/fcl/src/broad_phase_collision.cpp
index fbacd974..45699ea6 100644
--- a/trunk/fcl/src/broad_phase_collision.cpp
+++ b/trunk/fcl/src/broad_phase_collision.cpp
@@ -38,10 +38,11 @@
#include "fcl/broad_phase_collision.h"
#include "fcl/collision.h"
#include "fcl/distance.h"
+#include "fcl/BV.h"
+#include "fcl/geometric_shapes_utility.h"
#include <algorithm>
#include <set>
#include <deque>
-
#include <iostream>
namespace fcl
@@ -2342,6 +2343,140 @@ bool DynamicAABBTreeCollisionManager::selfDistanceRecurse(DynamicAABBNode* root,
return false;
}
+bool DynamicAABBTreeCollisionManager::collisionRecurse(DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const SimpleTransform& tf2, void* cdata, CollisionCallBack callback) const
+{
+ if(root1->isLeaf() && !root2->hasChildren())
+ {
+ if(tree2->isNodeOccupied(root2))
+ {
+ OBB obb1, obb2;
+ convertBV(root1->bv, SimpleTransform(), obb1);
+ convertBV(root2_bv, tf2, obb2);
+
+ if(obb1.overlap(obb2))
+ {
+ Box* box = new Box();
+ SimpleTransform box_tf;
+ constructBox(root2_bv, tf2, *box, box_tf);
+ CollisionObject obj(boost::shared_ptr<CollisionGeometry>(box), box_tf);
+ return callback(static_cast<CollisionObject*>(root1->data), &obj, cdata);
+ }
+ else return false;
+ }
+ else return false;
+ }
+
+ OBB obb1, obb2;
+ convertBV(root1->bv, SimpleTransform(), obb1);
+ convertBV(root2_bv, tf2, obb2);
+
+ if(!tree2->isNodeOccupied(root2) || !obb1.overlap(obb2)) return false;
+
+ if(!root2->hasChildren() || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size())))
+ {
+ if(collisionRecurse(root1->childs[0], tree2, root2, root2_bv, tf2, cdata, callback))
+ return true;
+ if(collisionRecurse(root1->childs[1], tree2, root2, root2_bv, tf2, cdata, callback))
+ return true;
+ }
+ else
+ {
+ for(unsigned int i = 0; i < 8; ++i)
+ {
+ if(root2->childExists(i))
+ {
+ const OcTree::OcTreeNode* child = root2->getChild(i);
+ AABB child_bv;
+ computeChildBV(root2_bv, i, child_bv);
+
+ if(collisionRecurse(root1, tree2, child, child_bv, tf2, cdata, callback))
+ return true;
+ }
+ }
+ }
+ return false;
+}
+
+bool DynamicAABBTreeCollisionManager::distanceRecurse(DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const SimpleTransform& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
+{
+ if(root1->isLeaf() && !root2->hasChildren())
+ {
+ if(tree2->isNodeOccupied(root2))
+ {
+ Box* box = new Box();
+ SimpleTransform box_tf;
+ constructBox(root2_bv, tf2, *box, box_tf);
+ CollisionObject obj(boost::shared_ptr<CollisionGeometry>(box), box_tf);
+ return callback(static_cast<CollisionObject*>(root1->data), &obj, cdata, min_dist);
+ }
+ else return false;
+ }
+
+ if(!tree2->isNodeOccupied(root2)) return false;
+
+ if(!root2->hasChildren() || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size())))
+ {
+ AABB aabb2;
+ convertBV(root2_bv, tf2, aabb2);
+
+ FCL_REAL d1 = aabb2.distance(root1->childs[0]->bv);
+ FCL_REAL d2 = aabb2.distance(root1->childs[1]->bv);
+
+ if(d2 < d1)
+ {
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse(root1->childs[1], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse(root1->childs[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
+ return true;
+ }
+ }
+ else
+ {
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse(root1->childs[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse(root1->childs[1], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
+ return true;
+ }
+ }
+ }
+ else
+ {
+ for(unsigned int i = 0; i < 8; ++i)
+ {
+ if(root2->childExists(i))
+ {
+ const OcTree::OcTreeNode* child = root2->getChild(i);
+ AABB child_bv;
+ computeChildBV(root2_bv, i, child_bv);
+
+ AABB aabb2;
+ convertBV(child_bv, tf2, aabb2);
+ FCL_REAL d = root1->bv.distance(aabb2);
+
+ if(d < min_dist)
+ {
+ if(distanceRecurse(root1, tree2, child, child_bv, tf2, cdata, callback, min_dist))
+ return true;
+ }
+ }
+ }
+ }
+
+ return false;
+}
+
@@ -2671,4 +2806,143 @@ bool DynamicAABBTreeCollisionManager2::selfDistanceRecurse(DynamicAABBNode* node
return false;
}
+
+
+bool DynamicAABBTreeCollisionManager2::collisionRecurse(DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const SimpleTransform& tf2, void* cdata, CollisionCallBack callback) const
+{
+ DynamicAABBNode* root1 = nodes1 + root1_id;
+ if(root1->isLeaf() && !root2->hasChildren())
+ {
+ if(tree2->isNodeOccupied(root2))
+ {
+ OBB obb1, obb2;
+ convertBV(root1->bv, SimpleTransform(), obb1);
+ convertBV(root2_bv, tf2, obb2);
+
+ if(obb1.overlap(obb2))
+ {
+ Box* box = new Box();
+ SimpleTransform box_tf;
+ constructBox(root2_bv, tf2, *box, box_tf);
+ CollisionObject obj(boost::shared_ptr<CollisionGeometry>(box), box_tf);
+ return callback(static_cast<CollisionObject*>(root1->data), &obj, cdata);
+ }
+ else return false;
+ }
+ else return false;
+ }
+
+ OBB obb1, obb2;
+ convertBV(root1->bv, SimpleTransform(), obb1);
+ convertBV(root2_bv, tf2, obb2);
+
+ if(!tree2->isNodeOccupied(root2) || !obb1.overlap(obb2)) return false;
+
+ if(!root2->hasChildren() || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size())))
+ {
+ if(collisionRecurse(nodes1, root1->childs[0], tree2, root2, root2_bv, tf2, cdata, callback))
+ return true;
+ if(collisionRecurse(nodes1, root1->childs[1], tree2, root2, root2_bv, tf2, cdata, callback))
+ return true;
+ }
+ else
+ {
+ for(unsigned int i = 0; i < 8; ++i)
+ {
+ if(root2->childExists(i))
+ {
+ const OcTree::OcTreeNode* child = root2->getChild(i);
+ AABB child_bv;
+ computeChildBV(root2_bv, i, child_bv);
+
+ if(collisionRecurse(nodes1, root1_id, tree2, child, child_bv, tf2, cdata, callback))
+ return true;
+ }
+ }
+ }
+
+ return false;
+}
+
+bool DynamicAABBTreeCollisionManager2::distanceRecurse(DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const SimpleTransform& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
+{
+ DynamicAABBNode* root1 = nodes1 + root1_id;
+ if(root1->isLeaf() && !root2->hasChildren())
+ {
+ if(tree2->isNodeOccupied(root2))
+ {
+ Box* box = new Box();
+ SimpleTransform box_tf;
+ constructBox(root2_bv, tf2, *box, box_tf);
+ CollisionObject obj(boost::shared_ptr<CollisionGeometry>(box), box_tf);
+ return callback(static_cast<CollisionObject*>(root1->data), &obj, cdata, min_dist);
+ }
+ else return false;
+ }
+
+ if(!tree2->isNodeOccupied(root2)) return false;
+
+ if(!root2->hasChildren() || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size())))
+ {
+ AABB aabb2;
+ convertBV(root2_bv, tf2, aabb2);
+
+ FCL_REAL d1 = aabb2.distance((nodes1 + root1->childs[0])->bv);
+ FCL_REAL d2 = aabb2.distance((nodes1 + root1->childs[1])->bv);
+
+ if(d2 < d1)
+ {
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse(nodes1, root1->childs[1], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse(nodes1, root1->childs[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
+ return true;
+ }
+ }
+ else
+ {
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse(nodes1, root1->childs[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse(nodes1, root1->childs[1], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
+ return true;
+ }
+ }
+ }
+ else
+ {
+ for(unsigned int i = 0; i < 8; ++i)
+ {
+ if(root2->childExists(i))
+ {
+ const OcTree::OcTreeNode* child = root2->getChild(i);
+ AABB child_bv;
+ computeChildBV(root2_bv, i, child_bv);
+
+ AABB aabb2;
+ convertBV(child_bv, tf2, aabb2);
+ FCL_REAL d = root1->bv.distance(aabb2);
+
+ if(d < min_dist)
+ {
+ if(distanceRecurse(nodes1, root1_id, tree2, child, child_bv, tf2, cdata, callback, min_dist))
+ return true;
+ }
+ }
+ }
+ }
+
+ return false;
+}
+
}
diff --git a/trunk/fcl/src/collision_func_matrix.cpp b/trunk/fcl/src/collision_func_matrix.cpp
index 148b3f70..09ffe9c4 100644
--- a/trunk/fcl/src/collision_func_matrix.cpp
+++ b/trunk/fcl/src/collision_func_matrix.cpp
@@ -37,11 +37,7 @@
#include "fcl/collision_func_matrix.h"
-#include "fcl/collision.h"
#include "fcl/simple_setup.h"
-#include "fcl/geometric_shapes.h"
-#include "fcl/BVH_model.h"
-#include "fcl/octree.h"
#include "fcl/collision_node.h"
#include "fcl/narrowphase/narrowphase.h"
@@ -49,13 +45,44 @@
namespace fcl
{
+template<typename T_SH>
+static inline int OcTreeShapeContactCollection(const std::vector<OcTreeShapeCollisionPair>& pairs, const OcTree* obj1, const T_SH* obj2,
+ int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+{
+ int num_contacts = pairs.size();
+ if(num_contacts > 0)
+ {
+ if((!exhaustive) && (num_contacts > num_max_contacts)) num_contacts = num_max_contacts;
+ contacts.resize(num_contacts);
+ if(!enable_contact)
+ {
+ for(int i = 0; i < num_contacts; ++i)
+ contacts[i] = Contact(obj1, obj2, pairs[i].node - obj1->getRoot(), 0);
+ }
+ else
+ {
+ for(int i = 0; i < num_contacts; ++i)
+ contacts[i] = Contact(obj1, obj2, pairs[i].node - obj1->getRoot(), 0, pairs[i].contact_point, pairs[i].normal, pairs[i].penetration_depth);
+ }
+ }
+
+ return num_contacts;
+}
+
template<typename T_SH, typename NarrowPhaseSolver>
int ShapeOcTreeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
{
+ ShapeOcTreeCollisionTraversalNode<T_SH, NarrowPhaseSolver> node;
const T_SH* obj1 = static_cast<const T_SH*>(o1);
const OcTree* obj2 = static_cast<const OcTree*>(o2);
+ OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
+ initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, num_max_contacts, exhaustive, enable_contact);
+ collide(&node);
+ int num_contacts = OcTreeShapeContactCollection(node.pairs, obj2, obj1, num_max_contacts, exhaustive, enable_contact, contacts);
+
+ return num_contacts;
}
template<typename T_SH, typename NarrowPhaseSolver>
@@ -63,32 +90,112 @@ int OcTreeShapeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1,
const NarrowPhaseSolver* nsolver,
int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
{
+ OcTreeShapeCollisionTraversalNode<T_SH, NarrowPhaseSolver> node;
const OcTree* obj1 = static_cast<const OcTree*>(o1);
const T_SH* obj2 = static_cast<const T_SH*>(o2);
+
+ OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
+ initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, num_max_contacts, exhaustive, enable_contact);
+ collide(&node);
+ int num_contacts = OcTreeShapeContactCollection(node.pairs, obj1, obj2, num_max_contacts, exhaustive, enable_contact, contacts);
+
+ return num_contacts;
}
+static inline int OcTreeContactCollection(const std::vector<OcTreeCollisionPair>& pairs, const OcTree* obj1, const OcTree* obj2,
+ int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+{
+ int num_contacts = pairs.size();
+ if(num_contacts > 0)
+ {
+ if((!exhaustive) && (num_contacts > num_max_contacts)) num_contacts = num_max_contacts;
+ contacts.resize(num_contacts);
+ if(!enable_contact)
+ {
+ for(int i = 0; i < num_contacts; ++i)
+ contacts[i] = Contact(obj1, obj2, pairs[i].node1 - obj1->getRoot(), pairs[i].node2 - obj2->getRoot());
+ }
+ else
+ {
+ for(int i = 0; i < num_contacts; ++i)
+ contacts[i] = Contact(obj1, obj2, pairs[i].node1 - obj1->getRoot(), pairs[i].node2 - obj2->getRoot(), pairs[i].contact_point, pairs[i].normal, pairs[i].penetration_depth);
+ }
+ }
+
+ return num_contacts;
+}
+
+template<typename NarrowPhaseSolver>
int OcTreeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
+ const NarrowPhaseSolver* nsolver,
int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
{
+ OcTreeCollisionTraversalNode<NarrowPhaseSolver> node;
const OcTree* obj1 = static_cast<const OcTree*>(o1);
const OcTree* obj2 = static_cast<const OcTree*>(o2);
+ OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
+ initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, num_max_contacts, exhaustive, enable_contact);
+ collide(&node);
+ int num_contacts = OcTreeContactCollection(node.pairs, obj1, obj2, num_max_contacts, exhaustive, enable_contact, contacts);
+ return num_contacts;
+}
+
+
+template<typename T_BVH>
+static inline int OcTreeBVHContactCollection(const std::vector<OcTreeMeshCollisionPair>& pairs, const OcTree* obj1, const BVHModel<T_BVH>* obj2,
+ int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+{
+ int num_contacts = pairs.size();
+ if(num_contacts > 0)
+ {
+ if((!exhaustive) && (num_contacts > num_max_contacts)) num_contacts = num_max_contacts;
+ contacts.resize(num_contacts);
+ if(!enable_contact)
+ {
+ for(int i = 0; i < num_contacts; ++i)
+ contacts[i] = Contact(obj1, obj2, pairs[i].node - obj1->getRoot(), pairs[i].id);
+ }
+ else
+ {
+ for(int i = 0; i < num_contacts; ++i)
+ contacts[i] = Contact(obj1, obj2, pairs[i].node - obj1->getRoot(), pairs[i].id, pairs[i].contact_point, pairs[i].normal, pairs[i].penetration_depth);
+ }
+ }
+
+ return num_contacts;
}
template<typename T_BVH, typename NarrowPhaseSolver>
int OcTreeBVHCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
+ const NarrowPhaseSolver* nsolver,
int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
{
+ OcTreeMeshCollisionTraversalNode<T_BVH, NarrowPhaseSolver> node;
const OcTree* obj1 = static_cast<const OcTree*>(o1);
const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>*>(o2);
+
+ OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
+ initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, num_max_contacts, exhaustive, enable_contact);
+ collide(&node);
+ int num_contacts = OcTreeBVHContactCollection(node.pairs, obj1, obj2, num_max_contacts, exhaustive, enable_contact, contacts);
+ return num_contacts;
}
template<typename T_BVH, typename NarrowPhaseSolver>
int BVHOcTreeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
+ const NarrowPhaseSolver* nsolver,
int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
{
+ MeshOcTreeCollisionTraversalNode<T_BVH, NarrowPhaseSolver> node;
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>*>(o1);
const OcTree* obj2 = static_cast<const OcTree*>(o2);
+
+ OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
+ initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, num_max_contacts, exhaustive, enable_contact);
+ collide(&node);
+ int num_contacts = OcTreeBVHContactCollection(node.pairs, obj2, obj1, num_max_contacts, exhaustive, enable_contact, contacts);
+ return num_contacts;
}
@@ -361,9 +468,9 @@ int BVHCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const Co
template<typename NarrowPhaseSolver>
CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
{
- for(int i = 0; i < NODE_COUNT - 1; ++i)
+ for(int i = 0; i < NODE_COUNT; ++i)
{
- for(int j = 0; j < NODE_COUNT - 1; ++j)
+ for(int j = 0; j < NODE_COUNT; ++j)
collision_matrix[i][j] = NULL;
}
@@ -495,6 +602,43 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
collision_matrix[BV_KDOP24][BV_KDOP24] = &BVHCollide<KDOP<24>, NarrowPhaseSolver>;
collision_matrix[BV_kIOS][BV_kIOS] = &BVHCollide<kIOS, NarrowPhaseSolver>;
collision_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHCollide<OBBRSS, NarrowPhaseSolver>;
+
+ collision_matrix[GEOM_OCTREE][GEOM_BOX] = &OcTreeShapeCollide<Box, NarrowPhaseSolver>;
+ collision_matrix[GEOM_OCTREE][GEOM_SPHERE] = &OcTreeShapeCollide<Sphere, NarrowPhaseSolver>;
+ collision_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &OcTreeShapeCollide<Capsule, NarrowPhaseSolver>;
+ collision_matrix[GEOM_OCTREE][GEOM_CONE] = &OcTreeShapeCollide<Cone, NarrowPhaseSolver>;
+ collision_matrix[GEOM_OCTREE][GEOM_CYLINDER] = &OcTreeShapeCollide<Cylinder, NarrowPhaseSolver>;
+ collision_matrix[GEOM_OCTREE][GEOM_CONVEX] = &OcTreeShapeCollide<Convex, NarrowPhaseSolver>;
+ collision_matrix[GEOM_OCTREE][GEOM_PLANE] = &OcTreeShapeCollide<Plane, NarrowPhaseSolver>;
+
+ collision_matrix[GEOM_BOX][GEOM_OCTREE] = &ShapeOcTreeCollide<Box, NarrowPhaseSolver>;
+ collision_matrix[GEOM_SPHERE][GEOM_OCTREE] = &ShapeOcTreeCollide<Sphere, NarrowPhaseSolver>;
+ collision_matrix[GEOM_CAPSULE][GEOM_OCTREE] = &ShapeOcTreeCollide<Capsule, NarrowPhaseSolver>;
+ collision_matrix[GEOM_CONE][GEOM_OCTREE] = &ShapeOcTreeCollide<Cone, NarrowPhaseSolver>;
+ collision_matrix[GEOM_CYLINDER][GEOM_OCTREE] = &ShapeOcTreeCollide<Cylinder, NarrowPhaseSolver>;
+ collision_matrix[GEOM_CONVEX][GEOM_OCTREE] = &ShapeOcTreeCollide<Convex, NarrowPhaseSolver>;
+ collision_matrix[GEOM_PLANE][GEOM_OCTREE] = &ShapeOcTreeCollide<Plane, NarrowPhaseSolver>;
+
+ collision_matrix[GEOM_OCTREE][GEOM_OCTREE] = &OcTreeCollide<NarrowPhaseSolver>;
+
+ collision_matrix[GEOM_OCTREE][BV_AABB] = &OcTreeBVHCollide<AABB, NarrowPhaseSolver>;
+ collision_matrix[GEOM_OCTREE][BV_OBB] = &OcTreeBVHCollide<OBB, NarrowPhaseSolver>;
+ collision_matrix[GEOM_OCTREE][BV_RSS] = &OcTreeBVHCollide<RSS, NarrowPhaseSolver>;
+ collision_matrix[GEOM_OCTREE][BV_OBBRSS] = &OcTreeBVHCollide<OBBRSS, NarrowPhaseSolver>;
+ collision_matrix[GEOM_OCTREE][BV_kIOS] = &OcTreeBVHCollide<kIOS, NarrowPhaseSolver>;
+ collision_matrix[GEOM_OCTREE][BV_KDOP16] = &OcTreeBVHCollide<KDOP<16>, NarrowPhaseSolver>;
+ collision_matrix[GEOM_OCTREE][BV_KDOP18] = &OcTreeBVHCollide<KDOP<18>, NarrowPhaseSolver>;
+ collision_matrix[GEOM_OCTREE][BV_KDOP24] = &OcTreeBVHCollide<KDOP<24>, NarrowPhaseSolver>;
+
+ collision_matrix[BV_AABB][GEOM_OCTREE] = &BVHOcTreeCollide<AABB, NarrowPhaseSolver>;
+ collision_matrix[BV_OBB][GEOM_OCTREE] = &BVHOcTreeCollide<OBB, NarrowPhaseSolver>;
+ collision_matrix[BV_RSS][GEOM_OCTREE] = &BVHOcTreeCollide<RSS, NarrowPhaseSolver>;
+ collision_matrix[BV_OBBRSS][GEOM_OCTREE] = &BVHOcTreeCollide<OBBRSS, NarrowPhaseSolver>;
+ collision_matrix[BV_kIOS][GEOM_OCTREE] = &BVHOcTreeCollide<kIOS, NarrowPhaseSolver>;
+ collision_matrix[BV_KDOP16][GEOM_OCTREE] = &BVHOcTreeCollide<KDOP<16>, NarrowPhaseSolver>;
+ collision_matrix[BV_KDOP18][GEOM_OCTREE] = &BVHOcTreeCollide<KDOP<18>, NarrowPhaseSolver>;
+ collision_matrix[BV_KDOP24][GEOM_OCTREE] = &BVHOcTreeCollide<KDOP<24>, NarrowPhaseSolver>;
+
}
diff --git a/trunk/fcl/src/distance_func_matrix.cpp b/trunk/fcl/src/distance_func_matrix.cpp
index 07973312..01a6d493 100644
--- a/trunk/fcl/src/distance_func_matrix.cpp
+++ b/trunk/fcl/src/distance_func_matrix.cpp
@@ -43,6 +43,73 @@
namespace fcl
{
+template<typename T_SH, typename NarrowPhaseSolver>
+FCL_REAL ShapeOcTreeDistance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver)
+{
+ ShapeOcTreeDistanceTraversalNode<T_SH, NarrowPhaseSolver> node;
+ const T_SH* obj1 = static_cast<const T_SH*>(o1);
+ const OcTree* obj2 = static_cast<const OcTree*>(o2);
+ OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
+ initialize(node, *obj1, tf1, *obj2, tf2, &otsolver);
+ distance(&node);
+
+ return node.min_distance;
+}
+
+template<typename T_SH, typename NarrowPhaseSolver>
+FCL_REAL OcTreeShapeDistance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver)
+{
+ OcTreeShapeDistanceTraversalNode<T_SH, NarrowPhaseSolver> node;
+ const OcTree* obj1 = static_cast<const OcTree*>(o1);
+ const T_SH* obj2 = static_cast<const T_SH*>(o2);
+ OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
+ initialize(node, *obj1, tf1, *obj2, tf2, &otsolver);
+ distance(&node);
+
+ return node.min_distance;
+}
+
+template<typename NarrowPhaseSolver>
+FCL_REAL OcTreeDistance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver)
+{
+ OcTreeDistanceTraversalNode<NarrowPhaseSolver> node;
+ const OcTree* obj1 = static_cast<const OcTree*>(o1);
+ const OcTree* obj2 = static_cast<const OcTree*>(o2);
+ OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
+ initialize(node, *obj1, tf1, *obj2, tf2, &otsolver);
+ distance(&node);
+
+ return node.min_distance;
+}
+
+template<typename T_BVH, typename NarrowPhaseSolver>
+FCL_REAL BVHOcTreeDistance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver)
+{
+ MeshOcTreeDistanceTraversalNode<T_BVH, NarrowPhaseSolver> node;
+ const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>*>(o1);
+ const OcTree* obj2 = static_cast<const OcTree*>(o2);
+ OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
+ initialize(node, *obj1, tf1, *obj2, tf2, &otsolver);
+ distance(&node);
+
+ return node.min_distance;
+}
+
+template<typename T_BVH, typename NarrowPhaseSolver>
+FCL_REAL OcTreeBVHDistance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver)
+{
+ OcTreeMeshDistanceTraversalNode<T_BVH, NarrowPhaseSolver> node;
+ const OcTree* obj1 = static_cast<const OcTree*>(o1);
+ const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>*>(o2);
+ OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
+ initialize(node, *obj1, tf1, *obj2, tf2, &otsolver);
+ distance(&node);
+
+ return node.min_distance;
+}
+
+
+
template<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>
FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver)
{
@@ -192,9 +259,9 @@ FCL_REAL BVHDistance(const CollisionGeometry* o1, const SimpleTransform& tf1, co
template<typename NarrowPhaseSolver>
DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
{
- for(int i = 0; i < NODE_COUNT-1; ++i)
+ for(int i = 0; i < NODE_COUNT; ++i)
{
- for(int j = 0; j < NODE_COUNT-1; ++j)
+ for(int j = 0; j < NODE_COUNT; ++j)
distance_matrix[i][j] = NULL;
}
@@ -328,6 +395,42 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
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>;
+
+ distance_matrix[GEOM_OCTREE][GEOM_BOX] = &OcTreeShapeDistance<Box, NarrowPhaseSolver>;
+ distance_matrix[GEOM_OCTREE][GEOM_SPHERE] = &OcTreeShapeDistance<Sphere, NarrowPhaseSolver>;
+ distance_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &OcTreeShapeDistance<Capsule, NarrowPhaseSolver>;
+ distance_matrix[GEOM_OCTREE][GEOM_CONE] = &OcTreeShapeDistance<Cone, NarrowPhaseSolver>;
+ distance_matrix[GEOM_OCTREE][GEOM_CYLINDER] = &OcTreeShapeDistance<Cylinder, NarrowPhaseSolver>;
+ distance_matrix[GEOM_OCTREE][GEOM_CONVEX] = &OcTreeShapeDistance<Convex, NarrowPhaseSolver>;
+ distance_matrix[GEOM_OCTREE][GEOM_PLANE] = &OcTreeShapeDistance<Plane, NarrowPhaseSolver>;
+
+ distance_matrix[GEOM_BOX][GEOM_OCTREE] = &ShapeOcTreeDistance<Box, NarrowPhaseSolver>;
+ distance_matrix[GEOM_SPHERE][GEOM_OCTREE] = &ShapeOcTreeDistance<Sphere, NarrowPhaseSolver>;
+ distance_matrix[GEOM_CAPSULE][GEOM_OCTREE] = &ShapeOcTreeDistance<Capsule, NarrowPhaseSolver>;
+ distance_matrix[GEOM_CONE][GEOM_OCTREE] = &ShapeOcTreeDistance<Cone, NarrowPhaseSolver>;
+ distance_matrix[GEOM_CYLINDER][GEOM_OCTREE] = &ShapeOcTreeDistance<Cylinder, NarrowPhaseSolver>;
+ distance_matrix[GEOM_CONVEX][GEOM_OCTREE] = &ShapeOcTreeDistance<Convex, NarrowPhaseSolver>;
+ distance_matrix[GEOM_PLANE][GEOM_OCTREE] = &ShapeOcTreeDistance<Plane, NarrowPhaseSolver>;
+
+ distance_matrix[GEOM_OCTREE][GEOM_OCTREE] = &OcTreeDistance<NarrowPhaseSolver>;
+
+ distance_matrix[GEOM_OCTREE][BV_AABB] = &OcTreeBVHDistance<AABB, NarrowPhaseSolver>;
+ distance_matrix[GEOM_OCTREE][BV_OBB] = &OcTreeBVHDistance<OBB, NarrowPhaseSolver>;
+ distance_matrix[GEOM_OCTREE][BV_RSS] = &OcTreeBVHDistance<RSS, NarrowPhaseSolver>;
+ distance_matrix[GEOM_OCTREE][BV_OBBRSS] = &OcTreeBVHDistance<OBBRSS, NarrowPhaseSolver>;
+ distance_matrix[GEOM_OCTREE][BV_kIOS] = &OcTreeBVHDistance<kIOS, NarrowPhaseSolver>;
+ distance_matrix[GEOM_OCTREE][BV_KDOP16] = &OcTreeBVHDistance<KDOP<16>, NarrowPhaseSolver>;
+ distance_matrix[GEOM_OCTREE][BV_KDOP18] = &OcTreeBVHDistance<KDOP<18>, NarrowPhaseSolver>;
+ distance_matrix[GEOM_OCTREE][BV_KDOP24] = &OcTreeBVHDistance<KDOP<24>, NarrowPhaseSolver>;
+
+ distance_matrix[BV_AABB][GEOM_OCTREE] = &BVHOcTreeDistance<AABB, NarrowPhaseSolver>;
+ distance_matrix[BV_OBB][GEOM_OCTREE] = &BVHOcTreeDistance<OBB, NarrowPhaseSolver>;
+ distance_matrix[BV_RSS][GEOM_OCTREE] = &BVHOcTreeDistance<RSS, NarrowPhaseSolver>;
+ distance_matrix[BV_OBBRSS][GEOM_OCTREE] = &BVHOcTreeDistance<OBBRSS, NarrowPhaseSolver>;
+ distance_matrix[BV_kIOS][GEOM_OCTREE] = &BVHOcTreeDistance<kIOS, NarrowPhaseSolver>;
+ distance_matrix[BV_KDOP16][GEOM_OCTREE] = &BVHOcTreeDistance<KDOP<16>, NarrowPhaseSolver>;
+ distance_matrix[BV_KDOP18][GEOM_OCTREE] = &BVHOcTreeDistance<KDOP<18>, NarrowPhaseSolver>;
+ distance_matrix[BV_KDOP24][GEOM_OCTREE] = &BVHOcTreeDistance<KDOP<24>, NarrowPhaseSolver>;
}
template struct DistanceFunctionMatrix<GJKSolver_libccd>;
diff --git a/trunk/fcl/src/geometric_shapes_utility.cpp b/trunk/fcl/src/geometric_shapes_utility.cpp
index c2f6fc96..7ef0596b 100644
--- a/trunk/fcl/src/geometric_shapes_utility.cpp
+++ b/trunk/fcl/src/geometric_shapes_utility.cpp
@@ -495,7 +495,6 @@ void constructBox(const OBBRSS& bv, Box& box, SimpleTransform& tf)
tf = SimpleTransform(Matrix3f(bv.obb.axis[0][0], bv.obb.axis[1][0], bv.obb.axis[2][0],
bv.obb.axis[0][1], bv.obb.axis[1][1], bv.obb.axis[2][1],
bv.obb.axis[0][2], bv.obb.axis[1][2], bv.obb.axis[2][2]), bv.obb.To);
-
}
void constructBox(const kIOS& bv, Box& box, SimpleTransform& tf)
@@ -504,7 +503,6 @@ void constructBox(const kIOS& bv, Box& box, SimpleTransform& tf)
tf = SimpleTransform(Matrix3f(bv.obb_bv.axis[0][0], bv.obb_bv.axis[1][0], bv.obb_bv.axis[2][0],
bv.obb_bv.axis[0][1], bv.obb_bv.axis[1][1], bv.obb_bv.axis[2][1],
bv.obb_bv.axis[0][2], bv.obb_bv.axis[1][2], bv.obb_bv.axis[2][2]), bv.obb_bv.To);
-
}
void constructBox(const RSS& bv, Box& box, SimpleTransform& tf)
@@ -555,7 +553,6 @@ void constructBox(const OBBRSS& bv, const SimpleTransform& tf_bv, Box& box, Simp
tf = tf_bv * SimpleTransform(Matrix3f(bv.obb.axis[0][0], bv.obb.axis[1][0], bv.obb.axis[2][0],
bv.obb.axis[0][1], bv.obb.axis[1][1], bv.obb.axis[2][1],
bv.obb.axis[0][2], bv.obb.axis[1][2], bv.obb.axis[2][2]), bv.obb.To);
-
}
void constructBox(const kIOS& bv, const SimpleTransform& tf_bv, Box& box, SimpleTransform& tf)
@@ -564,7 +561,6 @@ void constructBox(const kIOS& bv, const SimpleTransform& tf_bv, Box& box, Simple
tf = tf_bv * SimpleTransform(Matrix3f(bv.obb_bv.axis[0][0], bv.obb_bv.axis[1][0], bv.obb_bv.axis[2][0],
bv.obb_bv.axis[0][1], bv.obb_bv.axis[1][1], bv.obb_bv.axis[2][1],
bv.obb_bv.axis[0][2], bv.obb_bv.axis[1][2], bv.obb_bv.axis[2][2]), bv.obb_bv.To);
-
}
void constructBox(const RSS& bv, const SimpleTransform& tf_bv, Box& box, SimpleTransform& tf)
diff --git a/trunk/fcl/src/octomap_extension.cpp b/trunk/fcl/src/octomap_extension.cpp
index 2437a04d..115dad4f 100644
--- a/trunk/fcl/src/octomap_extension.cpp
+++ b/trunk/fcl/src/octomap_extension.cpp
@@ -40,41 +40,6 @@
namespace fcl
{
-static inline void computeChildBV(const AABB& root_bv, unsigned int i, AABB& child_bv)
-{
- if(i&1)
- {
- child_bv.min_[0] = (root_bv.min_[0] + root_bv.max_[0]) * 0.5;
- child_bv.max_[0] = root_bv.max_[0];
- }
- else
- {
- child_bv.min_[0] = root_bv.min_[0];
- child_bv.max_[0] = (root_bv.min_[0] + root_bv.max_[0]) * 0.5;
- }
-
- if(i&2)
- {
- child_bv.min_[1] = (root_bv.min_[1] + root_bv.max_[1]) * 0.5;
- child_bv.max_[1] = root_bv.max_[1];
- }
- else
- {
- child_bv.min_[1] = root_bv.min_[1];
- child_bv.max_[1] = (root_bv.min_[1] + root_bv.max_[1]) * 0.5;
- }
-
- if(i&4)
- {
- child_bv.min_[2] = (root_bv.min_[2] + root_bv.max_[2]) * 0.5;
- child_bv.max_[2] = root_bv.max_[2];
- }
- else
- {
- child_bv.min_[2] = root_bv.min_[2];
- child_bv.max_[2] = (root_bv.min_[2] + root_bv.max_[2]) * 0.5;
- }
-}
class ExtendedBox : public Box
{
--
GitLab