diff --git a/trunk/fcl/CMakeLists.txt b/trunk/fcl/CMakeLists.txt
index 458bd929138174042f7c731dc280e03b417f970c..79fc8fd57b8325d2f4ded8a30e2455258322d2f8 100644
--- a/trunk/fcl/CMakeLists.txt
+++ b/trunk/fcl/CMakeLists.txt
@@ -41,7 +41,7 @@ link_directories(${OCTOMAP_LIBRARY_DIRS})
add_definitions(-DUSE_SVMLIGHT=0)
-add_library(${PROJECT_NAME} SHARED src/BV/AABB.cpp src/BV/OBB.cpp src/BV/RSS.cpp src/BV/kIOS.cpp src/BV/OBBRSS.cpp src/BV/kDOP.cpp src/traversal_node_base.cpp src/traversal_node_bvhs.cpp src/intersect.cpp src/motion.cpp src/BV_fitter.cpp src/BV_splitter.cpp src/BVH_model.cpp src/BVH_utility.cpp src/transform.cpp src/simple_setup.cpp src/geometric_shapes.cpp src/geometric_shapes_utility.cpp src/collision_node.cpp src/traversal_recurse.cpp src/broad_phase_collision.cpp src/collision.cpp src/collision_func_matrix.cpp src/interval_tree.cpp src/conservative_advancement.cpp src/ccd/interval.cpp src/ccd/interval_vector.cpp src/ccd/interval_matrix.cpp src/ccd/taylor_model.cpp src/ccd/taylor_vector.cpp src/ccd/taylor_matrix.cpp src/distance_func_matrix.cpp src/distance.cpp src/narrowphase/gjk.cpp src/narrowphase/gjk_libccd.cpp src/narrowphase/narrowphase.cpp src/hierarchy_tree.cpp)
+add_library(${PROJECT_NAME} SHARED src/BV/AABB.cpp src/BV/OBB.cpp src/BV/RSS.cpp src/BV/kIOS.cpp src/BV/OBBRSS.cpp src/BV/kDOP.cpp src/traversal_node_base.cpp src/traversal_node_bvhs.cpp src/intersect.cpp src/motion.cpp src/BV_fitter.cpp src/BV_splitter.cpp src/BVH_model.cpp src/BVH_utility.cpp src/transform.cpp src/simple_setup.cpp src/geometric_shapes.cpp src/geometric_shapes_utility.cpp src/collision_node.cpp src/traversal_recurse.cpp src/broadphase/broadphase_bruteforce.cpp src/broadphase/broadphase_spatialhash.cpp src/broadphase/broadphase_SaP.cpp src/broadphase/broadphase_SSaP.cpp src/broadphase/broadphase_interval_tree.cpp src/broadphase/broadphase_dynamic_AABB_tree.cpp src/broadphase/broadphase_dynamic_AABB_tree_array.cpp src/collision.cpp src/collision_func_matrix.cpp src/broadphase/interval_tree.cpp src/conservative_advancement.cpp src/ccd/interval.cpp src/ccd/interval_vector.cpp src/ccd/interval_matrix.cpp src/ccd/taylor_model.cpp src/ccd/taylor_vector.cpp src/ccd/taylor_matrix.cpp src/distance_func_matrix.cpp src/distance.cpp src/narrowphase/gjk.cpp src/narrowphase/gjk_libccd.cpp src/narrowphase/narrowphase.cpp src/broadphase/hierarchy_tree.cpp)
target_link_libraries(${PROJECT_NAME} ${FLANN_LIBRARIES} ${CCD_LIBRARIES} ${OCTOMAP_LIBRARIES})
diff --git a/trunk/fcl/include/fcl/BV/OBB.h b/trunk/fcl/include/fcl/BV/OBB.h
index 2020b760b1258b30785a1216cd438756a0728030..92d0471e3af963eb96a1cf1e090b4eca9deda1ee 100644
--- a/trunk/fcl/include/fcl/BV/OBB.h
+++ b/trunk/fcl/include/fcl/BV/OBB.h
@@ -125,6 +125,10 @@ public:
FCL_REAL distance(const OBB& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
};
+
+/// @brief Translate the OBB bv
+OBB translate(const OBB& bv, const Vec3f& t);
+
/// @brief Check collision between two obbs, b1 is in configuration (R0, T0) and b2 is in identity.
bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2);
diff --git a/trunk/fcl/include/fcl/BV/OBBRSS.h b/trunk/fcl/include/fcl/BV/OBBRSS.h
index 9bdf0e5273de38ed02ebad2b6f2c9695d96171d2..8925ac7d1657fe05bc194a33ea033daed055c144 100644
--- a/trunk/fcl/include/fcl/BV/OBBRSS.h
+++ b/trunk/fcl/include/fcl/BV/OBBRSS.h
@@ -143,6 +143,8 @@ public:
}
};
+/// @brief Translate the OBBRSS bv
+OBBRSS translate(const OBBRSS& bv, const Vec3f& t);
/// @brief Check collision between two OBBRSS, b1 is in configuration (R0, T0) and b2 is in indentity
bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2);
diff --git a/trunk/fcl/include/fcl/BV/RSS.h b/trunk/fcl/include/fcl/BV/RSS.h
index 4a9b321dd2f05054028a607eccc2a90e2dc8725e..f0293eeb8eebc755dfd83f1d741d6487c7b8c6ec 100644
--- a/trunk/fcl/include/fcl/BV/RSS.h
+++ b/trunk/fcl/include/fcl/BV/RSS.h
@@ -130,6 +130,10 @@ public:
};
+
+/// @brief Translate the RSS bv
+RSS translate(const RSS& bv, const Vec3f& t);
+
/// @brief distance between two RSS bounding volumes
/// P and Q (optional return values) are the closest points in the rectangles, not the RSS. But the direction P - Q is the correct direction for cloest points
/// Notice that P and Q are both in the local frame of the first RSS (not global frame and not even the local frame of object 1)
diff --git a/trunk/fcl/include/fcl/BV/kDOP.h b/trunk/fcl/include/fcl/BV/kDOP.h
index 7784f7c60ba005b178a2cc7837d5f2e71584663a..660cdfbbfd1ffbc8ca811117ce3982b6999a3ca3 100644
--- a/trunk/fcl/include/fcl/BV/kDOP.h
+++ b/trunk/fcl/include/fcl/BV/kDOP.h
@@ -128,9 +128,25 @@ private:
/// @brief Origin's distances to N KDOP planes
FCL_REAL dist_[N];
+public:
+ inline FCL_REAL dist(std::size_t i) const
+ {
+ return dist_[i];
+ }
+
+ inline FCL_REAL& dist(std::size_t i)
+ {
+ return dist_[i];
+ }
+
};
+
+/// @brief translate the KDOP BV
+template<size_t N>
+KDOP<N> translate(const KDOP<N>& bv, const Vec3f& t);
+
}
#endif
diff --git a/trunk/fcl/include/fcl/BV/kIOS.h b/trunk/fcl/include/fcl/BV/kIOS.h
index bbab38bbde9c18334fd7c3efc1b420b0fcf916a2..b39dd3c67cd63c7edb43eec0108f1e2e186f8487 100644
--- a/trunk/fcl/include/fcl/BV/kIOS.h
+++ b/trunk/fcl/include/fcl/BV/kIOS.h
@@ -147,6 +147,10 @@ private:
};
+
+/// @brief Translate the kIOS BV
+kIOS translate(const kIOS& bv, const Vec3f& t);
+
/// @brief Check collision between two kIOSs, b1 is in configuration (R0, T0) and b2 is in identity.
/// @todo Not efficient
bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2);
diff --git a/trunk/fcl/include/fcl/BVH_internal.h b/trunk/fcl/include/fcl/BVH_internal.h
index 24f64d4a010263227af16eff1bbeecdb433fe590..2b099750ff82b889e6cb0ddb785583fdde0588de 100644
--- a/trunk/fcl/include/fcl/BVH_internal.h
+++ b/trunk/fcl/include/fcl/BVH_internal.h
@@ -42,46 +42,41 @@
namespace fcl
{
-/** \brief States for BVH construction
- * empty->begun->processed ->replace_begun->processed -> ......
- * |
- * |-> update_begun -> updated -> .....
- * */
-
/// @brief States for BVH construction
+/// empty->begun->processed ->replace_begun->processed -> ......
+/// |
+/// |-> update_begun -> updated -> .....
enum BVHBuildState
-{
- BVH_BUILD_STATE_EMPTY, // empty state, immediately after constructor
- BVH_BUILD_STATE_BEGUN, // after beginModel(), state for adding geometry primitives
- BVH_BUILD_STATE_PROCESSED, // after tree has been build, ready for cd use
- BVH_BUILD_STATE_UPDATE_BEGUN, // after beginUpdateModel(), state for updating geometry primitives
- BVH_BUILD_STATE_UPDATED, // after tree has been build for updated geometry, ready for ccd use
- BVH_BUILD_STATE_REPLACE_BEGUN, // after beginReplaceModel(), state for replacing geometry primitives
-};
+ {
+ BVH_BUILD_STATE_EMPTY, /// empty state, immediately after constructor
+ BVH_BUILD_STATE_BEGUN, /// after beginModel(), state for adding geometry primitives
+ BVH_BUILD_STATE_PROCESSED, /// after tree has been build, ready for cd use
+ BVH_BUILD_STATE_UPDATE_BEGUN, /// after beginUpdateModel(), state for updating geometry primitives
+ BVH_BUILD_STATE_UPDATED, /// after tree has been build for updated geometry, ready for ccd use
+ BVH_BUILD_STATE_REPLACE_BEGUN, /// after beginReplaceModel(), state for replacing geometry primitives
+ };
-/** \brief Error code for BVH */
+/// @brief Error code for BVH
enum BVHReturnCode
-{
- BVH_OK = 0,
- BVH_ERR_MODEL_OUT_OF_MEMORY = -1,
- BVH_ERR_OUT_OF_MEMORY = -2,
- BVH_ERR_UNPROCESSED_MODEL = -3,
- BVH_ERR_BUILD_OUT_OF_SEQUENCE = -4,
- BVH_ERR_BUILD_EMPTY_MODEL = -5,
- BVH_ERR_BUILD_EMPTY_PREVIOUS_FRAME = -6,
- BVH_ERR_UNSUPPORTED_FUNCTION = -7,
- BVH_ERR_UNUPDATED_MODEL = -8,
- BVH_ERR_INCORRECT_DATA = -9,
- BVH_ERR_UNKNOWN = -10
-};
+ {
+ BVH_OK = 0, /// BVH is valid
+ BVH_ERR_MODEL_OUT_OF_MEMORY = -1, /// Cannot allocate memory for vertices and triangles
+ BVH_ERR_BUILD_OUT_OF_SEQUENCE = -2, /// BVH construction does not follow correct sequence
+ BVH_ERR_BUILD_EMPTY_MODEL = -3, /// BVH geometry is not prepared
+ BVH_ERR_BUILD_EMPTY_PREVIOUS_FRAME = -4, /// BVH geometry in previous frame is not prepared
+ BVH_ERR_UNSUPPORTED_FUNCTION = -5, /// BVH funtion is not supported
+ BVH_ERR_UNUPDATED_MODEL = -6, /// BVH model update failed
+ BVH_ERR_INCORRECT_DATA = -7, /// BVH data is not valid
+ BVH_ERR_UNKNOWN = -8 /// Unknown failure
+ };
-/** \brief BVH model type */
+/// @brief BVH model type
enum BVHModelType
-{
- BVH_MODEL_UNKNOWN,
- BVH_MODEL_TRIANGLES,
- BVH_MODEL_POINTCLOUD
-};
+ {
+ BVH_MODEL_UNKNOWN, /// @brief unknown model type
+ BVH_MODEL_TRIANGLES, /// @brief triangle model
+ BVH_MODEL_POINTCLOUD /// @brief point cloud model
+ };
}
diff --git a/trunk/fcl/include/fcl/BVH_model.h b/trunk/fcl/include/fcl/BVH_model.h
index 67f5f0c6ed433a38faf2ae85f8aa048c1764667f..e46522720457dd17abf3d6eb7bd599e4b30b461c 100644
--- a/trunk/fcl/include/fcl/BVH_model.h
+++ b/trunk/fcl/include/fcl/BVH_model.h
@@ -47,33 +47,16 @@
#include <vector>
#include <boost/shared_ptr.hpp>
-/** \brief Main namespace */
namespace fcl
{
-/** \brief A class describing the bounding hierarchy of a mesh model */
+/// @brief A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as a degraded version of mesh)
template<typename BV>
class BVHModel : public CollisionGeometry
{
-private:
- int num_tris_allocated;
- int num_vertices_allocated;
- int num_bvs_allocated;
- int num_vertex_updated; // for ccd vertex update
- unsigned int* primitive_indices;
public:
-
- /** \brief The state of BVH building process */
- BVHBuildState build_state;
-
- /** \brief Split rule to split one BV node into two children */
- boost::shared_ptr<BVSplitterBase<BV> > bv_splitter;
-
- /** \brief Fitting rule to fit a BV node to a set of geometry primitives */
- boost::shared_ptr<BVFitterBase<BV> > bv_fitter;
-
- /** \brief Model type */
+ /// @brief Model type described by the instance
BVHModelType getModelType() const
{
if(num_tris && num_vertices)
@@ -84,32 +67,29 @@ public:
return BVH_MODEL_UNKNOWN;
}
- /** \brief Constructing an empty BVH */
- BVHModel()
+ /// @brief Constructing an empty BVH
+ BVHModel() : vertices(NULL),
+ tri_indices(NULL),
+ prev_vertices(NULL),
+ num_tris(0),
+ num_vertices(0),
+ build_state(BVH_BUILD_STATE_EMPTY),
+ bv_splitter(new BVSplitter<BV>(SPLIT_METHOD_MEAN)),
+ bv_fitter(new BVFitter<BV>()),
+ num_tris_allocated(0),
+ num_vertices_allocated(0),
+ num_bvs_allocated(0),
+ num_vertex_updated(0),
+ primitive_indices(NULL),
+ bvs(NULL),
+ num_bvs(0)
{
- vertices = NULL;
- tri_indices = NULL;
- bvs = NULL;
-
- num_tris = 0;
- num_tris_allocated = 0;
- num_vertices = 0;
- num_vertices_allocated = 0;
- num_bvs = 0;
- num_bvs_allocated = 0;
-
- prev_vertices = NULL;
-
- primitive_indices = NULL;
-
- build_state = BVH_BUILD_STATE_EMPTY;
-
- bv_splitter.reset(new BVSplitter<BV>(SPLIT_METHOD_MEAN));
- bv_fitter.reset(new BVFitter<BV>());
}
- BVHModel(const BVHModel& other);
+ /// @brief copy from another BVH
+ BVHModel(const BVHModel& other);
+ /// @brief deconstruction, delete mesh data related.
~BVHModel()
{
delete [] vertices;
@@ -117,162 +97,185 @@ public:
delete [] bvs;
delete [] prev_vertices;
-
delete [] primitive_indices;
}
- /** \brief We provide getBV() and getNumBVs() because BVH may be compressed (in future), so
- we must provide some flexibility here */
+ /// @brief We provide getBV() and getNumBVs() because BVH may be compressed (in future), so we must provide some flexibility here
- /** \brief Get the BV of index id */
+ /// @brief Access the bv giving the its index
const BVNode<BV>& getBV(int id) const
{
return bvs[id];
}
- /** \brief Get the BV of index id */
+ /// @brief Access the bv giving the its index
BVNode<BV>& getBV(int id)
{
return bvs[id];
}
- /** \brief Get the number of BVs */
+ /// @brief Get the number of bv in the BVH
int getNumBVs() const
{
return num_bvs;
}
- /** \brief Get the object type: it is a BVH */
+ /// @brief Get the object type: it is a BVH
OBJECT_TYPE getObjectType() const { return OT_BVH; }
- /** \brief Get the BV type */
+ /// @brief Get the BV type: default is unknown
NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
- /** \brief Compute the AABB for the BVH, used for broad-phase collision */
+ /// @brief Compute the AABB for the BVH, used for broad-phase collision
void computeLocalAABB();
- /** \brief Geometry point data */
- Vec3f* vertices;
-
- /** \brief Geometry triangle index data, will be NULL for point clouds */
- Triangle* tri_indices;
-
- /** \brief Geometry point data in previous frame */
- Vec3f* prev_vertices;
-
- /** \brief Number of triangles */
- int num_tris;
-
- /** \brief Number of points */
- int num_vertices;
-
- /** \brief Begin a new BVH model */
+ /// @brief Begin a new BVH model
int beginModel(int num_tris = 0, int num_vertices = 0);
- /** \brief Add one point in the new BVH model */
+ /// @brief Add one point in the new BVH model
int addVertex(const Vec3f& p);
- /** \brief Add one triangle in the new BVH model */
+ /// @brief Add one triangle in the new BVH model
int addTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3);
- /** \brief Add a set of triangles in the new BVH model */
+ /// @brief Add a set of triangles in the new BVH model
int addSubModel(const std::vector<Vec3f>& ps, const std::vector<Triangle>& ts);
- /** \brief Add a set of points in the new BVH model */
+ /// @brief Add a set of points in the new BVH model
int addSubModel(const std::vector<Vec3f>& ps);
- /** \brief End BVH model construction, will build the bounding volume hierarchy */
+ /// @brief End BVH model construction, will build the bounding volume hierarchy
int endModel();
- /** \brief Replace the geometry information of current frame (i.e. should have the same mesh topology with the previous frame) */
+ /// @brief Replace the geometry information of current frame (i.e. should have the same mesh topology with the previous frame)
int beginReplaceModel();
- /** \brief Replace one point in the old BVH model */
+ /// @brief Replace one point in the old BVH model
int replaceVertex(const Vec3f& p);
- /** \brief Replace one triangle in the old BVH model */
+ /// @brief Replace one triangle in the old BVH model
int replaceTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3);
- /** \brief Replace a set of points in the old BVH model */
+ /// @brief Replace a set of points in the old BVH model
int replaceSubModel(const std::vector<Vec3f>& ps);
- /** \brief End BVH model replacement, will also refit or rebuild the bounding volume hierarchy */
+ /// @brief End BVH model replacement, will also refit or rebuild the bounding volume hierarchy
int endReplaceModel(bool refit = true, bool bottomup = true);
- /** \brief Replace the geometry information of current frame (i.e. should have the same mesh topology with the previous frame).
- * The current frame will be saved as the previous frame in prev_vertices.
- * */
+ /// @brief Replace the geometry information of current frame (i.e. should have the same mesh topology with the previous frame).
+ /// The current frame will be saved as the previous frame in prev_vertices.
int beginUpdateModel();
- /** \brief Update one point in the old BVH model */
+ /// @brief Update one point in the old BVH model
int updateVertex(const Vec3f& p);
- /** \brief Update one triangle in the old BVH model */
+ /// @brief Update one triangle in the old BVH model
int updateTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3);
- /** \brief Update a set of points in the old BVH model */
+ /// @brief Update a set of points in the old BVH model
int updateSubModel(const std::vector<Vec3f>& ps);
- /** \brief End BVH model update, will also refit or rebuild the bounding volume hierarchy */
+ /// @brief End BVH model update, will also refit or rebuild the bounding volume hierarchy
int endUpdateModel(bool refit = true, bool bottomup = true);
- /** \brief Check the number of memory used */
+ /// @brief Check the number of memory used
int memUsage(int msg) const;
+ /// @brief This is a special acceleration: BVH_model default stores the BV's transform in world coordinate. However, we can also store each BV's transform related to its parent
+ /// BV node. When traversing the BVH, this can save one matrix transformation.
void makeParentRelative()
{
- makeParentRelativeRecurse(0, Matrix3f::getIdentity(), Vec3f());
+ Vec3f I[3] = {Vec3f(1, 0, 0), Vec3f(0, 1, 0), Vec3f(0, 0, 1)};
+ makeParentRelativeRecurse(0, I, Vec3f());
}
+public:
+ /// @brief Geometry point data
+ Vec3f* vertices;
+
+ /// @brief Geometry triangle index data, will be NULL for point clouds
+ Triangle* tri_indices;
+
+ /// @brief Geometry point data in previous frame
+ Vec3f* prev_vertices;
+
+ /// @brief Number of triangles
+ int num_tris;
+
+ /// @brief Number of points
+ int num_vertices;
+
+ /// @brief The state of BVH building process
+ BVHBuildState build_state;
+
+ /// @brief Split rule to split one BV node into two children
+ boost::shared_ptr<BVSplitterBase<BV> > bv_splitter;
+
+ /// @brief Fitting rule to fit a BV node to a set of geometry primitives
+ boost::shared_ptr<BVFitterBase<BV> > bv_fitter;
+
private:
- /** \brief Bounding volume hierarchy */
+ int num_tris_allocated;
+ int num_vertices_allocated;
+ int num_bvs_allocated;
+ int num_vertex_updated; /// for ccd vertex update
+ unsigned int* primitive_indices;
+
+
+ /// @brief Bounding volume hierarchy
BVNode<BV>* bvs;
- /** \brief Number of BV nodes in bounding volume hierarchy */
+ /// @brief Number of BV nodes in bounding volume hierarchy
int num_bvs;
- /** \brief Build the bounding volume hierarchy */
+ /// @brief Build the bounding volume hierarchy
int buildTree();
- /** \brief Refit the bounding volume hierarchy */
+ /// @brief Refit the bounding volume hierarchy
int refitTree(bool bottomup);
- /** \brief Refit the bounding volume hierarchy in a top-down way (slow but more compact)*/
+ /// @brief Refit the bounding volume hierarchy in a top-down way (slow but more compact)
int refitTree_topdown();
- /** \brief Refit the bounding volume hierarchy in a bottom-up way (fast but less compact) */
+ /// @brief Refit the bounding volume hierarchy in a bottom-up way (fast but less compact)
int refitTree_bottomup();
- /** \brief Recursive kernel for hierarchy construction */
+ /// @brief Recursive kernel for hierarchy construction
int recursiveBuildTree(int bv_id, int first_primitive, int num_primitives);
- /** \brief Recursive kernel for bottomup refitting */
+ /// @brief Recursive kernel for bottomup refitting
int recursiveRefitTree_bottomup(int bv_id);
- void makeParentRelativeRecurse(int bv_id, const Matrix3f& parentR, const Vec3f& parentT)
+ /// @recursively compute each bv's transform related to its parent. For default BV, only the translation works.
+ /// For oriented BV (OBB, RSS, OBBRSS), special implementation is provided.
+ void makeParentRelativeRecurse(int bv_id, Vec3f parent_axis[], const Vec3f& parent_c)
{
if(!bvs[bv_id].isLeaf())
{
- makeParentRelativeRecurse(bvs[bv_id].first_child, bvs[bv_id].getOrientation(), bvs[bv_id].getCenter());
+ makeParentRelativeRecurse(bvs[bv_id].first_child, parent_axis, bvs[bv_id].getCenter());
- makeParentRelativeRecurse(bvs[bv_id].first_child + 1, bvs[bv_id].getOrientation(), bvs[bv_id].getCenter());
+ makeParentRelativeRecurse(bvs[bv_id].first_child + 1, parent_axis, bvs[bv_id].getCenter());
}
- // make self parent relative
- Matrix3f Rpc = parentR.transposeTimes(bvs[bv_id].getOrientation());
- bvs[bv_id].setOrientation(Rpc);
-
- Vec3f Tpc = bvs[bv_id].getCenter() - parentT;
- bvs[bv_id].setCenter(parentR.transposeTimes(Tpc));
+ bvs[bv_id].bv = translate(bvs[bv_id].bv, -parent_c);
}
-
};
-/** Specialization of getNodeType() for BVHModel with different BV types */
+template<>
+void BVHModel<OBB>::makeParentRelativeRecurse(int bv_id, Vec3f parent_axis[], const Vec3f& parent_c);
+
+template<>
+void BVHModel<RSS>::makeParentRelativeRecurse(int bv_id, Vec3f parent_axis[], const Vec3f& parent_c);
+
+template<>
+void BVHModel<OBBRSS>::makeParentRelativeRecurse(int bv_id, Vec3f parent_axis[], const Vec3f& parent_c);
+
+
+/// @brief Specialization of getNodeType() for BVHModel with different BV types
template<>
NODE_TYPE BVHModel<AABB>::getNodeType() const;
diff --git a/trunk/fcl/include/fcl/BVH_utility.h b/trunk/fcl/include/fcl/BVH_utility.h
index bbe39353202fdb7bdae14065b40ccf275ddabae8..70b4d976b81a885fbf520382e095020958bc9a88 100644
--- a/trunk/fcl/include/fcl/BVH_utility.h
+++ b/trunk/fcl/include/fcl/BVH_utility.h
@@ -44,7 +44,7 @@
namespace fcl
{
-/** \brief Expand the BVH bounding boxes according to uncertainty */
+/// @brief Expand the BVH bounding boxes according to the variance matrix corresponding to the data stored within each BV node
template<typename BV>
void BVHExpand(BVHModel<BV>& model, const Variance3f* ucs, FCL_REAL r)
{
@@ -71,33 +71,29 @@ void BVHExpand(BVHModel<BV>& model, const Variance3f* ucs, FCL_REAL r)
}
}
-/** \brief Expand the BVH bounding boxes according to uncertainty, for OBB */
+/// @brief Expand the BVH bounding boxes according to the corresponding variance information, for OBB
void BVHExpand(BVHModel<OBB>& model, const Variance3f* ucs, FCL_REAL r);
-/** \brief Expand the BVH bounding boxes according to uncertainty, for RSS */
+/// @brief Expand the BVH bounding boxes according to the corresponding variance information, for RSS
void BVHExpand(BVHModel<RSS>& model, const Variance3f* ucs, FCL_REAL r);
-/** \brief Estimate the uncertainty of point clouds due to sampling procedure */
-void estimateSamplingUncertainty(Vec3f* vertices, int num_vertices, Variance3f* ucs);
+/// @brief Estimate the variance of point clouds due to sampling procedure
+void estimateSamplingVariance(Vec3f* vertices, int num_vertices, Variance3f* ucs);
-/** \brief Compute the covariance matrix for a set or subset of points. if ts = null, then indices refer to points directly; otherwise refer to triangles */
+/// @brief Compute the covariance matrix for a set or subset of points. if ts = null, then indices refer to points directly; otherwise refer to triangles
void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Matrix3f& M);
-/** \brief Compute the RSS bounding volume parameters: radius, rectangle size and the origin.
- * The bounding volume axes are known.
- */
+/// @brief Compute the RSS bounding volume parameters: radius, rectangle size and the origin, given the BV axises.
void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Vec3f axis[3], Vec3f& origin, FCL_REAL l[2], FCL_REAL& r);
-/** \brief Compute the bounding volume extent and center for a set or subset of points.
- * The bounding volume axes are known.
- */
+/// @brief Compute the bounding volume extent and center for a set or subset of points, given the BV axises.
void getExtentAndCenter(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Vec3f axis[3], Vec3f& center, Vec3f& extent);
-/** \brief Compute the center and radius for a triangle's circumcircle */
+/// @brief Compute the center and radius for a triangle's circumcircle
void circumCircleComputation(const Vec3f& a, const Vec3f& b, const Vec3f& c, Vec3f& center, FCL_REAL& radius);
-/** \brief Compute the maximum distance from a given center point to a point cloud */
+/// @brief Compute the maximum distance from a given center point to a point cloud
FCL_REAL maximumDistance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, const Vec3f& query);
diff --git a/trunk/fcl/include/fcl/BV_fitter.h b/trunk/fcl/include/fcl/BV_fitter.h
index 024391ca60d7e84d613de8926ba818d62cc31171..d88788ec07070d74f0cd7aa66492496d3de89bd0 100644
--- a/trunk/fcl/include/fcl/BV_fitter.h
+++ b/trunk/fcl/include/fcl/BV_fitter.h
@@ -47,11 +47,10 @@
#include "fcl/BV/OBBRSS.h"
#include <iostream>
-/** \brief Main namespace */
namespace fcl
{
-/** \brief Compute a bounding volume that fits a set of n points. */
+/// @brief Compute a bounding volume that fits a set of n points.
template<typename BV>
void fit(Vec3f* ps, int n, BV& bv)
{
@@ -74,28 +73,33 @@ template<>
void fit<OBBRSS>(Vec3f* ps, int n, OBBRSS& bv);
+/// @brief Interface for fitting a bv given the triangles or points inside it.
template<typename BV>
class BVFitterBase
{
public:
+ /// @brief Set the primitives to be processed by the fitter
virtual void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_) = 0;
+ /// @brief Set the primitives to be processed by the fitter, for deformable mesh.
virtual void set(Vec3f* vertices_, Vec3f* prev_vertices_, Triangle* tri_indices_, BVHModelType type_) = 0;
+ /// @brief Compute the fitting BV
virtual BV fit(unsigned int* primitive_indices, int num_primitives) = 0;
+ /// @brief clear the temporary data generated.
virtual void clear() = 0;
};
-/** \brief A class for fitting a bounding volume to a set of points */
+/// @brief The class for the default algorithm fitting a bounding volume to a set of points
template<typename BV>
class BVFitter : public BVFitterBase<BV>
{
public:
- /** \brief default deconstructor */
+ /// @brief default deconstructor
virtual ~BVFitter() {}
- /** \brief Prepare the geometry primitive data for fitting */
+ /// @brief Prepare the geometry primitive data for fitting
void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_)
{
vertices = vertices_;
@@ -104,7 +108,7 @@ public:
type = type_;
}
- /** \brief Prepare the geometry primitive data for fitting */
+ /// @brief Prepare the geometry primitive data for fitting, for deformable mesh
void set(Vec3f* vertices_, Vec3f* prev_vertices_, Triangle* tri_indices_, BVHModelType type_)
{
vertices = vertices_;
@@ -113,14 +117,13 @@ public:
type = type_;
}
- /** \brief Compute a bounding volume that fits a set of primitives (points or triangles).
- * The primitive data was set by set function and primitive_indices is the primitive index relative to the data
- */
+ /// @brief Compute a bounding volume that fits a set of primitives (points or triangles).
+ /// The primitive data was set by set function and primitive_indices is the primitive index relative to the data
BV fit(unsigned int* primitive_indices, int num_primitives)
{
BV bv;
- if(type == BVH_MODEL_TRIANGLES) /* The primitive is triangle */
+ if(type == BVH_MODEL_TRIANGLES) /// The primitive is triangle
{
for(int i = 0; i < num_primitives; ++i)
{
@@ -129,7 +132,7 @@ public:
bv += vertices[t[1]];
bv += vertices[t[2]];
- if(prev_vertices) /* When fitting both current and previous frame */
+ if(prev_vertices) /// can fitting both current and previous frame
{
bv += prev_vertices[t[0]];
bv += prev_vertices[t[1]];
@@ -137,13 +140,13 @@ public:
}
}
}
- else if(type == BVH_MODEL_POINTCLOUD) /* The primitive is point */
+ else if(type == BVH_MODEL_POINTCLOUD) /// The primitive is point
{
for(int i = 0; i < num_primitives; ++i)
{
bv += vertices[primitive_indices[i]];
- if(prev_vertices) /* When fitting both current and previous frame */
+ if(prev_vertices) /// can fitting both current and previous frame
{
bv += prev_vertices[primitive_indices[i]];
}
@@ -153,7 +156,7 @@ public:
return bv;
}
- /** \brief Clear the geometry primitive data */
+ /// @brief Clear the geometry primitive data
void clear()
{
vertices = NULL;
@@ -171,12 +174,12 @@ private:
};
-/** \brief Specification of BVFitter for OBB bounding volume */
+/// @brief Specification of BVFitter for OBB bounding volume
template<>
class BVFitter<OBB> : public BVFitterBase<OBB>
{
public:
- /** \brief Prepare the geometry primitive data for fitting */
+ /// @brief Prepare the geometry primitive data for fitting
void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_)
{
vertices = vertices_;
@@ -185,7 +188,7 @@ public:
type = type_;
}
- /** \brief Prepare the geometry primitive data for fitting */
+ /// @brief Prepare the geometry primitive data for fitting, for deformable mesh
void set(Vec3f* vertices_, Vec3f* prev_vertices_, Triangle* tri_indices_, BVHModelType type_)
{
vertices = vertices_;
@@ -194,12 +197,11 @@ public:
type = type_;
}
- /** \brief Compute a bounding volume that fits a set of primitives (points or triangles).
- * The primitive data was set by set function and primitive_indices is the primitive index relative to the data.
- */
+ /// @brief Compute a bounding volume that fits a set of primitives (points or triangles).
+ /// The primitive data was set by set function and primitive_indices is the primitive index relative to the data.
OBB fit(unsigned int* primitive_indices, int num_primitives);
- /** \brief Clear the geometry primitive data */
+ /// brief Clear the geometry primitive data
void clear()
{
vertices = NULL;
@@ -217,12 +219,12 @@ private:
};
-/** \brief Specification of BVFitter for RSS bounding volume */
+/// @brief Specification of BVFitter for RSS bounding volume
template<>
class BVFitter<RSS> : public BVFitterBase<RSS>
{
public:
- /** \brief Prepare the geometry primitive data for fitting */
+ /// brief Prepare the geometry primitive data for fitting
void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_)
{
vertices = vertices_;
@@ -231,7 +233,7 @@ public:
type = type_;
}
- /** \brief Prepare the geometry primitive data for fitting */
+ /// @brief Prepare the geometry primitive data for fitting, for deformable mesh
void set(Vec3f* vertices_, Vec3f* prev_vertices_, Triangle* tri_indices_, BVHModelType type_)
{
vertices = vertices_;
@@ -240,12 +242,11 @@ public:
type = type_;
}
- /** \brief Compute a bounding volume that fits a set of primitives (points or triangles).
- * The primitive data was set by set function and primitive_indices is the primitive index relative to the data.
- */
+ /// @brief Compute a bounding volume that fits a set of primitives (points or triangles).
+ /// The primitive data was set by set function and primitive_indices is the primitive index relative to the data.
RSS fit(unsigned int* primitive_indices, int num_primitives);
- /** \brief Clear the geometry primitive data */
+ /// @brief Clear the geometry primitive data
void clear()
{
vertices = NULL;
@@ -263,12 +264,12 @@ private:
};
-/** \brief Specification of BVFitter for kIOS bounding volume */
+/// @brief Specification of BVFitter for kIOS bounding volume
template<>
class BVFitter<kIOS> : public BVFitterBase<kIOS>
{
public:
- /** \brief Prepare the geometry primitive data for fitting */
+ /// @brief Prepare the geometry primitive data for fitting
void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_)
{
vertices = vertices_;
@@ -277,7 +278,7 @@ public:
type = type_;
}
- /** \brief Prepare the geometry primitive data for fitting */
+ /// @brief Prepare the geometry primitive data for fitting
void set(Vec3f* vertices_, Vec3f* prev_vertices_, Triangle* tri_indices_, BVHModelType type_)
{
vertices = vertices_;
@@ -286,12 +287,11 @@ public:
type = type_;
}
- /** \brief Compute a bounding volume that fits a set of primitives (points or triangles).
- * The primitive data was set by set function and primitive_indices is the primitive index relative to the data.
- */
+ /// @brief Compute a bounding volume that fits a set of primitives (points or triangles).
+ /// The primitive data was set by set function and primitive_indices is the primitive index relative to the data.
kIOS fit(unsigned int* primitive_indices, int num_primitives);
- /** \brief Clear the geometry primitive data */
+ /// @brief Clear the geometry primitive data
void clear()
{
vertices = NULL;
@@ -308,12 +308,12 @@ private:
};
-/** \brief Specification of BVFitter for OBBRSS bounding volume */
+/// @brief Specification of BVFitter for OBBRSS bounding volume
template<>
class BVFitter<OBBRSS> : public BVFitterBase<OBBRSS>
{
public:
- /** \brief Prepare the geometry primitive data for fitting */
+ /// @brief Prepare the geometry primitive data for fitting
void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_)
{
vertices = vertices_;
@@ -322,7 +322,7 @@ public:
type = type_;
}
- /** \brief Prepare the geometry primitive data for fitting */
+ /// @brief Prepare the geometry primitive data for fitting
void set(Vec3f* vertices_, Vec3f* prev_vertices_, Triangle* tri_indices_, BVHModelType type_)
{
vertices = vertices_;
@@ -331,12 +331,11 @@ public:
type = type_;
}
- /** \brief Compute a bounding volume that fits a set of primitives (points or triangles).
- * The primitive data was set by set function and primitive_indices is the primitive index relative to the data.
- */
+ /// @brief Compute a bounding volume that fits a set of primitives (points or triangles).
+ /// The primitive data was set by set function and primitive_indices is the primitive index relative to the data.
OBBRSS fit(unsigned int* primitive_indices, int num_primitives);
- /** \brief Clear the geometry primitive data */
+ /// @brief Clear the geometry primitive data
void clear()
{
vertices = NULL;
diff --git a/trunk/fcl/include/fcl/BV_node.h b/trunk/fcl/include/fcl/BV_node.h
index 28ab9fe4141aef01b4786e49bb5fe46f089cdcee..534e06195bec7712d96ae990de0f694c06cad2ec 100644
--- a/trunk/fcl/include/fcl/BV_node.h
+++ b/trunk/fcl/include/fcl/BV_node.h
@@ -41,135 +41,91 @@
#include "fcl/vec_3f.h"
#include "fcl/matrix_3f.h"
-#include "fcl/BV/OBB.h"
-#include "fcl/BV/RSS.h"
+#include "fcl/BV.h"
+#include <iostream>
-/** \brief Main namespace */
namespace fcl
{
+/// @brief BVNodeBase encodes the tree structure for BVH
struct BVNodeBase
{
- /** \brief An index for first child node or primitive
- * If the value is positive, it is the index of the first child bv node
- * If the value is negative, it is -(primitive index + 1)
- * Zero is not used.
- */
+ /// @brief An index for first child node or primitive
+ /// If the value is positive, it is the index of the first child bv node
+ /// If the value is negative, it is -(primitive index + 1)
+ /// Zero is not used.
int first_child;
- /** \brief The start id the primitive belonging to the current node. The index is referred to the primitive_indices in BVHModel and from that
- * we can obtain the primitive's index in original data indirectly.
- */
+ /// @brief The start id the primitive belonging to the current node. The index is referred to the primitive_indices in BVHModel and from that
+ /// we can obtain the primitive's index in original data indirectly.
int first_primitive;
- /** \brief The number of primitives belonging to the current node */
+ /// @brief The number of primitives belonging to the current node
int num_primitives;
- /** \brief Whether current node is a leaf node (i.e. contains a primitive index */
+ /// @brief Whether current node is a leaf node (i.e. contains a primitive index
inline bool isLeaf() const { return first_child < 0; }
- /** \brief Return the primitive index. The index is referred to the original data (i.e. vertices or tri_indices) in BVHModel */
+ /// @brief Return the primitive index. The index is referred to the original data (i.e. vertices or tri_indices) in BVHModel
inline int primitiveId() const { return -(first_child + 1); }
- /** \brief Return the index of the first child. The index is referred to the bounding volume array (i.e. bvs) in BVHModel */
+ /// @brief Return the index of the first child. The index is referred to the bounding volume array (i.e. bvs) in BVHModel
inline int leftChild() const { return first_child; }
- /** \brief Return the index of the second child. The index is referred to the bounding volume array (i.e. bvs) in BVHModel */
+ /// @brief Return the index of the second child. The index is referred to the bounding volume array (i.e. bvs) in BVHModel
inline int rightChild() const { return first_child + 1; }
};
-/** \brief A class describing a bounding volume node */
+/// @brief A class describing a bounding volume node. It includes the tree structure providing in BVNodeBase and also the geometry data provided in BV template parameter.
template<typename BV>
struct BVNode : public BVNodeBase
{
- /** \brief A bounding volume */
+ /// @brief bounding volume storing the geometry
BV bv;
- /** \brief Check whether two BVNode collide */
+ /// @brief Check whether two BVNode collide
bool overlap(const BVNode& other) const
{
return bv.overlap(other.bv);
}
+ /// @brief Compute the distance between two BVNode. P1 and P2, if not NULL and the underlying BV supports distance, return the nearest points.
FCL_REAL distance(const BVNode& other, Vec3f* P1 = NULL, Vec3f* P2 = NULL) const
{
return bv.distance(other.bv, P1, P2);
}
- Vec3f getCenter() const { return Vec3f(); }
- void setCenter(const Vec3f& v) {}
+ /// @brief Access the center of the BV
+ Vec3f getCenter() const { return bv.center(); }
+
+ /// @brief Access the orientation of the BV
Matrix3f getOrientation() const { return Matrix3f::getIdentity(); }
- void setOrientation(const Matrix3f& m) {}
};
template<>
-struct BVNode<OBB> : public BVNodeBase
+inline Matrix3f BVNode<OBB>::getOrientation() const
{
- /** \brief A bounding volume */
- OBB bv;
-
- /** \brief Check whether two BVNode collide */
- bool overlap(const BVNode& other) const
- {
- return bv.overlap(other.bv);
- }
-
- FCL_REAL distance(const BVNode& other, Vec3f* P1 = NULL, Vec3f* P2 = NULL) const
- {
- return bv.distance(other.bv, P1, P2);
- }
-
- Vec3f getCenter() const { return bv.To; }
- void setCenter(const Vec3f& v) { bv.To = v; }
- Matrix3f getOrientation() const
- {
- Matrix3f m(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
- bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
- bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]);
- return m;
- }
- void setOrientation(const Matrix3f& m)
- {
- bv.axis[0].setValue(m(0, 0), m(1, 0), m(2, 0));
- bv.axis[1].setValue(m(0, 1), m(1, 1), m(2, 1));
- bv.axis[2].setValue(m(0, 2), m(1, 2), m(2, 2));
- }
-};
+ return Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
+ bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
+ bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]);
+}
template<>
-struct BVNode<RSS> : public BVNodeBase
+inline Matrix3f BVNode<RSS>::getOrientation() const
{
- /** \brief A bounding volume */
- RSS bv;
-
- /** \brief Check whether two BVNode collide */
- bool overlap(const BVNode& other) const
- {
- return bv.overlap(other.bv);
- }
-
- FCL_REAL distance(const BVNode& other, Vec3f* P1 = NULL, Vec3f* P2 = NULL) const
- {
- return bv.distance(other.bv, P1, P2);
- }
+ return Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
+ bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
+ bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]);
+}
- Vec3f getCenter() const { return bv.Tr; }
- void setCenter(const Vec3f& v) { bv.Tr = v; }
- Matrix3f getOrientation() const
- {
- Matrix3f m(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
- bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
- bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]);
- return m;
- }
+template<>
+inline Matrix3f BVNode<OBBRSS>::getOrientation() const
+{
+ return 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]);
+}
- void setOrientation(const Matrix3f& m)
- {
- bv.axis[0].setValue(m(0, 0), m(1, 0), m(2, 0));
- bv.axis[1].setValue(m(0, 1), m(1, 1), m(2, 1));
- bv.axis[2].setValue(m(0, 2), m(1, 2), m(2, 2));
- }
-};
}
diff --git a/trunk/fcl/include/fcl/BV_splitter.h b/trunk/fcl/include/fcl/BV_splitter.h
index ae38295cca3b1d6ddb861e840718bdb3e6adf841..cde0a6e8c2658b128e869386f49f8df826615172 100644
--- a/trunk/fcl/include/fcl/BV_splitter.h
+++ b/trunk/fcl/include/fcl/BV_splitter.h
@@ -48,47 +48,46 @@
#include <vector>
#include <iostream>
-/** \brief Main namespace */
namespace fcl
{
-/** \brief Base class for BV splitting operation */
+/// @brief Base interface for BV splitting algorithm
template<typename BV>
class BVSplitterBase
{
public:
- /** \brief Set the geometry data needed by the split rule */
+ /// @brief Set the geometry data needed by the split rule
virtual void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_) = 0;
- /** \brief Compute the split rule according to a subset of geometry and the corresponding BV node */
+ /// @brief Compute the split rule according to a subset of geometry and the corresponding BV node
virtual void computeRule(const BV& bv, unsigned int* primitive_indices, int num_primitives) = 0;
- /** \brief Apply the split rule on a given point */
+ /// @brief Apply the split rule on a given point
virtual bool apply(const Vec3f& q) const = 0;
- /** \brief Clear the geometry data set before */
+ /// @brief Clear the geometry data set before
virtual void clear() = 0;
};
+/// @brief Three types of split algorithms are provided in FCL as default
enum SplitMethodType {SPLIT_METHOD_MEAN, SPLIT_METHOD_MEDIAN, SPLIT_METHOD_BV_CENTER};
-/** \brief A class describing the split rule that splits each BV node */
+/// @brief A class describing the split rule that splits each BV node
template<typename BV>
class BVSplitter : public BVSplitterBase<BV>
{
public:
- BVSplitter(SplitMethodType method)
+ BVSplitter(SplitMethodType method) : split_method(method)
{
- split_method = method;
}
- /** \brief Default deconstructor */
+ /// @brief Default deconstructor
virtual ~BVSplitter() {}
- /** \brief Set the geometry data needed by the split rule */
+ /// @brief Set the geometry data needed by the split rule
void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_)
{
vertices = vertices_;
@@ -96,32 +95,32 @@ public:
type = type_;
}
- /** \brief Compute the split rule according to a subset of geometry and the corresponding BV node */
+ /// @brief Compute the split rule according to a subset of geometry and the corresponding BV node
void computeRule(const BV& bv, unsigned int* primitive_indices, int num_primitives)
{
switch(split_method)
{
- case SPLIT_METHOD_MEAN:
- computeRule_mean(bv, primitive_indices, num_primitives);
- break;
- case SPLIT_METHOD_MEDIAN:
- computeRule_median(bv, primitive_indices, num_primitives);
- break;
- case SPLIT_METHOD_BV_CENTER:
- computeRule_bvcenter(bv, primitive_indices, num_primitives);
- break;
- default:
- std::cerr << "Split method not supported" << std::endl;
+ case SPLIT_METHOD_MEAN:
+ computeRule_mean(bv, primitive_indices, num_primitives);
+ break;
+ case SPLIT_METHOD_MEDIAN:
+ computeRule_median(bv, primitive_indices, num_primitives);
+ break;
+ case SPLIT_METHOD_BV_CENTER:
+ computeRule_bvcenter(bv, primitive_indices, num_primitives);
+ break;
+ default:
+ std::cerr << "Split method not supported" << std::endl;
}
}
- /** \brief Apply the split rule on a given point */
+ /// @brief Apply the split rule on a given point
bool apply(const Vec3f& q) const
{
return q[split_axis] > split_value;
}
- /** \brief Clear the geometry data set before */
+ /// @brief Clear the geometry data set before
void clear()
{
vertices = NULL;
@@ -131,18 +130,27 @@ public:
private:
- /** \brief The split axis */
+ /// @brief The axis based on which the split decision is made. For most BV, the axis is aligned with one of the world coordinate, so only split_axis is needed.
+ /// For oriented node, we can use a vector to make a better split decision.
int split_axis;
+ Vec3f split_vector;
- /** \brief The split threshold */
+ /// @brief The split threshold, different primitives are splitted according whether their projection on the split_axis is larger or smaller than the threshold
FCL_REAL split_value;
+ /// @brief The mesh vertices or points handled by the splitter
Vec3f* vertices;
+
+ /// @brief The triangles handled by the splitter
Triangle* tri_indices;
+
+ /// @brief Whether the geometry is mesh or point cloud
BVHModelType type;
+
+ /// @brief The split algorithm used
SplitMethodType split_method;
- /** \brief Split the node from center */
+ /// @brief Split algorithm 1: Split the node from center
void computeRule_bvcenter(const BV& bv, unsigned int* primitive_indices, int num_primitives)
{
Vec3f center = bv.center();
@@ -157,7 +165,7 @@ private:
split_value = center[axis];
}
- /** \brief Split the node according to the mean of the data contained */
+ /// @brief Split algorithm 2: Split the node according to the mean of the data contained
void computeRule_mean(const BV& bv, unsigned int* primitive_indices, int num_primitives)
{
int axis = 2;
@@ -191,7 +199,7 @@ private:
split_value = sum / num_primitives;
}
- /** \brief Split the node according to the median of the data contained */
+ /// @brief Split algorithm 3: Split the node according to the median of the data contained
void computeRule_median(const BV& bv, unsigned int* primitive_indices, int num_primitives)
{
int axis = 2;
@@ -232,295 +240,53 @@ private:
};
-
-/** \brief BVHSplitRule specialization for OBB */
template<>
-class BVSplitter<OBB> : public BVSplitterBase<OBB>
-{
-public:
-
- BVSplitter(SplitMethodType method)
- {
- split_method = method;
- }
-
- /** \brief Set the geometry data needed by the split rule */
- void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_)
- {
- vertices = vertices_;
- tri_indices = tri_indices_;
- type = type_;
- }
-
- /** \brief Compute the split rule according to a subset of geometry and the corresponding BV node */
- void computeRule(const OBB& bv, unsigned int* primitive_indices, int num_primitives)
- {
- switch(split_method)
- {
- case SPLIT_METHOD_MEAN:
- computeRule_mean(bv, primitive_indices, num_primitives);
- break;
- case SPLIT_METHOD_MEDIAN:
- computeRule_median(bv, primitive_indices, num_primitives);
- break;
- case SPLIT_METHOD_BV_CENTER:
- computeRule_bvcenter(bv, primitive_indices, num_primitives);
- break;
- default:
- std::cerr << "Split method not supported" << std::endl;
- }
- }
-
- /** \brief Apply the split rule on a given point */
- bool apply(const Vec3f& q) const
- {
- return split_vector.dot(Vec3f(q[0], q[1], q[2])) > split_value;
- }
-
- /** \brief Clear the geometry data set before */
- void clear()
- {
- vertices = NULL;
- tri_indices = NULL;
- type = BVH_MODEL_UNKNOWN;
- }
-
-private:
-
- /** \brief Split the node from center */
- void computeRule_bvcenter(const OBB& bv, unsigned int* primitive_indices, int num_primitives);
-
- /** \brief Split the node according to the mean of the data contained */
- void computeRule_mean(const OBB& bv, unsigned int* primitive_indices, int num_primitives);
-
- /** \brief Split the node according to the median of the data contained */
- void computeRule_median(const OBB& bv, unsigned int* primitive_indices, int num_primitives);
-
- FCL_REAL split_value;
- Vec3f split_vector;
-
- Vec3f* vertices;
- Triangle* tri_indices;
- BVHModelType type;
- SplitMethodType split_method;
-};
+bool BVSplitter<OBB>::apply(const Vec3f& q) const;
-
-/** \brief BVHSplitRule specialization for RSS */
template<>
-class BVSplitter<RSS> : public BVSplitterBase<RSS>
-{
-public:
+bool BVSplitter<RSS>::apply(const Vec3f& q) const;
- BVSplitter(SplitMethodType method)
- {
- split_method = method;
- }
-
- /** \brief Set the geometry data needed by the split rule */
- void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_)
- {
- vertices = vertices_;
- tri_indices = tri_indices_;
- type = type_;
- }
-
- /** \brief Compute the split rule according to a subset of geometry and the corresponding BV node */
- void computeRule(const RSS& bv, unsigned int* primitive_indices, int num_primitives)
- {
- switch(split_method)
- {
- case SPLIT_METHOD_MEAN:
- computeRule_mean(bv, primitive_indices, num_primitives);
- break;
- case SPLIT_METHOD_MEDIAN:
- computeRule_median(bv, primitive_indices, num_primitives);
- break;
- case SPLIT_METHOD_BV_CENTER:
- computeRule_bvcenter(bv, primitive_indices, num_primitives);
- break;
- default:
- std::cerr << "Split method not supported" << std::endl;
- }
- }
-
- /** \brief Apply the split rule on a given point */
- bool apply(const Vec3f& q) const
- {
- return split_vector.dot(Vec3f(q[0], q[1], q[2])) > split_value;
- }
-
- /** \brief Clear the geometry data set before */
- void clear()
- {
- vertices = NULL;
- tri_indices = NULL;
- type = BVH_MODEL_UNKNOWN;
- }
-
-private:
-
- /** \brief Split the node from center */
- void computeRule_bvcenter(const RSS& bv, unsigned int* primitive_indices, int num_primitives);
-
- /** \brief Split the node according to the mean of the data contained */
- void computeRule_mean(const RSS& bv, unsigned int* primitive_indices, int num_primitives);
-
- /** \brief Split the node according to the median of the data contained */
- void computeRule_median(const RSS& bv, unsigned int* primitive_indices, int num_primitives);
-
- FCL_REAL split_value;
- Vec3f split_vector;
-
- Vec3f* vertices;
- Triangle* tri_indices;
- BVHModelType type;
- SplitMethodType split_method;
-};
-
-
-/** \brief BVHSplitRule specialization for RSS */
template<>
-class BVSplitter<kIOS> : public BVSplitterBase<kIOS>
-{
-public:
-
- BVSplitter(SplitMethodType method)
- {
- split_method = method;
- }
-
- /** \brief Set the geometry data needed by the split rule */
- void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_)
- {
- vertices = vertices_;
- tri_indices = tri_indices_;
- type = type_;
- }
-
- /** \brief Compute the split rule according to a subset of geometry and the corresponding BV node */
- void computeRule(const kIOS& bv, unsigned int* primitive_indices, int num_primitives)
- {
- switch(split_method)
- {
- case SPLIT_METHOD_MEAN:
- computeRule_mean(bv, primitive_indices, num_primitives);
- break;
- case SPLIT_METHOD_MEDIAN:
- computeRule_median(bv, primitive_indices, num_primitives);
- break;
- case SPLIT_METHOD_BV_CENTER:
- computeRule_bvcenter(bv, primitive_indices, num_primitives);
- break;
- default:
- std::cerr << "Split method not supported" << std::endl;
- }
- }
-
- /** \brief Apply the split rule on a given point */
- bool apply(const Vec3f& q) const
- {
- return split_vector.dot(Vec3f(q[0], q[1], q[2])) > split_value;
- }
-
- /** \brief Clear the geometry data set before */
- void clear()
- {
- vertices = NULL;
- tri_indices = NULL;
- type = BVH_MODEL_UNKNOWN;
- }
-
-private:
-
- /** \brief Split the node from center */
- void computeRule_bvcenter(const kIOS& bv, unsigned int* primitive_indices, int num_primitives);
-
- /** \brief Split the node according to the mean of the data contained */
- void computeRule_mean(const kIOS& bv, unsigned int* primitive_indices, int num_primitives);
-
- /** \brief Split the node according to the median of the data contained */
- void computeRule_median(const kIOS& bv, unsigned int* primitive_indices, int num_primitives);
-
- FCL_REAL split_value;
- Vec3f split_vector;
-
- Vec3f* vertices;
- Triangle* tri_indices;
- BVHModelType type;
- SplitMethodType split_method;
-};
+bool BVSplitter<kIOS>::apply(const Vec3f& q) const;
template<>
-class BVSplitter<OBBRSS> : public BVSplitterBase<OBBRSS>
-{
-public:
+bool BVSplitter<OBBRSS>::apply(const Vec3f& q) const;
- BVSplitter(SplitMethodType method)
- {
- split_method = method;
- }
+template<>
+void BVSplitter<OBB>::computeRule_bvcenter(const OBB& bv, unsigned int* primitive_indices, int num_primitives);
- /** \brief Set the geometry data needed by the split rule */
- void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_)
- {
- vertices = vertices_;
- tri_indices = tri_indices_;
- type = type_;
- }
+template<>
+void BVSplitter<OBB>::computeRule_mean(const OBB& bv, unsigned int* primitive_indices, int num_primitives);
- /** \brief Compute the split rule according to a subset of geometry and the corresponding BV node */
- void computeRule(const OBBRSS& bv, unsigned int* primitive_indices, int num_primitives)
- {
- switch(split_method)
- {
- case SPLIT_METHOD_MEAN:
- computeRule_mean(bv, primitive_indices, num_primitives);
- break;
- case SPLIT_METHOD_MEDIAN:
- computeRule_median(bv, primitive_indices, num_primitives);
- break;
- case SPLIT_METHOD_BV_CENTER:
- computeRule_bvcenter(bv, primitive_indices, num_primitives);
- break;
- default:
- std::cerr << "Split method not supported" << std::endl;
- }
- }
+template<>
+void BVSplitter<OBB>::computeRule_median(const OBB& bv, unsigned int* primitive_indices, int num_primitives);
- /** \brief Apply the split rule on a given point */
- bool apply(const Vec3f& q) const
- {
- return split_vector.dot(Vec3f(q[0], q[1], q[2])) > split_value;
- }
+template<>
+void BVSplitter<RSS>::computeRule_bvcenter(const RSS& bv, unsigned int* primitive_indices, int num_primitives);
+
+template<>
+void BVSplitter<RSS>::computeRule_mean(const RSS& bv, unsigned int* primitive_indices, int num_primitives);
- /** \brief Clear the geometry data set before */
- void clear()
- {
- vertices = NULL;
- tri_indices = NULL;
- type = BVH_MODEL_UNKNOWN;
- }
+template<>
+void BVSplitter<RSS>::computeRule_median(const RSS& bv, unsigned int* primitive_indices, int num_primitives);
-private:
+template<>
+void BVSplitter<kIOS>::computeRule_bvcenter(const kIOS& bv, unsigned int* primitive_indices, int num_primitives);
- /** \brief Split the node from center */
- void computeRule_bvcenter(const OBBRSS& bv, unsigned int* primitive_indices, int num_primitives);
+template<>
+void BVSplitter<kIOS>::computeRule_mean(const kIOS& bv, unsigned int* primitive_indices, int num_primitives);
- /** \brief Split the node according to the mean of the data contained */
- void computeRule_mean(const OBBRSS& bv, unsigned int* primitive_indices, int num_primitives);
+template<>
+void BVSplitter<kIOS>::computeRule_median(const kIOS& bv, unsigned int* primitive_indices, int num_primitives);
- /** \brief Split the node according to the median of the data contained */
- void computeRule_median(const OBBRSS& bv, unsigned int* primitive_indices, int num_primitives);
+template<>
+void BVSplitter<OBBRSS>::computeRule_bvcenter(const OBBRSS& bv, unsigned int* primitive_indices, int num_primitives);
- FCL_REAL split_value;
- Vec3f split_vector;
+template<>
+void BVSplitter<OBBRSS>::computeRule_mean(const OBBRSS& bv, unsigned int* primitive_indices, int num_primitives);
- Vec3f* vertices;
- Triangle* tri_indices;
- BVHModelType type;
- SplitMethodType split_method;
-};
+template<>
+void BVSplitter<OBBRSS>::computeRule_median(const OBBRSS& bv, unsigned int* primitive_indices, int num_primitives);
}
diff --git a/trunk/fcl/include/fcl/broad_phase_collision.h b/trunk/fcl/include/fcl/broad_phase_collision.h
deleted file mode 100644
index b3d7d626ee50c98a3eebb013e2f9766ad3830535..0000000000000000000000000000000000000000
--- a/trunk/fcl/include/fcl/broad_phase_collision.h
+++ /dev/null
@@ -1,1646 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, Willow Garage, Inc.
- * 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 Jia Pan */
-
-
-
-#ifndef FCL_BROAD_PHASE_COLLISION_H
-#define FCL_BROAD_PHASE_COLLISION_H
-
-#include "fcl/collision_object.h"
-#include "fcl/collision_data.h"
-#include "fcl/BV/AABB.h"
-#include "fcl/interval_tree.h"
-#include "fcl/hierarchy_tree.h"
-#include "fcl/hash.h"
-#include "fcl/octree.h"
-#include <vector>
-#include <list>
-#include <iostream>
-#include <boost/unordered_map.hpp>
-#include <map>
-#include <boost/bind.hpp>
-
-namespace fcl
-{
-
-
-/** \brief return value is whether can stop now */
-typedef bool (*CollisionCallBack)(CollisionObject* o1, CollisionObject* o2, void* cdata);
-
-typedef bool (*DistanceCallBack)(CollisionObject* o1, CollisionObject* o2, void* cdata, FCL_REAL& dist);
-
-
-/** \brief Base class for broad phase collision */
-class BroadPhaseCollisionManager
-{
-public:
- BroadPhaseCollisionManager()
- {
- enable_tested_set_ = false;
- }
-
- virtual ~BroadPhaseCollisionManager() {}
-
- /** \brief add objects to the manager */
- virtual void registerObjects(const std::vector<CollisionObject*>& other_objs)
- {
- for(size_t i = 0; i < other_objs.size(); ++i)
- registerObject(other_objs[i]);
- }
-
- /** \brief add one object to the manager */
- virtual void registerObject(CollisionObject* obj) = 0;
-
- /** \brief remove one object from the manager */
- virtual void unregisterObject(CollisionObject* obj) = 0;
-
- /** \brief initialize the manager, related with the specific type of manager */
- virtual void setup() = 0;
-
- /** \brief update the condition of manager */
- virtual void update() = 0;
-
- /** \brief update the manager by explicitly given the object updated */
- virtual void update(CollisionObject* updated_obj)
- {
- update();
- }
-
- /** \brief update the manager by explicitly given the set of objects update */
- virtual void update(const std::vector<CollisionObject*>& updated_objs)
- {
- update();
- }
-
- /** \brief clear the manager */
- virtual void clear() = 0;
-
- /** \brief return the objects managed by the manager */
- virtual void getObjects(std::vector<CollisionObject*>& objs) const = 0;
-
- /** \brief perform collision test between one object and all the objects belonging to the manager */
- virtual void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const = 0;
-
- /** \brief perform distance computation between one object and all the objects belonging to the manager */
- virtual void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const = 0;
-
- /** \brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision) */
- virtual void collide(void* cdata, CollisionCallBack callback) const = 0;
-
- /** \brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance) */
- virtual void distance(void* cdata, DistanceCallBack callback) const = 0;
-
- /** \brief perform collision test with objects belonging to another manager */
- virtual void collide(BroadPhaseCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const = 0;
-
- /** \brief perform distance test with objects belonging to another manager */
- virtual void distance(BroadPhaseCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const = 0;
-
- /** \brief whether the manager is empty */
- virtual bool empty() const = 0;
-
- /** \brief the number of objects managed by the manager */
- virtual size_t size() const = 0;
-
-protected:
- mutable std::set<std::pair<CollisionObject*, CollisionObject*> > tested_set;
- mutable bool enable_tested_set_;
-
- bool inTestedSet(CollisionObject* a, CollisionObject* b) const
- {
- if(a < b) return tested_set.find(std::make_pair(a, b)) != tested_set.end();
- else return tested_set.find(std::make_pair(b, a)) != tested_set.end();
- }
-
- void insertTestedSet(CollisionObject* a, CollisionObject* b) const
- {
- if(a < b) tested_set.insert(std::make_pair(a, b));
- else tested_set.insert(std::make_pair(b, a));
- }
-
-};
-
-
-/** \brief Brute force N-body collision manager */
-class NaiveCollisionManager : public BroadPhaseCollisionManager
-{
-public:
- NaiveCollisionManager() {}
-
- /** \brief add objects to the manager */
- void registerObjects(const std::vector<CollisionObject*>& other_objs);
-
- /** \brief add one object to the manager */
- void registerObject(CollisionObject* obj);
-
- /** \brief remove one object from the manager */
- void unregisterObject(CollisionObject* obj);
-
- /** \brief initialize the manager, related with the specific type of manager */
- void setup();
-
- /** \brief update the condition of manager */
- void update();
-
- /** \brief clear the manager */
- void clear();
-
- /** \brief return the objects managed by the manager */
- void getObjects(std::vector<CollisionObject*>& objs) const;
-
- /** \brief perform collision test between one object and all the objects belonging to the manager */
- void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
-
- /** \brief perform distance computation between one object and all the objects belonging to the manager */
- void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const;
-
- /** \brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision) */
- void collide(void* cdata, CollisionCallBack callback) const;
-
- /** \brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance) */
- void distance(void* cdata, DistanceCallBack callback) const;
-
- /** \brief perform collision test with objects belonging to another manager */
- void collide(BroadPhaseCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const;
-
- /** \brief perform distance test with objects belonging to another manager */
- void distance(BroadPhaseCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const;
-
- /** \brief whether the manager is empty */
- bool empty() const;
-
- /** \brief the number of objects managed by the manager */
- inline size_t size() const { return objs.size(); }
-
-protected:
-
- /** \brief objects belonging to the manager are stored in a list structure */
- std::list<CollisionObject*> objs;
-};
-
-/** \brief Spatial hash function: hash an AABB to a set of integer values */
-struct SpatialHash
-{
- SpatialHash(const AABB& scene_limit_, FCL_REAL cell_size_)
- {
- cell_size = cell_size_;
- scene_limit = scene_limit_;
- width[0] = ceil(scene_limit.width() / cell_size);
- width[1] = ceil(scene_limit.height() / cell_size);
- width[2] = ceil(scene_limit.depth() / cell_size);
- }
-
- std::vector<unsigned int> operator() (const AABB& aabb) const
- {
- int min_x = floor((aabb.min_[0] - scene_limit.min_[0]) / cell_size);
- int max_x = ceil((aabb.max_[0] - scene_limit.min_[0]) / cell_size);
- int min_y = floor((aabb.min_[1] - scene_limit.min_[1]) / cell_size);
- int max_y = ceil((aabb.max_[1] - scene_limit.min_[1]) / cell_size);
- int min_z = floor((aabb.min_[2] - scene_limit.min_[2]) / cell_size);
- int max_z = ceil((aabb.max_[2] - scene_limit.min_[2]) / cell_size);
-
- std::vector<unsigned int> keys((max_x - min_x) * (max_y - min_y) * (max_z - min_z));
- int id = 0;
- for(int x = min_x; x < max_x; ++x)
- {
- for(int y = min_y; y < max_y; ++y)
- {
- for(int z = min_z; z < max_z; ++z)
- {
- keys[id++] = x + y * width[0] + z * width[0] * width[1];
- }
- }
- }
- return keys;
- }
-
-private:
-
- FCL_REAL cell_size;
- AABB scene_limit;
- unsigned int width[3];
-};
-
-/** \brief spatial hashing collision mananger */
-template<typename HashTable = SimpleHashTable<AABB, CollisionObject*, SpatialHash> >
-class SpatialHashingCollisionManager : public BroadPhaseCollisionManager
-{
-public:
- SpatialHashingCollisionManager(FCL_REAL cell_size, const Vec3f& scene_min, const Vec3f& scene_max, unsigned int default_table_size = 1000)
- {
- scene_limit = AABB(scene_min, scene_max);
- SpatialHash hasher(scene_limit, cell_size);
- hash_table = new HashTable(hasher);
- hash_table->init(default_table_size);
- }
-
- ~SpatialHashingCollisionManager()
- {
- delete hash_table;
- }
-
- /** \brief add one object to the manager */
- void registerObject(CollisionObject* obj);
-
- /** \brief remove one object from the manager */
- void unregisterObject(CollisionObject* obj);
-
- /** \brief initialize the manager, related with the specific type of manager */
- void setup();
-
- /** \brief update the condition of manager */
- void update();
-
- /** \brief update the manager by explicitly given the object updated */
- void update(CollisionObject* updated_obj);
-
- /** \brief update the manager by explicitly given the set of objects update */
- void update(const std::vector<CollisionObject*>& updated_objs);
-
- /** \brief clear the manager */
- void clear();
-
- /** \brief return the objects managed by the manager */
- void getObjects(std::vector<CollisionObject*>& objs) const;
-
- /** \brief perform collision test between one object and all the objects belonging to the manager */
- void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
-
- /** \brief perform distance computation between one object and all the objects belonging ot the manager */
- void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const;
-
- /** \brief perform collision test for the objects belonging to the manager (i.e, N^2 self collision) */
- void collide(void* cdata, CollisionCallBack callback) const;
-
- /** \brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance) */
- void distance(void* cdata, DistanceCallBack callback) const;
-
- /** \brief perform collision test with objects belonging to another manager */
- void collide(BroadPhaseCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const;
-
- /** \brief perform distance test with objects belonging to another manager */
- void distance(BroadPhaseCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const;
-
- /** \brief whether the manager is empty */
- bool empty() const;
-
- /** \brief the number of objects managed by the manager */
- size_t size() const;
-
- /** \brief compute the bound for the environent */
- static void computeBound(std::vector<CollisionObject*>& objs, Vec3f& l, Vec3f& u)
- {
- AABB bound;
- for(unsigned int i = 0; i < objs.size(); ++i)
- bound += objs[i]->getAABB();
-
- l = bound.min_;
- u = bound.max_;
- }
-
-protected:
-
- /** \brief perform collision test between one object and all the objects belonging to the manager */
- bool collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
-
- /** \brief perform distance computation between one object and all the objects belonging ot the manager */
- bool distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
-
-
- // all objects in the scene
- std::list<CollisionObject*> objs;
-
- // objects in the scene limit (given by scene_min and scene_max) are in the spatial hash table
- HashTable* hash_table;
- // objects outside the scene limit are in another list
- std::list<CollisionObject*> objs_outside_scene_limit;
-
- // the size of the scene
- AABB scene_limit;
-
- std::map<CollisionObject*, AABB> obj_aabb_map; // store the map between objects and their aabbs. will make update more convenient
-};
-
-
-template<typename HashTable>
-void SpatialHashingCollisionManager<HashTable>::registerObject(CollisionObject* obj)
-{
- objs.push_back(obj);
-
- const AABB& obj_aabb = obj->getAABB();
- AABB overlap_aabb;
-
- if(scene_limit.overlap(obj_aabb, overlap_aabb))
- {
- if(!scene_limit.contain(obj_aabb))
- objs_outside_scene_limit.push_back(obj);
-
- hash_table->insert(overlap_aabb, obj);
- }
- else
- objs_outside_scene_limit.push_back(obj);
-
- obj_aabb_map[obj] = obj_aabb;
-}
-
-template<typename HashTable>
-void SpatialHashingCollisionManager<HashTable>::unregisterObject(CollisionObject* obj)
-{
- objs.remove(obj);
-
- const AABB& obj_aabb = obj->getAABB();
- AABB overlap_aabb;
-
- if(scene_limit.overlap(obj_aabb, overlap_aabb))
- {
- if(!scene_limit.contain(obj_aabb))
- objs_outside_scene_limit.remove(obj);
-
- hash_table->remove(overlap_aabb, obj);
- }
- else
- objs_outside_scene_limit.remove(obj);
-
- obj_aabb_map.erase(obj);
-}
-
-template<typename HashTable>
-void SpatialHashingCollisionManager<HashTable>::setup()
-{}
-
-template<typename HashTable>
-void SpatialHashingCollisionManager<HashTable>::update()
-{
- hash_table->clear();
- objs_outside_scene_limit.clear();
-
- for(std::list<CollisionObject*>::const_iterator it = objs.begin(), end = objs.end();
- it != end; ++it)
- {
- CollisionObject* obj = *it;
- const AABB& obj_aabb = obj->getAABB();
- AABB overlap_aabb;
-
- if(scene_limit.overlap(obj_aabb, overlap_aabb))
- {
- if(!scene_limit.contain(obj_aabb))
- objs_outside_scene_limit.push_back(obj);
-
- hash_table->insert(overlap_aabb, obj);
- }
- else
- objs_outside_scene_limit.push_back(obj);
-
- obj_aabb_map[obj] = obj_aabb;
- }
-}
-
-template<typename HashTable>
-void SpatialHashingCollisionManager<HashTable>::update(CollisionObject* updated_obj)
-{
- const AABB& new_aabb = updated_obj->getAABB();
- const AABB& old_aabb = obj_aabb_map[updated_obj];
-
- if(!scene_limit.contain(old_aabb)) // previously not completely in scene limit
- {
- if(scene_limit.contain(new_aabb))
- {
- std::list<CollisionObject*>::iterator find_it = std::find(objs_outside_scene_limit.begin(),
- objs_outside_scene_limit.end(),
- updated_obj);
-
- objs_outside_scene_limit.erase(find_it);
- }
- }
- else if(!scene_limit.contain(new_aabb)) // previous completely in scenelimit, now not
- objs_outside_scene_limit.push_back(updated_obj);
-
- AABB old_overlap_aabb;
- if(scene_limit.overlap(old_aabb, old_overlap_aabb))
- hash_table->remove(old_overlap_aabb, updated_obj);
-
- AABB new_overlap_aabb;
- if(scene_limit.overlap(new_aabb, new_overlap_aabb))
- hash_table->insert(new_overlap_aabb, updated_obj);
-
- obj_aabb_map[updated_obj] = new_aabb;
-}
-
-template<typename HashTable>
-void SpatialHashingCollisionManager<HashTable>::update(const std::vector<CollisionObject*>& updated_objs)
-{
- for(size_t i = 0; i < updated_objs.size(); ++i)
- update(updated_objs[i]);
-}
-
-
-template<typename HashTable>
-void SpatialHashingCollisionManager<HashTable>::clear()
-{
- objs.clear();
- hash_table->clear();
- objs_outside_scene_limit.clear();
- obj_aabb_map.clear();
-}
-
-template<typename HashTable>
-void SpatialHashingCollisionManager<HashTable>::getObjects(std::vector<CollisionObject*>& objs_) const
-{
- objs_.resize(objs.size());
- std::copy(objs.begin(), objs.end(), objs_.begin());
-}
-
-template<typename HashTable>
-void SpatialHashingCollisionManager<HashTable>::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
-{
- if(size() == 0) return;
- collide_(obj, cdata, callback);
-}
-
-template<typename HashTable>
-void SpatialHashingCollisionManager<HashTable>::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const
-{
- if(size() == 0) return;
- FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
- distance_(obj, cdata, callback, min_dist);
-}
-
-template<typename HashTable>
-bool SpatialHashingCollisionManager<HashTable>::collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
-{
- const AABB& obj_aabb = obj->getAABB();
- AABB overlap_aabb;
-
- if(scene_limit.overlap(obj_aabb, overlap_aabb))
- {
- if(!scene_limit.contain(obj_aabb))
- {
- for(std::list<CollisionObject*>::const_iterator it = objs_outside_scene_limit.begin(), end = objs_outside_scene_limit.end();
- it != end; ++it)
- {
- if(obj == *it) continue;
- if(callback(obj, *it, cdata)) return true;
- }
- }
-
- std::vector<CollisionObject*> query_result = hash_table->query(overlap_aabb);
- for(unsigned int i = 0; i < query_result.size(); ++i)
- {
- if(obj == query_result[i]) continue;
- if(callback(obj, query_result[i], cdata)) return true;
- }
- }
- else
- {
- ;
- for(std::list<CollisionObject*>::const_iterator it = objs_outside_scene_limit.begin(), end = objs_outside_scene_limit.end();
- it != end; ++it)
- {
- if(obj == *it) continue;
- if(callback(obj, *it, cdata)) return true;
- }
- }
-
- return false;
-}
-
-template<typename HashTable>
-bool SpatialHashingCollisionManager<HashTable>::distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
-{
- Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
- AABB aabb = obj->getAABB();
- if(min_dist < std::numeric_limits<FCL_REAL>::max())
- {
- Vec3f min_dist_delta(min_dist, min_dist, min_dist);
- aabb.expand(min_dist_delta);
- }
-
- AABB overlap_aabb;
-
- int status = 1;
- FCL_REAL old_min_distance;
-
- while(1)
- {
- old_min_distance = min_dist;
-
- if(scene_limit.overlap(aabb, overlap_aabb))
- {
- if(!scene_limit.contain(aabb))
- {
- for(std::list<CollisionObject*>::const_iterator it = objs_outside_scene_limit.begin(), end = objs_outside_scene_limit.end();
- it != end; ++it)
- {
- if(obj == *it) continue;
- if(!enable_tested_set_)
- {
- if(obj->getAABB().distance((*it)->getAABB()) < min_dist)
- if(callback(obj, *it, cdata, min_dist)) return true;
- }
- else
- {
- if(!inTestedSet(obj, *it))
- {
- if(obj->getAABB().distance((*it)->getAABB()) < min_dist)
- if(callback(obj, *it, cdata, min_dist)) return true;
- insertTestedSet(obj, *it);
- }
- }
- }
- }
-
- std::vector<CollisionObject*> query_result = hash_table->query(overlap_aabb);
- for(unsigned int i = 0; i < query_result.size(); ++i)
- {
- if(obj == query_result[i]) continue;
- if(!enable_tested_set_)
- {
- if(obj->getAABB().distance(query_result[i]->getAABB()) < min_dist)
- if(callback(obj, query_result[i], cdata, min_dist)) return true;
- }
- else
- {
- if(!inTestedSet(obj, query_result[i]))
- {
- if(obj->getAABB().distance(query_result[i]->getAABB()) < min_dist)
- if(callback(obj, query_result[i], cdata, min_dist)) return true;
- insertTestedSet(obj, query_result[i]);
- }
- }
- }
- }
- else
- {
- for(std::list<CollisionObject*>::const_iterator it = objs_outside_scene_limit.begin(), end = objs_outside_scene_limit.end();
- it != end; ++it)
- {
- if(obj == *it) continue;
- if(!enable_tested_set_)
- {
- if(obj->getAABB().distance((*it)->getAABB()) < min_dist)
- if(callback(obj, *it, cdata, min_dist)) return true;
- }
- else
- {
- if(!inTestedSet(obj, *it))
- {
- if(obj->getAABB().distance((*it)->getAABB()) < min_dist)
- if(callback(obj, *it, cdata, min_dist)) return true;
- insertTestedSet(obj, *it);
- }
- }
- }
- }
-
- if(status == 1)
- {
- if(old_min_distance < std::numeric_limits<FCL_REAL>::max())
- break;
- else
- {
- if(min_dist < old_min_distance)
- {
- Vec3f min_dist_delta(min_dist, min_dist, min_dist);
- aabb = AABB(obj->getAABB(), min_dist_delta);
- status = 0;
- }
- else
- {
- if(aabb.equal(obj->getAABB()))
- aabb.expand(delta);
- else
- aabb.expand(obj->getAABB(), 2.0);
- }
- }
- }
- else if(status == 0)
- break;
- }
-
- return false;
-}
-
-template<typename HashTable>
-void SpatialHashingCollisionManager<HashTable>::collide(void* cdata, CollisionCallBack callback) const
-{
- if(size() == 0) return;
-
- for(std::list<CollisionObject*>::const_iterator it1 = objs.begin(), end1 = objs.end();
- it1 != end1; ++it1)
- {
- const AABB& obj_aabb = (*it1)->getAABB();
- AABB overlap_aabb;
-
- if(scene_limit.overlap(obj_aabb, overlap_aabb))
- {
- if(!scene_limit.contain(obj_aabb))
- {
- for(std::list<CollisionObject*>::const_iterator it2 = objs_outside_scene_limit.begin(), end2 = objs_outside_scene_limit.end();
- it2 != end2; ++it2)
- {
- if(*it1 < *it2) { if(callback(*it1, *it2, cdata)) return; }
- }
- }
-
- std::vector<CollisionObject*> query_result = hash_table->query(overlap_aabb);
- for(unsigned int i = 0; i < query_result.size(); ++i)
- {
- if(*it1 < query_result[i]) { if(callback(*it1, query_result[i], cdata)) return; }
- }
- }
- else
- {
- for(std::list<CollisionObject*>::const_iterator it2 = objs_outside_scene_limit.begin(), end2 = objs_outside_scene_limit.end();
- it2 != end2; ++it2)
- {
- if(*it1 < *it2) { if(callback(*it1, *it2, cdata)) return; }
- }
- }
- }
-}
-
-template<typename HashTable>
-void SpatialHashingCollisionManager<HashTable>::distance(void* cdata, DistanceCallBack callback) const
-{
- if(size() == 0) return;
-
- enable_tested_set_ = true;
- tested_set.clear();
-
- FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
-
- for(std::list<CollisionObject*>::const_iterator it = objs.begin(), end = objs.end(); it != end; ++it)
- if(distance_(*it, cdata, callback, min_dist)) break;
-
- enable_tested_set_ = false;
- tested_set.clear();
-}
-
-template<typename HashTable>
-void SpatialHashingCollisionManager<HashTable>::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const
-{
- SpatialHashingCollisionManager<HashTable>* other_manager = static_cast<SpatialHashingCollisionManager<HashTable>* >(other_manager_);
-
- if((size() == 0) || (other_manager->size() == 0)) return;
-
- if(this == other_manager)
- {
- collide(cdata, callback);
- return;
- }
-
- if(this->size() < other_manager->size())
- {
- for(std::list<CollisionObject*>::const_iterator it = objs.begin(), end = objs.end(); it != end; ++it)
- if(other_manager->collide_(*it, cdata, callback)) return;
- }
- else
- {
- for(std::list<CollisionObject*>::const_iterator it = other_manager->objs.begin(), end = other_manager->objs.end(); it != end; ++it)
- if(collide_(*it, cdata, callback)) return;
- }
-}
-
-template<typename HashTable>
-void SpatialHashingCollisionManager<HashTable>::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const
-{
- SpatialHashingCollisionManager<HashTable>* other_manager = static_cast<SpatialHashingCollisionManager<HashTable>* >(other_manager_);
-
- if((size() == 0) || (other_manager->size() == 0)) return;
-
- if(this == other_manager)
- {
- distance(cdata, callback);
- return;
- }
-
- FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
-
- if(this->size() < other_manager->size())
- {
- for(std::list<CollisionObject*>::const_iterator it = objs.begin(), end = objs.end(); it != end; ++it)
- if(other_manager->distance_(*it, cdata, callback, min_dist)) return;
- }
- else
- {
- for(std::list<CollisionObject*>::const_iterator it = other_manager->objs.begin(), end = other_manager->objs.end(); it != end; ++it)
- if(distance_(*it, cdata, callback, min_dist)) return;
- }
-}
-
-template<typename HashTable>
-bool SpatialHashingCollisionManager<HashTable>::empty() const
-{
- return objs.empty();
-}
-
-template<typename HashTable>
-size_t SpatialHashingCollisionManager<HashTable>::size() const
-{
- return objs.size();
-}
-
-
-/** Rigorous SAP collision manager */
-class SaPCollisionManager : public BroadPhaseCollisionManager
-{
-public:
-
- SaPCollisionManager()
- {
- elist[0] = NULL;
- elist[1] = NULL;
- elist[2] = NULL;
-
- optimal_axis = 0;
- }
-
- ~SaPCollisionManager()
- {
- clear();
- }
-
- /** \brief add objects to the manager */
- void registerObjects(const std::vector<CollisionObject*>& other_objs);
-
- /** \brief remove one object from the manager */
- void registerObject(CollisionObject* obj);
-
- /** \brief add one object to the manager */
- void unregisterObject(CollisionObject* obj);
-
- /** \brief initialize the manager, related with the specific type of manager */
- void setup();
-
- /** \brief update the condition of manager */
- void update();
-
- /** \brief update the manager by explicitly given the object updated */
- void update(CollisionObject* updated_obj);
-
- /** \brief update the manager by explicitly given the set of objects update */
- void update(const std::vector<CollisionObject*>& updated_objs);
-
- /** \brief clear the manager */
- void clear();
-
- /** \brief return the objects managed by the manager */
- void getObjects(std::vector<CollisionObject*>& objs) const;
-
- /** \brief perform collision test between one object and all the objects belonging to the manager */
- void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
-
- /** \brief perform distance computation between one object and all the objects belonging to the manager */
- void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const;
-
- /** \brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision) */
- void collide(void* cdata, CollisionCallBack callback) const;
-
- /** \brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance) */
- void distance(void* cdata, DistanceCallBack callback) const;
-
- /** \brief perform collision test with objects belonging to another manager */
- void collide(BroadPhaseCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const;
-
- /** \brief perform distance test with objects belonging to another manager */
- void distance(BroadPhaseCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const;
-
- /** \brief whether the manager is empty */
- bool empty() const;
-
- /** \brief the number of objects managed by the manager */
- inline size_t size() const { return AABB_arr.size(); }
-
-protected:
-
- struct EndPoint;
-
- /** \brief SAP interval for one object */
- struct SaPAABB
- {
- /** \brief object */
- CollisionObject* obj;
-
- /** \brief lower bound end point of the interval */
- EndPoint* lo;
-
- /** \brief higher bound end point of the interval */
- EndPoint* hi;
-
- /** \brief cached AABB value */
- AABB cached;
- };
-
- /** \brief End point for an interval */
- struct EndPoint
- {
- /** \brief tag for whether it is a lower bound or higher bound of an interval, 0 for lo, and 1 for hi */
- char minmax;
-
- /** \brief back pointer to SAP interval */
- SaPAABB* aabb;
-
- /** \brief the previous end point in the end point list */
- EndPoint* prev[3];
- /** \brief the next end point in the end point list */
- EndPoint* next[3];
-
- /** \brief get the value of the end point */
- inline const Vec3f& getVal() const
- {
- if(minmax) return aabb->cached.max_;
- else return aabb->cached.min_;
- }
-
- /** \brief set the value of the end point */
- inline Vec3f& getVal()
- {
- if(minmax) return aabb->cached.max_;
- else return aabb->cached.min_;
- }
-
- inline FCL_REAL getVal(size_t i) const
- {
- if(minmax) return aabb->cached.max_[i];
- else return aabb->cached.min_[i];
- }
-
- inline FCL_REAL& getVal(size_t i)
- {
- if(minmax) return aabb->cached.max_[i];
- else return aabb->cached.min_[i];
- }
-
- };
-
- /** \brief A pair of objects that are not culling away and should further check collision */
- struct SaPPair
- {
- SaPPair(CollisionObject* a, CollisionObject* b)
- {
- if(a < b)
- {
- obj1 = a;
- obj2 = b;
- }
- else
- {
- obj1 = b;
- obj2 = a;
- }
- }
-
- CollisionObject* obj1;
- CollisionObject* obj2;
-
- bool operator == (const SaPPair& other) const
- {
- return ((obj1 == other.obj1) && (obj2 == other.obj2));
- }
- };
-
- /** Functor to help unregister one object */
- class isUnregistered
- {
- CollisionObject* obj;
-
- public:
- isUnregistered(CollisionObject* obj_)
- {
- obj = obj_;
- }
-
- bool operator() (const SaPPair& pair)
- {
- return (pair.obj1 == obj) || (pair.obj2 == obj);
- }
- };
-
- /** Functor to help remove collision pairs no longer valid (i.e., should be culled away) */
- class isNotValidPair
- {
- CollisionObject* obj1;
- CollisionObject* obj2;
-
- public:
- isNotValidPair(CollisionObject* obj1_, CollisionObject* obj2_)
- {
- obj1 = obj1_;
- obj2 = obj2_;
- }
-
- bool operator() (const SaPPair& pair)
- {
- return (pair.obj1 == obj1) && (pair.obj2 == obj2);
- }
- };
-
- void update_(SaPAABB* updated_aabb);
-
- void updateVelist()
- {
- for(int coord = 0; coord < 3; ++coord)
- {
- velist[coord].resize(size() * 2);
- EndPoint* current = elist[coord];
- size_t id = 0;
- while(current)
- {
- velist[coord][id] = current;
- current = current->next[coord];
- id++;
- }
- }
- }
-
- /** \brief End point list for x, y, z coordinates */
- EndPoint* elist[3];
-
- /** \brief vector version of elist, for acceleration */
- std::vector<EndPoint*> velist[3];
-
- /** \brief SAP interval list */
- std::list<SaPAABB*> AABB_arr;
-
- /** \brief The pair of objects that should further check for collision */
- std::list<SaPPair> overlap_pairs;
-
- size_t optimal_axis;
-
- std::map<CollisionObject*, SaPAABB*> obj_aabb_map;
-
- bool distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
-
- bool collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
-
- void addToOverlapPairs(const SaPPair& p)
- {
- bool repeated = false;
- for(std::list<SaPPair>::iterator it = overlap_pairs.begin(), end = overlap_pairs.end();
- it != end;
- ++it)
- {
- if(*it == p)
- {
- repeated = true;
- break;
- }
- }
-
- if(!repeated)
- overlap_pairs.push_back(p);
- }
-
- void removeFromOverlapPairs(const SaPPair& p)
- {
- for(std::list<SaPPair>::iterator it = overlap_pairs.begin(), end = overlap_pairs.end();
- it != end;
- ++it)
- {
- if(*it == p)
- {
- overlap_pairs.erase(it);
- break;
- }
- }
-
- // or overlap_pairs.remove_if(isNotValidPair(p));
- }
-};
-
-
-
-
-/** Simple SAP collision manager */
-class SSaPCollisionManager : public BroadPhaseCollisionManager
-{
-public:
- SSaPCollisionManager()
- {
- setup_ = false;
- }
-
- /** \brief remove one object from the manager */
- void registerObject(CollisionObject* obj);
-
- /** \brief add one object to the manager */
- void unregisterObject(CollisionObject* obj);
-
- /** \brief initialize the manager, related with the specific type of manager */
- void setup();
-
- /** \brief update the condition of manager */
- void update();
-
- /** \brief clear the manager */
- void clear();
-
- /** \brief return the objects managed by the manager */
- void getObjects(std::vector<CollisionObject*>& objs) const;
-
- /** \brief perform collision test between one object and all the objects belonging to the manager */
- void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
-
- /** \brief perform distance computation between one object and all the objects belonging to the manager */
- void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const;
-
- /** \brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision) */
- void collide(void* cdata, CollisionCallBack callback) const;
-
- /** \brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance) */
- void distance(void* cdata, DistanceCallBack callback) const;
-
- /** \brief perform collision test with objects belonging to another manager */
- void collide(BroadPhaseCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const;
-
- /** \brief perform distance test with objects belonging to another manager */
- void distance(BroadPhaseCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const;
-
- /** \brief whether the manager is empty */
- bool empty() const;
-
- /** \brief the number of objects managed by the manager */
- inline size_t size() const { return objs_x.size(); }
-
-protected:
- /** \brief check collision between one object and a list of objects, return value is whether stop is possible */
- bool checkColl(std::vector<CollisionObject*>::const_iterator pos_start, std::vector<CollisionObject*>::const_iterator pos_end,
- CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
-
- /** \brief check distance between one object and a list of objects, return value is whether stop is possible */
- bool checkDis(std::vector<CollisionObject*>::const_iterator pos_start, std::vector<CollisionObject*>::const_iterator pos_end,
- CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
-
-
- bool collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
-
- bool distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
-
- static inline size_t selectOptimalAxis(const std::vector<CollisionObject*>& objs_x, const std::vector<CollisionObject*>& objs_y, const std::vector<CollisionObject*>& objs_z, std::vector<CollisionObject*>::const_iterator& it_beg, std::vector<CollisionObject*>::const_iterator& it_end)
- {
- // simple sweep and prune method
- double delta_x = (objs_x[objs_x.size() - 1])->getAABB().min_[0] - (objs_x[0])->getAABB().min_[0];
- double delta_y = (objs_x[objs_y.size() - 1])->getAABB().min_[1] - (objs_y[0])->getAABB().min_[1];
- double delta_z = (objs_z[objs_z.size() - 1])->getAABB().min_[2] - (objs_z[0])->getAABB().min_[2];
-
- int axis = 0;
- if(delta_y > delta_x && delta_y > delta_z)
- axis = 1;
- else if(delta_z > delta_y && delta_z > delta_x)
- axis = 2;
-
- switch(axis)
- {
- case 0:
- it_beg = objs_x.begin();
- it_end = objs_x.end();
- break;
- case 1:
- it_beg = objs_y.begin();
- it_end = objs_y.end();
- break;
- case 2:
- it_beg = objs_z.begin();
- it_end = objs_z.end();
- break;
- }
-
- return axis;
- }
-
-
- /** \brief Objects sorted according to lower x value */
- std::vector<CollisionObject*> objs_x;
-
- /** \brief Objects sorted according to lower y value */
- std::vector<CollisionObject*> objs_y;
-
- /** \brief Objects sorted according to lower z value */
- std::vector<CollisionObject*> objs_z;
-
- /** \brief tag about whether the environment is maintained suitably (i.e., the objs_x, objs_y, objs_z are sorted correctly */
- bool setup_;
-};
-
-/** Collision manager based on interval tree */
-class IntervalTreeCollisionManager : public BroadPhaseCollisionManager
-{
-public:
- IntervalTreeCollisionManager()
- {
- setup_ = false;
- for(int i = 0; i < 3; ++i)
- interval_trees[i] = NULL;
- }
-
- ~IntervalTreeCollisionManager()
- {
- clear();
- }
-
- /** \brief remove one object from the manager */
- void registerObject(CollisionObject* obj);
-
- /** \brief add one object to the manager */
- void unregisterObject(CollisionObject* obj);
-
- /** \brief initialize the manager, related with the specific type of manager */
- void setup();
-
- /** \brief update the condition of manager */
- void update();
-
- /** \brief update the manager by explicitly given the object updated */
- void update(CollisionObject* updated_obj);
-
- /** \brief update the manager by explicitly given the set of objects update */
- void update(const std::vector<CollisionObject*>& updated_objs);
-
- /** \brief clear the manager */
- void clear();
-
- /** \brief return the objects managed by the manager */
- void getObjects(std::vector<CollisionObject*>& objs) const;
-
- /** \brief perform collision test between one object and all the objects belonging to the manager */
- void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
-
- /** \brief perform distance computation between one object and all the objects belonging to the manager */
- void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const;
-
- /** \brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision) */
- void collide(void* cdata, CollisionCallBack callback) const;
-
- /** \brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance) */
- void distance(void* cdata, DistanceCallBack callback) const;
-
- /** \brief perform collision test with objects belonging to another manager */
- void collide(BroadPhaseCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const;
-
- /** \brief perform distance test with objects belonging to another manager */
- void distance(BroadPhaseCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const;
-
- /** \brief whether the manager is empty */
- bool empty() const;
-
- /** \brief the number of objects managed by the manager */
- inline size_t size() const { return endpoints[0].size() / 2; }
-
-protected:
-
-
- /** \brief SAP end point */
- struct EndPoint
- {
- /** \brief object related with the end point */
- CollisionObject* obj;
-
- /** \brief end point value */
- FCL_REAL value;
-
- /** \brief tag for whether it is a lower bound or higher bound of an interval, 0 for lo, and 1 for hi */
- char minmax;
- };
-
- /** \brief Extention interval tree's interval to SAP interval, adding more information */
- struct SAPInterval : public SimpleInterval
- {
- CollisionObject* obj;
- SAPInterval(double low_, double high_, CollisionObject* obj_) : SimpleInterval()
- {
- low = low_;
- high = high_;
- obj = obj_;
- }
- };
-
-
- bool checkColl(std::deque<SimpleInterval*>::const_iterator pos_start, std::deque<SimpleInterval*>::const_iterator pos_end, CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
-
- bool checkDist(std::deque<SimpleInterval*>::const_iterator pos_start, std::deque<SimpleInterval*>::const_iterator pos_end, CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
-
- bool collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
-
- bool distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
-
- /** \brief vector stores all the end points */
- std::vector<EndPoint> endpoints[3];
-
- /** \brief interval tree manages the intervals */
- IntervalTree* interval_trees[3];
-
- std::map<CollisionObject*, SAPInterval*> obj_interval_maps[3];
-
- /** \brief tag for whether the interval tree is maintained suitably */
- bool setup_;
-};
-
-class DynamicAABBTreeCollisionManager : public BroadPhaseCollisionManager
-{
-public:
- typedef NodeBase<AABB> DynamicAABBNode;
- typedef boost::unordered_map<CollisionObject*, DynamicAABBNode*> DynamicAABBTable;
-
- int max_tree_nonbalanced_level;
- int tree_incremental_balance_pass;
- int& tree_topdown_balance_threshold;
- int& tree_topdown_level;
- int tree_init_level;
-
- bool octree_as_geometry_collide;
- bool octree_as_geometry_distance;
-
-
- DynamicAABBTreeCollisionManager() : tree_topdown_balance_threshold(dtree.bu_threshold),
- tree_topdown_level(dtree.topdown_level)
- {
- max_tree_nonbalanced_level = 10;
- tree_incremental_balance_pass = 10;
- tree_topdown_balance_threshold = 2;
- tree_topdown_level = 0;
- tree_init_level = 0;
- setup_ = false;
-
- // from experiment, this is the optimal setting
- octree_as_geometry_collide = true;
- octree_as_geometry_distance = false;
- }
-
- /** \brief add objects to the manager */
- void registerObjects(const std::vector<CollisionObject*>& other_objs);
-
- /** \brief add one object to the manager */
- void registerObject(CollisionObject* obj);
-
- /** \brief remove one object from the manager */
- void unregisterObject(CollisionObject* obj);
-
- /** \brief initialize the manager, related with the specific type of manager */
- void setup();
-
- /** \brief update the condition of manager */
- void update();
-
- /** \brief update the manager by explicitly given the object updated */
- void update(CollisionObject* updated_obj);
-
- /** \brief update the manager by explicitly given the set of objects update */
- void update(const std::vector<CollisionObject*>& updated_objs);
-
- /** \brief clear the manager */
- void clear()
- {
- dtree.clear();
- table.clear();
- }
-
- /** \brief return the objects managed by the manager */
- void getObjects(std::vector<CollisionObject*>& objs) const
- {
- objs.resize(this->size());
- std::transform(table.begin(), table.end(), objs.begin(), boost::bind(&DynamicAABBTable::value_type::first, _1));
- }
-
- /** \brief perform collision test between one object and all the objects belonging to the manager */
- void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
- {
- if(size() == 0) return;
- switch(obj->getCollisionGeometry()->getNodeType())
- {
- case GEOM_OCTREE:
- {
- if(!octree_as_geometry_collide)
- {
- const OcTree* octree = static_cast<const OcTree*>(obj->getCollisionGeometry());
- collisionRecurse(dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback);
- }
- else
- collisionRecurse(dtree.getRoot(), obj, 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 */
- void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const
- {
- if(size() == 0) return;
- FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
- switch(obj->getCollisionGeometry()->getNodeType())
- {
- case GEOM_OCTREE:
- {
- if(!octree_as_geometry_distance)
- {
- const OcTree* octree = static_cast<const OcTree*>(obj->getCollisionGeometry());
- distanceRecurse(dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback, min_dist);
- }
- else
- distanceRecurse(dtree.getRoot(), obj, 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) */
- void collide(void* cdata, CollisionCallBack callback) const
- {
- if(size() == 0) return;
- selfCollisionRecurse(dtree.getRoot(), cdata, callback);
- }
-
- /** \brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance) */
- void distance(void* cdata, DistanceCallBack callback) const
- {
- if(size() == 0) return;
- FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
- selfDistanceRecurse(dtree.getRoot(), cdata, callback, min_dist);
- }
-
- /** \brief perform collision test with objects belonging to another manager */
- void collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const
- {
- DynamicAABBTreeCollisionManager* other_manager = static_cast<DynamicAABBTreeCollisionManager*>(other_manager_);
- if((size() == 0) || (other_manager->size() == 0)) return;
- collisionRecurse(dtree.getRoot(), other_manager->dtree.getRoot(), cdata, callback);
- }
-
- /** \brief perform distance test with objects belonging to another manager */
- void distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const
- {
- DynamicAABBTreeCollisionManager* other_manager = static_cast<DynamicAABBTreeCollisionManager*>(other_manager_);
- if((size() == 0) || (other_manager->size() == 0)) return;
- FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
- distanceRecurse(dtree.getRoot(), other_manager->dtree.getRoot(), cdata, callback, min_dist);
- }
-
- /** \brief whether the manager is empty */
- bool empty() const
- {
- return dtree.empty();
- }
-
- /** \brief the number of objects managed by the manager */
- size_t size() const
- {
- return dtree.size();
- }
-
- const HierarchyTree<AABB>& getTree() const { return dtree; }
-
-
-private:
- HierarchyTree<AABB> dtree;
- boost::unordered_map<CollisionObject*, DynamicAABBNode*> table;
-
- bool setup_;
-
- bool collisionRecurse(DynamicAABBNode* root1, DynamicAABBNode* root2, void* cdata, CollisionCallBack callback) const;
-
- bool collisionRecurse(DynamicAABBNode* root, CollisionObject* query, void* cdata, CollisionCallBack callback) const;
-
- bool selfCollisionRecurse(DynamicAABBNode* root, void* cdata, CollisionCallBack callback) const;
-
- bool distanceRecurse(DynamicAABBNode* root1, DynamicAABBNode* root2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
-
- bool distanceRecurse(DynamicAABBNode* root, CollisionObject* query, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
-
- 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 Transform3f& tf2, void* cdata, CollisionCallBack callback) const;
-
- bool distanceRecurse(DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
-
-};
-
-
-
-
-class DynamicAABBTreeCollisionManager2 : public BroadPhaseCollisionManager
-{
-public:
- typedef alternative::NodeBase<AABB> DynamicAABBNode;
- typedef boost::unordered_map<CollisionObject*, size_t> DynamicAABBTable;
-
- int max_tree_nonbalanced_level;
- int tree_incremental_balance_pass;
- int& tree_topdown_balance_threshold;
- int& tree_topdown_level;
- int tree_init_level;
-
- bool octree_as_geometry_collide;
- bool octree_as_geometry_distance;
-
- DynamicAABBTreeCollisionManager2() : tree_topdown_balance_threshold(dtree.bu_threshold),
- tree_topdown_level(dtree.topdown_level)
- {
- max_tree_nonbalanced_level = 10;
- tree_incremental_balance_pass = 10;
- tree_topdown_balance_threshold = 2;
- tree_topdown_level = 0;
- tree_init_level = 0;
- setup_ = false;
-
- // from experiment, this is the optimal setting
- octree_as_geometry_collide = true;
- octree_as_geometry_distance = false;
- }
-
- /** \brief add objects to the manager */
- void registerObjects(const std::vector<CollisionObject*>& other_objs);
-
- /** \brief add one object to the manager */
- void registerObject(CollisionObject* obj);
-
- /** \brief remove one object from the manager */
- void unregisterObject(CollisionObject* obj);
-
- /** \brief initialize the manager, related with the specific type of manager */
- void setup();
-
- /** \brief update the condition of manager */
- void update();
-
- /** \brief update the manager by explicitly given the object updated */
- void update(CollisionObject* updated_obj);
-
- /** \brief update the manager by explicitly given the set of objects update */
- void update(const std::vector<CollisionObject*>& updated_objs);
-
- /** \brief clear the manager */
- void clear()
- {
- dtree.clear();
- table.clear();
- }
-
- /** \brief return the objects managed by the manager */
- void getObjects(std::vector<CollisionObject*>& objs) const
- {
- objs.resize(this->size());
- std::transform(table.begin(), table.end(), objs.begin(), boost::bind(&DynamicAABBTable::value_type::first, _1));
- }
-
- /** \brief perform collision test between one object and all the objects belonging to the manager */
- void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
- {
- if(size() == 0) return;
- switch(obj->getCollisionGeometry()->getNodeType())
- {
- case GEOM_OCTREE:
- {
- if(!octree_as_geometry_collide)
- {
- const OcTree* octree = static_cast<const OcTree*>(obj->getCollisionGeometry());
- collisionRecurse(dtree.getNodes(), dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback);
- }
- else
- collisionRecurse(dtree.getNodes(), dtree.getRoot(), obj, 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 */
- void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const
- {
- if(size() == 0) return;
- FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
- switch(obj->getCollisionGeometry()->getNodeType())
- {
- case GEOM_OCTREE:
- {
- if(!octree_as_geometry_distance)
- {
- 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);
- }
- else
- distanceRecurse(dtree.getNodes(), dtree.getRoot(), obj, 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) */
- void collide(void* cdata, CollisionCallBack callback) const
- {
- if(size() == 0) return;
- selfCollisionRecurse(dtree.getNodes(), dtree.getRoot(), cdata, callback);
- }
-
- /** \brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance) */
- void distance(void* cdata, DistanceCallBack callback) const
- {
- if(size() == 0) return;
- FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
- selfDistanceRecurse(dtree.getNodes(), dtree.getRoot(), cdata, callback, min_dist);
- }
-
- /** \brief perform collision test with objects belonging to another manager */
- void collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const
- {
- DynamicAABBTreeCollisionManager2* other_manager = static_cast<DynamicAABBTreeCollisionManager2*>(other_manager_);
- if((size() == 0) || (other_manager->size() == 0)) return;
- collisionRecurse(dtree.getNodes(), dtree.getRoot(), other_manager->dtree.getNodes(), other_manager->dtree.getRoot(), cdata, callback);
- }
-
- /** \brief perform distance test with objects belonging to another manager */
- void distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const
- {
- DynamicAABBTreeCollisionManager2* other_manager = static_cast<DynamicAABBTreeCollisionManager2*>(other_manager_);
- if((size() == 0) || (other_manager->size() == 0)) return;
- FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
- distanceRecurse(dtree.getNodes(), dtree.getRoot(), other_manager->dtree.getNodes(), other_manager->dtree.getRoot(), cdata, callback, min_dist);
- }
-
- /** \brief whether the manager is empty */
- bool empty() const
- {
- return dtree.empty();
- }
-
- /** \brief the number of objects managed by the manager */
- size_t size() const
- {
- return dtree.size();
- }
-
-
- const alternative::HierarchyTree<AABB>& getTree() const { return dtree; }
-
-private:
- alternative::HierarchyTree<AABB> dtree;
- boost::unordered_map<CollisionObject*, size_t> table;
-
- bool setup_;
-
- bool collisionRecurse(DynamicAABBNode* nodes1, size_t root1, DynamicAABBNode* nodes2, size_t root2, void* cdata, CollisionCallBack callback) const;
-
- bool collisionRecurse(DynamicAABBNode* nodes, size_t root, CollisionObject* query, void* cdata, CollisionCallBack callback) const;
-
- bool selfCollisionRecurse(DynamicAABBNode* nodes, size_t root, void* cdata, CollisionCallBack callback) const;
-
- bool distanceRecurse(DynamicAABBNode* nodes1, size_t root1, DynamicAABBNode* nodes2, size_t root2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
-
- bool distanceRecurse(DynamicAABBNode* nodes, size_t root, CollisionObject* query, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
-
- 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 Transform3f& 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 Transform3f& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
-
-
-};
-
-}
-
-#endif
diff --git a/trunk/fcl/include/fcl/broadphase/broadphase.h b/trunk/fcl/include/fcl/broadphase/broadphase.h
new file mode 100644
index 0000000000000000000000000000000000000000..445deaac68f7ecb1fbc8ab76326debf127a064ed
--- /dev/null
+++ b/trunk/fcl/include/fcl/broadphase/broadphase.h
@@ -0,0 +1,160 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * 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 Jia Pan */
+
+
+
+#ifndef FCL_BROAD_PHASE_H
+#define FCL_BROAD_PHASE_H
+
+#include "fcl/collision_object.h"
+#include <set>
+#include <vector>
+
+namespace fcl
+{
+
+
+/// @brief Callback for collision between two objects. Return value is whether can stop now.
+typedef bool (*CollisionCallBack)(CollisionObject* o1, CollisionObject* o2, void* cdata);
+
+/// @brief Callback for distance between two objects, Return value is whether can stop now, also return the minimum distance till now.
+typedef bool (*DistanceCallBack)(CollisionObject* o1, CollisionObject* o2, void* cdata, FCL_REAL& dist);
+
+
+/// @brief Base class for broad phase collision. It helps to accelerate the collision/distance between N objects. Also support self collision, self distance and collision/distance with another M objects.
+class BroadPhaseCollisionManager
+{
+public:
+ BroadPhaseCollisionManager() : enable_tested_set_(false)
+ {
+ }
+
+ virtual ~BroadPhaseCollisionManager() {}
+
+ /// @brief add objects to the manager
+ virtual void registerObjects(const std::vector<CollisionObject*>& other_objs)
+ {
+ for(size_t i = 0; i < other_objs.size(); ++i)
+ registerObject(other_objs[i]);
+ }
+
+ /// @brief add one object to the manager
+ virtual void registerObject(CollisionObject* obj) = 0;
+
+ /// @brief remove one object from the manager
+ virtual void unregisterObject(CollisionObject* obj) = 0;
+
+ /// @brief initialize the manager, related with the specific type of manager
+ virtual void setup() = 0;
+
+ /// @brief update the condition of manager
+ virtual void update() = 0;
+
+ /// @brief update the manager by explicitly given the object updated
+ virtual void update(CollisionObject* updated_obj)
+ {
+ update();
+ }
+
+ /// @brief update the manager by explicitly given the set of objects update
+ virtual void update(const std::vector<CollisionObject*>& updated_objs)
+ {
+ update();
+ }
+
+ /// @brief clear the manager
+ virtual void clear() = 0;
+
+ /// @brief return the objects managed by the manager
+ virtual void getObjects(std::vector<CollisionObject*>& objs) const = 0;
+
+ /// @brief perform collision test between one object and all the objects belonging to the manager
+ virtual void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const = 0;
+
+ /// @brief perform distance computation between one object and all the objects belonging to the manager
+ virtual void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const = 0;
+
+ /// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision)
+ virtual void collide(void* cdata, CollisionCallBack callback) const = 0;
+
+ /// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance)
+ virtual void distance(void* cdata, DistanceCallBack callback) const = 0;
+
+ /// @brief perform collision test with objects belonging to another manager
+ virtual void collide(BroadPhaseCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const = 0;
+
+ /// @brief perform distance test with objects belonging to another manager
+ virtual void distance(BroadPhaseCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const = 0;
+
+ /// @brief whether the manager is empty
+ virtual bool empty() const = 0;
+
+ /// @brief the number of objects managed by the manager
+ virtual size_t size() const = 0;
+
+protected:
+
+ /// @brief tools help to avoid repeating collision or distance callback for the pairs of objects tested before. It can be useful for some of the broadphase algorithms.
+ mutable std::set<std::pair<CollisionObject*, CollisionObject*> > tested_set;
+ mutable bool enable_tested_set_;
+
+ bool inTestedSet(CollisionObject* a, CollisionObject* b) const
+ {
+ if(a < b) return tested_set.find(std::make_pair(a, b)) != tested_set.end();
+ else return tested_set.find(std::make_pair(b, a)) != tested_set.end();
+ }
+
+ void insertTestedSet(CollisionObject* a, CollisionObject* b) const
+ {
+ if(a < b) tested_set.insert(std::make_pair(a, b));
+ else tested_set.insert(std::make_pair(b, a));
+ }
+
+};
+
+
+}
+
+
+#include "fcl/broadphase/broadphase_bruteforce.h"
+#include "fcl/broadphase/broadphase_spatialhash.h"
+#include "fcl/broadphase/broadphase_SaP.h"
+#include "fcl/broadphase/broadphase_SSaP.h"
+#include "fcl/broadphase/broadphase_interval_tree.h"
+#include "fcl/broadphase/broadphase_dynamic_AABB_tree.h"
+#include "fcl/broadphase/broadphase_dynamic_AABB_tree_array.h"
+
+#endif
diff --git a/trunk/fcl/include/fcl/broadphase/broadphase_SSaP.h b/trunk/fcl/include/fcl/broadphase/broadphase_SSaP.h
new file mode 100644
index 0000000000000000000000000000000000000000..d72f6a81d675ebdfb2327053fca9c67ffddd3443
--- /dev/null
+++ b/trunk/fcl/include/fcl/broadphase/broadphase_SSaP.h
@@ -0,0 +1,157 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * 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 Jia Pan */
+
+
+#ifndef FCL_BROAD_PHASE_SSAP_H
+#define FCL_BROAD_PHASE_SSAP_H
+
+#include "fcl/broadphase/broadphase.h"
+
+namespace fcl
+{
+
+/// @brief Simple SAP collision manager
+class SSaPCollisionManager : public BroadPhaseCollisionManager
+{
+public:
+ SSaPCollisionManager() : setup_(false)
+ {}
+
+ /// @brief remove one object from the manager
+ void registerObject(CollisionObject* obj);
+
+ /// @brief add one object to the manager
+ void unregisterObject(CollisionObject* obj);
+
+ /// @brief initialize the manager, related with the specific type of manager
+ void setup();
+
+ /// @brief update the condition of manager
+ void update();
+
+ /// @brief clear the manager
+ void clear();
+
+ /// @brief return the objects managed by the manager
+ void getObjects(std::vector<CollisionObject*>& objs) const;
+
+ /// @brief perform collision test between one object and all the objects belonging to the manager
+ void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
+
+ /// @brief perform distance computation between one object and all the objects belonging to the manager
+ void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const;
+
+ /// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision)
+ void collide(void* cdata, CollisionCallBack callback) const;
+
+ /// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance)
+ void distance(void* cdata, DistanceCallBack callback) const;
+
+ /// @brief perform collision test with objects belonging to another manager
+ void collide(BroadPhaseCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const;
+
+ /// @brief perform distance test with objects belonging to another manager
+ void distance(BroadPhaseCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const;
+
+ /// @brief whether the manager is empty
+ bool empty() const;
+
+ /// @brief the number of objects managed by the manager
+ inline size_t size() const { return objs_x.size(); }
+
+protected:
+ /// @brief check collision between one object and a list of objects, return value is whether stop is possible
+ bool checkColl(std::vector<CollisionObject*>::const_iterator pos_start, std::vector<CollisionObject*>::const_iterator pos_end,
+ CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
+
+ /// @brief check distance between one object and a list of objects, return value is whether stop is possible
+ bool checkDis(std::vector<CollisionObject*>::const_iterator pos_start, std::vector<CollisionObject*>::const_iterator pos_end,
+ CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
+
+ bool collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
+
+ bool distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
+
+ static inline size_t selectOptimalAxis(const std::vector<CollisionObject*>& objs_x, const std::vector<CollisionObject*>& objs_y, const std::vector<CollisionObject*>& objs_z, std::vector<CollisionObject*>::const_iterator& it_beg, std::vector<CollisionObject*>::const_iterator& it_end)
+ {
+ /// simple sweep and prune method
+ double delta_x = (objs_x[objs_x.size() - 1])->getAABB().min_[0] - (objs_x[0])->getAABB().min_[0];
+ double delta_y = (objs_x[objs_y.size() - 1])->getAABB().min_[1] - (objs_y[0])->getAABB().min_[1];
+ double delta_z = (objs_z[objs_z.size() - 1])->getAABB().min_[2] - (objs_z[0])->getAABB().min_[2];
+
+ int axis = 0;
+ if(delta_y > delta_x && delta_y > delta_z)
+ axis = 1;
+ else if(delta_z > delta_y && delta_z > delta_x)
+ axis = 2;
+
+ switch(axis)
+ {
+ case 0:
+ it_beg = objs_x.begin();
+ it_end = objs_x.end();
+ break;
+ case 1:
+ it_beg = objs_y.begin();
+ it_end = objs_y.end();
+ break;
+ case 2:
+ it_beg = objs_z.begin();
+ it_end = objs_z.end();
+ break;
+ }
+
+ return axis;
+ }
+
+
+ /// @brief Objects sorted according to lower x value
+ std::vector<CollisionObject*> objs_x;
+
+ /// @brief Objects sorted according to lower y value
+ std::vector<CollisionObject*> objs_y;
+
+ /// @brief Objects sorted according to lower z value
+ std::vector<CollisionObject*> objs_z;
+
+ /// @brief tag about whether the environment is maintained suitably (i.e., the objs_x, objs_y, objs_z are sorted correctly
+ bool setup_;
+};
+
+
+}
+
+#endif
diff --git a/trunk/fcl/include/fcl/broadphase/broadphase_SaP.h b/trunk/fcl/include/fcl/broadphase/broadphase_SaP.h
new file mode 100644
index 0000000000000000000000000000000000000000..165fbd57ce956519de841b18209e30cb600ab23e
--- /dev/null
+++ b/trunk/fcl/include/fcl/broadphase/broadphase_SaP.h
@@ -0,0 +1,316 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * 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 Jia Pan */
+
+#ifndef FCL_BROAD_PHASE_SAP_H
+#define FCL_BROAD_PHASE_SAP_H
+
+#include "fcl/broadphase/broadphase.h"
+
+#include <map>
+#include <list>
+
+namespace fcl
+{
+
+/// @brief Rigorous SAP collision manager
+class SaPCollisionManager : public BroadPhaseCollisionManager
+{
+public:
+
+ SaPCollisionManager()
+ {
+ elist[0] = NULL;
+ elist[1] = NULL;
+ elist[2] = NULL;
+
+ optimal_axis = 0;
+ }
+
+ ~SaPCollisionManager()
+ {
+ clear();
+ }
+
+ /// @brief add objects to the manager
+ void registerObjects(const std::vector<CollisionObject*>& other_objs);
+
+ /// @brief remove one object from the manager
+ void registerObject(CollisionObject* obj);
+
+ /// @brief add one object to the manager
+ void unregisterObject(CollisionObject* obj);
+
+ /// @brief initialize the manager, related with the specific type of manager
+ void setup();
+
+ /// @brief update the condition of manager
+ void update();
+
+ /// @brief update the manager by explicitly given the object updated
+ void update(CollisionObject* updated_obj);
+
+ /// @brief update the manager by explicitly given the set of objects update
+ void update(const std::vector<CollisionObject*>& updated_objs);
+
+ /// @brief clear the manager
+ void clear();
+
+ /// @brief return the objects managed by the manager
+ void getObjects(std::vector<CollisionObject*>& objs) const;
+
+ /// @brief perform collision test between one object and all the objects belonging to the manager
+ void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
+
+ /// @brief perform distance computation between one object and all the objects belonging to the manager
+ void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const;
+
+ /// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision)
+ void collide(void* cdata, CollisionCallBack callback) const;
+
+ /// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance)
+ void distance(void* cdata, DistanceCallBack callback) const;
+
+ /// @brief perform collision test with objects belonging to another manager
+ void collide(BroadPhaseCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const;
+
+ /// @brief perform distance test with objects belonging to another manager
+ void distance(BroadPhaseCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const;
+
+ /// @brief whether the manager is empty
+ bool empty() const;
+
+ /// @brief the number of objects managed by the manager
+ inline size_t size() const { return AABB_arr.size(); }
+
+protected:
+
+ struct EndPoint;
+
+ /// @brief SAP interval for one object
+ struct SaPAABB
+ {
+ /// @brief object
+ CollisionObject* obj;
+
+ /// @brief lower bound end point of the interval
+ EndPoint* lo;
+
+ /// @brief higher bound end point of the interval
+ EndPoint* hi;
+
+ /// @brief cached AABB value
+ AABB cached;
+ };
+
+ /// @brief End point for an interval
+ struct EndPoint
+ {
+ /// @brief tag for whether it is a lower bound or higher bound of an interval, 0 for lo, and 1 for hi
+ char minmax;
+
+ /// @brief back pointer to SAP interval
+ SaPAABB* aabb;
+
+ /// @brief the previous end point in the end point list
+ EndPoint* prev[3];
+ /// @brief the next end point in the end point list
+ EndPoint* next[3];
+
+ /// @brief get the value of the end point
+ inline const Vec3f& getVal() const
+ {
+ if(minmax) return aabb->cached.max_;
+ else return aabb->cached.min_;
+ }
+
+ /// @brief set the value of the end point
+ inline Vec3f& getVal()
+ {
+ if(minmax) return aabb->cached.max_;
+ else return aabb->cached.min_;
+ }
+
+ inline FCL_REAL getVal(size_t i) const
+ {
+ if(minmax) return aabb->cached.max_[i];
+ else return aabb->cached.min_[i];
+ }
+
+ inline FCL_REAL& getVal(size_t i)
+ {
+ if(minmax) return aabb->cached.max_[i];
+ else return aabb->cached.min_[i];
+ }
+
+ };
+
+ /// @brief A pair of objects that are not culling away and should further check collision
+ struct SaPPair
+ {
+ SaPPair(CollisionObject* a, CollisionObject* b)
+ {
+ if(a < b)
+ {
+ obj1 = a;
+ obj2 = b;
+ }
+ else
+ {
+ obj1 = b;
+ obj2 = a;
+ }
+ }
+
+ CollisionObject* obj1;
+ CollisionObject* obj2;
+
+ bool operator == (const SaPPair& other) const
+ {
+ return ((obj1 == other.obj1) && (obj2 == other.obj2));
+ }
+ };
+
+ /// @brief Functor to help unregister one object
+ class isUnregistered
+ {
+ CollisionObject* obj;
+
+ public:
+ isUnregistered(CollisionObject* obj_) : obj(obj_)
+ {}
+
+ bool operator() (const SaPPair& pair) const
+ {
+ return (pair.obj1 == obj) || (pair.obj2 == obj);
+ }
+ };
+
+ /// @brief Functor to help remove collision pairs no longer valid (i.e., should be culled away)
+ class isNotValidPair
+ {
+ CollisionObject* obj1;
+ CollisionObject* obj2;
+
+ public:
+ isNotValidPair(CollisionObject* obj1_, CollisionObject* obj2_) : obj1(obj1_),
+ obj2(obj2_)
+ {}
+
+ bool operator() (const SaPPair& pair)
+ {
+ return (pair.obj1 == obj1) && (pair.obj2 == obj2);
+ }
+ };
+
+ void update_(SaPAABB* updated_aabb);
+
+ void updateVelist()
+ {
+ for(int coord = 0; coord < 3; ++coord)
+ {
+ velist[coord].resize(size() * 2);
+ EndPoint* current = elist[coord];
+ size_t id = 0;
+ while(current)
+ {
+ velist[coord][id] = current;
+ current = current->next[coord];
+ id++;
+ }
+ }
+ }
+
+ /// @brief End point list for x, y, z coordinates
+ EndPoint* elist[3];
+
+ /// @brief vector version of elist, for acceleration
+ std::vector<EndPoint*> velist[3];
+
+ /// @brief SAP interval list
+ std::list<SaPAABB*> AABB_arr;
+
+ /// @brief The pair of objects that should further check for collision
+ std::list<SaPPair> overlap_pairs;
+
+ size_t optimal_axis;
+
+ std::map<CollisionObject*, SaPAABB*> obj_aabb_map;
+
+ bool distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
+
+ bool collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
+
+ void addToOverlapPairs(const SaPPair& p)
+ {
+ bool repeated = false;
+ for(std::list<SaPPair>::iterator it = overlap_pairs.begin(), end = overlap_pairs.end();
+ it != end;
+ ++it)
+ {
+ if(*it == p)
+ {
+ repeated = true;
+ break;
+ }
+ }
+
+ if(!repeated)
+ overlap_pairs.push_back(p);
+ }
+
+ void removeFromOverlapPairs(const SaPPair& p)
+ {
+ for(std::list<SaPPair>::iterator it = overlap_pairs.begin(), end = overlap_pairs.end();
+ it != end;
+ ++it)
+ {
+ if(*it == p)
+ {
+ overlap_pairs.erase(it);
+ break;
+ }
+ }
+
+ // or overlap_pairs.remove_if(isNotValidPair(p));
+ }
+};
+
+
+
+}
+
+
+#endif
diff --git a/trunk/fcl/include/fcl/broadphase/broadphase_bruteforce.h b/trunk/fcl/include/fcl/broadphase/broadphase_bruteforce.h
new file mode 100644
index 0000000000000000000000000000000000000000..1b27ad88f753ee3bb19ff0def51770b5b4733d3d
--- /dev/null
+++ b/trunk/fcl/include/fcl/broadphase/broadphase_bruteforce.h
@@ -0,0 +1,107 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * 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 Jia Pan */
+
+#ifndef FCL_BROAD_PHASE_BRUTE_FORCE_H
+#define FCL_BROAD_PHASE_BRUTE_FORCE_H
+
+#include "fcl/broadphase/broadphase.h"
+#include <list>
+
+
+namespace fcl
+{
+
+/// @brief Brute force N-body collision manager
+class NaiveCollisionManager : public BroadPhaseCollisionManager
+{
+public:
+ NaiveCollisionManager() {}
+
+ /// @brief add objects to the manager
+ void registerObjects(const std::vector<CollisionObject*>& other_objs);
+
+ /// @brief add one object to the manager
+ void registerObject(CollisionObject* obj);
+
+ /// @brief remove one object from the manager
+ void unregisterObject(CollisionObject* obj);
+
+ /// @brief initialize the manager, related with the specific type of manager
+ void setup();
+
+ /// @brief update the condition of manager
+ void update();
+
+ /// @brief clear the manager
+ void clear();
+
+ /// @brief return the objects managed by the manager
+ void getObjects(std::vector<CollisionObject*>& objs) const;
+
+ /// @brief perform collision test between one object and all the objects belonging to the manager
+ void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
+
+ /// @brief perform distance computation between one object and all the objects belonging to the manager
+ void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const;
+
+ /// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision)
+ void collide(void* cdata, CollisionCallBack callback) const;
+
+ /// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance)
+ void distance(void* cdata, DistanceCallBack callback) const;
+
+ /// @brief perform collision test with objects belonging to another manager
+ void collide(BroadPhaseCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const;
+
+ /// @brief perform distance test with objects belonging to another manager
+ void distance(BroadPhaseCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const;
+
+ /// @brief whether the manager is empty
+ bool empty() const;
+
+ /// @brief the number of objects managed by the manager
+ inline size_t size() const { return objs.size(); }
+
+protected:
+
+ /// @brief objects belonging to the manager are stored in a list structure
+ std::list<CollisionObject*> objs;
+};
+
+
+}
+
+#endif
diff --git a/trunk/fcl/include/fcl/broadphase/broadphase_dynamic_AABB_tree.h b/trunk/fcl/include/fcl/broadphase/broadphase_dynamic_AABB_tree.h
new file mode 100644
index 0000000000000000000000000000000000000000..db8b49a04045d2843a0bc2ffbb3b7f147c87b9b8
--- /dev/null
+++ b/trunk/fcl/include/fcl/broadphase/broadphase_dynamic_AABB_tree.h
@@ -0,0 +1,243 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * 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 Jia Pan */
+
+
+#ifndef FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_H
+#define FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_H
+
+#include "fcl/broadphase/broadphase.h"
+#include "fcl/broadphase/hierarchy_tree.h"
+#include "fcl/octree.h"
+#include "fcl/BV.h"
+#include "fcl/geometric_shapes_utility.h"
+#include <boost/unordered_map.hpp>
+#include <boost/bind.hpp>
+#include <limits>
+
+
+namespace fcl
+{
+
+class DynamicAABBTreeCollisionManager : public BroadPhaseCollisionManager
+{
+public:
+ typedef NodeBase<AABB> DynamicAABBNode;
+ typedef boost::unordered_map<CollisionObject*, DynamicAABBNode*> DynamicAABBTable;
+
+ int max_tree_nonbalanced_level;
+ int tree_incremental_balance_pass;
+ int& tree_topdown_balance_threshold;
+ int& tree_topdown_level;
+ int tree_init_level;
+
+ bool octree_as_geometry_collide;
+ bool octree_as_geometry_distance;
+
+
+ DynamicAABBTreeCollisionManager() : tree_topdown_balance_threshold(dtree.bu_threshold),
+ tree_topdown_level(dtree.topdown_level)
+ {
+ max_tree_nonbalanced_level = 10;
+ tree_incremental_balance_pass = 10;
+ tree_topdown_balance_threshold = 2;
+ tree_topdown_level = 0;
+ tree_init_level = 0;
+ setup_ = false;
+
+ // from experiment, this is the optimal setting
+ octree_as_geometry_collide = true;
+ octree_as_geometry_distance = false;
+ }
+
+ /// @brief add objects to the manager
+ void registerObjects(const std::vector<CollisionObject*>& other_objs);
+
+ /// @brief add one object to the manager
+ void registerObject(CollisionObject* obj);
+
+ /// @brief remove one object from the manager
+ void unregisterObject(CollisionObject* obj);
+
+ /// @brief initialize the manager, related with the specific type of manager
+ void setup();
+
+ /// @brief update the condition of manager
+ void update();
+
+ /// @brief update the manager by explicitly given the object updated
+ void update(CollisionObject* updated_obj);
+
+ /// @brief update the manager by explicitly given the set of objects update
+ void update(const std::vector<CollisionObject*>& updated_objs);
+
+ /// @brief clear the manager
+ void clear()
+ {
+ dtree.clear();
+ table.clear();
+ }
+
+ /// @brief return the objects managed by the manager
+ void getObjects(std::vector<CollisionObject*>& objs) const
+ {
+ objs.resize(this->size());
+ std::transform(table.begin(), table.end(), objs.begin(), boost::bind(&DynamicAABBTable::value_type::first, _1));
+ }
+
+ /// @brief perform collision test between one object and all the objects belonging to the manager
+ void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
+ {
+ if(size() == 0) return;
+ switch(obj->getCollisionGeometry()->getNodeType())
+ {
+ case GEOM_OCTREE:
+ {
+ if(!octree_as_geometry_collide)
+ {
+ const OcTree* octree = static_cast<const OcTree*>(obj->getCollisionGeometry());
+ collisionRecurse(dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback);
+ }
+ else
+ collisionRecurse(dtree.getRoot(), obj, 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
+ void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const
+ {
+ if(size() == 0) return;
+ FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
+ switch(obj->getCollisionGeometry()->getNodeType())
+ {
+ case GEOM_OCTREE:
+ {
+ if(!octree_as_geometry_distance)
+ {
+ const OcTree* octree = static_cast<const OcTree*>(obj->getCollisionGeometry());
+ distanceRecurse(dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback, min_dist);
+ }
+ else
+ distanceRecurse(dtree.getRoot(), obj, 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)
+ void collide(void* cdata, CollisionCallBack callback) const
+ {
+ if(size() == 0) return;
+ selfCollisionRecurse(dtree.getRoot(), cdata, callback);
+ }
+
+ /// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance)
+ void distance(void* cdata, DistanceCallBack callback) const
+ {
+ if(size() == 0) return;
+ FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
+ selfDistanceRecurse(dtree.getRoot(), cdata, callback, min_dist);
+ }
+
+ /// @brief perform collision test with objects belonging to another manager
+ void collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const
+ {
+ DynamicAABBTreeCollisionManager* other_manager = static_cast<DynamicAABBTreeCollisionManager*>(other_manager_);
+ if((size() == 0) || (other_manager->size() == 0)) return;
+ collisionRecurse(dtree.getRoot(), other_manager->dtree.getRoot(), cdata, callback);
+ }
+
+ /// @brief perform distance test with objects belonging to another manager
+ void distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const
+ {
+ DynamicAABBTreeCollisionManager* other_manager = static_cast<DynamicAABBTreeCollisionManager*>(other_manager_);
+ if((size() == 0) || (other_manager->size() == 0)) return;
+ FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
+ distanceRecurse(dtree.getRoot(), other_manager->dtree.getRoot(), cdata, callback, min_dist);
+ }
+
+ /// @brief whether the manager is empty
+ bool empty() const
+ {
+ return dtree.empty();
+ }
+
+ /// @brief the number of objects managed by the manager
+ size_t size() const
+ {
+ return dtree.size();
+ }
+
+ const HierarchyTree<AABB>& getTree() const { return dtree; }
+
+
+private:
+ HierarchyTree<AABB> dtree;
+ boost::unordered_map<CollisionObject*, DynamicAABBNode*> table;
+
+ bool setup_;
+
+ bool collisionRecurse(DynamicAABBNode* root1, DynamicAABBNode* root2, void* cdata, CollisionCallBack callback) const;
+
+ bool collisionRecurse(DynamicAABBNode* root, CollisionObject* query, void* cdata, CollisionCallBack callback) const;
+
+ bool selfCollisionRecurse(DynamicAABBNode* root, void* cdata, CollisionCallBack callback) const;
+
+ bool distanceRecurse(DynamicAABBNode* root1, DynamicAABBNode* root2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
+
+ bool distanceRecurse(DynamicAABBNode* root, CollisionObject* query, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
+
+ 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 Transform3f& tf2, void* cdata, CollisionCallBack callback) const;
+
+ bool distanceRecurse(DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
+
+};
+
+
+
+}
+
+#endif
diff --git a/trunk/fcl/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h b/trunk/fcl/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h
new file mode 100644
index 0000000000000000000000000000000000000000..de146fe2e2730253fa57adfccd5e56c6f4c4f194
--- /dev/null
+++ b/trunk/fcl/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h
@@ -0,0 +1,241 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * 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 Jia Pan */
+
+#ifndef FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_H
+#define FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_H
+
+#include "fcl/broadphase/broadphase.h"
+#include "fcl/broadphase/hierarchy_tree.h"
+#include "fcl/octree.h"
+#include "fcl/BV.h"
+#include "fcl/geometric_shapes_utility.h"
+#include <boost/unordered_map.hpp>
+#include <boost/bind.hpp>
+#include <limits>
+
+
+namespace fcl
+{
+
+class DynamicAABBTreeCollisionManager_Array : public BroadPhaseCollisionManager
+{
+public:
+ typedef implementation_array::NodeBase<AABB> DynamicAABBNode;
+ typedef boost::unordered_map<CollisionObject*, size_t> DynamicAABBTable;
+
+ int max_tree_nonbalanced_level;
+ int tree_incremental_balance_pass;
+ int& tree_topdown_balance_threshold;
+ int& tree_topdown_level;
+ int tree_init_level;
+
+ bool octree_as_geometry_collide;
+ bool octree_as_geometry_distance;
+
+ DynamicAABBTreeCollisionManager_Array() : tree_topdown_balance_threshold(dtree.bu_threshold),
+ tree_topdown_level(dtree.topdown_level)
+ {
+ max_tree_nonbalanced_level = 10;
+ tree_incremental_balance_pass = 10;
+ tree_topdown_balance_threshold = 2;
+ tree_topdown_level = 0;
+ tree_init_level = 0;
+ setup_ = false;
+
+ // from experiment, this is the optimal setting
+ octree_as_geometry_collide = true;
+ octree_as_geometry_distance = false;
+ }
+
+ /// @brief add objects to the manager
+ void registerObjects(const std::vector<CollisionObject*>& other_objs);
+
+ /// @brief add one object to the manager
+ void registerObject(CollisionObject* obj);
+
+ /// @brief remove one object from the manager
+ void unregisterObject(CollisionObject* obj);
+
+ /// @brief initialize the manager, related with the specific type of manager
+ void setup();
+
+ /// @brief update the condition of manager
+ void update();
+
+ /// @brief update the manager by explicitly given the object updated
+ void update(CollisionObject* updated_obj);
+
+ /// @brief update the manager by explicitly given the set of objects update
+ void update(const std::vector<CollisionObject*>& updated_objs);
+
+ /// @brief clear the manager
+ void clear()
+ {
+ dtree.clear();
+ table.clear();
+ }
+
+ /// @brief return the objects managed by the manager
+ void getObjects(std::vector<CollisionObject*>& objs) const
+ {
+ objs.resize(this->size());
+ std::transform(table.begin(), table.end(), objs.begin(), boost::bind(&DynamicAABBTable::value_type::first, _1));
+ }
+
+ /// @brief perform collision test between one object and all the objects belonging to the manager
+ void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
+ {
+ if(size() == 0) return;
+ switch(obj->getCollisionGeometry()->getNodeType())
+ {
+ case GEOM_OCTREE:
+ {
+ if(!octree_as_geometry_collide)
+ {
+ const OcTree* octree = static_cast<const OcTree*>(obj->getCollisionGeometry());
+ collisionRecurse(dtree.getNodes(), dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback);
+ }
+ else
+ collisionRecurse(dtree.getNodes(), dtree.getRoot(), obj, 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
+ void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const
+ {
+ if(size() == 0) return;
+ FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
+ switch(obj->getCollisionGeometry()->getNodeType())
+ {
+ case GEOM_OCTREE:
+ {
+ if(!octree_as_geometry_distance)
+ {
+ 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);
+ }
+ else
+ distanceRecurse(dtree.getNodes(), dtree.getRoot(), obj, 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)
+ void collide(void* cdata, CollisionCallBack callback) const
+ {
+ if(size() == 0) return;
+ selfCollisionRecurse(dtree.getNodes(), dtree.getRoot(), cdata, callback);
+ }
+
+ /// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance)
+ void distance(void* cdata, DistanceCallBack callback) const
+ {
+ if(size() == 0) return;
+ FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
+ selfDistanceRecurse(dtree.getNodes(), dtree.getRoot(), cdata, callback, min_dist);
+ }
+
+ /// @brief perform collision test with objects belonging to another manager
+ void collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const
+ {
+ DynamicAABBTreeCollisionManager_Array* other_manager = static_cast<DynamicAABBTreeCollisionManager_Array*>(other_manager_);
+ if((size() == 0) || (other_manager->size() == 0)) return;
+ collisionRecurse(dtree.getNodes(), dtree.getRoot(), other_manager->dtree.getNodes(), other_manager->dtree.getRoot(), cdata, callback);
+ }
+
+ /// @brief perform distance test with objects belonging to another manager
+ void distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const
+ {
+ DynamicAABBTreeCollisionManager_Array* other_manager = static_cast<DynamicAABBTreeCollisionManager_Array*>(other_manager_);
+ if((size() == 0) || (other_manager->size() == 0)) return;
+ FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
+ distanceRecurse(dtree.getNodes(), dtree.getRoot(), other_manager->dtree.getNodes(), other_manager->dtree.getRoot(), cdata, callback, min_dist);
+ }
+
+ /// @brief whether the manager is empty
+ bool empty() const
+ {
+ return dtree.empty();
+ }
+
+ /// @brief the number of objects managed by the manager
+ size_t size() const
+ {
+ return dtree.size();
+ }
+
+
+ const implementation_array::HierarchyTree<AABB>& getTree() const { return dtree; }
+
+private:
+ implementation_array::HierarchyTree<AABB> dtree;
+ boost::unordered_map<CollisionObject*, size_t> table;
+
+ bool setup_;
+
+ bool collisionRecurse(DynamicAABBNode* nodes1, size_t root1, DynamicAABBNode* nodes2, size_t root2, void* cdata, CollisionCallBack callback) const;
+
+ bool collisionRecurse(DynamicAABBNode* nodes, size_t root, CollisionObject* query, void* cdata, CollisionCallBack callback) const;
+
+ bool selfCollisionRecurse(DynamicAABBNode* nodes, size_t root, void* cdata, CollisionCallBack callback) const;
+
+ bool distanceRecurse(DynamicAABBNode* nodes1, size_t root1, DynamicAABBNode* nodes2, size_t root2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
+
+ bool distanceRecurse(DynamicAABBNode* nodes, size_t root, CollisionObject* query, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
+
+ 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 Transform3f& 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 Transform3f& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
+
+
+};
+
+
+}
+
+#endif
diff --git a/trunk/fcl/include/fcl/broadphase/broadphase_interval_tree.h b/trunk/fcl/include/fcl/broadphase/broadphase_interval_tree.h
new file mode 100644
index 0000000000000000000000000000000000000000..ba16940b58beafa8377b138466213fbf509ec2bd
--- /dev/null
+++ b/trunk/fcl/include/fcl/broadphase/broadphase_interval_tree.h
@@ -0,0 +1,163 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * 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 Jia Pan */
+
+#ifndef FCL_BROAD_PHASE_INTERVAL_TREE_H
+#define FCL_BROAD_PHASE_INTERVAL_TREE_H
+
+#include "fcl/broadphase/broadphase.h"
+#include "fcl/broadphase/interval_tree.h"
+#include <deque>
+#include <map>
+
+namespace fcl
+{
+
+/// @brief Collision manager based on interval tree
+class IntervalTreeCollisionManager : public BroadPhaseCollisionManager
+{
+public:
+ IntervalTreeCollisionManager() : setup_(false)
+ {
+ for(int i = 0; i < 3; ++i)
+ interval_trees[i] = NULL;
+ }
+
+ ~IntervalTreeCollisionManager()
+ {
+ clear();
+ }
+
+ /// @brief remove one object from the manager
+ void registerObject(CollisionObject* obj);
+
+ /// @brief add one object to the manager
+ void unregisterObject(CollisionObject* obj);
+
+ /// @brief initialize the manager, related with the specific type of manager
+ void setup();
+
+ /// @brief update the condition of manager
+ void update();
+
+ /// @brief update the manager by explicitly given the object updated
+ void update(CollisionObject* updated_obj);
+
+ /// @brief update the manager by explicitly given the set of objects update
+ void update(const std::vector<CollisionObject*>& updated_objs);
+
+ /// @brief clear the manager
+ void clear();
+
+ /// @brief return the objects managed by the manager
+ void getObjects(std::vector<CollisionObject*>& objs) const;
+
+ /// @brief perform collision test between one object and all the objects belonging to the manager
+ void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
+
+ /// @brief perform distance computation between one object and all the objects belonging to the manager
+ void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const;
+
+ /// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision)
+ void collide(void* cdata, CollisionCallBack callback) const;
+
+ /// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance)
+ void distance(void* cdata, DistanceCallBack callback) const;
+
+ /// @brief perform collision test with objects belonging to another manager
+ void collide(BroadPhaseCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const;
+
+ /// @brief perform distance test with objects belonging to another manager
+ void distance(BroadPhaseCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const;
+
+ /// @brief whether the manager is empty
+ bool empty() const;
+
+ /// @brief the number of objects managed by the manager
+ inline size_t size() const { return endpoints[0].size() / 2; }
+
+protected:
+
+
+ /// @brief SAP end point
+ struct EndPoint
+ {
+ /// @brief object related with the end point
+ CollisionObject* obj;
+
+ /// @brief end point value
+ FCL_REAL value;
+
+ /// @brief tag for whether it is a lower bound or higher bound of an interval, 0 for lo, and 1 for hi
+ char minmax;
+ };
+
+ /// @brief Extention interval tree's interval to SAP interval, adding more information
+ struct SAPInterval : public SimpleInterval
+ {
+ CollisionObject* obj;
+ SAPInterval(double low_, double high_, CollisionObject* obj_) : SimpleInterval()
+ {
+ low = low_;
+ high = high_;
+ obj = obj_;
+ }
+ };
+
+
+ bool checkColl(std::deque<SimpleInterval*>::const_iterator pos_start, std::deque<SimpleInterval*>::const_iterator pos_end, CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
+
+ bool checkDist(std::deque<SimpleInterval*>::const_iterator pos_start, std::deque<SimpleInterval*>::const_iterator pos_end, CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
+
+ bool collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
+
+ bool distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
+
+ /// @brief vector stores all the end points
+ std::vector<EndPoint> endpoints[3];
+
+ /// @brief interval tree manages the intervals
+ IntervalTree* interval_trees[3];
+
+ std::map<CollisionObject*, SAPInterval*> obj_interval_maps[3];
+
+ /// @brief tag for whether the interval tree is maintained suitably
+ bool setup_;
+};
+
+
+}
+
+#endif
diff --git a/trunk/fcl/include/fcl/broadphase/broadphase_spatialhash.h b/trunk/fcl/include/fcl/broadphase/broadphase_spatialhash.h
new file mode 100644
index 0000000000000000000000000000000000000000..48ed2c3157457a5f98615f8d37a610a717e4628b
--- /dev/null
+++ b/trunk/fcl/include/fcl/broadphase/broadphase_spatialhash.h
@@ -0,0 +1,198 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * 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 Jia Pan */
+
+#ifndef FCL_BROAD_PHASE_SPATIAL_HASH_H
+#define FCL_BROAD_PHASE_SPATIAL_HASH_H
+
+#include "fcl/broadphase/broadphase.h"
+#include "fcl/broadphase/hash.h"
+#include "fcl/BV/AABB.h"
+#include <list>
+#include <map>
+
+namespace fcl
+{
+
+/// @brief Spatial hash function: hash an AABB to a set of integer values
+struct SpatialHash
+{
+ SpatialHash(const AABB& scene_limit_, FCL_REAL cell_size_) : cell_size(cell_size_),
+ scene_limit(scene_limit_)
+ {
+ width[0] = std::ceil(scene_limit.width() / cell_size);
+ width[1] = std::ceil(scene_limit.height() / cell_size);
+ width[2] = std::ceil(scene_limit.depth() / cell_size);
+ }
+
+ std::vector<unsigned int> operator() (const AABB& aabb) const
+ {
+ int min_x = std::floor((aabb.min_[0] - scene_limit.min_[0]) / cell_size);
+ int max_x = std::ceil((aabb.max_[0] - scene_limit.min_[0]) / cell_size);
+ int min_y = std::floor((aabb.min_[1] - scene_limit.min_[1]) / cell_size);
+ int max_y = std::ceil((aabb.max_[1] - scene_limit.min_[1]) / cell_size);
+ int min_z = std::floor((aabb.min_[2] - scene_limit.min_[2]) / cell_size);
+ int max_z = std::ceil((aabb.max_[2] - scene_limit.min_[2]) / cell_size);
+
+ std::vector<unsigned int> keys((max_x - min_x) * (max_y - min_y) * (max_z - min_z));
+ int id = 0;
+ for(int x = min_x; x < max_x; ++x)
+ {
+ for(int y = min_y; y < max_y; ++y)
+ {
+ for(int z = min_z; z < max_z; ++z)
+ {
+ keys[id++] = x + y * width[0] + z * width[0] * width[1];
+ }
+ }
+ }
+ return keys;
+ }
+
+private:
+
+ FCL_REAL cell_size;
+ AABB scene_limit;
+ unsigned int width[3];
+};
+
+/// @brief spatial hashing collision mananger
+template<typename HashTable = SimpleHashTable<AABB, CollisionObject*, SpatialHash> >
+class SpatialHashingCollisionManager : public BroadPhaseCollisionManager
+{
+public:
+ SpatialHashingCollisionManager(FCL_REAL cell_size, const Vec3f& scene_min, const Vec3f& scene_max, unsigned int default_table_size = 1000) : scene_limit(AABB(scene_min, scene_max)),
+ hash_table(new HashTable(SpatialHash(scene_limit, cell_size)))
+ {
+ hash_table->init(default_table_size);
+ }
+
+ ~SpatialHashingCollisionManager()
+ {
+ delete hash_table;
+ }
+
+ /// @brief add one object to the manager
+ void registerObject(CollisionObject* obj);
+
+ /// @brief remove one object from the manager
+ void unregisterObject(CollisionObject* obj);
+
+ /// @brief initialize the manager, related with the specific type of manager
+ void setup();
+
+ /// @brief update the condition of manager
+ void update();
+
+ /// @brief update the manager by explicitly given the object updated
+ void update(CollisionObject* updated_obj);
+
+ /// @brief update the manager by explicitly given the set of objects update
+ void update(const std::vector<CollisionObject*>& updated_objs);
+
+ /// @brief clear the manager
+ void clear();
+
+ /// @brief return the objects managed by the manager
+ void getObjects(std::vector<CollisionObject*>& objs) const;
+
+ /// @brief perform collision test between one object and all the objects belonging to the manager
+ void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
+
+ /// @brief perform distance computation between one object and all the objects belonging ot the manager
+ void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const;
+
+ /// @brief perform collision test for the objects belonging to the manager (i.e, N^2 self collision)
+ void collide(void* cdata, CollisionCallBack callback) const;
+
+ /// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance)
+ void distance(void* cdata, DistanceCallBack callback) const;
+
+ /// @brief perform collision test with objects belonging to another manager
+ void collide(BroadPhaseCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const;
+
+ /// @brief perform distance test with objects belonging to another manager
+ void distance(BroadPhaseCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const;
+
+ /// @brief whether the manager is empty
+ bool empty() const;
+
+ /// @brief the number of objects managed by the manager
+ size_t size() const;
+
+ /// @brief compute the bound for the environent
+ static void computeBound(std::vector<CollisionObject*>& objs, Vec3f& l, Vec3f& u)
+ {
+ AABB bound;
+ for(unsigned int i = 0; i < objs.size(); ++i)
+ bound += objs[i]->getAABB();
+
+ l = bound.min_;
+ u = bound.max_;
+ }
+
+protected:
+
+ /// @brief perform collision test between one object and all the objects belonging to the manager
+ bool collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
+
+ /// @brief perform distance computation between one object and all the objects belonging ot the manager
+ bool distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
+
+
+ /// @brief all objects in the scene
+ std::list<CollisionObject*> objs;
+
+ /// @brief objects outside the scene limit are in another list
+ std::list<CollisionObject*> objs_outside_scene_limit;
+
+ /// @brief the size of the scene
+ AABB scene_limit;
+
+ /// @brief store the map between objects and their aabbs. will make update more convenient
+ std::map<CollisionObject*, AABB> obj_aabb_map;
+
+ /// @brief objects in the scene limit (given by scene_min and scene_max) are in the spatial hash table
+ HashTable* hash_table;
+
+};
+
+
+}
+
+#include "fcl/broadphase/broadphase_spatialhash.hxx"
+
+
+#endif
diff --git a/trunk/fcl/include/fcl/broadphase/broadphase_spatialhash.hxx b/trunk/fcl/include/fcl/broadphase/broadphase_spatialhash.hxx
new file mode 100644
index 0000000000000000000000000000000000000000..0a64039402428358096c522f7432642c835740aa
--- /dev/null
+++ b/trunk/fcl/include/fcl/broadphase/broadphase_spatialhash.hxx
@@ -0,0 +1,458 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * 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 Jia Pan */
+
+namespace fcl
+{
+
+template<typename HashTable>
+void SpatialHashingCollisionManager<HashTable>::registerObject(CollisionObject* obj)
+{
+ objs.push_back(obj);
+
+ const AABB& obj_aabb = obj->getAABB();
+ AABB overlap_aabb;
+
+ if(scene_limit.overlap(obj_aabb, overlap_aabb))
+ {
+ if(!scene_limit.contain(obj_aabb))
+ objs_outside_scene_limit.push_back(obj);
+
+ hash_table->insert(overlap_aabb, obj);
+ }
+ else
+ objs_outside_scene_limit.push_back(obj);
+
+ obj_aabb_map[obj] = obj_aabb;
+}
+
+template<typename HashTable>
+void SpatialHashingCollisionManager<HashTable>::unregisterObject(CollisionObject* obj)
+{
+ objs.remove(obj);
+
+ const AABB& obj_aabb = obj->getAABB();
+ AABB overlap_aabb;
+
+ if(scene_limit.overlap(obj_aabb, overlap_aabb))
+ {
+ if(!scene_limit.contain(obj_aabb))
+ objs_outside_scene_limit.remove(obj);
+
+ hash_table->remove(overlap_aabb, obj);
+ }
+ else
+ objs_outside_scene_limit.remove(obj);
+
+ obj_aabb_map.erase(obj);
+}
+
+template<typename HashTable>
+void SpatialHashingCollisionManager<HashTable>::setup()
+{}
+
+template<typename HashTable>
+void SpatialHashingCollisionManager<HashTable>::update()
+{
+ hash_table->clear();
+ objs_outside_scene_limit.clear();
+
+ for(std::list<CollisionObject*>::const_iterator it = objs.begin(), end = objs.end();
+ it != end; ++it)
+ {
+ CollisionObject* obj = *it;
+ const AABB& obj_aabb = obj->getAABB();
+ AABB overlap_aabb;
+
+ if(scene_limit.overlap(obj_aabb, overlap_aabb))
+ {
+ if(!scene_limit.contain(obj_aabb))
+ objs_outside_scene_limit.push_back(obj);
+
+ hash_table->insert(overlap_aabb, obj);
+ }
+ else
+ objs_outside_scene_limit.push_back(obj);
+
+ obj_aabb_map[obj] = obj_aabb;
+ }
+}
+
+template<typename HashTable>
+void SpatialHashingCollisionManager<HashTable>::update(CollisionObject* updated_obj)
+{
+ const AABB& new_aabb = updated_obj->getAABB();
+ const AABB& old_aabb = obj_aabb_map[updated_obj];
+
+ if(!scene_limit.contain(old_aabb)) // previously not completely in scene limit
+ {
+ if(scene_limit.contain(new_aabb))
+ {
+ std::list<CollisionObject*>::iterator find_it = std::find(objs_outside_scene_limit.begin(),
+ objs_outside_scene_limit.end(),
+ updated_obj);
+
+ objs_outside_scene_limit.erase(find_it);
+ }
+ }
+ else if(!scene_limit.contain(new_aabb)) // previous completely in scenelimit, now not
+ objs_outside_scene_limit.push_back(updated_obj);
+
+ AABB old_overlap_aabb;
+ if(scene_limit.overlap(old_aabb, old_overlap_aabb))
+ hash_table->remove(old_overlap_aabb, updated_obj);
+
+ AABB new_overlap_aabb;
+ if(scene_limit.overlap(new_aabb, new_overlap_aabb))
+ hash_table->insert(new_overlap_aabb, updated_obj);
+
+ obj_aabb_map[updated_obj] = new_aabb;
+}
+
+template<typename HashTable>
+void SpatialHashingCollisionManager<HashTable>::update(const std::vector<CollisionObject*>& updated_objs)
+{
+ for(size_t i = 0; i < updated_objs.size(); ++i)
+ update(updated_objs[i]);
+}
+
+
+template<typename HashTable>
+void SpatialHashingCollisionManager<HashTable>::clear()
+{
+ objs.clear();
+ hash_table->clear();
+ objs_outside_scene_limit.clear();
+ obj_aabb_map.clear();
+}
+
+template<typename HashTable>
+void SpatialHashingCollisionManager<HashTable>::getObjects(std::vector<CollisionObject*>& objs_) const
+{
+ objs_.resize(objs.size());
+ std::copy(objs.begin(), objs.end(), objs_.begin());
+}
+
+template<typename HashTable>
+void SpatialHashingCollisionManager<HashTable>::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
+{
+ if(size() == 0) return;
+ collide_(obj, cdata, callback);
+}
+
+template<typename HashTable>
+void SpatialHashingCollisionManager<HashTable>::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const
+{
+ if(size() == 0) return;
+ FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
+ distance_(obj, cdata, callback, min_dist);
+}
+
+template<typename HashTable>
+bool SpatialHashingCollisionManager<HashTable>::collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
+{
+ const AABB& obj_aabb = obj->getAABB();
+ AABB overlap_aabb;
+
+ if(scene_limit.overlap(obj_aabb, overlap_aabb))
+ {
+ if(!scene_limit.contain(obj_aabb))
+ {
+ for(std::list<CollisionObject*>::const_iterator it = objs_outside_scene_limit.begin(), end = objs_outside_scene_limit.end();
+ it != end; ++it)
+ {
+ if(obj == *it) continue;
+ if(callback(obj, *it, cdata)) return true;
+ }
+ }
+
+ std::vector<CollisionObject*> query_result = hash_table->query(overlap_aabb);
+ for(unsigned int i = 0; i < query_result.size(); ++i)
+ {
+ if(obj == query_result[i]) continue;
+ if(callback(obj, query_result[i], cdata)) return true;
+ }
+ }
+ else
+ {
+ ;
+ for(std::list<CollisionObject*>::const_iterator it = objs_outside_scene_limit.begin(), end = objs_outside_scene_limit.end();
+ it != end; ++it)
+ {
+ if(obj == *it) continue;
+ if(callback(obj, *it, cdata)) return true;
+ }
+ }
+
+ return false;
+}
+
+template<typename HashTable>
+bool SpatialHashingCollisionManager<HashTable>::distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
+{
+ Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
+ AABB aabb = obj->getAABB();
+ if(min_dist < std::numeric_limits<FCL_REAL>::max())
+ {
+ Vec3f min_dist_delta(min_dist, min_dist, min_dist);
+ aabb.expand(min_dist_delta);
+ }
+
+ AABB overlap_aabb;
+
+ int status = 1;
+ FCL_REAL old_min_distance;
+
+ while(1)
+ {
+ old_min_distance = min_dist;
+
+ if(scene_limit.overlap(aabb, overlap_aabb))
+ {
+ if(!scene_limit.contain(aabb))
+ {
+ for(std::list<CollisionObject*>::const_iterator it = objs_outside_scene_limit.begin(), end = objs_outside_scene_limit.end();
+ it != end; ++it)
+ {
+ if(obj == *it) continue;
+ if(!enable_tested_set_)
+ {
+ if(obj->getAABB().distance((*it)->getAABB()) < min_dist)
+ if(callback(obj, *it, cdata, min_dist)) return true;
+ }
+ else
+ {
+ if(!inTestedSet(obj, *it))
+ {
+ if(obj->getAABB().distance((*it)->getAABB()) < min_dist)
+ if(callback(obj, *it, cdata, min_dist)) return true;
+ insertTestedSet(obj, *it);
+ }
+ }
+ }
+ }
+
+ std::vector<CollisionObject*> query_result = hash_table->query(overlap_aabb);
+ for(unsigned int i = 0; i < query_result.size(); ++i)
+ {
+ if(obj == query_result[i]) continue;
+ if(!enable_tested_set_)
+ {
+ if(obj->getAABB().distance(query_result[i]->getAABB()) < min_dist)
+ if(callback(obj, query_result[i], cdata, min_dist)) return true;
+ }
+ else
+ {
+ if(!inTestedSet(obj, query_result[i]))
+ {
+ if(obj->getAABB().distance(query_result[i]->getAABB()) < min_dist)
+ if(callback(obj, query_result[i], cdata, min_dist)) return true;
+ insertTestedSet(obj, query_result[i]);
+ }
+ }
+ }
+ }
+ else
+ {
+ for(std::list<CollisionObject*>::const_iterator it = objs_outside_scene_limit.begin(), end = objs_outside_scene_limit.end();
+ it != end; ++it)
+ {
+ if(obj == *it) continue;
+ if(!enable_tested_set_)
+ {
+ if(obj->getAABB().distance((*it)->getAABB()) < min_dist)
+ if(callback(obj, *it, cdata, min_dist)) return true;
+ }
+ else
+ {
+ if(!inTestedSet(obj, *it))
+ {
+ if(obj->getAABB().distance((*it)->getAABB()) < min_dist)
+ if(callback(obj, *it, cdata, min_dist)) return true;
+ insertTestedSet(obj, *it);
+ }
+ }
+ }
+ }
+
+ if(status == 1)
+ {
+ if(old_min_distance < std::numeric_limits<FCL_REAL>::max())
+ break;
+ else
+ {
+ if(min_dist < old_min_distance)
+ {
+ Vec3f min_dist_delta(min_dist, min_dist, min_dist);
+ aabb = AABB(obj->getAABB(), min_dist_delta);
+ status = 0;
+ }
+ else
+ {
+ if(aabb.equal(obj->getAABB()))
+ aabb.expand(delta);
+ else
+ aabb.expand(obj->getAABB(), 2.0);
+ }
+ }
+ }
+ else if(status == 0)
+ break;
+ }
+
+ return false;
+}
+
+template<typename HashTable>
+void SpatialHashingCollisionManager<HashTable>::collide(void* cdata, CollisionCallBack callback) const
+{
+ if(size() == 0) return;
+
+ for(std::list<CollisionObject*>::const_iterator it1 = objs.begin(), end1 = objs.end();
+ it1 != end1; ++it1)
+ {
+ const AABB& obj_aabb = (*it1)->getAABB();
+ AABB overlap_aabb;
+
+ if(scene_limit.overlap(obj_aabb, overlap_aabb))
+ {
+ if(!scene_limit.contain(obj_aabb))
+ {
+ for(std::list<CollisionObject*>::const_iterator it2 = objs_outside_scene_limit.begin(), end2 = objs_outside_scene_limit.end();
+ it2 != end2; ++it2)
+ {
+ if(*it1 < *it2) { if(callback(*it1, *it2, cdata)) return; }
+ }
+ }
+
+ std::vector<CollisionObject*> query_result = hash_table->query(overlap_aabb);
+ for(unsigned int i = 0; i < query_result.size(); ++i)
+ {
+ if(*it1 < query_result[i]) { if(callback(*it1, query_result[i], cdata)) return; }
+ }
+ }
+ else
+ {
+ for(std::list<CollisionObject*>::const_iterator it2 = objs_outside_scene_limit.begin(), end2 = objs_outside_scene_limit.end();
+ it2 != end2; ++it2)
+ {
+ if(*it1 < *it2) { if(callback(*it1, *it2, cdata)) return; }
+ }
+ }
+ }
+}
+
+template<typename HashTable>
+void SpatialHashingCollisionManager<HashTable>::distance(void* cdata, DistanceCallBack callback) const
+{
+ if(size() == 0) return;
+
+ enable_tested_set_ = true;
+ tested_set.clear();
+
+ FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
+
+ for(std::list<CollisionObject*>::const_iterator it = objs.begin(), end = objs.end(); it != end; ++it)
+ if(distance_(*it, cdata, callback, min_dist)) break;
+
+ enable_tested_set_ = false;
+ tested_set.clear();
+}
+
+template<typename HashTable>
+void SpatialHashingCollisionManager<HashTable>::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const
+{
+ SpatialHashingCollisionManager<HashTable>* other_manager = static_cast<SpatialHashingCollisionManager<HashTable>* >(other_manager_);
+
+ if((size() == 0) || (other_manager->size() == 0)) return;
+
+ if(this == other_manager)
+ {
+ collide(cdata, callback);
+ return;
+ }
+
+ if(this->size() < other_manager->size())
+ {
+ for(std::list<CollisionObject*>::const_iterator it = objs.begin(), end = objs.end(); it != end; ++it)
+ if(other_manager->collide_(*it, cdata, callback)) return;
+ }
+ else
+ {
+ for(std::list<CollisionObject*>::const_iterator it = other_manager->objs.begin(), end = other_manager->objs.end(); it != end; ++it)
+ if(collide_(*it, cdata, callback)) return;
+ }
+}
+
+template<typename HashTable>
+void SpatialHashingCollisionManager<HashTable>::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const
+{
+ SpatialHashingCollisionManager<HashTable>* other_manager = static_cast<SpatialHashingCollisionManager<HashTable>* >(other_manager_);
+
+ if((size() == 0) || (other_manager->size() == 0)) return;
+
+ if(this == other_manager)
+ {
+ distance(cdata, callback);
+ return;
+ }
+
+ FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
+
+ if(this->size() < other_manager->size())
+ {
+ for(std::list<CollisionObject*>::const_iterator it = objs.begin(), end = objs.end(); it != end; ++it)
+ if(other_manager->distance_(*it, cdata, callback, min_dist)) return;
+ }
+ else
+ {
+ for(std::list<CollisionObject*>::const_iterator it = other_manager->objs.begin(), end = other_manager->objs.end(); it != end; ++it)
+ if(distance_(*it, cdata, callback, min_dist)) return;
+ }
+}
+
+template<typename HashTable>
+bool SpatialHashingCollisionManager<HashTable>::empty() const
+{
+ return objs.empty();
+}
+
+template<typename HashTable>
+size_t SpatialHashingCollisionManager<HashTable>::size() const
+{
+ return objs.size();
+}
+
+}
diff --git a/trunk/fcl/include/fcl/hash.h b/trunk/fcl/include/fcl/broadphase/hash.h
similarity index 85%
rename from trunk/fcl/include/fcl/hash.h
rename to trunk/fcl/include/fcl/broadphase/hash.h
index b8b15c84ef0811ce3181aae18715687f79a3e14e..a54aacc3dda2b65301c1e2e6f31f15e11ce3bd99 100644
--- a/trunk/fcl/include/fcl/hash.h
+++ b/trunk/fcl/include/fcl/broadphase/hash.h
@@ -46,20 +46,25 @@
namespace fcl
{
-// HashFnc is any extended hash function: HashFnc(key) = {index1, index2, ..., }
+/// @brief A simple hash table implemented as multiple buckets. HashFnc is any extended hash function: HashFnc(key) = {index1, index2, ..., }
template<typename Key, typename Data, typename HashFnc>
class SimpleHashTable
{
protected:
typedef std::list<Data> Bin;
+
std::vector<Bin> table_;
+
HashFnc h_;
+
size_t table_size_;
+
public:
SimpleHashTable(const HashFnc& h) : h_(h)
{
}
+ ///@ brief Init the number of bins in the hash table
void init(size_t size)
{
if(size == 0)
@@ -71,6 +76,7 @@ public:
table_size_ = size;
}
+ //// @brief Insert a key-value pair into the table
void insert(Key key, Data value)
{
std::vector<unsigned int> indices = h_(key);
@@ -79,6 +85,7 @@ public:
table_[indices[i] % range].push_back(value);
}
+ /// @brief Find the elements in the hash table whose key is the same as query key.
std::vector<Data> query(Key key) const
{
size_t range = table_.size();
@@ -93,6 +100,7 @@ public:
return std::vector<Data>(result.begin(), result.end());
}
+ /// @brief remove the key-value pair from the table
void remove(Key key, Data value)
{
size_t range = table_.size();
@@ -104,6 +112,7 @@ public:
}
}
+ /// @brief clear the hash table
void clear()
{
table_.clear();
@@ -115,7 +124,7 @@ public:
template<typename U, typename V>
class unordered_map_hash_table : public boost::unordered_map<U, V> {};
-
+/// @brief A hash table implemented using unordered_map
template<typename Key, typename Data, typename HashFnc, template<typename, typename> class TableT = unordered_map_hash_table>
class SparseHashTable
{
@@ -128,8 +137,10 @@ protected:
public:
SparseHashTable(const HashFnc& h) : h_(h) {}
+ /// @brief Init the hash table. The bucket size is dynamically decided.
void init(size_t) { table_.clear(); }
+ /// @brief insert one key-value pair into the hash table
void insert(Key key, Data value)
{
std::vector<unsigned int> indices = h_(key);
@@ -137,6 +148,7 @@ public:
table_[indices[i]].push_back(value);
}
+ /// @brief find the elements whose key is the same as the query
std::vector<Data> query(Key key) const
{
std::vector<unsigned int> indices = h_(key);
@@ -152,6 +164,7 @@ public:
return std::vector<Data>(result.begin(), result.end());
}
+ /// @brief remove one key-value pair from the hash table
void remove(Key key, Data value)
{
std::vector<unsigned int> indices = h_(key);
@@ -162,6 +175,7 @@ public:
}
}
+ /// @brief clear the hash table
void clear()
{
table_.clear();
diff --git a/trunk/fcl/include/fcl/broadphase/hierarchy_tree.h b/trunk/fcl/include/fcl/broadphase/hierarchy_tree.h
new file mode 100644
index 0000000000000000000000000000000000000000..2787f1dac34c575666564d1257cc2d2f6352a9c5
--- /dev/null
+++ b/trunk/fcl/include/fcl/broadphase/hierarchy_tree.h
@@ -0,0 +1,593 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * 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 Jia Pan */
+
+#ifndef FCL_HIERARCHY_TREE_H
+#define FCL_HIERARCHY_TREE_H
+
+#include <vector>
+#include <map>
+#include "fcl/BV/AABB.h"
+#include "fcl/vec_3f.h"
+#include "fcl/broadphase/morton.h"
+#include <boost/bind.hpp>
+#include <boost/iterator/zip_iterator.hpp>
+
+namespace fcl
+{
+
+/// @brief dynamic AABB tree node
+template<typename BV>
+struct NodeBase
+{
+ /// @brief the bounding volume for the node
+ BV bv;
+
+ /// @brief pointer to parent node
+ NodeBase<BV>* parent;
+
+ /// @brief whether is a leaf
+ bool isLeaf() const { return (children[1] == NULL); }
+
+ /// @brief whether is internal node
+ bool isInternal() const { return !isLeaf(); }
+
+ union
+ {
+ /// @brief for leaf node, children nodes
+ NodeBase<BV>* children[2];
+ void* data;
+ };
+
+ /// @brief morton code for current BV
+ FCL_UINT32 code;
+
+ NodeBase()
+ {
+ parent = NULL;
+ children[0] = NULL;
+ children[1] = NULL;
+ }
+};
+
+
+/// @brief Compare two nodes accoording to the d-th dimension of node center
+template<typename BV>
+bool nodeBaseLess(NodeBase<BV>* a, NodeBase<BV>* b, int d)
+{
+ if(a->bv.center()[d] < b->bv.center()[d]) return true;
+ return false;
+}
+
+/// @brief select from node1 and node2 which is close to a given query. 0 for node1 and 1 for node2
+template<typename BV>
+size_t select(const NodeBase<BV>& query, const NodeBase<BV>& node1, const NodeBase<BV>& node2)
+{
+ return 0;
+}
+
+template<>
+size_t select(const NodeBase<AABB>& node, const NodeBase<AABB>& node1, const NodeBase<AABB>& node2);
+
+/// @brief select from node1 and node2 which is close to a given query bounding volume. 0 for node1 and 1 for node2
+template<typename BV>
+size_t select(const BV& query, const NodeBase<BV>& node1, const NodeBase<BV>& node2)
+{
+ return 0;
+}
+
+template<>
+size_t select(const AABB& query, const NodeBase<AABB>& node1, const NodeBase<AABB>& node2);
+
+
+/// @brief Class for hierarchy tree structure
+template<typename BV>
+class HierarchyTree
+{
+ typedef NodeBase<BV> NodeType;
+ typedef typename std::vector<NodeBase<BV>* >::iterator NodeVecIterator;
+ typedef typename std::vector<NodeBase<BV>* >::const_iterator NodeVecConstIterator;
+
+ struct SortByMorton
+ {
+ bool operator() (NodeType* a, NodeType* b) const
+ {
+ return a->code < b->code;
+ }
+ };
+
+public:
+
+ /// @brief Create hierarchy tree with suitable setting.
+ /// bu_threshold decides the height of tree node to start bottom-up construction / optimization;
+ /// topdown_level decides different methods to construct tree in topdown manner.
+ /// lower level method constructs tree with better quality but is slower.
+ HierarchyTree(int bu_threshold_ = 16, int topdown_level_ = 0);
+
+ ~HierarchyTree();
+
+ /// @brief Initialize the tree by a set of leaves using algorithm with a given level.
+ void init(std::vector<NodeType*>& leaves, int level = 0);
+
+ /// @brief Insest a node
+ NodeType* insert(const BV& bv, void* data);
+
+ /// @brief Remove a leaf node
+ void remove(NodeType* leaf);
+
+ /// @brief Clear the tree
+ void clear();
+
+ /// @brief Whether the tree is empty
+ bool empty() const;
+
+ /// @brief update one leaf node
+ void update(NodeType* leaf, int lookahead_level = -1);
+
+ /// @brief update the tree when the bounding volume of a given leaf has changed
+ bool update(NodeType* leaf, const BV& bv);
+
+ /// @brief update one leaf's bounding volume, with prediction
+ bool update(NodeType* leaf, const BV& bv, const Vec3f& vel, FCL_REAL margin);
+
+ /// @brief update one leaf's bounding volume, with prediction
+ bool update(NodeType* leaf, const BV& bv, const Vec3f& vel);
+
+ /// @brief get the max height of the tree
+ size_t getMaxHeight() const;
+
+ /// @brief get the max depth of the tree
+ size_t getMaxDepth() const;
+
+ /// @brief balance the tree from bottom
+ void balanceBottomup();
+
+ /// @brief balance the tree from top
+ void balanceTopdown();
+
+ /// @brief balance the tree in an incremental way
+ void balanceIncremental(int iterations);
+
+ /// @brief refit the tree, i.e., when the leaf nodes' bounding volumes change, update the entire tree in a bottom-up manner
+ void refit();
+
+ /// @brief extract all the leaves of the tree
+ void extractLeaves(const NodeType* root, std::vector<NodeType*>& leaves) const;
+
+ /// @brief number of leaves in the tree
+ size_t size() const;
+
+ /// @brief get the root of the tree
+ NodeType* getRoot() const;
+
+ NodeType*& getRoot();
+
+ /// @brief print the tree in a recursive way
+ void print(NodeType* root, int depth);
+
+private:
+
+ /// @brief construct a tree for a set of leaves from bottom -- very heavy way
+ void bottomup(const NodeVecIterator lbeg, const NodeVecIterator lend);
+
+ /// @brief construct a tree for a set of leaves from top
+ NodeType* topdown(const NodeVecIterator lbeg, const NodeVecIterator lend);
+
+ /// @brief compute the maximum height of a subtree rooted from a given node
+ size_t getMaxHeight(NodeType* node) const;
+
+ /// @brief compute the maximum depth of a subtree rooted from a given node
+ void getMaxDepth(NodeType* node, size_t depth, size_t& max_depth) const;
+
+ /// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a topdown manner.
+ /// During construction, first compute the best split axis as the axis along with the longest AABB edge.
+ /// Then compute the median of all nodes' center projection onto the axis and using it as the split threshold.
+ NodeType* topdown_0(const NodeVecIterator lbeg, const NodeVecIterator lend);
+
+ /// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a topdown manner.
+ /// During construction, first compute the best split thresholds for different axes as the average of all nodes' center.
+ /// Then choose the split axis as the axis whose threshold can divide the nodes into two parts with almost similar size.
+ /// This construction is more expensive then topdown_0, but also can provide tree with better quality.
+ NodeType* topdown_1(const NodeVecIterator lbeg, const NodeVecIterator lend);
+
+ /// @brief init tree from leaves in the topdown manner (topdown_0 or topdown_1)
+ void init_0(std::vector<NodeType*>& leaves);
+
+ /// @brief init tree from leaves using morton code. It uses morton_0, i.e., for nodes which is of depth more than the maximum bits of the morton code,
+ /// we use bottomup method to construct the subtree, which is slow but can construct tree with high quality.
+ void init_1(std::vector<NodeType*>& leaves);
+
+ /// @brief init tree from leaves using morton code. It uses morton_0, i.e., for nodes which is of depth more than the maximum bits of the morton code,
+ /// we split the leaves into two parts with the same size simply using the node index.
+ void init_2(std::vector<NodeType*>& leaves);
+
+ /// @brief init tree from leaves using morton code. It uses morton_2, i.e., for all nodes, we simply divide the leaves into parts with the same size simply using the node index.
+ void init_3(std::vector<NodeType*>& leaves);
+
+ NodeType* mortonRecurse_0(const NodeVecIterator lbeg, const NodeVecIterator lend, const FCL_UINT32& split, int bits);
+
+ NodeType* mortonRecurse_1(const NodeVecIterator lbeg, const NodeVecIterator lend, const FCL_UINT32& split, int bits);
+
+ NodeType* mortonRecurse_2(const NodeVecIterator lbeg, const NodeVecIterator lend);
+
+ /// @brief update one leaf node's bounding volume
+ void update_(NodeType* leaf, const BV& bv);
+
+ /// @brief sort node n and its parent according to their memory position
+ NodeType* sort(NodeType* n, NodeType*& r);
+
+ /// @brief Insert a leaf node and also update its ancestors
+ void insertLeaf(NodeType* root, NodeType* leaf);
+
+ /// @brief Remove a leaf. The leaf node itself is not deleted yet, but all the unnecessary internal nodes are deleted.
+ /// return the node with the smallest depth and is influenced by the remove operation
+ NodeType* removeLeaf(NodeType* leaf);
+
+ /// @brief Delete all internal nodes and return all leaves nodes with given depth from root
+ void fetchLeaves(NodeType* root, std::vector<NodeType*>& leaves, int depth = -1);
+
+ static size_t indexOf(NodeType* node);
+
+ /// @brief create one node (leaf or internal)
+ NodeType* createNode(NodeType* parent,
+ const BV& bv,
+ void* data);
+
+ NodeType* createNode(NodeType* parent,
+ const BV& bv1,
+ const BV& bv2,
+ void* data);
+
+ NodeType* createNode(NodeType* parent,
+ void* data);
+
+ void deleteNode(NodeType* node);
+
+ void recurseDeleteNode(NodeType* node);
+
+ void recurseRefit(NodeType* node);
+
+ static BV bounds(const std::vector<NodeType*>& leaves);
+
+ static BV bounds(const NodeVecIterator lbeg, const NodeVecIterator lend);
+
+protected:
+ NodeType* root_node;
+
+ size_t n_leaves;
+
+ unsigned int opath;
+
+ /// This is a one NodeType cache, the reason is that we need to remove a node and then add it again frequently.
+ NodeType* free_node;
+
+ int max_lookahead_level;
+
+public:
+ /// @brief decide which topdown algorithm to use
+ int topdown_level;
+
+ /// @brief decide the depth to use expensive bottom-up algorithm
+ int bu_threshold;
+};
+
+
+template<>
+bool HierarchyTree<AABB>::update(NodeBase<AABB>* leaf, const AABB& bv_, const Vec3f& vel, FCL_REAL margin);
+
+template<>
+bool HierarchyTree<AABB>::update(NodeBase<AABB>* leaf, const AABB& bv_, const Vec3f& vel);
+
+
+namespace implementation_array
+{
+
+template<typename BV>
+struct NodeBase
+{
+ BV bv;
+
+ union
+ {
+ size_t parent;
+ size_t next;
+ };
+
+ union
+ {
+ size_t children[2];
+ void* data;
+ };
+
+ FCL_UINT32 code;
+
+ bool isLeaf() const { return (children[1] == (size_t)(-1)); }
+ bool isInternal() const { return !isLeaf(); }
+};
+
+
+
+/// @brief Functor comparing two nodes
+template<typename BV>
+struct nodeBaseLess
+{
+ nodeBaseLess(const NodeBase<BV>* nodes_, size_t d_) : nodes(nodes_),
+ d(d_)
+ {}
+
+ bool operator() (size_t i, size_t j) const
+ {
+ if(nodes[i].bv.center()[d] < nodes[j].bv.center()[d])
+ return true;
+ return false;
+ }
+
+private:
+
+ /// @brief the nodes array
+ const NodeBase<BV>* nodes;
+
+ /// @brief the dimension to compare
+ size_t d;
+};
+
+
+/// @brief select the node from node1 and node2 which is close to the query-th node in the nodes. 0 for node1 and 1 for node2.
+template<typename BV>
+size_t select(size_t query, size_t node1, size_t node2, NodeBase<BV>* nodes)
+{
+ return 0;
+}
+
+template<>
+size_t select(size_t query, size_t node1, size_t node2, NodeBase<AABB>* nodes);
+
+/// @brief select the node from node1 and node2 which is close to the query AABB. 0 for node1 and 1 for node2.
+template<typename BV>
+size_t select(const BV& query, size_t node1, size_t node2, NodeBase<BV>* nodes)
+{
+ return 0;
+}
+
+template<>
+size_t select(const AABB& query, size_t node1, size_t node2, NodeBase<AABB>* nodes);
+
+/// @brief Class for hierarchy tree structure
+template<typename BV>
+class HierarchyTree
+{
+ typedef NodeBase<BV> NodeType;
+
+ struct SortByMorton
+ {
+ bool operator() (size_t a, size_t b) const
+ {
+ if((a != NULL_NODE) && (b != NULL_NODE))
+ return nodes[a].code < nodes[b].code;
+ else if(a == NULL_NODE)
+ return split < nodes[b].code;
+ else if(b == NULL_NODE)
+ return nodes[a].code < split;
+
+ return false;
+ }
+
+ NodeType* nodes;
+ FCL_UINT32 split;
+ };
+
+public:
+ /// @brief Create hierarchy tree with suitable setting.
+ /// bu_threshold decides the height of tree node to start bottom-up construction / optimization;
+ /// topdown_level decides different methods to construct tree in topdown manner.
+ /// lower level method constructs tree with better quality but is slower.
+ HierarchyTree(int bu_threshold_ = 16, int topdown_level_ = 0);
+
+ ~HierarchyTree();
+
+ /// @brief Initialize the tree by a set of leaves using algorithm with a given level.
+ void init(NodeType* leaves, int n_leaves_, int level = 0);
+
+ /// @brief Initialize the tree by a set of leaves using algorithm with a given level.
+ size_t insert(const BV& bv, void* data);
+
+ /// @brief Remove a leaf node
+ void remove(size_t leaf);
+
+ /// @brief Clear the tree
+ void clear();
+
+ /// @brief Whether the tree is empty
+ bool empty() const;
+
+ /// @brief update one leaf node
+ void update(size_t leaf, int lookahead_level = -1);
+
+ /// @brief update the tree when the bounding volume of a given leaf has changed
+ bool update(size_t leaf, const BV& bv);
+
+ /// @brief update one leaf's bounding volume, with prediction
+ bool update(size_t leaf, const BV& bv, const Vec3f& vel, FCL_REAL margin);
+
+ /// @brief update one leaf's bounding volume, with prediction
+ bool update(size_t leaf, const BV& bv, const Vec3f& vel);
+
+ /// @brief get the max height of the tree
+ size_t getMaxHeight() const;
+
+ /// @brief get the max depth of the tree
+ size_t getMaxDepth() const;
+
+ /// @brief balance the tree from bottom
+ void balanceBottomup();
+
+ /// @brief balance the tree from top
+ void balanceTopdown();
+
+ /// @brief balance the tree in an incremental way
+ void balanceIncremental(int iterations);
+
+ /// @brief refit the tree, i.e., when the leaf nodes' bounding volumes change, update the entire tree in a bottom-up manner
+ void refit();
+
+ /// @brief extract all the leaves of the tree
+ void extractLeaves(size_t root, NodeType*& leaves) const;
+
+ /// @brief number of leaves in the tree
+ size_t size() const;
+
+ /// @brief get the root of the tree
+ size_t getRoot() const;
+
+ /// @brief get the pointer to the nodes array
+ NodeType* getNodes() const;
+
+ /// @brief print the tree in a recursive way
+ void print(size_t root, int depth);
+
+private:
+
+ /// @brief construct a tree for a set of leaves from bottom -- very heavy way
+ void bottomup(size_t* lbeg, size_t* lend);
+
+ /// @brief construct a tree for a set of leaves from top
+ size_t topdown(size_t* lbeg, size_t* lend);
+
+ /// @brief compute the maximum height of a subtree rooted from a given node
+ size_t getMaxHeight(size_t node) const;
+
+ /// @brief compute the maximum depth of a subtree rooted from a given node
+ void getMaxDepth(size_t node, size_t depth, size_t& max_depth) const;
+
+ /// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a topdown manner.
+ /// During construction, first compute the best split axis as the axis along with the longest AABB edge.
+ /// Then compute the median of all nodes' center projection onto the axis and using it as the split threshold.
+ size_t topdown_0(size_t* lbeg, size_t* lend);
+
+ /// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a topdown manner.
+ /// During construction, first compute the best split thresholds for different axes as the average of all nodes' center.
+ /// Then choose the split axis as the axis whose threshold can divide the nodes into two parts with almost similar size.
+ /// This construction is more expensive then topdown_0, but also can provide tree with better quality.
+ size_t topdown_1(size_t* lbeg, size_t* lend);
+
+ /// @brief init tree from leaves in the topdown manner (topdown_0 or topdown_1)
+ void init_0(NodeType* leaves, int n_leaves_);
+
+ /// @brief init tree from leaves using morton code. It uses morton_0, i.e., for nodes which is of depth more than the maximum bits of the morton code,
+ /// we use bottomup method to construct the subtree, which is slow but can construct tree with high quality.
+ void init_1(NodeType* leaves, int n_leaves_);
+
+ /// @brief init tree from leaves using morton code. It uses morton_0, i.e., for nodes which is of depth more than the maximum bits of the morton code,
+ /// we split the leaves into two parts with the same size simply using the node index.
+ void init_2(NodeType* leaves, int n_leaves_);
+
+ /// @brief init tree from leaves using morton code. It uses morton_2, i.e., for all nodes, we simply divide the leaves into parts with the same size simply using the node index.
+ void init_3(NodeType* leaves, int n_leaves_);
+
+ size_t mortonRecurse_0(size_t* lbeg, size_t* lend, const FCL_UINT32& split, int bits);
+
+ size_t mortonRecurse_1(size_t* lbeg, size_t* lend, const FCL_UINT32& split, int bits);
+
+ size_t mortonRecurse_2(size_t* lbeg, size_t* lend);
+
+ /// @brief update one leaf node's bounding volume
+ void update_(size_t leaf, const BV& bv);
+
+ /// @brief Insert a leaf node and also update its ancestors
+ void insertLeaf(size_t root, size_t leaf);
+
+ /// @brief Remove a leaf. The leaf node itself is not deleted yet, but all the unnecessary internal nodes are deleted.
+ /// return the node with the smallest depth and is influenced by the remove operation
+ size_t removeLeaf(size_t leaf);
+
+ /// @brief Delete all internal nodes and return all leaves nodes with given depth from root
+ void fetchLeaves(size_t root, NodeType*& leaves, int depth = -1);
+
+ size_t indexOf(size_t node);
+
+ size_t allocateNode();
+
+ /// @brief create one node (leaf or internal)
+ size_t createNode(size_t parent,
+ const BV& bv1,
+ const BV& bv2,
+ void* data);
+
+ size_t createNode(size_t parent,
+ const BV& bv,
+ void* data);
+
+ size_t createNode(size_t parent,
+ void* data);
+
+ void deleteNode(size_t node);
+
+ void recurseRefit(size_t node);
+
+protected:
+ size_t root_node;
+ NodeType* nodes;
+ size_t n_nodes;
+ size_t n_nodes_alloc;
+
+ size_t n_leaves;
+ size_t freelist;
+ unsigned int opath;
+
+ int max_lookahead_level;
+
+public:
+ /// @brief decide which topdown algorithm to use
+ int topdown_level;
+
+ /// @brief decide the depth to use expensive bottom-up algorithm
+ int bu_threshold;
+
+public:
+ static const size_t NULL_NODE = -1;
+};
+
+template<typename BV>
+const size_t HierarchyTree<BV>::NULL_NODE;
+
+}
+
+}
+
+
+#include "fcl/broadphase/hierarchy_tree.hxx"
+
+
+#endif
diff --git a/trunk/fcl/include/fcl/broadphase/hierarchy_tree.hxx b/trunk/fcl/include/fcl/broadphase/hierarchy_tree.hxx
new file mode 100644
index 0000000000000000000000000000000000000000..05e01cde22a9bd697f7dfb34919c3b410e8f5c97
--- /dev/null
+++ b/trunk/fcl/include/fcl/broadphase/hierarchy_tree.hxx
@@ -0,0 +1,1876 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * 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 Jia Pan */
+
+namespace fcl
+{
+
+template<typename BV>
+HierarchyTree<BV>::HierarchyTree(int bu_threshold_, int topdown_level_)
+{
+ root_node = NULL;
+ n_leaves = 0;
+ free_node = NULL;
+ max_lookahead_level = -1;
+ opath = 0;
+ bu_threshold = bu_threshold_;
+ topdown_level = topdown_level_;
+}
+
+template<typename BV>
+HierarchyTree<BV>::~HierarchyTree()
+{
+ clear();
+}
+
+template<typename BV>
+void HierarchyTree<BV>::init(std::vector<NodeType*>& leaves, int level)
+{
+ switch(level)
+ {
+ case 0:
+ init_0(leaves);
+ break;
+ case 1:
+ init_1(leaves);
+ break;
+ case 2:
+ init_2(leaves);
+ break;
+ case 3:
+ init_3(leaves);
+ break;
+ default:
+ init_0(leaves);
+ }
+}
+
+template<typename BV>
+typename HierarchyTree<BV>::NodeType* HierarchyTree<BV>::insert(const BV& bv, void* data)
+{
+ NodeType* leaf = createNode(NULL, bv, data);
+ insertLeaf(root_node, leaf);
+ ++n_leaves;
+ return leaf;
+}
+
+template<typename BV>
+void HierarchyTree<BV>::remove(NodeType* leaf)
+{
+ removeLeaf(leaf);
+ deleteNode(leaf);
+ --n_leaves;
+}
+
+template<typename BV>
+void HierarchyTree<BV>::clear()
+{
+ if(root_node)
+ recurseDeleteNode(root_node);
+ n_leaves = 0;
+ delete free_node;
+ free_node = NULL;
+ max_lookahead_level = -1;
+ opath = 0;
+}
+
+template<typename BV>
+bool HierarchyTree<BV>::empty() const
+{
+ return (NULL == root_node);
+}
+
+template<typename BV>
+void HierarchyTree<BV>::update(NodeType* leaf, int lookahead_level)
+{
+ NodeType* root = removeLeaf(leaf);
+ if(root)
+ {
+ if(lookahead_level > 0)
+ {
+ for(int i = 0; (i < lookahead_level) && root->parent; ++i)
+ root = root->parent;
+ }
+ else
+ root = root_node;
+ }
+ insertLeaf(root, leaf);
+}
+
+template<typename BV>
+bool HierarchyTree<BV>::update(NodeType* leaf, const BV& bv)
+{
+ if(leaf->bv.contain(bv)) return false;
+ update_(leaf, bv);
+ return true;
+}
+
+template<typename BV>
+bool HierarchyTree<BV>::update(NodeType* leaf, const BV& bv, const Vec3f& vel, FCL_REAL margin)
+{
+ if(leaf->bv.contain(bv)) return false;
+ update_(leaf, bv);
+ return true;
+}
+
+template<typename BV>
+bool HierarchyTree<BV>::update(NodeType* leaf, const BV& bv, const Vec3f& vel)
+{
+ if(leaf->bv.contain(bv)) return false;
+ update_(leaf, bv);
+ return true;
+}
+
+template<typename BV>
+size_t HierarchyTree<BV>::getMaxHeight() const
+{
+ if(!root_node)
+ return 0;
+ return getMaxHeight(root_node);
+}
+
+template<typename BV>
+size_t HierarchyTree<BV>::getMaxDepth() const
+{
+ if(!root_node) return 0;
+
+ size_t max_depth;
+ getMaxDepth(root_node, 0, max_depth);
+ return max_depth;
+}
+
+template<typename BV>
+void HierarchyTree<BV>::balanceBottomup()
+{
+ if(root_node)
+ {
+ std::vector<NodeType*> leaves;
+ leaves.reserve(n_leaves);
+ fetchLeaves(root_node, leaves);
+ bottomup(leaves.begin(), leaves.end());
+ root_node = leaves[0];
+ }
+}
+
+template<typename BV>
+void HierarchyTree<BV>::balanceTopdown()
+{
+ if(root_node)
+ {
+ std::vector<NodeType*> leaves;
+ leaves.reserve(n_leaves);
+ fetchLeaves(root_node, leaves);
+ root_node = topdown(leaves.begin(), leaves.end());
+ }
+}
+
+template<typename BV>
+void HierarchyTree<BV>::balanceIncremental(int iterations)
+{
+ if(iterations < 0) iterations = n_leaves;
+ if(root_node && (iterations > 0))
+ {
+ for(int i = 0; i < iterations; ++i)
+ {
+ NodeType* node = root_node;
+ unsigned int bit = 0;
+ while(!node->isLeaf())
+ {
+ node = sort(node, root_node)->children[(opath>>bit)&1];
+ bit = (bit+1)&(sizeof(unsigned int) * 8-1);
+ }
+ update(node);
+ ++opath;
+ }
+ }
+}
+
+template<typename BV>
+void HierarchyTree<BV>::refit()
+{
+ if(root_node)
+ recurseRefit(root_node);
+}
+
+template<typename BV>
+void HierarchyTree<BV>::extractLeaves(const NodeType* root, std::vector<NodeType*>& leaves) const
+{
+ if(!root->isLeaf())
+ {
+ extractLeaves(root->children[0], leaves);
+ extractLeaves(root->children[1], leaves);
+ }
+ else
+ leaves.push_back(root);
+}
+
+template<typename BV>
+size_t HierarchyTree<BV>::size() const
+{
+ return n_leaves;
+}
+
+template<typename BV>
+typename HierarchyTree<BV>::NodeType* HierarchyTree<BV>::getRoot() const
+{
+ return root_node;
+}
+
+template<typename BV>
+typename HierarchyTree<BV>::NodeType*& HierarchyTree<BV>::getRoot()
+{
+ return root_node;
+}
+
+template<typename BV>
+void HierarchyTree<BV>::print(NodeType* root, int depth)
+{
+ for(int i = 0; i < depth; ++i)
+ std::cout << " ";
+ std::cout << " (" << root->bv.min_[0] << ", " << root->bv.min_[1] << ", " << root->bv.min_[2] << "; " << root->bv.max_[0] << ", " << root->bv.max_[1] << ", " << root->bv.max_[2] << ")" << std::endl;
+ if(root->isLeaf())
+ {
+ }
+ else
+ {
+ print(root->children[0], depth+1);
+ print(root->children[1], depth+1);
+ }
+}
+
+
+
+template<typename BV>
+void HierarchyTree<BV>::bottomup(const NodeVecIterator lbeg, const NodeVecIterator lend)
+{
+ NodeVecIterator lcur_end = lend;
+ while(lbeg < lcur_end - 1)
+ {
+ NodeVecIterator min_it1, min_it2;
+ FCL_REAL min_size = std::numeric_limits<FCL_REAL>::max();
+ for(NodeVecIterator it1 = lbeg; it1 < lcur_end; ++it1)
+ {
+ for(NodeVecIterator it2 = it1 + 1; it2 < lcur_end; ++it2)
+ {
+ FCL_REAL cur_size = ((*it1)->bv + (*it2)->bv).size();
+ if(cur_size < min_size)
+ {
+ min_size = cur_size;
+ min_it1 = it1;
+ min_it2 = it2;
+ }
+ }
+ }
+
+ NodeType* n[2] = {*min_it1, *min_it2};
+ NodeType* p = createNode(NULL, n[0]->bv, n[1]->bv, NULL);
+ p->children[0] = n[0];
+ p->children[1] = n[1];
+ n[0]->parent = p;
+ n[1]->parent = p;
+ *min_it1 = p;
+ NodeType* tmp = *min_it2;
+ lcur_end--;
+ *min_it2 = *lcur_end;
+ *lcur_end = tmp;
+ }
+}
+
+template<typename BV>
+typename HierarchyTree<BV>::NodeType* HierarchyTree<BV>::topdown(const NodeVecIterator lbeg, const NodeVecIterator lend)
+{
+ switch(topdown_level)
+ {
+ case 0:
+ return topdown_0(lbeg, lend);
+ break;
+ case 1:
+ return topdown_1(lbeg, lend);
+ break;
+ default:
+ return topdown_0(lbeg, lend);
+ }
+}
+
+template<typename BV>
+size_t HierarchyTree<BV>::getMaxHeight(NodeType* node) const
+{
+ if(!node->isLeaf())
+ {
+ size_t height1 = getMaxHeight(node->children[0]);
+ size_t height2 = getMaxHeight(node->children[1]);
+ return std::max(height1, height2) + 1;
+ }
+ else
+ return 0;
+}
+
+template<typename BV>
+void HierarchyTree<BV>::getMaxDepth(NodeType* node, size_t depth, size_t& max_depth) const
+{
+ if(!node->isLeaf())
+ {
+ getMaxDepth(node->children[0], depth+1, max_depth);
+ getMaxDepth(node->children[1], depth+1, max_depth);
+ }
+ else
+ max_depth = std::max(max_depth, depth);
+}
+
+template<typename BV>
+typename HierarchyTree<BV>::NodeType* HierarchyTree<BV>::topdown_0(const NodeVecIterator lbeg, const NodeVecIterator lend)
+{
+ int num_leaves = lend - lbeg;
+ if(num_leaves > 1)
+ {
+ if(num_leaves > bu_threshold)
+ {
+ BV vol = (*lbeg)->bv;
+ for(NodeVecIterator it = lbeg + 1; it < lend; ++it)
+ vol += (*it)->bv;
+
+ int best_axis = 0;
+ FCL_REAL extent[3] = {vol.width(), vol.height(), vol.depth()};
+ if(extent[1] > extent[0]) best_axis = 1;
+ if(extent[2] > extent[best_axis]) best_axis = 2;
+
+ // compute median
+ NodeVecIterator lcenter = lbeg + num_leaves / 2;
+ std::nth_element(lbeg, lcenter, lend, boost::bind(&nodeBaseLess<BV>, _1, _2, boost::ref(best_axis)));
+
+ NodeType* node = createNode(NULL, vol, NULL);
+ node->children[0] = topdown_0(lbeg, lcenter);
+ node->children[1] = topdown_0(lcenter, lend);
+ node->children[0]->parent = node;
+ node->children[1]->parent = node;
+ return node;
+ }
+ else
+ {
+ bottomup(lbeg, lend);
+ return *lbeg;
+ }
+ }
+ return *lbeg;
+}
+
+template<typename BV>
+typename HierarchyTree<BV>::NodeType* HierarchyTree<BV>::topdown_1(const NodeVecIterator lbeg, const NodeVecIterator lend)
+{
+ int num_leaves = lend - lbeg;
+ if(num_leaves > 1)
+ {
+ if(num_leaves > bu_threshold)
+ {
+ Vec3f split_p = (*lbeg)->bv.center();
+ BV vol = (*lbeg)->bv;
+ NodeVecIterator it;
+ for(it = lbeg + 1; it < lend; ++it)
+ {
+ split_p += (*it)->bv.center();
+ vol += (*it)->bv;
+ }
+ split_p /= (FCL_REAL)(num_leaves);
+ int best_axis = -1;
+ int bestmidp = num_leaves;
+ int splitcount[3][2] = {{0,0}, {0,0}, {0,0}};
+ for(it = lbeg; it < lend; ++it)
+ {
+ Vec3f x = (*it)->bv.center() - split_p;
+ for(size_t j = 0; j < 3; ++j)
+ ++splitcount[j][x[j] > 0 ? 1 : 0];
+ }
+
+ for(size_t i = 0; i < 3; ++i)
+ {
+ if((splitcount[i][0] > 0) && (splitcount[i][1] > 0))
+ {
+ int midp = std::abs(splitcount[i][0] - splitcount[i][1]);
+ if(midp < bestmidp)
+ {
+ best_axis = i;
+ bestmidp = midp;
+ }
+ }
+ }
+
+ if(best_axis < 0) best_axis = 0;
+
+ FCL_REAL split_value = split_p[best_axis];
+ NodeVecIterator lcenter = lbeg;
+ for(it = lbeg; it < lend; ++it)
+ {
+ if((*it)->bv.center()[best_axis] < split_value)
+ {
+ NodeType* temp = *it;
+ *it = *lcenter;
+ *lcenter = temp;
+ ++lcenter;
+ }
+ }
+
+ NodeType* node = createNode(NULL, vol, NULL);
+ node->children[0] = topdown_1(lbeg, lcenter);
+ node->children[1] = topdown_1(lcenter, lend);
+ node->children[0]->parent = node;
+ node->children[1]->parent = node;
+ return node;
+ }
+ else
+ {
+ bottomup(lbeg, lend);
+ return *lbeg;
+ }
+ }
+ return *lbeg;
+}
+
+template<typename BV>
+void HierarchyTree<BV>::init_0(std::vector<NodeType*>& leaves)
+{
+ clear();
+ root_node = topdown(leaves.begin(), leaves.end());
+ n_leaves = leaves.size();
+ max_lookahead_level = -1;
+ opath = 0;
+}
+
+template<typename BV>
+void HierarchyTree<BV>::init_1(std::vector<NodeType*>& leaves)
+{
+ clear();
+
+ BV bound_bv;
+ if(leaves.size() > 0)
+ bound_bv = leaves[0]->bv;
+ for(size_t i = 1; i < leaves.size(); ++i)
+ bound_bv += leaves[i]->bv;
+
+ morton_functor<FCL_UINT32> coder(bound_bv);
+ for(size_t i = 0; i < leaves.size(); ++i)
+ leaves[i]->code = coder(leaves[i]->bv.center());
+
+ std::sort(leaves.begin(), leaves.end(), SortByMorton());
+
+ root_node = mortonRecurse_0(leaves.begin(), leaves.end(), (1 << (coder.bits()-1)), coder.bits()-1);
+
+ refit();
+ n_leaves = leaves.size();
+ max_lookahead_level = -1;
+ opath = 0;
+}
+
+template<typename BV>
+void HierarchyTree<BV>::init_2(std::vector<NodeType*>& leaves)
+{
+ clear();
+
+ BV bound_bv;
+ if(leaves.size() > 0)
+ bound_bv = leaves[0]->bv;
+ for(size_t i = 1; i < leaves.size(); ++i)
+ bound_bv += leaves[i]->bv;
+
+ morton_functor<FCL_UINT32> coder(bound_bv);
+ for(size_t i = 0; i < leaves.size(); ++i)
+ leaves[i]->code = coder(leaves[i]->bv.center());
+
+ std::sort(leaves.begin(), leaves.end(), SortByMorton());
+
+ root_node = mortonRecurse_1(leaves.begin(), leaves.end(), (1 << (coder.bits()-1)), coder.bits()-1);
+
+ refit();
+ n_leaves = leaves.size();
+ max_lookahead_level = -1;
+ opath = 0;
+}
+
+template<typename BV>
+void HierarchyTree<BV>::init_3(std::vector<NodeType*>& leaves)
+{
+ clear();
+
+ BV bound_bv;
+ if(leaves.size() > 0)
+ bound_bv = leaves[0]->bv;
+ for(size_t i = 1; i < leaves.size(); ++i)
+ bound_bv += leaves[i]->bv;
+
+ morton_functor<FCL_UINT32> coder(bound_bv);
+ for(size_t i = 0; i < leaves.size(); ++i)
+ leaves[i]->code = coder(leaves[i]->bv.center());
+
+ std::sort(leaves.begin(), leaves.end(), SortByMorton());
+
+ root_node = mortonRecurse_2(leaves.begin(), leaves.end());
+
+ refit();
+ n_leaves = leaves.size();
+ max_lookahead_level = -1;
+ opath = 0;
+}
+
+template<typename BV>
+typename HierarchyTree<BV>::NodeType* HierarchyTree<BV>::mortonRecurse_0(const NodeVecIterator lbeg, const NodeVecIterator lend, const FCL_UINT32& split, int bits)
+{
+ int num_leaves = lend - lbeg;
+ if(num_leaves > 1)
+ {
+ if(bits > 0)
+ {
+ NodeType dummy;
+ dummy.code = split;
+ NodeVecIterator lcenter = std::lower_bound(lbeg, lend, &dummy, SortByMorton());
+
+ if(lcenter == lbeg)
+ {
+ FCL_UINT32 split2 = split | (1 << (bits - 1));
+ return mortonRecurse_0(lbeg, lend, split2, bits - 1);
+ }
+ else if(lcenter == lend)
+ {
+ FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
+ return mortonRecurse_0(lbeg, lend, split1, bits - 1);
+ }
+ else
+ {
+ FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
+ FCL_UINT32 split2 = split | (1 << (bits - 1));
+
+ NodeType* child1 = mortonRecurse_0(lbeg, lcenter, split1, bits - 1);
+ NodeType* child2 = mortonRecurse_0(lcenter, lend, split2, bits - 1);
+ NodeType* node = createNode(NULL, NULL);
+ node->children[0] = child1;
+ node->children[1] = child2;
+ child1->parent = node;
+ child2->parent = node;
+ return node;
+ }
+ }
+ else
+ {
+ NodeType* node = topdown(lbeg, lend);
+ return node;
+ }
+ }
+ else
+ return *lbeg;
+}
+
+template<typename BV>
+typename HierarchyTree<BV>::NodeType* HierarchyTree<BV>::mortonRecurse_1(const NodeVecIterator lbeg, const NodeVecIterator lend, const FCL_UINT32& split, int bits)
+{
+ int num_leaves = lend - lbeg;
+ if(num_leaves > 1)
+ {
+ if(bits > 0)
+ {
+ NodeType dummy;
+ dummy.code = split;
+ NodeVecIterator lcenter = std::lower_bound(lbeg, lend, &dummy, SortByMorton());
+
+ if(lcenter == lbeg)
+ {
+ FCL_UINT32 split2 = split | (1 << (bits - 1));
+ return mortonRecurse_1(lbeg, lend, split2, bits - 1);
+ }
+ else if(lcenter == lend)
+ {
+ FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
+ return mortonRecurse_1(lbeg, lend, split1, bits - 1);
+ }
+ else
+ {
+ FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
+ FCL_UINT32 split2 = split | (1 << (bits - 1));
+
+ NodeType* child1 = mortonRecurse_1(lbeg, lcenter, split1, bits - 1);
+ NodeType* child2 = mortonRecurse_1(lcenter, lend, split2, bits - 1);
+ NodeType* node = createNode(NULL, NULL);
+ node->children[0] = child1;
+ node->children[1] = child2;
+ child1->parent = node;
+ child2->parent = node;
+ return node;
+ }
+ }
+ else
+ {
+ NodeType* child1 = mortonRecurse_1(lbeg, lbeg + num_leaves / 2, 0, bits - 1);
+ NodeType* child2 = mortonRecurse_1(lbeg + num_leaves / 2, lend, 0, bits - 1);
+ NodeType* node = createNode(NULL, NULL);
+ node->children[0] = child1;
+ node->children[1] = child2;
+ child1->parent = node;
+ child2->parent = node;
+ return node;
+ }
+ }
+ else
+ return *lbeg;
+}
+
+template<typename BV>
+typename HierarchyTree<BV>::NodeType* HierarchyTree<BV>::mortonRecurse_2(const NodeVecIterator lbeg, const NodeVecIterator lend)
+{
+ int num_leaves = lend - lbeg;
+ if(num_leaves > 1)
+ {
+ NodeType* child1 = mortonRecurse_2(lbeg, lbeg + num_leaves / 2);
+ NodeType* child2 = mortonRecurse_2(lbeg + num_leaves / 2, lend);
+ NodeType* node = createNode(NULL, NULL);
+ node->children[0] = child1;
+ node->children[1] = child2;
+ child1->parent = node;
+ child2->parent = node;
+ return node;
+ }
+ else
+ return *lbeg;
+}
+
+
+template<typename BV>
+void HierarchyTree<BV>::update_(NodeType* leaf, const BV& bv)
+{
+ NodeType* root = removeLeaf(leaf);
+ if(root)
+ {
+ if(max_lookahead_level >= 0)
+ {
+ for(int i = 0; (i < max_lookahead_level) && root->parent; ++i)
+ root = root->parent;
+ }
+ else
+ root = root_node;
+ }
+
+ leaf->bv = bv;
+ insertLeaf(root, leaf);
+}
+
+template<typename BV>
+typename HierarchyTree<BV>::NodeType* HierarchyTree<BV>::sort(NodeType* n, NodeType*& r)
+{
+ NodeType* p = n->parent;
+ if(p > n)
+ {
+ int i = indexOf(n);
+ int j = 1 - i;
+ NodeType* s = p->children[j];
+ NodeType* q = p->parent;
+ if(q) q->children[indexOf(p)] = n; else r = n;
+ s->parent = n;
+ p->parent = n;
+ n->parent = q;
+ p->children[0] = n->children[0];
+ p->children[1] = n->children[1];
+ n->children[0]->parent = p;
+ n->children[1]->parent = p;
+ n->children[i] = p;
+ n->children[j] = s;
+ std::swap(p->bv, n->bv);
+ return p;
+ }
+ return n;
+}
+
+template<typename BV>
+void HierarchyTree<BV>::insertLeaf(NodeType* root, NodeType* leaf)
+{
+ if(!root_node)
+ {
+ root_node = leaf;
+ leaf->parent = NULL;
+ }
+ else
+ {
+ if(!root->isLeaf())
+ {
+ do
+ {
+ root = root->children[select(*leaf, *(root->children[0]), *(root->children[1]))];
+ }
+ while(!root->isLeaf());
+ }
+
+ NodeType* prev = root->parent;
+ NodeType* node = createNode(prev, leaf->bv, root->bv, NULL);
+ if(prev)
+ {
+ prev->children[indexOf(root)] = node;
+ node->children[0] = root; root->parent = node;
+ node->children[1] = leaf; leaf->parent = node;
+ do
+ {
+ if(!prev->bv.contain(node->bv))
+ prev->bv = prev->children[0]->bv + prev->children[1]->bv;
+ else
+ break;
+ node = prev;
+ } while (NULL != (prev = node->parent));
+ }
+ else
+ {
+ node->children[0] = root; root->parent = node;
+ node->children[1] = leaf; leaf->parent = node;
+ root_node = node;
+ }
+ }
+}
+
+template<typename BV>
+typename HierarchyTree<BV>::NodeType* HierarchyTree<BV>::removeLeaf(NodeType* leaf)
+{
+ if(leaf == root_node)
+ {
+ root_node = NULL;
+ return NULL;
+ }
+ else
+ {
+ NodeType* parent = leaf->parent;
+ NodeType* prev = parent->parent;
+ NodeType* sibling = parent->children[1-indexOf(leaf)];
+ if(prev)
+ {
+ prev->children[indexOf(parent)] = sibling;
+ sibling->parent = prev;
+ deleteNode(parent);
+ while(prev)
+ {
+ BV new_bv = prev->children[0]->bv + prev->children[1]->bv;
+ if(!new_bv.equal(prev->bv))
+ {
+ prev->bv = new_bv;
+ prev = prev->parent;
+ }
+ else break;
+ }
+
+ return prev ? prev : root_node;
+ }
+ else
+ {
+ root_node = sibling;
+ sibling->parent = NULL;
+ deleteNode(parent);
+ return root_node;
+ }
+ }
+}
+
+template<typename BV>
+void HierarchyTree<BV>::fetchLeaves(NodeType* root, std::vector<NodeType*>& leaves, int depth)
+{
+ if((!root->isLeaf()) && depth)
+ {
+ fetchLeaves(root->children[0], leaves, depth-1);
+ fetchLeaves(root->children[1], leaves, depth-1);
+ deleteNode(root);
+ }
+ else
+ {
+ leaves.push_back(root);
+ }
+}
+
+
+template<typename BV>
+size_t HierarchyTree<BV>::indexOf(NodeType* node)
+{
+ // node cannot be NULL
+ return (node->parent->children[1] == node);
+}
+
+template<typename BV>
+typename HierarchyTree<BV>::NodeType* HierarchyTree<BV>::createNode(NodeType* parent,
+ const BV& bv,
+ void* data)
+{
+ NodeType* node = createNode(parent, data);
+ node->bv = bv;
+ return node;
+}
+
+template<typename BV>
+typename HierarchyTree<BV>::NodeType* HierarchyTree<BV>::createNode(NodeType* parent,
+ const BV& bv1,
+ const BV& bv2,
+ void* data)
+{
+ NodeType* node = createNode(parent, data);
+ node->bv = bv1 + bv2;
+ return node;
+}
+
+
+template<typename BV>
+typename HierarchyTree<BV>::NodeType* HierarchyTree<BV>::createNode(NodeType* parent, void* data)
+{
+ NodeType* node = NULL;
+ if(free_node)
+ {
+ node = free_node;
+ free_node = NULL;
+ }
+ else
+ node = new NodeType();
+ node->parent = parent;
+ node->data = data;
+ node->children[1] = 0;
+ return node;
+}
+
+template<typename BV>
+void HierarchyTree<BV>::deleteNode(NodeType* node)
+{
+ if(free_node != node)
+ {
+ delete free_node;
+ free_node = node;
+ }
+}
+
+template<typename BV>
+void HierarchyTree<BV>::recurseDeleteNode(NodeType* node)
+{
+ if(!node->isLeaf())
+ {
+ recurseDeleteNode(node->children[0]);
+ recurseDeleteNode(node->children[1]);
+ }
+
+ if(node == root_node) root_node = NULL;
+ deleteNode(node);
+}
+
+template<typename BV>
+void HierarchyTree<BV>::recurseRefit(NodeType* node)
+{
+ if(!node->isLeaf())
+ {
+ recurseRefit(node->children[0]);
+ recurseRefit(node->children[1]);
+ node->bv = node->children[0]->bv + node->children[1]->bv;
+ }
+ else
+ return;
+}
+
+template<typename BV>
+BV HierarchyTree<BV>::bounds(const std::vector<NodeType*>& leaves)
+{
+ if(leaves.size() == 0) return BV();
+ BV bv = leaves[0]->bv;
+ for(size_t i = 1; i < leaves.size(); ++i)
+ {
+ bv += leaves[i]->bv;
+ }
+
+ return bv;
+}
+
+template<typename BV>
+BV HierarchyTree<BV>::bounds(const NodeVecIterator lbeg, const NodeVecIterator lend)
+{
+ if(lbeg == lend) return BV();
+ BV bv = *lbeg;
+ for(NodeVecIterator it = lbeg + 1; it < lend; ++it)
+ {
+ bv += (*it)->bv;
+ }
+
+ return bv;
+}
+
+}
+
+
+namespace fcl
+{
+namespace implementation_array
+{
+
+template<typename BV>
+HierarchyTree<BV>::HierarchyTree(int bu_threshold_, int topdown_level_)
+{
+ root_node = NULL_NODE;
+ n_nodes = 0;
+ n_nodes_alloc = 16;
+ nodes = new NodeType[n_nodes_alloc];
+ for(size_t i = 0; i < n_nodes_alloc - 1; ++i)
+ nodes[i].next = i + 1;
+ nodes[n_nodes_alloc - 1].next = NULL_NODE;
+ n_leaves = 0;
+ freelist = 0;
+ opath = 0;
+ max_lookahead_level = -1;
+ bu_threshold = bu_threshold_;
+ topdown_level = topdown_level_;
+}
+
+template<typename BV>
+HierarchyTree<BV>::~HierarchyTree()
+{
+ delete [] nodes;
+}
+
+template<typename BV>
+void HierarchyTree<BV>::init(NodeType* leaves, int n_leaves_, int level)
+{
+ switch(level)
+ {
+ case 0:
+ init_0(leaves, n_leaves_);
+ break;
+ case 1:
+ init_1(leaves, n_leaves_);
+ break;
+ case 2:
+ init_2(leaves, n_leaves_);
+ break;
+ case 3:
+ init_3(leaves, n_leaves_);
+ break;
+ default:
+ init_0(leaves, n_leaves_);
+ }
+}
+
+template<typename BV>
+void HierarchyTree<BV>::init_0(NodeType* leaves, int n_leaves_)
+{
+ clear();
+
+ n_leaves = n_leaves_;
+ root_node = NULL_NODE;
+ nodes = new NodeType[n_leaves * 2];
+ memcpy(nodes, leaves, sizeof(NodeType) * n_leaves);
+ freelist = n_leaves;
+ n_nodes = n_leaves;
+ n_nodes_alloc = 2 * n_leaves;
+ for(size_t i = n_leaves; i < n_nodes_alloc; ++i)
+ nodes[i].next = i + 1;
+ nodes[n_nodes_alloc - 1].next = NULL_NODE;
+
+ size_t* ids = new size_t[n_leaves];
+ for(size_t i = 0; i < n_leaves; ++i)
+ ids[i] = i;
+
+ root_node = topdown(ids, ids + n_leaves);
+ delete [] ids;
+
+ opath = 0;
+ max_lookahead_level = -1;
+}
+
+template<typename BV>
+void HierarchyTree<BV>::init_1(NodeType* leaves, int n_leaves_)
+{
+ clear();
+
+ n_leaves = n_leaves_;
+ root_node = NULL_NODE;
+ nodes = new NodeType[n_leaves * 2];
+ memcpy(nodes, leaves, sizeof(NodeType) * n_leaves);
+ freelist = n_leaves;
+ n_nodes = n_leaves;
+ n_nodes_alloc = 2 * n_leaves;
+ for(size_t i = n_leaves; i < n_nodes_alloc; ++i)
+ nodes[i].next = i + 1;
+ nodes[n_nodes_alloc - 1].next = NULL_NODE;
+
+
+ BV bound_bv;
+ if(n_leaves > 0)
+ bound_bv = nodes[0].bv;
+ for(size_t i = 1; i < n_leaves; ++i)
+ bound_bv += nodes[i].bv;
+
+ morton_functor<FCL_UINT32> coder(bound_bv);
+ for(size_t i = 0; i < n_leaves; ++i)
+ nodes[i].code = coder(nodes[i].bv.center());
+
+
+ size_t* ids = new size_t[n_leaves];
+ for(size_t i = 0; i < n_leaves; ++i)
+ ids[i] = i;
+
+ SortByMorton comp;
+ comp.nodes = nodes;
+ std::sort(ids, ids + n_leaves, comp);
+ root_node = mortonRecurse_0(ids, ids + n_leaves, (1 << coder.bits()-1), coder.bits()-1);
+ delete [] ids;
+
+ refit();
+
+ opath = 0;
+ max_lookahead_level = -1;
+}
+
+template<typename BV>
+void HierarchyTree<BV>::init_2(NodeType* leaves, int n_leaves_)
+{
+ clear();
+
+ n_leaves = n_leaves_;
+ root_node = NULL_NODE;
+ nodes = new NodeType[n_leaves * 2];
+ memcpy(nodes, leaves, sizeof(NodeType) * n_leaves);
+ freelist = n_leaves;
+ n_nodes = n_leaves;
+ n_nodes_alloc = 2 * n_leaves;
+ for(size_t i = n_leaves; i < n_nodes_alloc; ++i)
+ nodes[i].next = i + 1;
+ nodes[n_nodes_alloc - 1].next = NULL_NODE;
+
+
+ BV bound_bv;
+ if(n_leaves > 0)
+ bound_bv = nodes[0].bv;
+ for(size_t i = 1; i < n_leaves; ++i)
+ bound_bv += nodes[i].bv;
+
+ morton_functor<FCL_UINT32> coder(bound_bv);
+ for(size_t i = 0; i < n_leaves; ++i)
+ nodes[i].code = coder(nodes[i].bv.center());
+
+
+ size_t* ids = new size_t[n_leaves];
+ for(size_t i = 0; i < n_leaves; ++i)
+ ids[i] = i;
+
+ SortByMorton comp;
+ comp.nodes = nodes;
+ std::sort(ids, ids + n_leaves, comp);
+ root_node = mortonRecurse_1(ids, ids + n_leaves, (1 << coder.bits()-1), coder.bits()-1);
+ delete [] ids;
+
+ refit();
+
+ opath = 0;
+ max_lookahead_level = -1;
+}
+
+template<typename BV>
+void HierarchyTree<BV>::init_3(NodeType* leaves, int n_leaves_)
+{
+ clear();
+
+ n_leaves = n_leaves_;
+ root_node = NULL_NODE;
+ nodes = new NodeType[n_leaves * 2];
+ memcpy(nodes, leaves, sizeof(NodeType) * n_leaves);
+ freelist = n_leaves;
+ n_nodes = n_leaves;
+ n_nodes_alloc = 2 * n_leaves;
+ for(size_t i = n_leaves; i < n_nodes_alloc; ++i)
+ nodes[i].next = i + 1;
+ nodes[n_nodes_alloc - 1].next = NULL_NODE;
+
+
+ BV bound_bv;
+ if(n_leaves > 0)
+ bound_bv = nodes[0].bv;
+ for(size_t i = 1; i < n_leaves; ++i)
+ bound_bv += nodes[i].bv;
+
+ morton_functor<FCL_UINT32> coder(bound_bv);
+ for(size_t i = 0; i < n_leaves; ++i)
+ nodes[i].code = coder(nodes[i].bv.center());
+
+
+ size_t* ids = new size_t[n_leaves];
+ for(size_t i = 0; i < n_leaves; ++i)
+ ids[i] = i;
+
+ SortByMorton comp;
+ comp.nodes = nodes;
+ std::sort(ids, ids + n_leaves, comp);
+ root_node = mortonRecurse_2(ids, ids + n_leaves);
+ delete [] ids;
+
+ refit();
+
+ opath = 0;
+ max_lookahead_level = -1;
+}
+
+template<typename BV>
+size_t HierarchyTree<BV>::insert(const BV& bv, void* data)
+{
+ size_t node = createNode(NULL_NODE, bv, data);
+ insertLeaf(root_node, node);
+ ++n_leaves;
+ return node;
+}
+
+template<typename BV>
+void HierarchyTree<BV>::remove(size_t leaf)
+{
+ removeLeaf(leaf);
+ deleteNode(leaf);
+ --n_leaves;
+}
+
+template<typename BV>
+void HierarchyTree<BV>::clear()
+{
+ delete [] nodes;
+ root_node = NULL_NODE;
+ n_nodes = 0;
+ n_nodes_alloc = 16;
+ nodes = new NodeType[n_nodes_alloc];
+ for(size_t i = 0; i < n_nodes_alloc; ++i)
+ nodes[i].next = i + 1;
+ nodes[n_nodes_alloc - 1].next = NULL_NODE;
+ n_leaves = 0;
+ freelist = 0;
+ opath = 0;
+ max_lookahead_level = -1;
+}
+
+template<typename BV>
+bool HierarchyTree<BV>::empty() const
+{
+ return (n_nodes == 0);
+}
+
+template<typename BV>
+void HierarchyTree<BV>::update(size_t leaf, int lookahead_level)
+{
+ size_t root = removeLeaf(leaf);
+ if(root != NULL_NODE)
+ {
+ if(lookahead_level > 0)
+ {
+ for(int i = 0; (i < lookahead_level) && (nodes[root].parent != NULL_NODE); ++i)
+ root = nodes[root].parent;
+ }
+ else
+ root = root_node;
+ }
+ insertLeaf(root, leaf);
+}
+
+template<typename BV>
+bool HierarchyTree<BV>::update(size_t leaf, const BV& bv)
+{
+ if(nodes[leaf].bv.contain(bv)) return false;
+ update_(leaf, bv);
+ return true;
+}
+
+template<typename BV>
+bool HierarchyTree<BV>::update(size_t leaf, const BV& bv, const Vec3f& vel, FCL_REAL margin)
+{
+ if(nodes[leaf].bv.contain(bv)) return false;
+ update_(leaf, bv);
+ return true;
+}
+
+template<typename BV>
+bool HierarchyTree<BV>::update(size_t leaf, const BV& bv, const Vec3f& vel)
+{
+ if(nodes[leaf].bv.contain(bv)) return false;
+ update_(leaf, bv);
+ return true;
+}
+
+template<typename BV>
+size_t HierarchyTree<BV>::getMaxHeight() const
+{
+ if(root_node == NULL_NODE) return 0;
+
+ return getMaxHeight(root_node);
+}
+
+template<typename BV>
+size_t HierarchyTree<BV>::getMaxDepth() const
+{
+ if(root_node == NULL_NODE) return 0;
+
+ size_t max_depth;
+ getMaxDepth(root_node, 0, max_depth);
+ return max_depth;
+}
+
+template<typename BV>
+void HierarchyTree<BV>::balanceBottomup()
+{
+ if(root_node != NULL_NODE)
+ {
+ NodeType* leaves = new NodeType[n_leaves];
+ NodeType* leaves_ = leaves;
+ extractLeaves(root_node, leaves_);
+ root_node = NULL_NODE;
+ memcpy(nodes, leaves, sizeof(NodeType) * n_leaves);
+ freelist = n_leaves;
+ n_nodes = n_leaves;
+ for(size_t i = n_leaves; i < n_nodes_alloc; ++i)
+ nodes[i].next = i + 1;
+ nodes[n_nodes_alloc - 1].next = NULL_NODE;
+
+
+ size_t* ids = new size_t[n_leaves];
+ for(size_t i = 0; i < n_leaves; ++i)
+ ids[i] = i;
+
+ bottomup(ids, ids + n_leaves);
+ root_node = *ids;
+
+ delete [] ids;
+ }
+}
+
+template<typename BV>
+void HierarchyTree<BV>::balanceTopdown()
+{
+ if(root_node != NULL_NODE)
+ {
+ NodeType* leaves = new NodeType[n_leaves];
+ NodeType* leaves_ = leaves;
+ extractLeaves(root_node, leaves_);
+ root_node = NULL_NODE;
+ memcpy(nodes, leaves, sizeof(NodeType) * n_leaves);
+ freelist = n_leaves;
+ n_nodes = n_leaves;
+ for(size_t i = n_leaves; i < n_nodes_alloc; ++i)
+ nodes[i].next = i + 1;
+ nodes[n_nodes_alloc - 1].next = NULL_NODE;
+
+ size_t* ids = new size_t[n_leaves];
+ for(size_t i = 0; i < n_leaves; ++i)
+ ids[i] = i;
+
+ root_node = topdown(ids, ids + n_leaves);
+ delete [] ids;
+ }
+}
+
+template<typename BV>
+void HierarchyTree<BV>::balanceIncremental(int iterations)
+{
+ if(iterations < 0) iterations = n_leaves;
+ if((root_node != NULL_NODE) && (iterations > 0))
+ {
+ for(int i = 0; i < iterations; ++i)
+ {
+ size_t node = root_node;
+ unsigned int bit = 0;
+ while(!nodes[node].isLeaf())
+ {
+ node = nodes[node].children[(opath>>bit)&1];
+ bit = (bit+1)&(sizeof(unsigned int) * 8-1);
+ }
+ update(node);
+ ++opath;
+ }
+ }
+}
+
+template<typename BV>
+void HierarchyTree<BV>::refit()
+{
+ if(root_node != NULL_NODE)
+ recurseRefit(root_node);
+}
+
+template<typename BV>
+void HierarchyTree<BV>::extractLeaves(size_t root, NodeType*& leaves) const
+{
+ if(!nodes[root].isLeaf())
+ {
+ extractLeaves(nodes[root].children[0], leaves);
+ extractLeaves(nodes[root].children[1], leaves);
+ }
+ else
+ {
+ *leaves = nodes[root];
+ leaves++;
+ }
+}
+
+template<typename BV>
+size_t HierarchyTree<BV>::size() const
+{
+ return n_leaves;
+}
+
+template<typename BV>
+size_t HierarchyTree<BV>::getRoot() const
+{
+ return root_node;
+}
+
+template<typename BV>
+typename HierarchyTree<BV>::NodeType* HierarchyTree<BV>::getNodes() const
+{
+ return nodes;
+}
+
+template<typename BV>
+void HierarchyTree<BV>::print(size_t root, int depth)
+{
+ for(int i = 0; i < depth; ++i)
+ std::cout << " ";
+ NodeType* n = nodes + root;
+ std::cout << " (" << n->bv.min_[0] << ", " << n->bv.min_[1] << ", " << n->bv.min_[2] << "; " << n->bv.max_[0] << ", " << n->bv.max_[1] << ", " << n->bv.max_[2] << ")" << std::endl;
+ if(n->isLeaf())
+ {
+ }
+ else
+ {
+ print(n->children[0], depth+1);
+ print(n->children[1], depth+1);
+ }
+}
+
+template<typename BV>
+size_t HierarchyTree<BV>::getMaxHeight(size_t node) const
+{
+ if(!nodes[node].isLeaf())
+ {
+ size_t height1 = getMaxHeight(nodes[node].children[0]);
+ size_t height2 = getMaxHeight(nodes[node].children[1]);
+ return std::max(height1, height2) + 1;
+ }
+ else
+ return 0;
+}
+
+template<typename BV>
+void HierarchyTree<BV>::getMaxDepth(size_t node, size_t depth, size_t& max_depth) const
+{
+ if(!nodes[node].isLeaf())
+ {
+ getMaxDepth(nodes[node].children[0], depth+1, max_depth);
+ getmaxDepth(nodes[node].children[1], depth+1, max_depth);
+ }
+ else
+ max_depth = std::max(max_depth, depth);
+}
+
+template<typename BV>
+void HierarchyTree<BV>::bottomup(size_t* lbeg, size_t* lend)
+{
+ size_t* lcur_end = lend;
+ while(lbeg < lcur_end - 1)
+ {
+ size_t* min_it1, *min_it2;
+ FCL_REAL min_size = std::numeric_limits<FCL_REAL>::max();
+ for(size_t* it1 = lbeg; it1 < lcur_end; ++it1)
+ {
+ for(size_t* it2 = it1 + 1; it2 < lcur_end; ++it2)
+ {
+ FCL_REAL cur_size = (nodes[*it1].bv + nodes[*it2].bv).size();
+ if(cur_size < min_size)
+ {
+ min_size = cur_size;
+ min_it1 = it1;
+ min_it2 = it2;
+ }
+ }
+ }
+
+ size_t p = createNode(NULL_NODE, nodes[*min_it1].bv, nodes[*min_it2].bv, NULL);
+ nodes[p].children[0] = *min_it1;
+ nodes[p].children[1] = *min_it2;
+ nodes[*min_it1].parent = p;
+ nodes[*min_it2].parent = p;
+ *min_it1 = p;
+ size_t tmp = *min_it2;
+ lcur_end--;
+ *min_it2 = *lcur_end;
+ *lcur_end = tmp;
+ }
+}
+
+template<typename BV>
+size_t HierarchyTree<BV>::topdown(size_t* lbeg, size_t* lend)
+{
+ switch(topdown_level)
+ {
+ case 0:
+ return topdown_0(lbeg, lend);
+ break;
+ case 1:
+ return topdown_1(lbeg, lend);
+ break;
+ default:
+ return topdown_0(lbeg, lend);
+ }
+}
+
+template<typename BV>
+size_t HierarchyTree<BV>::topdown_0(size_t* lbeg, size_t* lend)
+{
+ int num_leaves = lend - lbeg;
+ if(num_leaves > 1)
+ {
+ if(num_leaves > bu_threshold)
+ {
+ BV vol = nodes[*lbeg].bv;
+ for(size_t* i = lbeg + 1; i < lend; ++i)
+ vol += nodes[*i].bv;
+
+ int best_axis = 0;
+ FCL_REAL extent[3] = {vol.width(), vol.height(), vol.depth()};
+ if(extent[1] > extent[0]) best_axis = 1;
+ if(extent[2] > extent[best_axis]) best_axis = 2;
+
+ nodeBaseLess<BV> comp(nodes, best_axis);
+ size_t* lcenter = lbeg + num_leaves / 2;
+ std::nth_element(lbeg, lcenter, lend, comp);
+
+ size_t node = createNode(NULL_NODE, vol, NULL);
+ nodes[node].children[0] = topdown_0(lbeg, lcenter);
+ nodes[node].children[1] = topdown_0(lcenter, lend);
+ nodes[nodes[node].children[0]].parent = node;
+ nodes[nodes[node].children[1]].parent = node;
+ return node;
+ }
+ else
+ {
+ bottomup(lbeg, lend);
+ return *lbeg;
+ }
+ }
+ return *lbeg;
+}
+
+template<typename BV>
+size_t HierarchyTree<BV>::topdown_1(size_t* lbeg, size_t* lend)
+{
+ int num_leaves = lend - lbeg;
+ if(num_leaves > 1)
+ {
+ if(num_leaves > bu_threshold)
+ {
+ Vec3f split_p = nodes[*lbeg].bv.center();
+ BV vol = nodes[*lbeg].bv;
+ for(size_t* i = lbeg + 1; i < lend; ++i)
+ {
+ split_p += nodes[*i].bv.center();
+ vol += nodes[*i].bv;
+ }
+ split_p /= (FCL_REAL)(num_leaves);
+ int best_axis = -1;
+ int bestmidp = num_leaves;
+ int splitcount[3][2] = {{0,0}, {0,0}, {0,0}};
+ for(size_t* i = lbeg; i < lend; ++i)
+ {
+ Vec3f x = nodes[*i].bv.center() - split_p;
+ for(size_t j = 0; j < 3; ++j)
+ ++splitcount[j][x[j] > 0 ? 1 : 0];
+ }
+
+ for(size_t i = 0; i < 3; ++i)
+ {
+ if((splitcount[i][0] > 0) && (splitcount[i][1] > 0))
+ {
+ int midp = std::abs(splitcount[i][0] - splitcount[i][1]);
+ if(midp < bestmidp)
+ {
+ best_axis = i;
+ bestmidp = midp;
+ }
+ }
+ }
+
+ if(best_axis < 0) best_axis = 0;
+
+ FCL_REAL split_value = split_p[best_axis];
+ size_t* lcenter = lbeg;
+ for(size_t* i = lbeg; i < lend; ++i)
+ {
+ if(nodes[*i].bv.center()[best_axis] < split_value)
+ {
+ size_t temp = *i;
+ *i = *lcenter;
+ *lcenter = temp;
+ ++lcenter;
+ }
+ }
+
+ size_t node = createNode(NULL_NODE, vol, NULL);
+ nodes[node].children[0] = topdown_1(lbeg, lcenter);
+ nodes[node].children[1] = topdown_1(lcenter, lend);
+ nodes[nodes[node].children[0]].parent = node;
+ nodes[nodes[node].children[1]].parent = node;
+ return node;
+ }
+ else
+ {
+ bottomup(lbeg, lend);
+ return *lbeg;
+ }
+ }
+ return *lbeg;
+}
+
+template<typename BV>
+size_t HierarchyTree<BV>::mortonRecurse_0(size_t* lbeg, size_t* lend, const FCL_UINT32& split, int bits)
+{
+ int num_leaves = lend - lbeg;
+ if(num_leaves > 1)
+ {
+ if(bits > 0)
+ {
+ SortByMorton comp;
+ comp.nodes = nodes;
+ comp.split = split;
+ size_t* lcenter = std::lower_bound(lbeg, lend, NULL_NODE, comp);
+
+ if(lcenter == lbeg)
+ {
+ FCL_UINT32 split2 = split | (1 << (bits - 1));
+ return mortonRecurse_0(lbeg, lend, split2, bits - 1);
+ }
+ else if(lcenter == lend)
+ {
+ FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
+ return mortonRecurse_0(lbeg, lend, split1, bits - 1);
+ }
+ else
+ {
+ FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
+ FCL_UINT32 split2 = split | (1 << (bits - 1));
+
+ size_t child1 = mortonRecurse_0(lbeg, lcenter, split1, bits - 1);
+ size_t child2 = mortonRecurse_0(lcenter, lend, split2, bits - 1);
+ size_t node = createNode(NULL_NODE, NULL);
+ nodes[node].children[0] = child1;
+ nodes[node].children[1] = child2;
+ nodes[child1].parent = node;
+ nodes[child2].parent = node;
+ return node;
+ }
+ }
+ else
+ {
+ size_t node = topdown(lbeg, lend);
+ return node;
+ }
+ }
+ else
+ return *lbeg;
+}
+
+template<typename BV>
+size_t HierarchyTree<BV>::mortonRecurse_1(size_t* lbeg, size_t* lend, const FCL_UINT32& split, int bits)
+{
+ int num_leaves = lend - lbeg;
+ if(num_leaves > 1)
+ {
+ if(bits > 0)
+ {
+ SortByMorton comp;
+ comp.nodes = nodes;
+ comp.split = split;
+ size_t* lcenter = std::lower_bound(lbeg, lend, NULL_NODE, comp);
+
+ if(lcenter == lbeg)
+ {
+ FCL_UINT32 split2 = split | (1 << (bits - 1));
+ return mortonRecurse_1(lbeg, lend, split2, bits - 1);
+ }
+ else if(lcenter == lend)
+ {
+ FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
+ return mortonRecurse_1(lbeg, lend, split1, bits - 1);
+ }
+ else
+ {
+ FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
+ FCL_UINT32 split2 = split | (1 << (bits - 1));
+
+ size_t child1 = mortonRecurse_1(lbeg, lcenter, split1, bits - 1);
+ size_t child2 = mortonRecurse_1(lcenter, lend, split2, bits - 1);
+ size_t node = createNode(NULL_NODE, NULL);
+ nodes[node].children[0] = child1;
+ nodes[node].children[1] = child2;
+ nodes[child1].parent = node;
+ nodes[child2].parent = node;
+ return node;
+ }
+ }
+ else
+ {
+ size_t child1 = mortonRecurse_1(lbeg, lbeg + num_leaves / 2, 0, bits - 1);
+ size_t child2 = mortonRecurse_1(lbeg + num_leaves / 2, lend, 0, bits - 1);
+ size_t node = createNode(NULL_NODE, NULL);
+ nodes[node].children[0] = child1;
+ nodes[node].children[1] = child2;
+ nodes[child1].parent = node;
+ nodes[child2].parent = node;
+ return node;
+ }
+ }
+ else
+ return *lbeg;
+}
+
+template<typename BV>
+size_t HierarchyTree<BV>::mortonRecurse_2(size_t* lbeg, size_t* lend)
+{
+ int num_leaves = lend - lbeg;
+ if(num_leaves > 1)
+ {
+ size_t child1 = mortonRecurse_2(lbeg, lbeg + num_leaves / 2);
+ size_t child2 = mortonRecurse_2(lbeg + num_leaves / 2, lend);
+ size_t node = createNode(NULL_NODE, NULL);
+ nodes[node].children[0] = child1;
+ nodes[node].children[1] = child2;
+ nodes[child1].parent = node;
+ nodes[child2].parent = node;
+ return node;
+ }
+ else
+ return *lbeg;
+}
+
+template<typename BV>
+void HierarchyTree<BV>::insertLeaf(size_t root, size_t leaf)
+{
+ if(root_node == NULL_NODE)
+ {
+ root_node = leaf;
+ nodes[leaf].parent = NULL_NODE;
+ }
+ else
+ {
+ if(!nodes[root].isLeaf())
+ {
+ do
+ {
+ root = nodes[root].children[select(leaf, nodes[root].children[0], nodes[root].children[1], nodes)];
+ }
+ while(!nodes[root].isLeaf());
+ }
+
+ size_t prev = nodes[root].parent;
+ size_t node = createNode(prev, nodes[leaf].bv, nodes[root].bv, NULL);
+ if(prev != NULL_NODE)
+ {
+ nodes[prev].children[indexOf(root)] = node;
+ nodes[node].children[0] = root; nodes[root].parent = node;
+ nodes[node].children[1] = leaf; nodes[leaf].parent = node;
+ do
+ {
+ if(!nodes[prev].bv.contain(nodes[node].bv))
+ nodes[prev].bv = nodes[nodes[prev].children[0]].bv + nodes[nodes[prev].children[1]].bv;
+ else
+ break;
+ node = prev;
+ } while (NULL_NODE != (prev = nodes[node].parent));
+ }
+ else
+ {
+ nodes[node].children[0] = root; nodes[root].parent = node;
+ nodes[node].children[1] = leaf; nodes[leaf].parent = node;
+ root_node = node;
+ }
+ }
+}
+
+template<typename BV>
+size_t HierarchyTree<BV>::removeLeaf(size_t leaf)
+{
+ if(leaf == root_node)
+ {
+ root_node = NULL_NODE;
+ return NULL_NODE;
+ }
+ else
+ {
+ size_t parent = nodes[leaf].parent;
+ size_t prev = nodes[parent].parent;
+ size_t sibling = nodes[parent].children[1-indexOf(leaf)];
+
+ if(prev != NULL_NODE)
+ {
+ nodes[prev].children[indexOf(parent)] = sibling;
+ nodes[sibling].parent = prev;
+ deleteNode(parent);
+ while(prev != NULL_NODE)
+ {
+ BV new_bv = nodes[nodes[prev].children[0]].bv + nodes[nodes[prev].children[1]].bv;
+ if(!new_bv.equal(nodes[prev].bv))
+ {
+ nodes[prev].bv = new_bv;
+ prev = nodes[prev].parent;
+ }
+ else break;
+ }
+
+ return (prev != NULL_NODE) ? prev : root_node;
+ }
+ else
+ {
+ root_node = sibling;
+ nodes[sibling].parent = NULL_NODE;
+ deleteNode(parent);
+ return root_node;
+ }
+ }
+}
+
+template<typename BV>
+void HierarchyTree<BV>::update_(size_t leaf, const BV& bv)
+{
+ size_t root = removeLeaf(leaf);
+ if(root != NULL_NODE)
+ {
+ if(max_lookahead_level >= 0)
+ {
+ for(int i = 0; (i < max_lookahead_level) && (nodes[root].parent != NULL_NODE); ++i)
+ root = nodes[root].parent;
+ }
+
+ nodes[leaf].bv = bv;
+ insertLeaf(root, leaf);
+ }
+}
+
+template<typename BV>
+size_t HierarchyTree<BV>::indexOf(size_t node)
+{
+ return (nodes[nodes[node].parent].children[1] == node);
+}
+
+template<typename BV>
+size_t HierarchyTree<BV>::allocateNode()
+{
+ if(freelist == NULL_NODE)
+ {
+ NodeType* old_nodes = nodes;
+ n_nodes_alloc *= 2;
+ nodes = new NodeType[n_nodes_alloc];
+ memcpy(nodes, old_nodes, n_nodes * sizeof(NodeType));
+ delete [] old_nodes;
+
+ for(size_t i = n_nodes; i < n_nodes_alloc - 1; ++i)
+ nodes[i].next = i + 1;
+ nodes[n_nodes_alloc - 1].next = NULL_NODE;
+ freelist = n_nodes;
+ }
+
+ size_t node_id = freelist;
+ freelist = nodes[node_id].next;
+ nodes[node_id].parent = NULL_NODE;
+ nodes[node_id].children[0] = NULL_NODE;
+ nodes[node_id].children[1] = NULL_NODE;
+ ++n_nodes;
+ return node_id;
+}
+
+template<typename BV>
+size_t HierarchyTree<BV>::createNode(size_t parent,
+ const BV& bv1,
+ const BV& bv2,
+ void* data)
+{
+ size_t node = allocateNode();
+ nodes[node].parent = parent;
+ nodes[node].data = data;
+ nodes[node].bv = bv1 + bv2;
+ return node;
+}
+
+template<typename BV>
+size_t HierarchyTree<BV>::createNode(size_t parent,
+ const BV& bv,
+ void* data)
+{
+ size_t node = allocateNode();
+ nodes[node].parent = parent;
+ nodes[node].data = data;
+ nodes[node].bv = bv;
+ return node;
+}
+
+template<typename BV>
+size_t HierarchyTree<BV>::createNode(size_t parent,
+ void* data)
+{
+ size_t node = allocateNode();
+ nodes[node].parent = parent;
+ nodes[node].data = data;
+ return node;
+}
+
+template<typename BV>
+void HierarchyTree<BV>::deleteNode(size_t node)
+{
+ nodes[node].next = freelist;
+ freelist = node;
+ --n_nodes;
+}
+
+template<typename BV>
+void HierarchyTree<BV>::recurseRefit(size_t node)
+{
+ if(!nodes[node].isLeaf())
+ {
+ recurseRefit(nodes[node].children[0]);
+ recurseRefit(nodes[node].children[1]);
+ nodes[node].bv = nodes[nodes[node].children[0]].bv + nodes[nodes[node].children[1]].bv;
+ }
+ else
+ return;
+}
+
+template<typename BV>
+void HierarchyTree<BV>::fetchLeaves(size_t root, NodeType*& leaves, int depth)
+{
+ if((!nodes[root].isLeaf()) && depth)
+ {
+ fetchLeaves(nodes[root].children[0], leaves, depth-1);
+ fetchLeaves(nodes[root].children[1], leaves, depth-1);
+ deleteNode(root);
+ }
+ else
+ {
+ *leaves = nodes[root];
+ leaves++;
+ }
+}
+
+
+
+}
+}
diff --git a/trunk/fcl/include/fcl/interval_tree.h b/trunk/fcl/include/fcl/broadphase/interval_tree.h
similarity index 70%
rename from trunk/fcl/include/fcl/interval_tree.h
rename to trunk/fcl/include/fcl/broadphase/interval_tree.h
index 6624c9c6f2ff42ce8371e28f8d470f7f33a9ffb2..cab4da64e7acefbd60d49e9546117f8336b8d187 100644
--- a/trunk/fcl/include/fcl/interval_tree.h
+++ b/trunk/fcl/include/fcl/broadphase/interval_tree.h
@@ -41,40 +41,41 @@
#include <deque>
#include <limits>
-/** \brief Main namespace */
namespace fcl
{
-/** \brief Interval trees implemented using red-black-trees as described in
- * the book Introduction_To_Algorithms_ by Cormen, Leisserson, and Rivest.
- * Can be replaced in part by boost::icl::interval_set, which is only supported after boost 1.46 and does not support delete node routine.
- */
-
+/// @brief Interval trees implemented using red-black-trees as described in
+/// the book Introduction_To_Algorithms_ by Cormen, Leisserson, and Rivest.
+/// Can be replaced in part by boost::icl::interval_set, which is only supported after boost 1.46 and does not support delete node routine.
struct SimpleInterval
{
public:
- SimpleInterval() {}
virtual ~SimpleInterval() {}
+
virtual void print() {}
+ /// @brief interval is defined as [low, high]
double low, high;
};
+/// @brief The node for interval tree
class IntervalTreeNode
{
friend class IntervalTree;
public:
- /** \brief Print the interval node information: set left = nil and right = root */
+ /// @brief Print the interval node information: set left = nil and right = root
void print(IntervalTreeNode* left, IntervalTreeNode* right) const;
-
+
+ /// @brief Create an empty node
IntervalTreeNode();
+ /// @brief Create an node storing the interval
IntervalTreeNode(SimpleInterval* new_interval);
~IntervalTreeNode();
protected:
-
+ /// @brief interval stored in the node
SimpleInterval* stored_interval;
double key;
@@ -83,7 +84,8 @@ protected:
double max_high;
- bool red; /* if red = false then the node is black */
+ /// @brief red or black node: if red = false then the node is black
+ bool red;
IntervalTreeNode* left;
@@ -92,22 +94,9 @@ protected:
IntervalTreeNode* parent;
};
-/** \brief Class describes the information needed when we take the
- * right branch in searching for intervals but possibly come back
- * and check the left branch as well.
- */
-struct it_recursion_node
-{
-public:
- IntervalTreeNode* start_node;
-
- unsigned int parent_index;
-
- bool try_right_branch;
-};
-
+struct it_recursion_node;
-/** \brief Interval tree */
+/// @brief Interval tree
class IntervalTree
{
public:
@@ -116,25 +105,25 @@ public:
~IntervalTree();
- /** \brief Print the whole interval tree */
+ /// @brief Print the whole interval tree
void print() const;
- /** \brief Delete one node of the interval tree */
+ /// @brief Delete one node of the interval tree
SimpleInterval* deleteNode(IntervalTreeNode* node);
+ /// @brief delete node stored a given interval
void deleteNode(SimpleInterval* ivl);
-
- /** \brief Insert one node of the interval tree */
+ /// @brief Insert one node of the interval tree
IntervalTreeNode* insert(SimpleInterval* new_interval);
- /** \brief get the predecessor of a given node */
+ /// @brief get the predecessor of a given node
IntervalTreeNode* getPredecessor(IntervalTreeNode* node) const;
- /** \brief Get the successor of a given node */
+ /// @brief Get the successor of a given node
IntervalTreeNode* getSuccessor(IntervalTreeNode* node) const;
- /** \brief Return result for a given query */
+ /// @brief Return result for a given query
std::deque<SimpleInterval*> query(double low, double high);
protected:
@@ -143,22 +132,22 @@ protected:
IntervalTreeNode* nil;
- /** \brief left rotation of tree node */
+ /// @brief left rotation of tree node
void leftRotate(IntervalTreeNode* node);
- /** \brief right rotation of tree node */
+ /// @brief right rotation of tree node
void rightRotate(IntervalTreeNode* node);
- /** \brief recursively insert a node */
+ /// @brief recursively insert a node
void recursiveInsert(IntervalTreeNode* node);
- /** \brief recursively print a subtree */
+ /// @brief recursively print a subtree
void recursivePrint(IntervalTreeNode* node) const;
- /** \brief recursively find the node corresponding to the interval */
+ /// @brief recursively find the node corresponding to the interval
IntervalTreeNode* recursiveSearch(IntervalTreeNode* node, SimpleInterval* ivl) const;
- /** \brief Travels up to the root fixing the max_high fields after an insertion or deletion */
+ /// @brief Travels up to the root fixing the max_high fields after an insertion or deletion
void fixupMaxHigh(IntervalTreeNode* node);
void deleteFixup(IntervalTreeNode* node);
diff --git a/trunk/fcl/include/fcl/morton.h b/trunk/fcl/include/fcl/broadphase/morton.h
similarity index 84%
rename from trunk/fcl/include/fcl/morton.h
rename to trunk/fcl/include/fcl/broadphase/morton.h
index bf0004268f6fad33a156058b7c357a30b549e4b2..98eeaea722c252e8b3030ad1e8c2cb4dfea0f7b1 100644
--- a/trunk/fcl/include/fcl/morton.h
+++ b/trunk/fcl/include/fcl/broadphase/morton.h
@@ -44,11 +44,16 @@
namespace fcl
{
+/// @cond IGNORE
+namespace details
+{
+
static inline FCL_UINT32 quantize(FCL_REAL x, FCL_UINT32 n)
{
return std::max(std::min((FCL_UINT32)(x * (FCL_REAL)n), FCL_UINT32(n-1)), FCL_UINT32(0));
}
+/// @brief compute 30 bit morton code
static inline FCL_UINT32 morton_code(FCL_UINT32 x, FCL_UINT32 y, FCL_UINT32 z)
{
x = (x | (x << 16)) & 0x030000FF;
@@ -69,6 +74,7 @@ static inline FCL_UINT32 morton_code(FCL_UINT32 x, FCL_UINT32 y, FCL_UINT32 z)
return x | (y << 1) | (z << 2);
}
+/// @brief compute 60 bit morton code
static inline FCL_UINT64 morton_code60(FCL_UINT32 x, FCL_UINT32 y, FCL_UINT32 z)
{
FCL_UINT32 lo_x = x & 1023u;
@@ -81,9 +87,16 @@ static inline FCL_UINT64 morton_code60(FCL_UINT32 x, FCL_UINT32 y, FCL_UINT32 z)
return (FCL_UINT64(morton_code(hi_x, hi_y, hi_z)) << 30) | FCL_UINT64(morton_code(lo_x, lo_y, lo_z));
}
+}
+/// @endcond
+
+
+/// @brief Functor to compute the morton code for a given AABB
template<typename T>
struct morton_functor {};
+
+/// @brief Functor to compute 30 bit morton code for a given AABB
template<>
struct morton_functor<FCL_UINT32>
{
@@ -95,11 +108,11 @@ struct morton_functor<FCL_UINT32>
FCL_UINT32 operator() (const Vec3f& point) const
{
- FCL_UINT32 x = quantize((point[0] - base[0]) * inv[0], 1024u);
- FCL_UINT32 y = quantize((point[1] - base[1]) * inv[1], 1024u);
- FCL_UINT32 z = quantize((point[2] - base[2]) * inv[2], 1024u);
+ FCL_UINT32 x = details::quantize((point[0] - base[0]) * inv[0], 1024u);
+ FCL_UINT32 y = details::quantize((point[1] - base[1]) * inv[1], 1024u);
+ FCL_UINT32 z = details::quantize((point[2] - base[2]) * inv[2], 1024u);
- return morton_code(x, y, z);
+ return details::morton_code(x, y, z);
}
const Vec3f base;
@@ -109,6 +122,7 @@ struct morton_functor<FCL_UINT32>
};
+/// @brief Functor to compute 60 bit morton code for a given AABB
template<>
struct morton_functor<FCL_UINT64>
{
@@ -120,11 +134,11 @@ struct morton_functor<FCL_UINT64>
FCL_UINT64 operator() (const Vec3f& point) const
{
- FCL_UINT32 x = quantize((point[0] - base[0]) * inv[0], 1u << 20);
- FCL_UINT32 y = quantize((point[1] - base[1]) * inv[1], 1u << 20);
- FCL_UINT32 z = quantize((point[2] - base[2]) * inv[2], 1u << 20);
+ FCL_UINT32 x = details::quantize((point[0] - base[0]) * inv[0], 1u << 20);
+ FCL_UINT32 y = details::quantize((point[1] - base[1]) * inv[1], 1u << 20);
+ FCL_UINT32 z = details::quantize((point[2] - base[2]) * inv[2], 1u << 20);
- return morton_code60(x, y, z);
+ return details::morton_code60(x, y, z);
}
const Vec3f base;
@@ -133,6 +147,7 @@ struct morton_functor<FCL_UINT64>
size_t bits() const { return 60; }
};
+/// @brief Functor to compute n bit morton code for a given AABB
template<>
struct morton_functor<boost::dynamic_bitset<> >
{
diff --git a/trunk/fcl/include/fcl/ccd/interval.h b/trunk/fcl/include/fcl/ccd/interval.h
index 6a4b697fbe9c8cb98e3b6b38fa39425eb4ccc2ad..6870ba0011be5dd6027537b008d8a6bd9a77568d 100644
--- a/trunk/fcl/include/fcl/ccd/interval.h
+++ b/trunk/fcl/include/fcl/ccd/interval.h
@@ -44,41 +44,49 @@
namespace fcl
{
+/// @brief Interval class for [a, b]
struct Interval
{
FCL_REAL i_[2];
Interval() { i_[0] = i_[1] = 0; }
+
explicit Interval(FCL_REAL v)
{
i_[0] = i_[1] = v;
}
+ /// @brief construct interval [left, right]
Interval(FCL_REAL left, FCL_REAL right)
{
i_[0] = left; i_[1] = right;
}
+ /// @brief construct interval [left, right]
inline void setValue(FCL_REAL a, FCL_REAL b)
{
i_[0] = a; i_[1] = b;
}
+ /// @brief construct zero interval [x, x]
inline void setValue(FCL_REAL x)
{
i_[0] = i_[1] = x;
}
+ /// @brief access the interval endpoints: 0 for left, 1 for right end
inline FCL_REAL operator [] (size_t i) const
{
return i_[i];
}
+ /// @brief access the interval endpoints: 0 for left, 1 for right end
inline FCL_REAL& operator [] (size_t i)
{
return i_[i];
}
+ /// @brief whether two intervals are the same
inline bool operator == (const Interval& other) const
{
if(i_[0] != other.i_[0]) return false;
@@ -86,11 +94,13 @@ struct Interval
return true;
}
+ /// @brief add two intervals
inline Interval operator + (const Interval& other) const
{
return Interval(i_[0] + other.i_[0], i_[1] + other.i_[1]);
}
+ /// @brief minus another interval
inline Interval operator - (const Interval& other) const
{
return Interval(i_[0] - other.i_[1], i_[1] - other.i_[0]);
@@ -137,12 +147,12 @@ struct Interval
return *this;
}
- /** \brief other must not contain 0 */
+ /// @brief other must not contain 0
Interval operator / (const Interval& other) const;
Interval& operator /= (const Interval& other);
- /** \brief determine whether the intersection between intervals is empty */
+ /// @brief determine whether the intersection between intervals is empty
inline bool overlap(const Interval& other) const
{
if(i_[1] < other.i_[0]) return false;
@@ -164,7 +174,7 @@ struct Interval
return Interval(-i_[1], -i_[0]);
}
- /** \brief Return the nearest distance for points within the interval to zero */
+ /// @brief Return the nearest distance for points within the interval to zero
inline FCL_REAL getAbsLower() const
{
if(i_[0] >= 0) return i_[0];
@@ -172,7 +182,7 @@ struct Interval
return -i_[1];
}
- /** \brief Return the farthest distance for points within the interval to zero */
+ /// @brief Return the farthest distance for points within the interval to zero
inline FCL_REAL getAbsUpper() const
{
if(i_[0] + i_[1] >= 0) return i_[1];
@@ -187,7 +197,7 @@ struct Interval
return true;
}
- /** \brief Compute the minimum interval contains v and original interval */
+ /// @brief Compute the minimum interval contains v and original interval
inline Interval& bound(FCL_REAL v)
{
if(v < i_[0]) i_[0] = v;
@@ -196,7 +206,7 @@ struct Interval
}
- /** \brief Compute the minimum interval contains other and original interval */
+ /// @brief Compute the minimum interval contains other and original interval
inline Interval& bound(const Interval& other)
{
if(other.i_[0] < i_[0]) i_[0] = other.i_[0];
diff --git a/trunk/fcl/include/fcl/ccd/taylor_model.h b/trunk/fcl/include/fcl/ccd/taylor_model.h
index 3249067f9aabac87cdf02249bdc3ec33adb84904..9012de5a16541431f102993ed4194c8bc485b803 100644
--- a/trunk/fcl/include/fcl/ccd/taylor_model.h
+++ b/trunk/fcl/include/fcl/ccd/taylor_model.h
@@ -45,7 +45,7 @@ namespace fcl
struct TimeInterval
{
- /** \brief time interval and different powers */
+ /// @brief time interval and different powers
Interval t_; // [t1, t2]
Interval t2_; // [t1, t2]^2
Interval t3_; // [t1, t2]^3
@@ -54,19 +54,18 @@ struct TimeInterval
Interval t6_; // [t1, t2]^6
};
-/** \brief TaylorModel implements a third order Taylor model, i.e., a cubic approximation of a function
- * over a time interval, with an interval remainder.
- * All the operations on two Taylor models assume their time intervals are the same.
- */
+/// @brief TaylorModel implements a third order Taylor model, i.e., a cubic approximation of a function
+/// over a time interval, with an interval remainder.
+/// All the operations on two Taylor models assume their time intervals are the same.
struct TaylorModel
{
- /** \brief time interval */
+ /// @brief time interval
boost::shared_ptr<TimeInterval> time_interval_;
- /** \brief Coefficients of the cubic polynomial approximation */
+ /// @brief Coefficients of the cubic polynomial approximation
FCL_REAL coeffs_[4];
- /** \brief interval remainder */
+ /// @brief interval remainder
Interval r_;
void setTimeInterval(FCL_REAL l, FCL_REAL r);
diff --git a/trunk/fcl/include/fcl/collision.h b/trunk/fcl/include/fcl/collision.h
index df3fa31de08ff5a019b6b6350c1ec8965e794244..d91c2169f3d8c9039308bd17284b01e6cf2d4c34 100644
--- a/trunk/fcl/include/fcl/collision.h
+++ b/trunk/fcl/include/fcl/collision.h
@@ -42,15 +42,13 @@
#include "fcl/collision_object.h"
#include "fcl/collision_data.h"
-/** \brief Main namespace */
namespace fcl
{
-/** \brief Main collision interface: given two collision objects, and the requirements for contacts, including num of max contacts, whether perform exhaustive collision (i.e., returning
- * returning all the contact points), whether return detailed contact information (i.e., normal, contact point, depth; otherwise only contact primitive id is returned), this function
- * performs the collision between them.
- * Return value is the number of contacts generated between the two objects.
- */
+/// @brief Main collision interface: given two collision objects, and the requirements for contacts, including num of max contacts, whether perform exhaustive collision (i.e., returning
+/// returning all the contact points), whether return detailed contact information (i.e., normal, contact point, depth; otherwise only contact primitive id is returned), this function
+/// performs the collision between them.
+/// Return value is the number of contacts generated between the two objects.
template<typename NarrowPhaseSolver>
std::size_t collide(const CollisionObject* o1, const CollisionObject* o2,
diff --git a/trunk/fcl/include/fcl/collision_data.h b/trunk/fcl/include/fcl/collision_data.h
index 3dd5e93e9ab7f481a16f3bcb52f205453a47e34f..ebb24a58ce834a9f88755d743deddbc4557eeb27 100644
--- a/trunk/fcl/include/fcl/collision_data.h
+++ b/trunk/fcl/include/fcl/collision_data.h
@@ -11,45 +11,62 @@
namespace fcl
{
+/// @brief Contact information returned by collision
struct Contact
{
- FCL_REAL penetration_depth;
- Vec3f normal;
- Vec3f pos;
+ /// @brief collision object 1
const CollisionGeometry* o1;
+
+ /// @brief collision object 2
const CollisionGeometry* o2;
+
+ /// @brief contact primitive in object 1
+ /// if object 1 is mesh or point cloud, it is the triangle or point id
+ /// if object 1 is geometry shape, it is NONE (-1),
+ /// if object 1 is octree, it is the id of the cell
int b1;
+
+
+ /// @brief contact primitive in object 2
+ /// if object 2 is mesh or point cloud, it is the triangle or point id
+ /// if object 2 is geometry shape, it is NONE (-1),
+ /// if object 2 is octree, it is the id of the cell
int b2;
-
+
+ /// @brief contact normal, pointing from o1 to o2
+ Vec3f normal;
+
+ /// @brief contact position, in world space
+ Vec3f pos;
+
+ /// @brief penetration depth
+ FCL_REAL penetration_depth;
+
+
+ /// @brief invalid contact primitive information
static const int NONE = -1;
- Contact()
- {
- o1 = NULL;
- o2 = NULL;
- b1 = -1;
- b2 = -1;
- }
+ Contact() : o1(NULL),
+ o2(NULL),
+ b1(NONE),
+ b2(NONE)
+ {}
- Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_)
- {
- o1 = o1_;
- o2 = o2_;
- b1 = b1_;
- b2 = b2_;
- }
+ Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_) : o1(o1_),
+ o2(o2_),
+ b1(b1_),
+ b2(b2_)
+ {}
Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_,
- const Vec3f& pos_, const Vec3f& normal_, FCL_REAL depth_)
- {
- o1 = o1_;
- o2 = o2_;
- b1 = b1_;
- b2 = b2_;
- normal = normal_;
- pos = pos_;
- penetration_depth = depth_;
- }
+ const Vec3f& pos_, const Vec3f& normal_, FCL_REAL depth_) : o1(o1_),
+ o2(o2_),
+ b1(b1_),
+ b2(b2_),
+ normal(normal_),
+ pos(pos_),
+ penetration_depth(depth_)
+ {}
bool operator < (const Contact& other) const
{
@@ -59,10 +76,16 @@ struct Contact
}
};
+/// @brief Cost source describes an area with a cost. The area is described by an AABB region.
struct CostSource
{
+ /// @brief aabb lower bound
Vec3f aabb_min;
+
+ /// @brief aabb upper bound
Vec3f aabb_max;
+
+ /// @brief cost density in the AABB region
FCL_REAL cost_density;
CostSource(const Vec3f& aabb_min_, const Vec3f& aabb_max_, FCL_REAL cost_density_) : aabb_min(aabb_min_),
@@ -85,11 +108,19 @@ struct CostSource
}
};
+/// @brief request to the collision algorithm
struct CollisionRequest
{
+ /// @brief The maximum number of contacts will return
size_t num_max_contacts;
+
+ /// @brief whether the contact information (normal, penetration depth and contact position) will return
bool enable_contact;
+
+ /// @brief The maximum number of cost sources will return
size_t num_max_cost_sources;
+
+ /// @brief whether the cost sources will be computed
bool enable_cost;
CollisionRequest(size_t num_max_contacts_ = 1,
@@ -104,12 +135,14 @@ struct CollisionRequest
};
-
+/// @brief collision result
struct CollisionResult
{
private:
+ /// @brief contact information
std::vector<Contact> contacts;
+ /// @brief cost sources
std::set<CostSource> cost_sources;
const CollisionRequest* request;
@@ -125,11 +158,13 @@ public:
request = &request_;
}
+ /// @brief add one contact into result structure
inline void addContact(const Contact& c)
{
contacts.push_back(c);
}
+ /// @brief add one cost source into result structure
inline void addCostSource(const CostSource& c)
{
if(request)
@@ -144,21 +179,25 @@ public:
}
}
+ /// @brief return binary collision result
bool isCollision() const
{
return contacts.size() > 0;
}
+ /// @brief number of contacts found
size_t numContacts() const
{
return contacts.size();
}
+ /// @brief number of cost sources found
size_t numCostSources() const
{
return cost_sources.size();
}
+ /// @brief get the i-th contact calculated
const Contact& getContact(size_t i) const
{
if(i < contacts.size())
@@ -167,18 +206,21 @@ public:
return contacts.back();
}
+ /// @brief get all the contacts
void getContacts(std::vector<Contact>& contacts_)
{
contacts_.resize(contacts.size());
std::copy(contacts.begin(), contacts.end(), contacts_.begin());
}
+ /// @brief get all the cost sources
void getCostSources(std::vector<CostSource>& cost_sources_)
{
cost_sources_.resize(cost_sources.size());
std::copy(cost_sources.begin(), cost_sources.end(), cost_sources_.begin());
}
+ /// @brief clear the results obtained
void clear()
{
contacts.clear();
@@ -188,9 +230,10 @@ public:
-
+/// @brief request to the distance computation
struct DistanceRequest
{
+ /// @brief whether to return the nearest points
bool enable_nearest_points;
DistanceRequest(bool enable_nearest_points_ = false) : enable_nearest_points(enable_nearest_points_)
@@ -198,6 +241,7 @@ struct DistanceRequest
}
};
+/// @brief distance result
struct DistanceResult
{
private:
@@ -205,22 +249,38 @@ private:
public:
+ /// @brief minimum distance between two objects
FCL_REAL min_distance;
+ /// @brief nearest points
Vec3f nearest_points[2];
+ /// @brief collision object 1
const CollisionGeometry* o1;
+
+ /// @brief collision object 2
const CollisionGeometry* o2;
+
+ /// @brief information about the nearest point in object 1
+ /// if object 1 is mesh or point cloud, it is the triangle or point id
+ /// if object 1 is geometry shape, it is NONE (-1),
+ /// if object 1 is octree, it is the id of the cell
int b1;
+
+ /// @brief information about the nearest point in object 2
+ /// if object 2 is mesh or point cloud, it is the triangle or point id
+ /// if object 2 is geometry shape, it is NONE (-1),
+ /// if object 2 is octree, it is the id of the cell
int b2;
+ /// @brief invalid contact primitive information
static const int NONE = -1;
DistanceResult(FCL_REAL min_distance_ = std::numeric_limits<FCL_REAL>::max()) : min_distance(min_distance_),
o1(NULL),
o2(NULL),
- b1(-1),
- b2(-1)
+ b1(NONE),
+ b2(NONE)
{
request = NULL;
}
@@ -230,6 +290,7 @@ public:
request = &request_;
}
+ /// @brief add distance information into the result
void update(FCL_REAL distance, const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_)
{
if(min_distance > distance)
@@ -242,6 +303,7 @@ public:
}
}
+ /// @brief add distance information into the result
void update(FCL_REAL distance, const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_, const Vec3f& p1, const Vec3f& p2)
{
if(min_distance > distance)
@@ -256,6 +318,7 @@ public:
}
}
+ /// @brief add distance information into the result
void update(const DistanceResult& other_result)
{
if(min_distance > other_result.min_distance)
@@ -270,13 +333,14 @@ public:
}
}
+ /// @brief clear the result
void clear()
{
min_distance = std::numeric_limits<FCL_REAL>::max();
o1 = NULL;
o2 = NULL;
- b1 = -1;
- b2 = -1;
+ b1 = NONE;
+ b2 = NONE;
}
};
diff --git a/trunk/fcl/include/fcl/collision_node.h b/trunk/fcl/include/fcl/collision_node.h
index b269cdf1d812a353c5e431136e905fc495e77427..58a7ee73906b733901d1dbb0eab4c2d9eac2612c 100644
--- a/trunk/fcl/include/fcl/collision_node.h
+++ b/trunk/fcl/include/fcl/collision_node.h
@@ -42,17 +42,26 @@
#include "fcl/traversal_node_bvhs.h"
#include "fcl/BVH_front.h"
+
+
+
namespace fcl
{
+
+/// @brief collision on collision traversal node; can use front list to accelerate
void collide(CollisionTraversalNodeBase* node, BVHFrontList* front_list = NULL);
+/// @brief self collision on collision traversal node; can use front list to accelerate
void selfCollide(CollisionTraversalNodeBase* node, BVHFrontList* front_list = NULL);
+/// @brief distance computation on distance traversal node; can use front list to accelerate
void distance(DistanceTraversalNodeBase* node, BVHFrontList* front_list = NULL, int qsize = 2);
+/// @brief special collision on OBB traversal node
void collide2(MeshCollisionTraversalNodeOBB* node, BVHFrontList* front_list = NULL);
+/// @brief special collision on RSS traversal node
void collide2(MeshCollisionTraversalNodeRSS* node, BVHFrontList* front_list = NULL);
}
diff --git a/trunk/fcl/include/fcl/collision_object.h b/trunk/fcl/include/fcl/collision_object.h
index 6576194de5d4d1a081e91774e6b39e6e3ef6ac28..fad09584c0bec9ca9397824c66b3dba0e9931fc2 100644
--- a/trunk/fcl/include/fcl/collision_object.h
+++ b/trunk/fcl/include/fcl/collision_object.h
@@ -45,66 +45,78 @@
namespace fcl
{
+/// @brief object type: BVH (mesh, points), basic geometry, octree
enum OBJECT_TYPE {OT_UNKNOWN, OT_BVH, OT_GEOM, OT_OCTREE, OT_COUNT};
+/// @brief traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18, kDOP24), basic shape (box, sphere, capsule, cone, cylinder, convex, plane, triangle), and octree
enum NODE_TYPE {BV_UNKNOWN, BV_AABB, BV_OBB, BV_RSS, BV_kIOS, BV_OBBRSS, BV_KDOP16, BV_KDOP18, BV_KDOP24,
GEOM_BOX, GEOM_SPHERE, GEOM_CAPSULE, GEOM_CONE, GEOM_CYLINDER, GEOM_CONVEX, GEOM_PLANE, GEOM_TRIANGLE, GEOM_OCTREE, NODE_COUNT};
+/// @brief The geometry for the object for collision or distance computation
class CollisionGeometry
{
public:
- CollisionGeometry()
+ CollisionGeometry() : cost_density(1),
+ threshold_occupied(1),
+ threshold_free(0)
{
- cost_density = 1;
- threshold_occupied = 1;
- threshold_free = 0;
}
virtual ~CollisionGeometry() {}
+ /// @brief get the type of the object
virtual OBJECT_TYPE getObjectType() const { return OT_UNKNOWN; }
+ /// @brief get the node type
virtual NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
+ /// @brief compute the AABB for object in local coordinate
virtual void computeLocalAABB() = 0;
+ /// @brief get user data in geometry
void* getUserData() const
{
return user_data;
}
+ /// @brief set user data in geometry
void setUserData(void *data)
{
user_data = data;
}
-
+ /// @brief whether the object is completely occupied
inline bool isOccupied() const { return cost_density >= threshold_occupied; }
+
+ /// @brief whether the object is completely free
inline bool isFree() const { return cost_density <= threshold_free; }
+
+ /// @brief whether the object has some uncertainty
inline bool isUncertain() const { return !isOccupied() && !isFree(); }
- /// AABB center in local coordinate
+ /// @brief AABB center in local coordinate
Vec3f aabb_center;
- /// AABB radius
+ /// @brief AABB radius
FCL_REAL aabb_radius;
- /// AABB in local coordinate, used for tight AABB when only translation transform
+ /// @brief AABB in local coordinate, used for tight AABB when only translation transform
AABB aabb_local;
- /// pointer to user defined data specific to this object
+ /// @brief pointer to user defined data specific to this object
void *user_data;
- /// collision cost for unit volume
+ /// @brief collision cost for unit volume
FCL_REAL cost_density;
- /// threshold for occupied ( >= is occupied)
+ /// @brief threshold for occupied ( >= is occupied)
FCL_REAL threshold_occupied;
- /// threshold for free (<= is free)
+ /// @brief threshold for free (<= is free)
FCL_REAL threshold_free;
};
+/// @brief the object for collision or distance computation, contains the geometry and the transform information
class CollisionObject
{
public:
@@ -135,21 +147,25 @@ public:
{
}
+ /// @brief get the type of the object
OBJECT_TYPE getObjectType() const
{
return cgeom->getObjectType();
}
+ /// @brief get the node type
NODE_TYPE getNodeType() const
{
return cgeom->getNodeType();
}
+ /// @brief get the AABB in world space
inline const AABB& getAABB() const
{
return aabb;
}
+ /// @brief compute the AABB in world space
inline void computeAABB()
{
if(t.getQuatRotation().isIdentity())
@@ -165,101 +181,121 @@ public:
}
}
+ /// @brief get user data in object
void* getUserData() const
{
return user_data;
}
+ /// @brief set user data in object
void setUserData(void *data)
{
user_data = data;
}
+ /// @brief get translation of the object
inline const Vec3f& getTranslation() const
{
return t.getTranslation();
}
+ /// @brief get matrix rotation of the object
inline const Matrix3f& getRotation() const
{
return t.getRotation();
}
+ /// @brief get quaternion rotation of the object
inline const Quaternion3f& getQuatRotation() const
{
return t.getQuatRotation();
}
+ /// @brief get object's transform
inline const Transform3f& getTransform() const
{
return t;
}
+ /// @brief set object's rotation matrix
void setRotation(const Matrix3f& R)
{
t.setRotation(R);
}
+ /// @brief set object's translation
void setTranslation(const Vec3f& T)
{
t.setTranslation(T);
}
+ /// @brief set object's quatenrion rotation
void setQuatRotation(const Quaternion3f& q)
{
t.setQuatRotation(q);
}
+ /// @brief set object's transform
void setTransform(const Matrix3f& R, const Vec3f& T)
{
t.setTransform(R, T);
}
+ /// @brief set object's transform
void setTransform(const Quaternion3f& q, const Vec3f& T)
{
t.setTransform(q, T);
}
+ /// @brief set object's transform
void setTransform(const Transform3f& tf)
{
t = tf;
}
+ /// @brief whether the object is in local coordinate
bool isIdentityTransform() const
{
return t.isIdentity();
}
+ /// @brief set the object in local coordinate
void setIdentityTransform()
{
t.setIdentity();
}
+ /// @brief get geometry from the object instance
const CollisionGeometry* getCollisionGeometry() const
{
return cgeom.get();
}
+ /// @brief get object's cost density
FCL_REAL getCostDensity() const
{
return cgeom->cost_density;
}
+ /// @brief set object's cost density
void setCostDensity(FCL_REAL c)
{
cgeom->cost_density = c;
}
+ /// @brief whether the object is completely occupied
inline bool isOccupied() const
{
return cgeom->isOccupied();
}
+ /// @brief whether the object is completely free
inline bool isFree() const
{
return cgeom->isFree();
}
+ /// @brief whether the object is uncertain
inline bool isUncertain() const
{
return cgeom->isUncertain();
@@ -271,10 +307,10 @@ protected:
Transform3f t;
- /// AABB in global coordinate
+ /// @brief AABB in global coordinate
mutable AABB aabb;
- /// pointer to user defined data specific to this object
+ /// @brief pointer to user defined data specific to this object
void *user_data;
};
diff --git a/trunk/fcl/include/fcl/hierarchy_tree.h b/trunk/fcl/include/fcl/hierarchy_tree.h
deleted file mode 100644
index 254ed18084a8c3deb3e41d456f8f8bd54c7f86d4..0000000000000000000000000000000000000000
--- a/trunk/fcl/include/fcl/hierarchy_tree.h
+++ /dev/null
@@ -1,2053 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, Willow Garage, Inc.
- * 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 Jia Pan */
-
-#ifndef FCL_HIERARCHY_TREE_H
-#define FCL_HIERARCHY_TREE_H
-
-#include <vector>
-#include <map>
-#include "fcl/BV/AABB.h"
-#include "fcl/vec_3f.h"
-#include "fcl/morton.h"
-#include <boost/bind.hpp>
-#include <boost/iterator/zip_iterator.hpp>
-
-namespace fcl
-{
-
-
-template<typename BV>
-struct NodeBase
-{
- BV bv;
- NodeBase<BV>* parent;
- bool isLeaf() const { return (childs[1] == NULL); }
- bool isInternal() const { return !isLeaf(); }
- union
- {
- NodeBase<BV>* childs[2];
- void* data;
- };
-
- FCL_UINT32 code;
-
- NodeBase()
- {
- parent = NULL;
- childs[0] = NULL;
- childs[1] = NULL;
- }
-};
-
-template<typename BV>
-bool nodeBaseLess(NodeBase<BV>* a, NodeBase<BV>* b, int d)
-{
- if(a->bv.center()[d] < b->bv.center()[d]) return true;
- return false;
-}
-
-template<typename BV>
-size_t select(const NodeBase<BV>& query, const NodeBase<BV>& node1, const NodeBase<BV>& node2)
-{
- return 0;
-}
-
-template<>
-size_t select(const NodeBase<AABB>& node, const NodeBase<AABB>& node1, const NodeBase<AABB>& node2);
-
-template<typename BV>
-size_t select(const BV& query, const NodeBase<BV>& node1, const NodeBase<BV>& node2)
-{
- return 0;
-}
-
-template<>
-size_t select(const AABB& query, const NodeBase<AABB>& node1, const NodeBase<AABB>& node2);
-
-
-
-template<typename BV>
-class HierarchyTree
-{
- typedef NodeBase<BV> NodeType;
- typedef typename std::vector<NodeBase<BV>* >::iterator NodeVecIterator;
- typedef typename std::vector<NodeBase<BV>* >::const_iterator NodeVecConstIterator;
-
- struct SortByMorton
- {
- bool operator() (NodeType* a, NodeType* b) const
- {
- return a->code < b->code;
- }
- };
-
-public:
-
- HierarchyTree(int bu_threshold_ = 16, int topdown_level_ = 0)
- {
- root_node = NULL;
- n_leaves = 0;
- free_node = NULL;
- max_lookahead_level = -1;
- opath = 0;
- bu_threshold = bu_threshold_;
- topdown_level = topdown_level_;
- }
-
- ~HierarchyTree()
- {
- clear();
- }
-
- void init(std::vector<NodeType*>& leaves, int level = 0)
- {
- switch(level)
- {
- case 0:
- init_0(leaves);
- break;
- case 1:
- init_1(leaves);
- break;
- case 2:
- init_2(leaves);
- break;
- case 3:
- init_3(leaves);
- break;
- default:
- init_0(leaves);
- }
- }
-
- /** \brief Insest a node */
- NodeType* insert(const BV& bv, void* data)
- {
- NodeType* leaf = createNode(NULL, bv, data);
- insertLeaf(root_node, leaf);
- ++n_leaves;
- return leaf;
- }
-
- /** \brief Remove a leaf node */
- void remove(NodeType* leaf)
- {
- removeLeaf(leaf);
- deleteNode(leaf);
- --n_leaves;
- }
-
- /** \brief Clear the tree */
- void clear()
- {
- if(root_node)
- recurseDeleteNode(root_node);
- n_leaves = 0;
- delete free_node;
- free_node = NULL;
- max_lookahead_level = -1;
- opath = 0;
- }
-
- /** \brief Whether the tree is empty */
- bool empty() const
- {
- return (NULL == root_node);
- }
-
- /** \brief update one leaf node */
- void update(NodeType* leaf, int lookahead_level = -1)
- {
- NodeType* root = removeLeaf(leaf);
- if(root)
- {
- if(lookahead_level > 0)
- {
- for(int i = 0; (i < lookahead_level) && root->parent; ++i)
- root = root->parent;
- }
- else
- root = root_node;
- }
- insertLeaf(root, leaf);
- }
-
- bool update(NodeType* leaf, const BV& bv)
- {
- if(leaf->bv.contain(bv)) return false;
- update_(leaf, bv);
- return true;
- }
-
- /** \brief update one leaf's bounding volume, with prediction */
- bool update(NodeType* leaf, const BV& bv, const Vec3f& vel, FCL_REAL margin)
- {
- if(leaf->bv.contain(bv)) return false;
- update_(leaf, bv);
- return true;
- }
-
- /** \brief update one leaf's bounding volume, with prediction */
- bool update(NodeType* leaf, const BV& bv, const Vec3f& vel)
- {
- if(leaf->bv.contain(bv)) return false;
- update_(leaf, bv);
- return true;
- }
-
- size_t getMaxHeight() const
- {
- if(!root_node)
- return 0;
- return getMaxHeight(root_node);
- }
-
- size_t getMaxDepth() const
- {
- if(!root_node) return 0;
-
- size_t max_depth;
- getMaxDepth(root_node, 0, max_depth);
- return max_depth;
- }
-
- /** \brief balance the tree from bottom */
- void balanceBottomup()
- {
- if(root_node)
- {
- std::vector<NodeType*> leaves;
- leaves.reserve(n_leaves);
- fetchLeaves(root_node, leaves);
- bottomup(leaves.begin(), leaves.end());
- root_node = leaves[0];
- }
- }
-
- /** \brief balance the tree from top */
- void balanceTopdown()
- {
- if(root_node)
- {
- std::vector<NodeType*> leaves;
- leaves.reserve(n_leaves);
- fetchLeaves(root_node, leaves);
- root_node = topdown(leaves.begin(), leaves.end());
- }
- }
-
-
- /** \brief balance the tree in an incremental way */
- void balanceIncremental(int iterations)
- {
- if(iterations < 0) iterations = n_leaves;
- if(root_node && (iterations > 0))
- {
- for(int i = 0; i < iterations; ++i)
- {
- NodeType* node = root_node;
- unsigned int bit = 0;
- while(!node->isLeaf())
- {
- node = sort(node, root_node)->childs[(opath>>bit)&1];
- bit = (bit+1)&(sizeof(unsigned int) * 8-1);
- }
- update(node);
- ++opath;
- }
- }
- }
-
- /** \brief refit */
- void refit()
- {
- if(root_node)
- recurseRefit(root_node);
- }
-
- /** \brief extract all the leaves of the tree */
- void extractLeaves(const NodeType* root, std::vector<NodeType*>& leaves) const
- {
- if(!root->isLeaf())
- {
- extractLeaves(root->childs[0], leaves);
- extractLeaves(root->childs[1], leaves);
- }
- else
- leaves.push_back(root);
- }
-
- size_t size() const
- {
- return n_leaves;
- }
-
- inline NodeType* getRoot() const
- {
- return root_node;
- }
-
- inline NodeType*& getRoot()
- {
- return root_node;
- }
-
- void print(NodeType* root, int depth)
- {
- for(int i = 0; i < depth; ++i)
- std::cout << " ";
- std::cout << " (" << root->bv.min_[0] << ", " << root->bv.min_[1] << ", " << root->bv.min_[2] << "; " << root->bv.max_[0] << ", " << root->bv.max_[1] << ", " << root->bv.max_[2] << ")" << std::endl;
- if(root->isLeaf())
- {
- }
- else
- {
- print(root->childs[0], depth+1);
- print(root->childs[1], depth+1);
- }
- }
-
-
-
-private:
-
- /** \brief construct a tree for a set of leaves from bottom -- very heavy way */
- void bottomup(const NodeVecIterator lbeg, const NodeVecIterator lend)
- {
- NodeVecIterator lcur_end = lend;
- while(lbeg < lcur_end - 1)
- {
- NodeVecIterator min_it1, min_it2;
- FCL_REAL min_size = std::numeric_limits<FCL_REAL>::max();
- for(NodeVecIterator it1 = lbeg; it1 < lcur_end; ++it1)
- {
- for(NodeVecIterator it2 = it1 + 1; it2 < lcur_end; ++it2)
- {
- FCL_REAL cur_size = ((*it1)->bv + (*it2)->bv).size();
- if(cur_size < min_size)
- {
- min_size = cur_size;
- min_it1 = it1;
- min_it2 = it2;
- }
- }
- }
-
- NodeType* n[2] = {*min_it1, *min_it2};
- NodeType* p = createNode(NULL, n[0]->bv, n[1]->bv, NULL);
- p->childs[0] = n[0];
- p->childs[1] = n[1];
- n[0]->parent = p;
- n[1]->parent = p;
- *min_it1 = p;
- NodeType* tmp = *min_it2;
- lcur_end--;
- *min_it2 = *lcur_end;
- *lcur_end = tmp;
- }
- }
-
- /** \brief construct a tree for a set of leaves from top */
- NodeType* topdown(const NodeVecIterator lbeg, const NodeVecIterator lend)
- {
- switch(topdown_level)
- {
- case 0:
- return topdown_0(lbeg, lend);
- break;
- case 1:
- return topdown_1(lbeg, lend);
- break;
- default:
- return topdown_0(lbeg, lend);
- }
- }
-
-
- size_t getMaxHeight(NodeType* node) const
- {
- if(!node->isLeaf())
- {
- size_t height1 = getMaxHeight(node->childs[0]);
- size_t height2 = getMaxHeight(node->childs[1]);
- return std::max(height1, height2) + 1;
- }
- else
- return 0;
- }
-
- void getMaxDepth(NodeType* node, size_t depth, size_t& max_depth) const
- {
- if(!node->isLeaf())
- {
- getMaxDepth(node->childs[0], depth+1, max_depth);
- getMaxDepth(node->childs[1], depth+1, max_depth);
- }
- else
- max_depth = std::max(max_depth, depth);
- }
-
-
- NodeType* topdown_0(const NodeVecIterator lbeg, const NodeVecIterator lend)
- {
- int num_leaves = lend - lbeg;
- if(num_leaves > 1)
- {
- if(num_leaves > bu_threshold)
- {
- BV vol = (*lbeg)->bv;
- for(NodeVecIterator it = lbeg + 1; it < lend; ++it)
- vol += (*it)->bv;
-
- int best_axis = 0;
- FCL_REAL extent[3] = {vol.width(), vol.height(), vol.depth()};
- if(extent[1] > extent[0]) best_axis = 1;
- if(extent[2] > extent[best_axis]) best_axis = 2;
-
- // compute median
- NodeVecIterator lcenter = lbeg + num_leaves / 2;
- std::nth_element(lbeg, lcenter, lend, boost::bind(&nodeBaseLess<BV>, _1, _2, boost::ref(best_axis)));
-
- NodeType* node = createNode(NULL, vol, NULL);
- node->childs[0] = topdown_0(lbeg, lcenter);
- node->childs[1] = topdown_0(lcenter, lend);
- node->childs[0]->parent = node;
- node->childs[1]->parent = node;
- return node;
- }
- else
- {
- bottomup(lbeg, lend);
- return *lbeg;
- }
- }
- return *lbeg;
- }
-
-
- NodeType* topdown_1(const NodeVecIterator lbeg, const NodeVecIterator lend)
- {
- int num_leaves = lend - lbeg;
- if(num_leaves > 1)
- {
- if(num_leaves > bu_threshold)
- {
- Vec3f split_p = (*lbeg)->bv.center();
- BV vol = (*lbeg)->bv;
- NodeVecIterator it;
- for(it = lbeg + 1; it < lend; ++it)
- {
- split_p += (*it)->bv.center();
- vol += (*it)->bv;
- }
- split_p /= (FCL_REAL)(num_leaves);
- int best_axis = -1;
- int bestmidp = num_leaves;
- int splitcount[3][2] = {{0,0}, {0,0}, {0,0}};
- for(it = lbeg; it < lend; ++it)
- {
- Vec3f x = (*it)->bv.center() - split_p;
- for(size_t j = 0; j < 3; ++j)
- ++splitcount[j][x[j] > 0 ? 1 : 0];
- }
-
- for(size_t i = 0; i < 3; ++i)
- {
- if((splitcount[i][0] > 0) && (splitcount[i][1] > 0))
- {
- int midp = std::abs(splitcount[i][0] - splitcount[i][1]);
- if(midp < bestmidp)
- {
- best_axis = i;
- bestmidp = midp;
- }
- }
- }
-
- if(best_axis < 0) best_axis = 0;
-
- FCL_REAL split_value = split_p[best_axis];
- NodeVecIterator lcenter = lbeg;
- for(it = lbeg; it < lend; ++it)
- {
- if((*it)->bv.center()[best_axis] < split_value)
- {
- NodeType* temp = *it;
- *it = *lcenter;
- *lcenter = temp;
- ++lcenter;
- }
- }
-
- NodeType* node = createNode(NULL, vol, NULL);
- node->childs[0] = topdown_1(lbeg, lcenter);
- node->childs[1] = topdown_1(lcenter, lend);
- node->childs[0]->parent = node;
- node->childs[1]->parent = node;
- return node;
- }
- else
- {
- bottomup(lbeg, lend);
- return *lbeg;
- }
- }
- return *lbeg;
- }
-
-
- void init_0(std::vector<NodeType*>& leaves)
- {
- clear();
- root_node = topdown(leaves.begin(), leaves.end());
- n_leaves = leaves.size();
- max_lookahead_level = -1;
- opath = 0;
- }
-
- void init_1(std::vector<NodeType*>& leaves)
- {
- clear();
-
- BV bound_bv;
- if(leaves.size() > 0)
- bound_bv = leaves[0]->bv;
- for(size_t i = 1; i < leaves.size(); ++i)
- bound_bv += leaves[i]->bv;
-
- morton_functor<FCL_UINT32> coder(bound_bv);
- for(size_t i = 0; i < leaves.size(); ++i)
- leaves[i]->code = coder(leaves[i]->bv.center());
-
- std::sort(leaves.begin(), leaves.end(), SortByMorton());
-
- root_node = mortonRecurse_0(leaves.begin(), leaves.end(), (1 << (coder.bits()-1)), coder.bits()-1);
-
- refit();
- n_leaves = leaves.size();
- max_lookahead_level = -1;
- opath = 0;
- }
-
- void init_2(std::vector<NodeType*>& leaves)
- {
- clear();
-
- BV bound_bv;
- if(leaves.size() > 0)
- bound_bv = leaves[0]->bv;
- for(size_t i = 1; i < leaves.size(); ++i)
- bound_bv += leaves[i]->bv;
-
- morton_functor<FCL_UINT32> coder(bound_bv);
- for(size_t i = 0; i < leaves.size(); ++i)
- leaves[i]->code = coder(leaves[i]->bv.center());
-
- std::sort(leaves.begin(), leaves.end(), SortByMorton());
-
- root_node = mortonRecurse_1(leaves.begin(), leaves.end(), (1 << (coder.bits()-1)), coder.bits()-1);
-
- refit();
- n_leaves = leaves.size();
- max_lookahead_level = -1;
- opath = 0;
- }
-
- void init_3(std::vector<NodeType*>& leaves)
- {
- clear();
-
- BV bound_bv;
- if(leaves.size() > 0)
- bound_bv = leaves[0]->bv;
- for(size_t i = 1; i < leaves.size(); ++i)
- bound_bv += leaves[i]->bv;
-
- morton_functor<FCL_UINT32> coder(bound_bv);
- for(size_t i = 0; i < leaves.size(); ++i)
- leaves[i]->code = coder(leaves[i]->bv.center());
-
- std::sort(leaves.begin(), leaves.end(), SortByMorton());
-
- root_node = mortonRecurse_2(leaves.begin(), leaves.end());
-
- refit();
- n_leaves = leaves.size();
- max_lookahead_level = -1;
- opath = 0;
- }
-
-
- NodeType* mortonRecurse_0(const NodeVecIterator lbeg, const NodeVecIterator lend, const FCL_UINT32& split, int bits)
- {
- int num_leaves = lend - lbeg;
- if(num_leaves > 1)
- {
- if(bits > 0)
- {
- NodeType dummy;
- dummy.code = split;
- NodeVecIterator lcenter = std::lower_bound(lbeg, lend, &dummy, SortByMorton());
-
- if(lcenter == lbeg)
- {
- FCL_UINT32 split2 = split | (1 << (bits - 1));
- return mortonRecurse_0(lbeg, lend, split2, bits - 1);
- }
- else if(lcenter == lend)
- {
- FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
- return mortonRecurse_0(lbeg, lend, split1, bits - 1);
- }
- else
- {
- FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
- FCL_UINT32 split2 = split | (1 << (bits - 1));
-
- NodeType* child1 = mortonRecurse_0(lbeg, lcenter, split1, bits - 1);
- NodeType* child2 = mortonRecurse_0(lcenter, lend, split2, bits - 1);
- NodeType* node = createNode(NULL, NULL);
- node->childs[0] = child1;
- node->childs[1] = child2;
- child1->parent = node;
- child2->parent = node;
- return node;
- }
- }
- else
- {
- NodeType* node = topdown(lbeg, lend);
- return node;
- }
- }
- else
- return *lbeg;
- }
-
- NodeType* mortonRecurse_1(const NodeVecIterator lbeg, const NodeVecIterator lend, const FCL_UINT32& split, int bits)
- {
- int num_leaves = lend - lbeg;
- if(num_leaves > 1)
- {
- if(bits > 0)
- {
- NodeType dummy;
- dummy.code = split;
- NodeVecIterator lcenter = std::lower_bound(lbeg, lend, &dummy, SortByMorton());
-
- if(lcenter == lbeg)
- {
- FCL_UINT32 split2 = split | (1 << (bits - 1));
- return mortonRecurse_1(lbeg, lend, split2, bits - 1);
- }
- else if(lcenter == lend)
- {
- FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
- return mortonRecurse_1(lbeg, lend, split1, bits - 1);
- }
- else
- {
- FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
- FCL_UINT32 split2 = split | (1 << (bits - 1));
-
- NodeType* child1 = mortonRecurse_1(lbeg, lcenter, split1, bits - 1);
- NodeType* child2 = mortonRecurse_1(lcenter, lend, split2, bits - 1);
- NodeType* node = createNode(NULL, NULL);
- node->childs[0] = child1;
- node->childs[1] = child2;
- child1->parent = node;
- child2->parent = node;
- return node;
- }
- }
- else
- {
- NodeType* child1 = mortonRecurse_1(lbeg, lbeg + num_leaves / 2, 0, bits - 1);
- NodeType* child2 = mortonRecurse_1(lbeg + num_leaves / 2, lend, 0, bits - 1);
- NodeType* node = createNode(NULL, NULL);
- node->childs[0] = child1;
- node->childs[1] = child2;
- child1->parent = node;
- child2->parent = node;
- return node;
- }
- }
- else
- return *lbeg;
- }
-
- NodeType* mortonRecurse_2(const NodeVecIterator lbeg, const NodeVecIterator lend)
- {
- int num_leaves = lend - lbeg;
- if(num_leaves > 1)
- {
- NodeType* child1 = mortonRecurse_2(lbeg, lbeg + num_leaves / 2);
- NodeType* child2 = mortonRecurse_2(lbeg + num_leaves / 2, lend);
- NodeType* node = createNode(NULL, NULL);
- node->childs[0] = child1;
- node->childs[1] = child2;
- child1->parent = node;
- child2->parent = node;
- return node;
- }
- else
- return *lbeg;
- }
-
-
- /** \brief update one leaf node's bounding volume */
- void update_(NodeType* leaf, const BV& bv)
- {
- NodeType* root = removeLeaf(leaf);
- if(root)
- {
- if(max_lookahead_level >= 0)
- {
- for(int i = 0; (i < max_lookahead_level) && root->parent; ++i)
- root = root->parent;
- }
- else
- root = root_node;
- }
-
- leaf->bv = bv;
- insertLeaf(root, leaf);
- }
-
- /** \brief sort node n and its parent according to their memory position */
- NodeType* sort(NodeType* n, NodeType*& r)
- {
- NodeType* p = n->parent;
- if(p > n)
- {
- int i = indexOf(n);
- int j = 1 - i;
- NodeType* s = p->childs[j];
- NodeType* q = p->parent;
- if(q) q->childs[indexOf(p)] = n; else r = n;
- s->parent = n;
- p->parent = n;
- n->parent = q;
- p->childs[0] = n->childs[0];
- p->childs[1] = n->childs[1];
- n->childs[0]->parent = p;
- n->childs[1]->parent = p;
- n->childs[i] = p;
- n->childs[j] = s;
- std::swap(p->bv, n->bv);
- return p;
- }
- return n;
- }
-
- /** \brief Insert a leaf node and also update its ancestors */
- void insertLeaf(NodeType* root, NodeType* leaf)
- {
- if(!root_node)
- {
- root_node = leaf;
- leaf->parent = NULL;
- }
- else
- {
- if(!root->isLeaf())
- {
- do
- {
- root = root->childs[select(*leaf, *(root->childs[0]), *(root->childs[1]))];
- }
- while(!root->isLeaf());
- }
-
- NodeType* prev = root->parent;
- NodeType* node = createNode(prev, leaf->bv, root->bv, NULL);
- if(prev)
- {
- prev->childs[indexOf(root)] = node;
- node->childs[0] = root; root->parent = node;
- node->childs[1] = leaf; leaf->parent = node;
- do
- {
- if(!prev->bv.contain(node->bv))
- prev->bv = prev->childs[0]->bv + prev->childs[1]->bv;
- else
- break;
- node = prev;
- } while (NULL != (prev = node->parent));
- }
- else
- {
- node->childs[0] = root; root->parent = node;
- node->childs[1] = leaf; leaf->parent = node;
- root_node = node;
- }
- }
- }
-
- /** \brief Remove a leaf. The leaf node itself is not deleted yet, but all the unnecessary internal nodes are deleted.
- return the node with the smallest depth and is influenced by the remove operation */
- NodeType* removeLeaf(NodeType* leaf)
- {
- if(leaf == root_node)
- {
- root_node = NULL;
- return NULL;
- }
- else
- {
- NodeType* parent = leaf->parent;
- NodeType* prev = parent->parent;
- NodeType* sibling = parent->childs[1-indexOf(leaf)];
- if(prev)
- {
- prev->childs[indexOf(parent)] = sibling;
- sibling->parent = prev;
- deleteNode(parent);
- while(prev)
- {
- BV new_bv = prev->childs[0]->bv + prev->childs[1]->bv;
- if(!new_bv.equal(prev->bv))
- {
- prev->bv = new_bv;
- prev = prev->parent;
- }
- else break;
- }
-
- return prev ? prev : root_node;
- }
- else
- {
- root_node = sibling;
- sibling->parent = NULL;
- deleteNode(parent);
- return root_node;
- }
- }
- }
-
- /** \brief Delete all internal nodes and return all leaves nodes with given depth from root */
- void fetchLeaves(NodeType* root, std::vector<NodeType*>& leaves, int depth = -1)
- {
- if((!root->isLeaf()) && depth)
- {
- fetchLeaves(root->childs[0], leaves, depth-1);
- fetchLeaves(root->childs[1], leaves, depth-1);
- deleteNode(root);
- }
- else
- {
- leaves.push_back(root);
- }
- }
-
- static size_t indexOf(NodeType* node)
- {
- // node cannot be NULL
- return (node->parent->childs[1] == node);
- }
-
- NodeType* createNode(NodeType* parent,
- const BV& bv,
- void* data)
- {
- NodeType* node = createNode(parent, data);
- node->bv = bv;
- return node;
- }
-
- /** \brief create one node (leaf or internal */
- NodeType* createNode(NodeType* parent,
- const BV& bv1,
- const BV& bv2,
- void* data)
- {
- NodeType* node = createNode(parent, data);
- node->bv = bv1 + bv2;
- return node;
- }
-
-
- inline NodeType* createNode(NodeType* parent,
- void* data)
- {
- NodeType* node = NULL;
- if(free_node)
- {
- node = free_node;
- free_node = NULL;
- }
- else
- node = new NodeType();
- node->parent = parent;
- node->data = data;
- node->childs[1] = 0;
- return node;
- }
-
- inline void deleteNode(NodeType* node)
- {
- if(free_node != node)
- {
- delete free_node;
- free_node = node;
- }
- }
-
- void recurseDeleteNode(NodeType* node)
- {
- if(!node->isLeaf())
- {
- recurseDeleteNode(node->childs[0]);
- recurseDeleteNode(node->childs[1]);
- }
-
- if(node == root_node) root_node = NULL;
- deleteNode(node);
- }
-
- void recurseRefit(NodeType* node)
- {
- if(!node->isLeaf())
- {
- recurseRefit(node->childs[0]);
- recurseRefit(node->childs[1]);
- node->bv = node->childs[0]->bv + node->childs[1]->bv;
- }
- else
- return;
- }
-
-
- static BV bounds(const std::vector<NodeType*>& leaves)
- {
- if(leaves.size() == 0) return BV();
- BV bv = leaves[0]->bv;
- for(size_t i = 1; i < leaves.size(); ++i)
- {
- bv += leaves[i]->bv;
- }
-
- return bv;
- }
-
- static BV bounds(const NodeVecIterator lbeg, const NodeVecIterator lend)
- {
- if(lbeg == lend) return BV();
- BV bv = *lbeg;
- for(NodeVecIterator it = lbeg + 1; it < lend; ++it)
- {
- bv += (*it)->bv;
- }
-
- return bv;
- }
-
-protected:
- NodeType* root_node;
- size_t n_leaves;
-
- unsigned int opath;
-
- NodeType* free_node; // This is a one NodeType cache, the reason is that we need to remove a node and then add it again frequently.
-
- int max_lookahead_level;
-
-public:
-
- int topdown_level;
- int bu_threshold;
-
-
-};
-
-
-template<>
-bool HierarchyTree<AABB>::update(NodeBase<AABB>* leaf, const AABB& bv_, const Vec3f& vel, FCL_REAL margin);
-
-template<>
-bool HierarchyTree<AABB>::update(NodeBase<AABB>* leaf, const AABB& bv_, const Vec3f& vel);
-
-
-namespace alternative
-{
-
-template<typename BV>
-struct NodeBase
-{
- BV bv;
-
- union
- {
- size_t parent;
- size_t next;
- };
-
- union
- {
- size_t childs[2];
- void* data;
- };
-
- FCL_UINT32 code;
-
- bool isLeaf() const { return (childs[1] == (size_t)(-1)); }
- bool isInternal() const { return !isLeaf(); }
-};
-
-template<typename BV>
-struct nodeBaseLess
-{
- nodeBaseLess(const NodeBase<BV>* nodes_, size_t d_)
- {
- nodes = nodes_;
- d = d_;
- }
-
- bool operator() (size_t i, size_t j) const
- {
- if(nodes[i].bv.center()[d] < nodes[j].bv.center()[d])
- return true;
- return false;
- }
-
- const NodeBase<BV>* nodes;
- size_t d;
-};
-
-
-
-
-template<typename BV>
-size_t select(size_t query, size_t node1, size_t node2, NodeBase<BV>* nodes)
-{
- return 0;
-}
-
-template<>
-size_t select(size_t query, size_t node1, size_t node2, NodeBase<AABB>* nodes);
-
-template<typename BV>
-size_t select(const BV& query, size_t node1, size_t node2, NodeBase<BV>* nodes)
-{
- return 0;
-}
-
-template<>
-size_t select(const AABB& query, size_t node1, size_t node2, NodeBase<AABB>* nodes);
-
-
-template<typename BV>
-class HierarchyTree
-{
- typedef NodeBase<BV> NodeType;
-
- struct SortByMorton
- {
- bool operator() (size_t a, size_t b) const
- {
- if((a != NULL_NODE) && (b != NULL_NODE))
- {
- return nodes[a].code < nodes[b].code;
- }
- else if(a == NULL_NODE)
- {
- return split < nodes[b].code;
- }
- else if(b == NULL_NODE)
- {
- return nodes[a].code < split;
- }
- return false;
- }
-
- NodeType* nodes;
- FCL_UINT32 split;
- };
-
-public:
- HierarchyTree(int bu_threshold_ = 16, int topdown_level_ = 0)
- {
- root_node = NULL_NODE;
- n_nodes = 0;
- n_nodes_alloc = 16;
- nodes = new NodeType[n_nodes_alloc];
- for(size_t i = 0; i < n_nodes_alloc - 1; ++i)
- nodes[i].next = i + 1;
- nodes[n_nodes_alloc - 1].next = NULL_NODE;
- n_leaves = 0;
- freelist = 0;
- opath = 0;
- max_lookahead_level = -1;
- bu_threshold = bu_threshold_;
- topdown_level = topdown_level_;
- }
-
- ~HierarchyTree()
- {
- delete [] nodes;
- }
-
- void init(NodeType* leaves, int n_leaves_, int level = 0)
- {
- switch(level)
- {
- case 0:
- init_0(leaves, n_leaves_);
- break;
- case 1:
- init_1(leaves, n_leaves_);
- break;
- case 2:
- init_2(leaves, n_leaves_);
- break;
- case 3:
- init_3(leaves, n_leaves_);
- break;
- default:
- init_0(leaves, n_leaves_);
- }
- }
-
- void init_0(NodeType* leaves, int n_leaves_)
- {
- clear();
-
- n_leaves = n_leaves_;
- root_node = NULL_NODE;
- nodes = new NodeType[n_leaves * 2];
- memcpy(nodes, leaves, sizeof(NodeType) * n_leaves);
- freelist = n_leaves;
- n_nodes = n_leaves;
- n_nodes_alloc = 2 * n_leaves;
- for(size_t i = n_leaves; i < n_nodes_alloc; ++i)
- nodes[i].next = i + 1;
- nodes[n_nodes_alloc - 1].next = NULL_NODE;
-
- size_t* ids = new size_t[n_leaves];
- for(size_t i = 0; i < n_leaves; ++i)
- ids[i] = i;
-
- root_node = topdown(ids, ids + n_leaves);
- delete [] ids;
-
- opath = 0;
- max_lookahead_level = -1;
- }
-
- void init_1(NodeType* leaves, int n_leaves_)
- {
- clear();
-
- n_leaves = n_leaves_;
- root_node = NULL_NODE;
- nodes = new NodeType[n_leaves * 2];
- memcpy(nodes, leaves, sizeof(NodeType) * n_leaves);
- freelist = n_leaves;
- n_nodes = n_leaves;
- n_nodes_alloc = 2 * n_leaves;
- for(size_t i = n_leaves; i < n_nodes_alloc; ++i)
- nodes[i].next = i + 1;
- nodes[n_nodes_alloc - 1].next = NULL_NODE;
-
-
- BV bound_bv;
- if(n_leaves > 0)
- bound_bv = nodes[0].bv;
- for(size_t i = 1; i < n_leaves; ++i)
- bound_bv += nodes[i].bv;
-
- morton_functor<FCL_UINT32> coder(bound_bv);
- for(size_t i = 0; i < n_leaves; ++i)
- nodes[i].code = coder(nodes[i].bv.center());
-
-
- size_t* ids = new size_t[n_leaves];
- for(size_t i = 0; i < n_leaves; ++i)
- ids[i] = i;
-
- SortByMorton comp;
- comp.nodes = nodes;
- std::sort(ids, ids + n_leaves, comp);
- root_node = mortonRecurse_0(ids, ids + n_leaves, (1 << coder.bits()-1), coder.bits()-1);
- delete [] ids;
-
- refit();
-
- opath = 0;
- max_lookahead_level = -1;
- }
-
- void init_2(NodeType* leaves, int n_leaves_)
- {
- clear();
-
- n_leaves = n_leaves_;
- root_node = NULL_NODE;
- nodes = new NodeType[n_leaves * 2];
- memcpy(nodes, leaves, sizeof(NodeType) * n_leaves);
- freelist = n_leaves;
- n_nodes = n_leaves;
- n_nodes_alloc = 2 * n_leaves;
- for(size_t i = n_leaves; i < n_nodes_alloc; ++i)
- nodes[i].next = i + 1;
- nodes[n_nodes_alloc - 1].next = NULL_NODE;
-
-
- BV bound_bv;
- if(n_leaves > 0)
- bound_bv = nodes[0].bv;
- for(size_t i = 1; i < n_leaves; ++i)
- bound_bv += nodes[i].bv;
-
- morton_functor<FCL_UINT32> coder(bound_bv);
- for(size_t i = 0; i < n_leaves; ++i)
- nodes[i].code = coder(nodes[i].bv.center());
-
-
- size_t* ids = new size_t[n_leaves];
- for(size_t i = 0; i < n_leaves; ++i)
- ids[i] = i;
-
- SortByMorton comp;
- comp.nodes = nodes;
- std::sort(ids, ids + n_leaves, comp);
- root_node = mortonRecurse_1(ids, ids + n_leaves, (1 << coder.bits()-1), coder.bits()-1);
- delete [] ids;
-
- refit();
-
- opath = 0;
- max_lookahead_level = -1;
- }
-
- void init_3(NodeType* leaves, int n_leaves_)
- {
- clear();
-
- n_leaves = n_leaves_;
- root_node = NULL_NODE;
- nodes = new NodeType[n_leaves * 2];
- memcpy(nodes, leaves, sizeof(NodeType) * n_leaves);
- freelist = n_leaves;
- n_nodes = n_leaves;
- n_nodes_alloc = 2 * n_leaves;
- for(size_t i = n_leaves; i < n_nodes_alloc; ++i)
- nodes[i].next = i + 1;
- nodes[n_nodes_alloc - 1].next = NULL_NODE;
-
-
- BV bound_bv;
- if(n_leaves > 0)
- bound_bv = nodes[0].bv;
- for(size_t i = 1; i < n_leaves; ++i)
- bound_bv += nodes[i].bv;
-
- morton_functor<FCL_UINT32> coder(bound_bv);
- for(size_t i = 0; i < n_leaves; ++i)
- nodes[i].code = coder(nodes[i].bv.center());
-
-
- size_t* ids = new size_t[n_leaves];
- for(size_t i = 0; i < n_leaves; ++i)
- ids[i] = i;
-
- SortByMorton comp;
- comp.nodes = nodes;
- std::sort(ids, ids + n_leaves, comp);
- root_node = mortonRecurse_2(ids, ids + n_leaves);
- delete [] ids;
-
- refit();
-
- opath = 0;
- max_lookahead_level = -1;
- }
-
-
-
-
- size_t insert(const BV& bv, void* data)
- {
- size_t node = createNode(NULL_NODE, bv, data);
- insertLeaf(root_node, node);
- ++n_leaves;
- return node;
- }
-
- void remove(size_t leaf)
- {
- removeLeaf(leaf);
- deleteNode(leaf);
- --n_leaves;
- }
-
- void clear()
- {
- delete [] nodes;
- root_node = NULL_NODE;
- n_nodes = 0;
- n_nodes_alloc = 16;
- nodes = new NodeType[n_nodes_alloc];
- for(size_t i = 0; i < n_nodes_alloc; ++i)
- nodes[i].next = i + 1;
- nodes[n_nodes_alloc - 1].next = NULL_NODE;
- n_leaves = 0;
- freelist = 0;
- opath = 0;
- max_lookahead_level = -1;
- }
-
- bool empty() const
- {
- return (n_nodes == 0);
- }
-
-
- void update(size_t leaf, int lookahead_level = -1)
- {
- size_t root = removeLeaf(leaf);
- if(root != NULL_NODE)
- {
- if(lookahead_level > 0)
- {
- for(int i = 0; (i < lookahead_level) && (nodes[root].parent != NULL_NODE); ++i)
- root = nodes[root].parent;
- }
- else
- root = root_node;
- }
- insertLeaf(root, leaf);
- }
-
- bool update(size_t leaf, const BV& bv)
- {
- if(nodes[leaf].bv.contain(bv)) return false;
- update_(leaf, bv);
- return true;
- }
-
- bool update(size_t leaf, const BV& bv, const Vec3f& vel, FCL_REAL margin)
- {
- if(nodes[leaf].bv.contain(bv)) return false;
- update_(leaf, bv);
- return true;
- }
-
- bool update(size_t leaf, const BV& bv, const Vec3f& vel)
- {
- if(nodes[leaf].bv.contain(bv)) return false;
- update_(leaf, bv);
- return true;
- }
-
-
- size_t getMaxHeight() const
- {
- if(root_node == NULL_NODE) return 0;
-
- return getMaxHeight(root_node);
- }
-
- size_t getMaxDepth() const
- {
- if(root_node == NULL_NODE) return 0;
-
- size_t max_depth;
- getMaxDepth(root_node, 0, max_depth);
- return max_depth;
- }
-
- void balanceBottomup()
- {
- if(root_node != NULL_NODE)
- {
- NodeType* leaves = new NodeType[n_leaves];
- NodeType* leaves_ = leaves;
- extractLeaves(root_node, leaves_);
- root_node = NULL_NODE;
- memcpy(nodes, leaves, sizeof(NodeType) * n_leaves);
- freelist = n_leaves;
- n_nodes = n_leaves;
- for(size_t i = n_leaves; i < n_nodes_alloc; ++i)
- nodes[i].next = i + 1;
- nodes[n_nodes_alloc - 1].next = NULL_NODE;
-
-
- size_t* ids = new size_t[n_leaves];
- for(size_t i = 0; i < n_leaves; ++i)
- ids[i] = i;
-
- bottomup(ids, ids + n_leaves);
- root_node = *ids;
-
- delete [] ids;
- }
- }
-
- void balanceTopdown()
- {
- if(root_node != NULL_NODE)
- {
- NodeType* leaves = new NodeType[n_leaves];
- NodeType* leaves_ = leaves;
- extractLeaves(root_node, leaves_);
- root_node = NULL_NODE;
- memcpy(nodes, leaves, sizeof(NodeType) * n_leaves);
- freelist = n_leaves;
- n_nodes = n_leaves;
- for(size_t i = n_leaves; i < n_nodes_alloc; ++i)
- nodes[i].next = i + 1;
- nodes[n_nodes_alloc - 1].next = NULL_NODE;
-
- size_t* ids = new size_t[n_leaves];
- for(size_t i = 0; i < n_leaves; ++i)
- ids[i] = i;
-
- root_node = topdown(ids, ids + n_leaves);
- delete [] ids;
- }
- }
-
- void balanceIncremental(int iterations)
- {
- if(iterations < 0) iterations = n_leaves;
- if((root_node != NULL_NODE) && (iterations > 0))
- {
- for(int i = 0; i < iterations; ++i)
- {
- size_t node = root_node;
- unsigned int bit = 0;
- while(!nodes[node].isLeaf())
- {
- node = nodes[node].childs[(opath>>bit)&1];
- bit = (bit+1)&(sizeof(unsigned int) * 8-1);
- }
- update(node);
- ++opath;
- }
- }
- }
-
- void refit()
- {
- if(root_node != NULL_NODE)
- recurseRefit(root_node);
- }
-
- void extractLeaves(size_t root, NodeType*& leaves) const
- {
- if(!nodes[root].isLeaf())
- {
- extractLeaves(nodes[root].childs[0], leaves);
- extractLeaves(nodes[root].childs[1], leaves);
- }
- else
- {
- *leaves = nodes[root];
- leaves++;
- }
- }
-
- size_t size() const
- {
- return n_leaves;
- }
-
- size_t getRoot() const
- {
- return root_node;
- }
-
- NodeType* getNodes() const
- {
- return nodes;
- }
-
- void print(size_t root, int depth)
- {
- for(int i = 0; i < depth; ++i)
- std::cout << " ";
- NodeType* n = nodes + root;
- std::cout << " (" << n->bv.min_[0] << ", " << n->bv.min_[1] << ", " << n->bv.min_[2] << "; " << n->bv.max_[0] << ", " << n->bv.max_[1] << ", " << n->bv.max_[2] << ")" << std::endl;
- if(n->isLeaf())
- {
- }
- else
- {
- print(n->childs[0], depth+1);
- print(n->childs[1], depth+1);
- }
- }
-
-
-
-private:
-
- size_t getMaxHeight(size_t node) const
- {
- if(!nodes[node].isLeaf())
- {
- size_t height1 = getMaxHeight(nodes[node].childs[0]);
- size_t height2 = getMaxHeight(nodes[node].childs[1]);
- return std::max(height1, height2) + 1;
- }
- else
- return 0;
- }
-
- void getMaxDepth(size_t node, size_t depth, size_t& max_depth) const
- {
- if(!nodes[node].isLeaf())
- {
- getMaxDepth(nodes[node].childs[0], depth+1, max_depth);
- getmaxDepth(nodes[node].childs[1], depth+1, max_depth);
- }
- else
- max_depth = std::max(max_depth, depth);
- }
-
-
- void bottomup(size_t* lbeg, size_t* lend)
- {
- size_t* lcur_end = lend;
- while(lbeg < lcur_end - 1)
- {
- size_t* min_it1, *min_it2;
- FCL_REAL min_size = std::numeric_limits<FCL_REAL>::max();
- for(size_t* it1 = lbeg; it1 < lcur_end; ++it1)
- {
- for(size_t* it2 = it1 + 1; it2 < lcur_end; ++it2)
- {
- FCL_REAL cur_size = (nodes[*it1].bv + nodes[*it2].bv).size();
- if(cur_size < min_size)
- {
- min_size = cur_size;
- min_it1 = it1;
- min_it2 = it2;
- }
- }
- }
-
- size_t p = createNode(NULL_NODE, nodes[*min_it1].bv, nodes[*min_it2].bv, NULL);
- nodes[p].childs[0] = *min_it1;
- nodes[p].childs[1] = *min_it2;
- nodes[*min_it1].parent = p;
- nodes[*min_it2].parent = p;
- *min_it1 = p;
- size_t tmp = *min_it2;
- lcur_end--;
- *min_it2 = *lcur_end;
- *lcur_end = tmp;
- }
- }
-
- size_t topdown(size_t* lbeg, size_t* lend)
- {
- switch(topdown_level)
- {
- case 0:
- return topdown_0(lbeg, lend);
- break;
- case 1:
- return topdown_1(lbeg, lend);
- break;
- default:
- return topdown_0(lbeg, lend);
- }
- }
-
-
-
- size_t topdown_0(size_t* lbeg, size_t* lend)
- {
- int num_leaves = lend - lbeg;
- if(num_leaves > 1)
- {
- if(num_leaves > bu_threshold)
- {
- BV vol = nodes[*lbeg].bv;
- for(size_t* i = lbeg + 1; i < lend; ++i)
- vol += nodes[*i].bv;
-
- int best_axis = 0;
- FCL_REAL extent[3] = {vol.width(), vol.height(), vol.depth()};
- if(extent[1] > extent[0]) best_axis = 1;
- if(extent[2] > extent[best_axis]) best_axis = 2;
-
- nodeBaseLess<BV> comp(nodes, best_axis);
- size_t* lcenter = lbeg + num_leaves / 2;
- std::nth_element(lbeg, lcenter, lend, comp);
-
- size_t node = createNode(NULL_NODE, vol, NULL);
- nodes[node].childs[0] = topdown_0(lbeg, lcenter);
- nodes[node].childs[1] = topdown_0(lcenter, lend);
- nodes[nodes[node].childs[0]].parent = node;
- nodes[nodes[node].childs[1]].parent = node;
- return node;
- }
- else
- {
- bottomup(lbeg, lend);
- return *lbeg;
- }
- }
- return *lbeg;
- }
-
- size_t topdown_1(size_t* lbeg, size_t* lend)
- {
- int num_leaves = lend - lbeg;
- if(num_leaves > 1)
- {
- if(num_leaves > bu_threshold)
- {
- Vec3f split_p = nodes[*lbeg].bv.center();
- BV vol = nodes[*lbeg].bv;
- for(size_t* i = lbeg + 1; i < lend; ++i)
- {
- split_p += nodes[*i].bv.center();
- vol += nodes[*i].bv;
- }
- split_p /= (FCL_REAL)(num_leaves);
- int best_axis = -1;
- int bestmidp = num_leaves;
- int splitcount[3][2] = {{0,0}, {0,0}, {0,0}};
- for(size_t* i = lbeg; i < lend; ++i)
- {
- Vec3f x = nodes[*i].bv.center() - split_p;
- for(size_t j = 0; j < 3; ++j)
- ++splitcount[j][x[j] > 0 ? 1 : 0];
- }
-
- for(size_t i = 0; i < 3; ++i)
- {
- if((splitcount[i][0] > 0) && (splitcount[i][1] > 0))
- {
- int midp = std::abs(splitcount[i][0] - splitcount[i][1]);
- if(midp < bestmidp)
- {
- best_axis = i;
- bestmidp = midp;
- }
- }
- }
-
- if(best_axis < 0) best_axis = 0;
-
- FCL_REAL split_value = split_p[best_axis];
- size_t* lcenter = lbeg;
- for(size_t* i = lbeg; i < lend; ++i)
- {
- if(nodes[*i].bv.center()[best_axis] < split_value)
- {
- size_t temp = *i;
- *i = *lcenter;
- *lcenter = temp;
- ++lcenter;
- }
- }
-
- size_t node = createNode(NULL_NODE, vol, NULL);
- nodes[node].childs[0] = topdown_1(lbeg, lcenter);
- nodes[node].childs[1] = topdown_1(lcenter, lend);
- nodes[nodes[node].childs[0]].parent = node;
- nodes[nodes[node].childs[1]].parent = node;
- return node;
- }
- else
- {
- bottomup(lbeg, lend);
- return *lbeg;
- }
- }
- return *lbeg;
- }
-
- size_t mortonRecurse_0(size_t* lbeg, size_t* lend, const FCL_UINT32& split, int bits)
- {
- int num_leaves = lend - lbeg;
- if(num_leaves > 1)
- {
- if(bits > 0)
- {
- SortByMorton comp;
- comp.nodes = nodes;
- comp.split = split;
- size_t* lcenter = std::lower_bound(lbeg, lend, NULL_NODE, comp);
-
- if(lcenter == lbeg)
- {
- FCL_UINT32 split2 = split | (1 << (bits - 1));
- return mortonRecurse_0(lbeg, lend, split2, bits - 1);
- }
- else if(lcenter == lend)
- {
- FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
- return mortonRecurse_0(lbeg, lend, split1, bits - 1);
- }
- else
- {
- FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
- FCL_UINT32 split2 = split | (1 << (bits - 1));
-
- size_t child1 = mortonRecurse_0(lbeg, lcenter, split1, bits - 1);
- size_t child2 = mortonRecurse_0(lcenter, lend, split2, bits - 1);
- size_t node = createNode(NULL_NODE, NULL);
- nodes[node].childs[0] = child1;
- nodes[node].childs[1] = child2;
- nodes[child1].parent = node;
- nodes[child2].parent = node;
- return node;
- }
- }
- else
- {
- size_t node = topdown(lbeg, lend);
- return node;
- }
- }
- else
- return *lbeg;
- }
-
- size_t mortonRecurse_1(size_t* lbeg, size_t* lend, const FCL_UINT32& split, int bits)
- {
- int num_leaves = lend - lbeg;
- if(num_leaves > 1)
- {
- if(bits > 0)
- {
- SortByMorton comp;
- comp.nodes = nodes;
- comp.split = split;
- size_t* lcenter = std::lower_bound(lbeg, lend, NULL_NODE, comp);
-
- if(lcenter == lbeg)
- {
- FCL_UINT32 split2 = split | (1 << (bits - 1));
- return mortonRecurse_1(lbeg, lend, split2, bits - 1);
- }
- else if(lcenter == lend)
- {
- FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
- return mortonRecurse_1(lbeg, lend, split1, bits - 1);
- }
- else
- {
- FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
- FCL_UINT32 split2 = split | (1 << (bits - 1));
-
- size_t child1 = mortonRecurse_1(lbeg, lcenter, split1, bits - 1);
- size_t child2 = mortonRecurse_1(lcenter, lend, split2, bits - 1);
- size_t node = createNode(NULL_NODE, NULL);
- nodes[node].childs[0] = child1;
- nodes[node].childs[1] = child2;
- nodes[child1].parent = node;
- nodes[child2].parent = node;
- return node;
- }
- }
- else
- {
- size_t child1 = mortonRecurse_1(lbeg, lbeg + num_leaves / 2, 0, bits - 1);
- size_t child2 = mortonRecurse_1(lbeg + num_leaves / 2, lend, 0, bits - 1);
- size_t node = createNode(NULL_NODE, NULL);
- nodes[node].childs[0] = child1;
- nodes[node].childs[1] = child2;
- nodes[child1].parent = node;
- nodes[child2].parent = node;
- return node;
- }
- }
- else
- return *lbeg;
- }
-
- size_t mortonRecurse_2(size_t* lbeg, size_t* lend)
- {
- int num_leaves = lend - lbeg;
- if(num_leaves > 1)
- {
- size_t child1 = mortonRecurse_2(lbeg, lbeg + num_leaves / 2);
- size_t child2 = mortonRecurse_2(lbeg + num_leaves / 2, lend);
- size_t node = createNode(NULL_NODE, NULL);
- nodes[node].childs[0] = child1;
- nodes[node].childs[1] = child2;
- nodes[child1].parent = node;
- nodes[child2].parent = node;
- return node;
- }
- else
- return *lbeg;
- }
-
-
- void insertLeaf(size_t root, size_t leaf)
- {
- if(root_node == NULL_NODE)
- {
- root_node = leaf;
- nodes[leaf].parent = NULL_NODE;
- }
- else
- {
- if(!nodes[root].isLeaf())
- {
- do
- {
- root = nodes[root].childs[select(leaf, nodes[root].childs[0], nodes[root].childs[1], nodes)];
- }
- while(!nodes[root].isLeaf());
- }
-
- size_t prev = nodes[root].parent;
- size_t node = createNode(prev, nodes[leaf].bv, nodes[root].bv, NULL);
- if(prev != NULL_NODE)
- {
- nodes[prev].childs[indexOf(root)] = node;
- nodes[node].childs[0] = root; nodes[root].parent = node;
- nodes[node].childs[1] = leaf; nodes[leaf].parent = node;
- do
- {
- if(!nodes[prev].bv.contain(nodes[node].bv))
- nodes[prev].bv = nodes[nodes[prev].childs[0]].bv + nodes[nodes[prev].childs[1]].bv;
- else
- break;
- node = prev;
- } while (NULL_NODE != (prev = nodes[node].parent));
- }
- else
- {
- nodes[node].childs[0] = root; nodes[root].parent = node;
- nodes[node].childs[1] = leaf; nodes[leaf].parent = node;
- root_node = node;
- }
- }
- }
-
- size_t removeLeaf(size_t leaf)
- {
- if(leaf == root_node)
- {
- root_node = NULL_NODE;
- return NULL_NODE;
- }
- else
- {
- size_t parent = nodes[leaf].parent;
- size_t prev = nodes[parent].parent;
- size_t sibling = nodes[parent].childs[1-indexOf(leaf)];
-
- if(prev != NULL_NODE)
- {
- nodes[prev].childs[indexOf(parent)] = sibling;
- nodes[sibling].parent = prev;
- deleteNode(parent);
- while(prev != NULL_NODE)
- {
- BV new_bv = nodes[nodes[prev].childs[0]].bv + nodes[nodes[prev].childs[1]].bv;
- if(!new_bv.equal(nodes[prev].bv))
- {
- nodes[prev].bv = new_bv;
- prev = nodes[prev].parent;
- }
- else break;
- }
-
- return (prev != NULL_NODE) ? prev : root_node;
- }
- else
- {
- root_node = sibling;
- nodes[sibling].parent = NULL_NODE;
- deleteNode(parent);
- return root_node;
- }
- }
- }
-
-
- void update_(size_t leaf, const BV& bv)
- {
- size_t root = removeLeaf(leaf);
- if(root != NULL_NODE)
- {
- if(max_lookahead_level >= 0)
- {
- for(int i = 0; (i < max_lookahead_level) && (nodes[root].parent != NULL_NODE); ++i)
- root = nodes[root].parent;
- }
-
- nodes[leaf].bv = bv;
- insertLeaf(root, leaf);
- }
- }
-
- inline size_t indexOf(size_t node)
- {
- return (nodes[nodes[node].parent].childs[1] == node);
- }
-
-
- size_t allocateNode()
- {
- if(freelist == NULL_NODE)
- {
- NodeType* old_nodes = nodes;
- n_nodes_alloc *= 2;
- nodes = new NodeType[n_nodes_alloc];
- memcpy(nodes, old_nodes, n_nodes * sizeof(NodeType));
- delete [] old_nodes;
-
- for(size_t i = n_nodes; i < n_nodes_alloc - 1; ++i)
- nodes[i].next = i + 1;
- nodes[n_nodes_alloc - 1].next = NULL_NODE;
- freelist = n_nodes;
- }
-
- size_t node_id = freelist;
- freelist = nodes[node_id].next;
- nodes[node_id].parent = NULL_NODE;
- nodes[node_id].childs[0] = NULL_NODE;
- nodes[node_id].childs[1] = NULL_NODE;
- ++n_nodes;
- return node_id;
- }
-
- size_t createNode(size_t parent,
- const BV& bv1,
- const BV& bv2,
- void* data)
- {
- size_t node = allocateNode();
- nodes[node].parent = parent;
- nodes[node].data = data;
- nodes[node].bv = bv1 + bv2;
- return node;
- }
-
- size_t createNode(size_t parent,
- const BV& bv,
- void* data)
- {
- size_t node = allocateNode();
- nodes[node].parent = parent;
- nodes[node].data = data;
- nodes[node].bv = bv;
- return node;
- }
-
- size_t createNode(size_t parent,
- void* data)
- {
- size_t node = allocateNode();
- nodes[node].parent = parent;
- nodes[node].data = data;
- return node;
- }
-
- void deleteNode(size_t node)
- {
- nodes[node].next = freelist;
- freelist = node;
- --n_nodes;
- }
-
- void recurseRefit(size_t node)
- {
- if(!nodes[node].isLeaf())
- {
- recurseRefit(nodes[node].childs[0]);
- recurseRefit(nodes[node].childs[1]);
- nodes[node].bv = nodes[nodes[node].childs[0]].bv + nodes[nodes[node].childs[1]].bv;
- }
- else
- return;
- }
-
- void fetchLeaves(size_t root, NodeType*& leaves, int depth = -1)
- {
- if((!nodes[root].isLeaf()) && depth)
- {
- fetchLeaves(nodes[root].childs[0], leaves, depth-1);
- fetchLeaves(nodes[root].childs[1], leaves, depth-1);
- deleteNode(root);
- }
- else
- {
- *leaves = nodes[root];
- leaves++;
- }
- }
-
-
-
-protected:
- size_t root_node;
- NodeType* nodes;
- size_t n_nodes;
- size_t n_nodes_alloc;
-
- size_t n_leaves;
- size_t freelist;
- unsigned int opath;
-
- int max_lookahead_level;
-
-public:
- int bu_threshold;
- int topdown_level;
-
-public:
- static const size_t NULL_NODE = -1;
-};
-
-template<typename BV>
-const size_t HierarchyTree<BV>::NULL_NODE;
-
-
-
-
-
-}
-
-
-
-}
-
-
-#endif
diff --git a/trunk/fcl/src/BV/OBB.cpp b/trunk/fcl/src/BV/OBB.cpp
index 68fc42c30f7500fe94a89e8e35573db391e5c781..4de8f8d0e29c1f53e8ee9595e12b5e156b4ef1fa 100644
--- a/trunk/fcl/src/BV/OBB.cpp
+++ b/trunk/fcl/src/BV/OBB.cpp
@@ -384,4 +384,11 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2)
return !obbDisjoint(R, T, b1.extent, b2.extent);
}
+OBB translate(const OBB& bv, const Vec3f& t)
+{
+ OBB res(bv);
+ res.To += t;
+ return res;
+}
+
}
diff --git a/trunk/fcl/src/BV/OBBRSS.cpp b/trunk/fcl/src/BV/OBBRSS.cpp
index 5df9821b3c55fef7ebbae7e9f5d8147a6c84dd83..3a5fc39af8732f206ef1571696eda94c3885e0e0 100644
--- a/trunk/fcl/src/BV/OBBRSS.cpp
+++ b/trunk/fcl/src/BV/OBBRSS.cpp
@@ -49,5 +49,12 @@ FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const O
return distance(R0, T0, b1.rss, b2.rss, P, Q);
}
+OBBRSS translate(const OBBRSS& bv, const Vec3f& t)
+{
+ OBBRSS res(bv);
+ res.obb.To += t;
+ res.rss.Tr += t;
+}
+
}
diff --git a/trunk/fcl/src/BV/RSS.cpp b/trunk/fcl/src/BV/RSS.cpp
index ae98c25bda2092320774215fa9a09e787bb019fd..ee296fcee80eddf9d5ff50fbbc2039963fc7834a 100644
--- a/trunk/fcl/src/BV/RSS.cpp
+++ b/trunk/fcl/src/BV/RSS.cpp
@@ -1136,7 +1136,12 @@ FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS&
return (dist < (FCL_REAL)0.0) ? (FCL_REAL)0.0 : dist;
}
-
+RSS translate(const RSS& bv, const Vec3f& t)
+{
+ RSS res(bv);
+ res.Tr += t;
+ return res;
+}
diff --git a/trunk/fcl/src/BV/kDOP.cpp b/trunk/fcl/src/BV/kDOP.cpp
index d99c7727d8d9ddeb500846e8c7a18e3ca2318979..2b2b0f815df0f7d15210ffe0ddfbd3c912e678c9 100644
--- a/trunk/fcl/src/BV/kDOP.cpp
+++ b/trunk/fcl/src/BV/kDOP.cpp
@@ -104,6 +104,7 @@ inline void getDistances<9>(const Vec3f& p, FCL_REAL* d)
}
+
template<size_t N>
KDOP<N>::KDOP()
{
@@ -225,8 +226,34 @@ FCL_REAL KDOP<N>::distance(const KDOP<N>& other, Vec3f* P, Vec3f* Q) const
}
+template<size_t N>
+KDOP<N> translate(const KDOP<N>& bv, const Vec3f& t)
+{
+ KDOP<N> res(bv);
+ for(size_t i = 0; i < 3; ++i)
+ {
+ res.dist(i) += t[i];
+ res.dist(N / 2 + i) += t[i];
+ }
+
+ FCL_REAL d[(N - 6) / 2];
+ getDistances<(N - 6) / 2>(t, d);
+ for(size_t i = 0; i < (N - 6) / 2; ++i)
+ {
+ res.dist(3 + i) += d[i];
+ res.dist(3 + i + N / 2) += d[i];
+ }
+
+ return res;
+}
+
+
template class KDOP<16>;
template class KDOP<18>;
template class KDOP<24>;
+template KDOP<16> translate<16>(const KDOP<16>&, const Vec3f&);
+template KDOP<18> translate<18>(const KDOP<18>&, const Vec3f&);
+template KDOP<24> translate<24>(const KDOP<24>&, const Vec3f&);
+
}
diff --git a/trunk/fcl/src/BV/kIOS.cpp b/trunk/fcl/src/BV/kIOS.cpp
index 94e3bcd3f563c55f87f6448f3e5883fdf37e3578..30171aff59d20fb798cfeefb8acae2bf01407169 100644
--- a/trunk/fcl/src/BV/kIOS.cpp
+++ b/trunk/fcl/src/BV/kIOS.cpp
@@ -196,4 +196,16 @@ FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIO
}
+kIOS translate(const kIOS& bv, const Vec3f& t)
+{
+ kIOS res(bv);
+ for(size_t i = 0; i < res.num_spheres; ++i)
+ {
+ res.spheres[i].o += t;
+ }
+
+ translate(res.obb, t);
+}
+
+
}
diff --git a/trunk/fcl/src/BVH_model.cpp b/trunk/fcl/src/BVH_model.cpp
index 607c69d5f074067eb8cb323802d4bcfdb55f5c6d..73f768bc48a22ec00f190fba3471240b01eab5ff 100644
--- a/trunk/fcl/src/BVH_model.cpp
+++ b/trunk/fcl/src/BVH_model.cpp
@@ -43,10 +43,15 @@ namespace fcl
{
template<typename BV>
-BVHModel<BV>::BVHModel(const BVHModel<BV>& other) : CollisionGeometry(other)
+BVHModel<BV>::BVHModel(const BVHModel<BV>& other) : CollisionGeometry(other),
+ num_tris(other.num_tris),
+ num_vertices(other.num_vertices),
+ build_state(other.build_state),
+ bv_splitter(other.bv_splitter),
+ bv_fitter(other.bv_fitter),
+ num_tris_allocated(other.num_tris),
+ num_vertices_allocated(other.num_vertices)
{
- num_tris = num_tris_allocated = other.num_tris;
- num_vertices = num_vertices_allocated = other.num_vertices;
if(other.vertices)
{
vertices = new Vec3f[num_vertices];
@@ -100,12 +105,6 @@ BVHModel<BV>::BVHModel(const BVHModel<BV>& other) : CollisionGeometry(other)
}
else
bvs = NULL;
-
- build_state = other.build_state;
-
- bv_splitter = other.bv_splitter;
-
- bv_fitter = other.bv_fitter;
}
@@ -875,6 +874,75 @@ void BVHModel<BV>::computeLocalAABB()
aabb_local = aabb_;
}
+
+template<>
+void BVHModel<OBB>::makeParentRelativeRecurse(int bv_id, Vec3f parent_axis[], const Vec3f& parent_c)
+{
+ OBB& obb = bvs[bv_id].bv;
+ if(!bvs[bv_id].isLeaf())
+ {
+ makeParentRelativeRecurse(bvs[bv_id].first_child, obb.axis, obb.To);
+
+ makeParentRelativeRecurse(bvs[bv_id].first_child + 1, obb.axis, obb.To);
+ }
+
+ // make self parent relative
+ obb.axis[0] = Vec3f(parent_axis[0].dot(obb.axis[0]), parent_axis[1].dot(obb.axis[0]), parent_axis[2].dot(obb.axis[0]));
+ obb.axis[1] = Vec3f(parent_axis[0].dot(obb.axis[1]), parent_axis[1].dot(obb.axis[1]), parent_axis[2].dot(obb.axis[1]));
+ obb.axis[2] = Vec3f(parent_axis[0].dot(obb.axis[2]), parent_axis[1].dot(obb.axis[2]), parent_axis[2].dot(obb.axis[2]));
+
+ Vec3f t = obb.To - parent_c;
+ obb.To = Vec3f(parent_axis[0].dot(t), parent_axis[1].dot(t), parent_axis[2].dot(t));
+}
+
+template<>
+void BVHModel<RSS>::makeParentRelativeRecurse(int bv_id, Vec3f parent_axis[], const Vec3f& parent_c)
+{
+ RSS& rss = bvs[bv_id].bv;
+ if(!bvs[bv_id].isLeaf())
+ {
+ makeParentRelativeRecurse(bvs[bv_id].first_child, rss.axis, rss.Tr);
+
+ makeParentRelativeRecurse(bvs[bv_id].first_child + 1, rss.axis, rss.Tr);
+ }
+
+ // make self parent relative
+ rss.axis[0] = Vec3f(parent_axis[0].dot(rss.axis[0]), parent_axis[1].dot(rss.axis[0]), parent_axis[2].dot(rss.axis[0]));
+ rss.axis[1] = Vec3f(parent_axis[0].dot(rss.axis[1]), parent_axis[1].dot(rss.axis[1]), parent_axis[2].dot(rss.axis[1]));
+ rss.axis[2] = Vec3f(parent_axis[0].dot(rss.axis[2]), parent_axis[1].dot(rss.axis[2]), parent_axis[2].dot(rss.axis[2]));
+
+ Vec3f t = rss.Tr - parent_c;
+ rss.Tr = Vec3f(parent_axis[0].dot(t), parent_axis[1].dot(t), parent_axis[2].dot(t));
+}
+
+template<>
+void BVHModel<OBBRSS>::makeParentRelativeRecurse(int bv_id, Vec3f parent_axis[], const Vec3f& parent_c)
+{
+ OBB& obb = bvs[bv_id].bv.obb;
+ RSS& rss = bvs[bv_id].bv.rss;
+ if(!bvs[bv_id].isLeaf())
+ {
+ makeParentRelativeRecurse(bvs[bv_id].first_child, obb.axis, obb.To);
+
+ makeParentRelativeRecurse(bvs[bv_id].first_child + 1, obb.axis, obb.To);
+ }
+
+ // make self parent relative
+ obb.axis[0] = Vec3f(parent_axis[0].dot(obb.axis[0]), parent_axis[1].dot(obb.axis[0]), parent_axis[2].dot(obb.axis[0]));
+ obb.axis[1] = Vec3f(parent_axis[0].dot(obb.axis[1]), parent_axis[1].dot(obb.axis[1]), parent_axis[2].dot(obb.axis[1]));
+ obb.axis[2] = Vec3f(parent_axis[0].dot(obb.axis[2]), parent_axis[1].dot(obb.axis[2]), parent_axis[2].dot(obb.axis[2]));
+
+ rss.axis[0] = obb.axis[0];
+ rss.axis[1] = obb.axis[1];
+ rss.axis[2] = obb.axis[2];
+
+ Vec3f t = obb.To - parent_c;
+ obb.To = Vec3f(parent_axis[0].dot(t), parent_axis[1].dot(t), parent_axis[2].dot(t));
+ rss.Tr = obb.To;
+}
+
+
+
template<>
NODE_TYPE BVHModel<AABB>::getNodeType() const
{
@@ -925,6 +993,9 @@ NODE_TYPE BVHModel<KDOP<24> >::getNodeType() const
+
+
+
template class BVHModel<KDOP<16> >;
template class BVHModel<KDOP<18> >;
template class BVHModel<KDOP<24> >;
diff --git a/trunk/fcl/src/BV_splitter.cpp b/trunk/fcl/src/BV_splitter.cpp
index d28405323b829a8da6efaa37b7e820c34e6ebf93..b657363e2fd744ab1df3ce0ccd028ce53185b99c 100644
--- a/trunk/fcl/src/BV_splitter.cpp
+++ b/trunk/fcl/src/BV_splitter.cpp
@@ -169,75 +169,84 @@ void computeSplitValue_median(const BV& bv, Vec3f* vertices, Triangle* triangles
}
}
-
+template<>
void BVSplitter<OBB>::computeRule_bvcenter(const OBB& bv, unsigned int* primitive_indices, int num_primitives)
{
computeSplitVector<OBB>(bv, split_vector);
computeSplitValue_bvcenter<OBB>(bv, split_value);
}
+template<>
void BVSplitter<OBB>::computeRule_mean(const OBB& bv, unsigned int* primitive_indices, int num_primitives)
{
computeSplitVector<OBB>(bv, split_vector);
computeSplitValue_mean<OBB>(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value);
}
+template<>
void BVSplitter<OBB>::computeRule_median(const OBB& bv, unsigned int* primitive_indices, int num_primitives)
{
computeSplitVector<OBB>(bv, split_vector);
computeSplitValue_median<OBB>(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value);
}
-
+template<>
void BVSplitter<RSS>::computeRule_bvcenter(const RSS& bv, unsigned int* primitive_indices, int num_primitives)
{
computeSplitVector<RSS>(bv, split_vector);
computeSplitValue_bvcenter<RSS>(bv, split_value);
}
-
+
+template<>
void BVSplitter<RSS>::computeRule_mean(const RSS& bv, unsigned int* primitive_indices, int num_primitives)
{
computeSplitVector<RSS>(bv, split_vector);
computeSplitValue_mean<RSS>(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value);
}
+template<>
void BVSplitter<RSS>::computeRule_median(const RSS& bv, unsigned int* primitive_indices, int num_primitives)
{
computeSplitVector<RSS>(bv, split_vector);
computeSplitValue_median<RSS>(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value);
}
+template<>
void BVSplitter<kIOS>::computeRule_bvcenter(const kIOS& bv, unsigned int* primitive_indices, int num_primitives)
{
computeSplitVector<kIOS>(bv, split_vector);
computeSplitValue_bvcenter<kIOS>(bv, split_value);
}
+template<>
void BVSplitter<kIOS>::computeRule_mean(const kIOS& bv, unsigned int* primitive_indices, int num_primitives)
{
computeSplitVector<kIOS>(bv, split_vector);
computeSplitValue_mean<kIOS>(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value);
}
+template<>
void BVSplitter<kIOS>::computeRule_median(const kIOS& bv, unsigned int* primitive_indices, int num_primitives)
{
computeSplitVector<kIOS>(bv, split_vector);
computeSplitValue_median<kIOS>(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value);
}
-
+template<>
void BVSplitter<OBBRSS>::computeRule_bvcenter(const OBBRSS& bv, unsigned int* primitive_indices, int num_primitives)
{
computeSplitVector<OBBRSS>(bv, split_vector);
computeSplitValue_bvcenter<OBBRSS>(bv, split_value);
}
+template<>
void BVSplitter<OBBRSS>::computeRule_mean(const OBBRSS& bv, unsigned int* primitive_indices, int num_primitives)
{
computeSplitVector<OBBRSS>(bv, split_vector);
computeSplitValue_mean<OBBRSS>(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value);
}
+template<>
void BVSplitter<OBBRSS>::computeRule_median(const OBBRSS& bv, unsigned int* primitive_indices, int num_primitives)
{
computeSplitVector<OBBRSS>(bv, split_vector);
@@ -245,4 +254,29 @@ void BVSplitter<OBBRSS>::computeRule_median(const OBBRSS& bv, unsigned int* prim
}
+template<>
+bool BVSplitter<OBB>::apply(const Vec3f& q) const
+{
+ return split_vector.dot(Vec3f(q[0], q[1], q[2])) > split_value;
+}
+
+template<>
+bool BVSplitter<RSS>::apply(const Vec3f& q) const
+{
+ return split_vector.dot(Vec3f(q[0], q[1], q[2])) > split_value;
+}
+
+template<>
+bool BVSplitter<kIOS>::apply(const Vec3f& q) const
+{
+ return split_vector.dot(Vec3f(q[0], q[1], q[2])) > split_value;
+}
+
+template<>
+bool BVSplitter<OBBRSS>::apply(const Vec3f& q) const
+{
+ return split_vector.dot(Vec3f(q[0], q[1], q[2])) > split_value;
+}
+
+
}
diff --git a/trunk/fcl/src/broad_phase_collision.cpp b/trunk/fcl/src/broad_phase_collision.cpp
deleted file mode 100644
index ae45d64e9cd7954fddb1ab5a360e8f4165336bb2..0000000000000000000000000000000000000000
--- a/trunk/fcl/src/broad_phase_collision.cpp
+++ /dev/null
@@ -1,3209 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, Willow Garage, Inc.
- * 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 Jia Pan */
-
-
-#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
-{
-
-void NaiveCollisionManager::registerObjects(const std::vector<CollisionObject*>& other_objs)
-{
- std::copy(other_objs.begin(), other_objs.end(), std::back_inserter(objs));
-}
-
-void NaiveCollisionManager::unregisterObject(CollisionObject* obj)
-{
- objs.remove(obj);
-}
-
-void NaiveCollisionManager::registerObject(CollisionObject* obj)
-{
- objs.push_back(obj);
-}
-
-void NaiveCollisionManager::setup()
-{
-
-}
-
-void NaiveCollisionManager::update()
-{
-
-}
-
-void NaiveCollisionManager::clear()
-{
- objs.clear();
-}
-
-void NaiveCollisionManager::getObjects(std::vector<CollisionObject*>& objs_) const
-{
- objs_.resize(objs.size());
- std::copy(objs.begin(), objs.end(), objs_.begin());
-}
-
-void NaiveCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
-{
- if(size() == 0) return;
-
- for(std::list<CollisionObject*>::const_iterator it = objs.begin(), end = objs.end(); it != end; ++it)
- {
- if(callback(obj, *it, cdata))
- return;
- }
-}
-
-void NaiveCollisionManager::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const
-{
- if(size() == 0) return;
-
- FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
- for(std::list<CollisionObject*>::const_iterator it = objs.begin(), end = objs.end();
- it != end; ++it)
- {
- if(obj->getAABB().distance((*it)->getAABB()) < min_dist)
- {
- if(callback(obj, *it, cdata, min_dist))
- return;
- }
- }
-}
-
-void NaiveCollisionManager::collide(void* cdata, CollisionCallBack callback) const
-{
- if(size() == 0) return;
-
- for(std::list<CollisionObject*>::const_iterator it1 = objs.begin(), end = objs.end();
- it1 != end; ++it1)
- {
- std::list<CollisionObject*>::const_iterator it2 = it1; it2++;
- for(; it2 != end; ++it2)
- {
- if((*it1)->getAABB().overlap((*it2)->getAABB()))
- if(callback(*it1, *it2, cdata))
- return;
- }
- }
-}
-
-void NaiveCollisionManager::distance(void* cdata, DistanceCallBack callback) const
-{
- if(size() == 0) return;
-
- FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
- for(std::list<CollisionObject*>::const_iterator it1 = objs.begin(), end = objs.end(); it1 != end; ++it1)
- {
- std::list<CollisionObject*>::const_iterator it2 = it1; it2++;
- for(; it2 != end; ++it2)
- {
- if((*it1)->getAABB().distance((*it2)->getAABB()) < min_dist)
- {
- if(callback(*it1, *it2, cdata, min_dist))
- return;
- }
- }
- }
-}
-
-void NaiveCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const
-{
- NaiveCollisionManager* other_manager = static_cast<NaiveCollisionManager*>(other_manager_);
-
- if((size() == 0) || (other_manager->size() == 0)) return;
-
- if(this == other_manager)
- {
- collide(cdata, callback);
- return;
- }
-
- for(std::list<CollisionObject*>::const_iterator it1 = objs.begin(), end1 = objs.end(); it1 != end1; ++it1)
- {
- for(std::list<CollisionObject*>::const_iterator it2 = other_manager->objs.begin(), end2 = other_manager->objs.end(); it2 != end2; ++it2)
- {
- if((*it1)->getAABB().overlap((*it2)->getAABB()))
- if(callback((*it1), (*it2), cdata))
- return;
- }
- }
-}
-
-void NaiveCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const
-{
- NaiveCollisionManager* other_manager = static_cast<NaiveCollisionManager*>(other_manager_);
-
- if((size() == 0) || (other_manager->size() == 0)) return;
-
- if(this == other_manager)
- {
- distance(cdata, callback);
- return;
- }
-
- FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
- for(std::list<CollisionObject*>::const_iterator it1 = objs.begin(), end1 = objs.end(); it1 != end1; ++it1)
- {
- for(std::list<CollisionObject*>::const_iterator it2 = other_manager->objs.begin(), end2 = other_manager->objs.end(); it2 != end2; ++it2)
- {
- if((*it1)->getAABB().distance((*it2)->getAABB()) < min_dist)
- {
- if(callback(*it1, *it2, cdata, min_dist))
- return;
- }
- }
- }
-}
-
-bool NaiveCollisionManager::empty() const
-{
- return objs.empty();
-}
-
-
-/** \brief Functor sorting objects according to the AABB lower x bound */
-struct SortByXLow
-{
- bool operator()(const CollisionObject* a, const CollisionObject* b) const
- {
- if(a->getAABB().min_[0] < b->getAABB().min_[0])
- return true;
- return false;
- }
-};
-
-/** \brief Functor sorting objects according to the AABB lower y bound */
-struct SortByYLow
-{
- bool operator()(const CollisionObject* a, const CollisionObject* b) const
- {
- if(a->getAABB().min_[1] < b->getAABB().min_[1])
- return true;
- return false;
- }
-};
-
-/** \brief Functor sorting objects according to the AABB lower z bound */
-struct SortByZLow
-{
- bool operator()(const CollisionObject* a, const CollisionObject* b) const
- {
- if(a->getAABB().min_[2] < b->getAABB().min_[2])
- return true;
- return false;
- }
-};
-
-/** \brief Dummy collision object with a point AABB */
-class DummyCollisionObject : public CollisionObject
-{
-public:
- DummyCollisionObject(const AABB& aabb_) : CollisionObject()
- {
- aabb = aabb_;
- }
-
- void computeLocalAABB() {}
-};
-
-
-void SSaPCollisionManager::unregisterObject(CollisionObject* obj)
-{
- setup();
-
- DummyCollisionObject dummyHigh(AABB(obj->getAABB().max_));
-
- std::vector<CollisionObject*>::iterator pos_start1 = objs_x.begin();
- std::vector<CollisionObject*>::iterator pos_end1 = std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow());
-
- while(pos_start1 < pos_end1)
- {
- if(*pos_start1 == obj)
- {
- objs_x.erase(pos_start1);
- break;
- }
- ++pos_start1;
- }
-
- std::vector<CollisionObject*>::iterator pos_start2 = objs_y.begin();
- std::vector<CollisionObject*>::iterator pos_end2 = std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow());
-
- while(pos_start2 < pos_end2)
- {
- if(*pos_start2 == obj)
- {
- objs_y.erase(pos_start2);
- break;
- }
- ++pos_start2;
- }
-
- std::vector<CollisionObject*>::iterator pos_start3 = objs_z.begin();
- std::vector<CollisionObject*>::iterator pos_end3 = std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow());
-
- while(pos_start3 < pos_end3)
- {
- if(*pos_start3 == obj)
- {
- objs_z.erase(pos_start3);
- break;
- }
- ++pos_start3;
- }
-}
-
-
-void SSaPCollisionManager::registerObject(CollisionObject* obj)
-{
- objs_x.push_back(obj);
- objs_y.push_back(obj);
- objs_z.push_back(obj);
- setup_ = false;
-}
-
-void SSaPCollisionManager::setup()
-{
- if(!setup_)
- {
- std::sort(objs_x.begin(), objs_x.end(), SortByXLow());
- std::sort(objs_y.begin(), objs_y.end(), SortByYLow());
- std::sort(objs_z.begin(), objs_z.end(), SortByZLow());
- setup_ = true;
- }
-}
-
-void SSaPCollisionManager::update()
-{
- setup_ = false;
- setup();
-}
-
-void SSaPCollisionManager::clear()
-{
- objs_x.clear();
- objs_y.clear();
- objs_z.clear();
- setup_ = false;
-}
-
-void SSaPCollisionManager::getObjects(std::vector<CollisionObject*>& objs) const
-{
- objs.resize(objs_x.size());
- std::copy(objs_x.begin(), objs_x.end(), objs.begin());
-}
-
-bool SSaPCollisionManager::checkColl(std::vector<CollisionObject*>::const_iterator pos_start, std::vector<CollisionObject*>::const_iterator pos_end,
- CollisionObject* obj, void* cdata, CollisionCallBack callback) const
-{
- while(pos_start < pos_end)
- {
- if(*pos_start != obj) // no collision between the same object
- {
- if((*pos_start)->getAABB().overlap(obj->getAABB()))
- {
- if(callback(*pos_start, obj, cdata))
- return true;
- }
- }
- pos_start++;
- }
- return false;
-}
-
-bool SSaPCollisionManager::checkDis(std::vector<CollisionObject*>::const_iterator pos_start, std::vector<CollisionObject*>::const_iterator pos_end, CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
-{
- while(pos_start < pos_end)
- {
- if(*pos_start != obj) // no distance between the same object
- {
- if((*pos_start)->getAABB().distance(obj->getAABB()) < min_dist)
- {
- if(callback(*pos_start, obj, cdata, min_dist))
- return true;
- }
- }
- pos_start++;
- }
-
- return false;
-}
-
-
-
-void SSaPCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
-{
- if(size() == 0) return;
-
- collide_(obj, cdata, callback);
-}
-
-bool SSaPCollisionManager::collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
-{
- static const unsigned int CUTOFF = 100;
-
- DummyCollisionObject dummyHigh(AABB(obj->getAABB().max_));
- bool coll_res = false;
-
- std::vector<CollisionObject*>::const_iterator pos_start1 = objs_x.begin();
- std::vector<CollisionObject*>::const_iterator pos_end1 = std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow());
- unsigned int d1 = pos_end1 - pos_start1;
-
- if(d1 > CUTOFF)
- {
- std::vector<CollisionObject*>::const_iterator pos_start2 = objs_y.begin();
- std::vector<CollisionObject*>::const_iterator pos_end2 = std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow());
- unsigned int d2 = pos_end2 - pos_start2;
-
- if(d2 > CUTOFF)
- {
- std::vector<CollisionObject*>::const_iterator pos_start3 = objs_z.begin();
- std::vector<CollisionObject*>::const_iterator pos_end3 = std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow());
- unsigned int d3 = pos_end3 - pos_start3;
-
- if(d3 > CUTOFF)
- {
- if(d3 <= d2 && d3 <= d1)
- coll_res = checkColl(pos_start3, pos_end3, obj, cdata, callback);
- else
- {
- if(d2 <= d3 && d2 <= d1)
- coll_res = checkColl(pos_start2, pos_end2, obj, cdata, callback);
- else
- coll_res = checkColl(pos_start1, pos_end1, obj, cdata, callback);
- }
- }
- else
- coll_res = checkColl(pos_start3, pos_end3, obj, cdata, callback);
- }
- else
- coll_res = checkColl(pos_start2, pos_end2, obj, cdata, callback);
- }
- else
- coll_res = checkColl(pos_start1, pos_end1, obj, cdata, callback);
-
- return coll_res;
-}
-
-
-void SSaPCollisionManager::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const
-{
- if(size() == 0) return;
-
- FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
- distance_(obj, cdata, callback, min_dist);
-}
-
-bool SSaPCollisionManager::distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
-{
- static const unsigned int CUTOFF = 100;
- Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
- Vec3f dummy_vector = obj->getAABB().max_;
- if(min_dist < std::numeric_limits<FCL_REAL>::max())
- dummy_vector += Vec3f(min_dist, min_dist, min_dist);
-
- std::vector<CollisionObject*>::const_iterator pos_start1 = objs_x.begin();
- std::vector<CollisionObject*>::const_iterator pos_start2 = objs_y.begin();
- std::vector<CollisionObject*>::const_iterator pos_start3 = objs_z.begin();
- std::vector<CollisionObject*>::const_iterator pos_end1 = objs_x.end();
- std::vector<CollisionObject*>::const_iterator pos_end2 = objs_y.end();
- std::vector<CollisionObject*>::const_iterator pos_end3 = objs_z.end();
-
- int status = 1;
- FCL_REAL old_min_distance;
-
- while(1)
- {
- old_min_distance = min_dist;
- DummyCollisionObject dummyHigh((AABB(dummy_vector)));
-
- pos_end1 = std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow());
- unsigned int d1 = pos_end1 - pos_start1;
-
- bool dist_res = false;
-
- if(d1 > CUTOFF)
- {
- pos_end2 = std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow());
- unsigned int d2 = pos_end2 - pos_start2;
-
- if(d2 > CUTOFF)
- {
- pos_end3 = std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow());
- unsigned int d3 = pos_end3 - pos_start3;
-
- if(d3 > CUTOFF)
- {
- if(d3 <= d2 && d3 <= d1)
- dist_res = checkDis(pos_start3, pos_end3, obj, cdata, callback, min_dist);
- else
- {
- if(d2 <= d3 && d2 <= d1)
- dist_res = checkDis(pos_start2, pos_end2, obj, cdata, callback, min_dist);
- else
- dist_res = checkDis(pos_start1, pos_end1, obj, cdata, callback, min_dist);
- }
- }
- else
- dist_res = checkDis(pos_start3, pos_end3, obj, cdata, callback, min_dist);
- }
- else
- dist_res = checkDis(pos_start2, pos_end2, obj, cdata, callback, min_dist);
- }
- else
- {
- dist_res = checkDis(pos_start1, pos_end1, obj, cdata, callback, min_dist);
- }
-
- if(dist_res) return true;
-
- if(status == 1)
- {
- if(old_min_distance < std::numeric_limits<FCL_REAL>::max())
- break;
- else
- {
- // from infinity to a finite one, only need one additional loop
- // to check the possible missed ones to the right of the objs array
- if(min_dist < old_min_distance)
- {
- dummy_vector = obj->getAABB().max_ + Vec3f(min_dist, min_dist, min_dist);
- status = 0;
- }
- else // need more loop
- {
- if(dummy_vector.equal(obj->getAABB().max_))
- dummy_vector = dummy_vector + delta;
- else
- dummy_vector = dummy_vector * 2 - obj->getAABB().max_;
- }
- }
-
- // yes, following is wrong, will result in too large distance.
- // if(pos_end1 != objs_x.end()) pos_start1 = pos_end1;
- // if(pos_end2 != objs_y.end()) pos_start2 = pos_end2;
- // if(pos_end3 != objs_z.end()) pos_start3 = pos_end3;
- }
- else if(status == 0)
- break;
- }
-
- return false;
-}
-
-void SSaPCollisionManager::collide(void* cdata, CollisionCallBack callback) const
-{
- if(size() == 0) return;
-
- std::vector<CollisionObject*>::const_iterator pos, run_pos, pos_end;
- size_t axis = selectOptimalAxis(objs_x, objs_y, objs_z,
- pos, pos_end);
- size_t axis2 = (axis + 1 > 2) ? 0 : (axis + 1);
- size_t axis3 = (axis2 + 1 > 2) ? 0 : (axis2 + 1);
-
- run_pos = pos;
-
- while((run_pos < pos_end) && (pos < pos_end))
- {
- CollisionObject* obj = *(pos++);
-
- while(1)
- {
- if((*run_pos)->getAABB().min_[axis] < obj->getAABB().min_[axis])
- {
- run_pos++;
- if(run_pos == pos_end) break;
- continue;
- }
- else
- {
- run_pos++;
- break;
- }
- }
-
- if(run_pos < pos_end)
- {
- std::vector<CollisionObject*>::const_iterator run_pos2 = run_pos;
-
- while((*run_pos2)->getAABB().min_[axis] <= obj->getAABB().max_[axis])
- {
- CollisionObject* obj2 = *run_pos2;
- run_pos2++;
-
- if((obj->getAABB().max_[axis2] >= obj2->getAABB().min_[axis2]) && (obj2->getAABB().max_[axis2] >= obj->getAABB().min_[axis2]))
- {
- if((obj->getAABB().max_[axis3] >= obj2->getAABB().min_[axis3]) && (obj2->getAABB().max_[axis3] >= obj->getAABB().min_[axis3]))
- {
- if(callback(obj, obj2, cdata))
- return;
- }
- }
-
- if(run_pos2 == pos_end) break;
- }
- }
- }
-}
-
-
-void SSaPCollisionManager::distance(void* cdata, DistanceCallBack callback) const
-{
- if(size() == 0) return;
-
- std::vector<CollisionObject*>::const_iterator it, it_end;
- size_t axis = selectOptimalAxis(objs_x, objs_y, objs_z, it, it_end);
-
- FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
- for(; it != it_end; ++it)
- {
- if(distance_(*it, cdata, callback, min_dist))
- return;
- }
-}
-
-void SSaPCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const
-{
- SSaPCollisionManager* other_manager = static_cast<SSaPCollisionManager*>(other_manager_);
-
- if((size() == 0) || (other_manager->size() == 0)) return;
-
- if(this == other_manager)
- {
- collide(cdata, callback);
- return;
- }
-
-
- std::vector<CollisionObject*>::const_iterator it, end;
- if(this->size() < other_manager->size())
- {
- for(it = objs_x.begin(), end = objs_x.end(); it != end; ++it)
- if(other_manager->collide_(*it, cdata, callback)) return;
- }
- else
- {
- for(it = other_manager->objs_x.begin(), end != other_manager->objs_x.end(); it != end; ++it)
- if(collide_(*it, cdata, callback)) return;
- }
-}
-
-void SSaPCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const
-{
- SSaPCollisionManager* other_manager = static_cast<SSaPCollisionManager*>(other_manager_);
-
- if((size() == 0) || (other_manager->size() == 0)) return;
-
- if(this == other_manager)
- {
- distance(cdata, callback);
- return;
- }
-
- FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
- std::vector<CollisionObject*>::const_iterator it, end;
- if(this->size() < other_manager->size())
- {
- for(it = objs_x.begin(), end = objs_x.end(); it != end; ++it)
- if(other_manager->distance_(*it, cdata, callback, min_dist)) return;
- }
- else
- {
- for(it = other_manager->objs_x.begin(), end = other_manager->objs_x.end(); it != end; ++it)
- if(distance_(*it, cdata, callback, min_dist)) return;
- }
-}
-
-bool SSaPCollisionManager::empty() const
-{
- return objs_x.empty();
-}
-
-
-void SaPCollisionManager::unregisterObject(CollisionObject* obj)
-{
- std::list<SaPAABB*>::iterator it = AABB_arr.begin();
- for(std::list<SaPAABB*>::iterator end = AABB_arr.end(); it != end; ++it)
- {
- if((*it)->obj == obj)
- break;
- }
-
- AABB_arr.erase(it);
- obj_aabb_map.erase(obj);
-
- if(it == AABB_arr.end())
- return;
-
- SaPAABB* curr = *it;
- *it = NULL;
-
- for(int coord = 0; coord < 3; ++coord)
- {
- //first delete the lo endpoint of the interval.
- if(curr->lo->prev[coord] == NULL)
- elist[coord] = curr->lo->next[coord];
- else
- curr->lo->prev[coord]->next[coord] = curr->lo->next[coord];
-
- curr->lo->next[coord]->prev[coord] = curr->lo->prev[coord];
-
- //then, delete the "hi" endpoint.
- if(curr->hi->prev[coord] == NULL)
- elist[coord] = curr->hi->next[coord];
- else
- curr->hi->prev[coord]->next[coord] = curr->hi->next[coord];
-
- if(curr->hi->next[coord] != NULL)
- curr->hi->next[coord]->prev[coord] = curr->hi->prev[coord];
- }
-
- delete curr->lo;
- delete curr->hi;
- delete curr;
-
- overlap_pairs.remove_if(isUnregistered(obj));
-}
-
-void SaPCollisionManager::registerObjects(const std::vector<CollisionObject*>& other_objs)
-{
- if(size() > 0)
- BroadPhaseCollisionManager::registerObjects(other_objs);
- else
- {
- std::vector<EndPoint*> endpoints(2 * other_objs.size());
-
- for(size_t i = 0; i < other_objs.size(); ++i)
- {
- SaPAABB* sapaabb = new SaPAABB();
- sapaabb->obj = other_objs[i];
- sapaabb->lo = new EndPoint();
- sapaabb->hi = new EndPoint();
- sapaabb->cached = other_objs[i]->getAABB();
- endpoints[2 * i] = sapaabb->lo;
- endpoints[2 * i + 1] = sapaabb->hi;
- sapaabb->lo->minmax = 0;
- sapaabb->hi->minmax = 1;
- sapaabb->lo->aabb = sapaabb;
- sapaabb->hi->aabb = sapaabb;
- AABB_arr.push_back(sapaabb);
- obj_aabb_map[other_objs[i]] = sapaabb;
- }
-
-
- FCL_REAL scale[3];
- for(size_t coord = 0; coord < 3; ++coord)
- {
- std::sort(endpoints.begin(), endpoints.end(),
- boost::bind(std::less<FCL_REAL>(),
- boost::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const >(&EndPoint::getVal), _1, coord),
- boost::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const >(&EndPoint::getVal), _2, coord)));
-
- endpoints[0]->prev[coord] = NULL;
- endpoints[0]->next[coord] = endpoints[1];
- for(int i = 1; i < endpoints.size() - 1; ++i)
- {
- endpoints[i]->prev[coord] = endpoints[i-1];
- endpoints[i]->next[coord] = endpoints[i+1];
- }
- endpoints[endpoints.size() - 1]->prev[coord] = endpoints[endpoints.size() - 2];
- endpoints[endpoints.size() - 1]->next[coord] = NULL;
-
- elist[coord] = endpoints[0];
-
- scale[coord] = endpoints.back()->aabb->cached.max_[coord] - endpoints[0]->aabb->cached.min_[coord];
- }
-
- int axis = 0;
- if(scale[axis] < scale[1]) axis = 1;
- if(scale[axis] < scale[2]) axis = 2;
-
- EndPoint* pos = elist[axis];
-
- while(pos != NULL)
- {
- EndPoint* pos_next = NULL;
- SaPAABB* aabb = pos->aabb;
- EndPoint* pos_it = pos->next[axis];
-
- while(pos_it != NULL)
- {
- if(pos_it->aabb == aabb)
- {
- if(pos_next == NULL) pos_next = pos_it;
- break;
- }
-
- if(pos_it->minmax == 0)
- {
- if(pos_next == NULL) pos_next = pos_it;
- if(pos_it->aabb->cached.overlap(aabb->cached))
- overlap_pairs.push_back(SaPPair(pos_it->aabb->obj, aabb->obj));
- }
- pos_it = pos_it->next[axis];
- }
-
- pos = pos_next;
- }
- }
-
- updateVelist();
-}
-
-void SaPCollisionManager::registerObject(CollisionObject* obj)
-{
- SaPAABB* curr = new SaPAABB;
- curr->cached = obj->getAABB();
- curr->obj = obj;
- curr->lo = new EndPoint;
- curr->lo->minmax = 0;
- curr->lo->aabb = curr;
-
- curr->hi = new EndPoint;
- curr->hi->minmax = 1;
- curr->hi->aabb = curr;
-
- for(int coord = 0; coord < 3; ++coord)
- {
- EndPoint* current = elist[coord];
-
- // first insert the lo end point
- if(current == NULL) // empty list
- {
- elist[coord] = curr->lo;
- curr->lo->prev[coord] = curr->lo->next[coord] = NULL;
- }
- else // otherwise, find the correct location in the list and insert
- {
- EndPoint* curr_lo = curr->lo;
- FCL_REAL curr_lo_val = curr_lo->getVal()[coord];
- while((current->getVal()[coord] < curr_lo_val) && (current->next[coord] != NULL))
- current = current->next[coord];
-
- if(current->getVal()[coord] >= curr_lo_val)
- {
- curr_lo->prev[coord] = current->prev[coord];
- curr_lo->next[coord] = current;
- if(current->prev[coord] == NULL)
- elist[coord] = curr_lo;
- else
- current->prev[coord]->next[coord] = curr_lo;
-
- current->prev[coord] = curr_lo;
- }
- else
- {
- curr_lo->prev[coord] = current;
- curr_lo->next[coord] = NULL;
- current->next[coord] = curr_lo;
- }
- }
-
- // now insert hi end point
- current = curr->lo;
-
- EndPoint* curr_hi = curr->hi;
- FCL_REAL curr_hi_val = curr_hi->getVal()[coord];
-
- if(coord == 0)
- {
- while((current->getVal()[coord] < curr_hi_val) && (current->next[coord] != NULL))
- {
- if(current != curr->lo)
- if(current->aabb->cached.overlap(curr->cached))
- overlap_pairs.push_back(SaPPair(current->aabb->obj, obj));
-
- current = current->next[coord];
- }
- }
- else
- {
- while((current->getVal()[coord] < curr_hi_val) && (current->next[coord] != NULL))
- current = current->next[coord];
- }
-
- if(current->getVal()[coord] >= curr_hi_val)
- {
- curr_hi->prev[coord] = current->prev[coord];
- curr_hi->next[coord] = current;
- if(current->prev[coord] == NULL)
- elist[coord] = curr_hi;
- else
- current->prev[coord]->next[coord] = curr_hi;
-
- current->prev[coord] = curr_hi;
- }
- else
- {
- curr_hi->prev[coord] = current;
- curr_hi->next[coord] = NULL;
- current->next[coord] = curr_hi;
- }
- }
-
- AABB_arr.push_back(curr);
-
- obj_aabb_map[obj] = curr;
-
- updateVelist();
-}
-
-void SaPCollisionManager::setup()
-{
- FCL_REAL scale[3];
- scale[0] = (velist[0].back())->getVal(0) - velist[0][0]->getVal(0);
- scale[1] = (velist[1].back())->getVal(1) - velist[1][0]->getVal(1);;
- scale[2] = (velist[2].back())->getVal(2) - velist[2][0]->getVal(2);
- size_t axis = 0;
- if(scale[axis] < scale[1]) axis = 1;
- if(scale[axis] < scale[2]) axis = 2;
- optimal_axis = axis;
-}
-
-void SaPCollisionManager::update_(SaPAABB* updated_aabb)
-{
- if(updated_aabb->cached.equal(updated_aabb->obj->getAABB()))
- return;
-
- SaPAABB* current = updated_aabb;
-
- Vec3f new_min = current->obj->getAABB().min_;
- Vec3f new_max = current->obj->getAABB().max_;
-
- SaPAABB dummy;
- dummy.cached = current->obj->getAABB();
-
- for(int coord = 0; coord < 3; ++coord)
- {
- int direction; // -1 reverse, 0 nochange, 1 forward
- EndPoint* temp;
-
- if(current->lo->getVal(coord) > new_min[coord])
- direction = -1;
- else if(current->lo->getVal(coord) < new_min[coord])
- direction = 1;
- else direction = 0;
-
- if(direction == -1)
- {
- //first update the "lo" endpoint of the interval
- if(current->lo->prev[coord] != NULL)
- {
- temp = current->lo;
- while((temp != NULL) && (temp->getVal(coord) > new_min[coord]))
- {
- if(temp->minmax == 1)
- if(temp->aabb->cached.overlap(dummy.cached))
- addToOverlapPairs(SaPPair(temp->aabb->obj, current->obj));
- temp = temp->prev[coord];
- }
-
- if(temp == NULL)
- {
- current->lo->prev[coord]->next[coord] = current->lo->next[coord];
- current->lo->next[coord]->prev[coord] = current->lo->prev[coord];
- current->lo->prev[coord] = NULL;
- current->lo->next[coord] = elist[coord];
- elist[coord]->prev[coord] = current->lo;
- elist[coord] = current->lo;
- }
- else
- {
- current->lo->prev[coord]->next[coord] = current->lo->next[coord];
- current->lo->next[coord]->prev[coord] = current->lo->prev[coord];
- current->lo->prev[coord] = temp;
- current->lo->next[coord] = temp->next[coord];
- temp->next[coord]->prev[coord] = current->lo;
- temp->next[coord] = current->lo;
- }
- }
-
- current->lo->getVal(coord) = new_min[coord];
-
- // update hi end point
- temp = current->hi;
- while(temp->getVal(coord) > new_max[coord])
- {
- if((temp->minmax == 0) && (temp->aabb->cached.overlap(current->cached)))
- removeFromOverlapPairs(SaPPair(temp->aabb->obj, current->obj));
- temp = temp->prev[coord];
- }
-
- current->hi->prev[coord]->next[coord] = current->hi->next[coord];
- if(current->hi->next[coord] != NULL)
- current->hi->next[coord]->prev[coord] = current->hi->prev[coord];
- current->hi->prev[coord] = temp;
- current->hi->next[coord] = temp->next[coord];
- if(temp->next[coord] != NULL)
- temp->next[coord]->prev[coord] = current->hi;
- temp->next[coord] = current->hi;
-
- current->hi->getVal(coord) = new_max[coord];
- }
- else if(direction == 1)
- {
- //here, we first update the "hi" endpoint.
- if(current->hi->next[coord] != NULL)
- {
- temp = current->hi;
- while((temp->next[coord] != NULL) && (temp->getVal(coord) < new_max[coord]))
- {
- if(temp->minmax == 0)
- if(temp->aabb->cached.overlap(dummy.cached))
- addToOverlapPairs(SaPPair(temp->aabb->obj, current->obj));
- temp = temp->next[coord];
- }
-
- if(temp->getVal(coord) < new_max[coord])
- {
- current->hi->prev[coord]->next[coord] = current->hi->next[coord];
- current->hi->next[coord]->prev[coord] = current->hi->prev[coord];
- current->hi->prev[coord] = temp;
- current->hi->next[coord] = NULL;
- temp->next[coord] = current->hi;
- }
- else
- {
- current->hi->prev[coord]->next[coord] = current->hi->next[coord];
- current->hi->next[coord]->prev[coord] = current->hi->prev[coord];
- current->hi->prev[coord] = temp->prev[coord];
- current->hi->next[coord] = temp;
- temp->prev[coord]->next[coord] = current->hi;
- temp->prev[coord] = current->hi;
- }
- }
-
- current->hi->getVal(coord) = new_max[coord];
-
- //then, update the "lo" endpoint of the interval.
- temp = current->lo;
-
- while(temp->getVal(coord) < new_min[coord])
- {
- if((temp->minmax == 1) && (temp->aabb->cached.overlap(current->cached)))
- removeFromOverlapPairs(SaPPair(temp->aabb->obj, current->obj));
- temp = temp->next[coord];
- }
-
- if(current->lo->prev[coord] != NULL)
- current->lo->prev[coord]->next[coord] = current->lo->next[coord];
- else
- elist[coord] = current->lo->next[coord];
- current->lo->next[coord]->prev[coord] = current->lo->prev[coord];
- current->lo->prev[coord] = temp->prev[coord];
- current->lo->next[coord] = temp;
- if(temp->prev[coord] != NULL)
- temp->prev[coord]->next[coord] = current->lo;
- else
- elist[coord] = current->lo;
- temp->prev[coord] = current->lo;
- current->lo->getVal(coord) = new_min[coord];
- }
- }
-}
-
-void SaPCollisionManager::update(CollisionObject* updated_obj)
-{
- update_(obj_aabb_map[updated_obj]);
-
- updateVelist();
-
- setup();
-}
-
-void SaPCollisionManager::update(const std::vector<CollisionObject*>& updated_objs)
-{
- for(size_t i = 0; i < updated_objs.size(); ++i)
- update_(obj_aabb_map[updated_objs[i]]);
-
- updateVelist();
-
- setup();
-}
-
-void SaPCollisionManager::update()
-{
- for(std::list<SaPAABB*>::const_iterator it = AABB_arr.begin(), end = AABB_arr.end(); it != end; ++it)
- {
- update_(*it);
- }
-
- updateVelist();
-
- setup();
-}
-
-
-void SaPCollisionManager::clear()
-{
- for(std::list<SaPAABB*>::iterator it = AABB_arr.begin(), end = AABB_arr.end();
- it != end;
- ++it)
- {
- delete (*it)->hi;
- delete (*it)->lo;
- delete *it;
- *it = NULL;
- }
-
- AABB_arr.clear();
- overlap_pairs.clear();
-
- elist[0] = NULL;
- elist[1] = NULL;
- elist[2] = NULL;
-
- velist[0].clear();
- velist[1].clear();
- velist[2].clear();
-
- obj_aabb_map.clear();
-}
-
-void SaPCollisionManager::getObjects(std::vector<CollisionObject*>& objs) const
-{
- objs.resize(AABB_arr.size());
- int i = 0;
- for(std::list<SaPAABB*>::const_iterator it = AABB_arr.begin(), end = AABB_arr.end(); it != end; ++it, ++i)
- {
- objs[i] = (*it)->obj;
- }
-}
-
-bool SaPCollisionManager::collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
-{
- size_t axis = optimal_axis;
- const AABB& obj_aabb = obj->getAABB();
-
- FCL_REAL min_val = obj_aabb.min_[axis];
- FCL_REAL max_val = obj_aabb.max_[axis];
-
- EndPoint dummy;
- SaPAABB dummy_aabb;
- dummy_aabb.cached = obj_aabb;
- dummy.minmax = 1;
- dummy.aabb = &dummy_aabb;
-
- // compute stop_pos by binary search, this is cheaper than check it in while iteration linearly
- std::vector<EndPoint*>::const_iterator res_it = std::upper_bound(velist[axis].begin(), velist[axis].end(), &dummy,
- boost::bind(std::less<FCL_REAL>(),
- boost::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const>(&EndPoint::getVal), _1, axis),
- boost::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const>(&EndPoint::getVal), _2, axis)));
-
- EndPoint* end_pos = NULL;
- if(res_it != velist[axis].end())
- end_pos = *res_it;
-
- EndPoint* pos = elist[axis];
-
- while(pos != end_pos)
- {
- if(pos->aabb->obj != obj)
- {
- if((pos->minmax == 0) && (pos->aabb->hi->getVal(axis) >= min_val))
- {
- if(pos->aabb->cached.overlap(obj->getAABB()))
- if(callback(obj, pos->aabb->obj, cdata))
- return true;
- }
- }
- pos = pos->next[axis];
- }
-
- return false;
-}
-
-void SaPCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
-{
- if(size() == 0) return;
-
- collide_(obj, cdata, callback);
-}
-
-bool SaPCollisionManager::distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
-{
- Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
- AABB aabb = obj->getAABB();
-
- if(min_dist < std::numeric_limits<FCL_REAL>::max())
- {
- Vec3f min_dist_delta(min_dist, min_dist, min_dist);
- aabb.expand(min_dist_delta);
- }
-
- size_t axis = optimal_axis;
-
- int status = 1;
- FCL_REAL old_min_distance;
-
- EndPoint* start_pos = elist[axis];
-
- while(1)
- {
- old_min_distance = min_dist;
- FCL_REAL min_val = aabb.min_[axis];
- FCL_REAL max_val = aabb.max_[axis];
-
- EndPoint dummy;
- SaPAABB dummy_aabb;
- dummy_aabb.cached = aabb;
- dummy.minmax = 1;
- dummy.aabb = &dummy_aabb;
-
-
- std::vector<EndPoint*>::const_iterator res_it = std::upper_bound(velist[axis].begin(), velist[axis].end(), &dummy,
- boost::bind(std::less<FCL_REAL>(),
- boost::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const>(&EndPoint::getVal), _1, axis),
- boost::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const>(&EndPoint::getVal), _2, axis)));
-
- EndPoint* end_pos = NULL;
- if(res_it != velist[axis].end())
- end_pos = *res_it;
-
- EndPoint* pos = start_pos;
-
- while(pos != end_pos)
- {
- // can change to pos->aabb->hi->getVal(axis) >= min_val - min_dist, and then update start_pos to end_pos.
- // but this seems slower.
- if((pos->minmax == 0) && (pos->aabb->hi->getVal(axis) >= min_val))
- {
- CollisionObject* curr_obj = pos->aabb->obj;
- if(curr_obj != obj)
- {
- if(!enable_tested_set_)
- {
- if(pos->aabb->cached.distance(obj->getAABB()) < min_dist)
- {
- if(callback(curr_obj, obj, cdata, min_dist))
- return true;
- }
- }
- else
- {
- if(!inTestedSet(curr_obj, obj))
- {
- if(pos->aabb->cached.distance(obj->getAABB()) < min_dist)
- {
- if(callback(curr_obj, obj, cdata, min_dist))
- return true;
- }
-
- insertTestedSet(curr_obj, obj);
- }
- }
- }
- }
-
- pos = pos->next[axis];
- }
-
- if(status == 1)
- {
- if(old_min_distance < std::numeric_limits<FCL_REAL>::max())
- break;
- else
- {
- if(min_dist < old_min_distance)
- {
- Vec3f min_dist_delta(min_dist, min_dist, min_dist);
- aabb = AABB(obj->getAABB(), min_dist_delta);
- status = 0;
- }
- else
- {
- if(aabb.equal(obj->getAABB()))
- aabb.expand(delta);
- else
- aabb.expand(obj->getAABB(), 2.0);
- }
- }
- }
- else if(status == 0)
- break;
- }
-
- return false;
-}
-
-void SaPCollisionManager::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const
-{
- if(size() == 0) return;
-
- FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
-
- distance_(obj, cdata, callback, min_dist);
-}
-
-void SaPCollisionManager::collide(void* cdata, CollisionCallBack callback) const
-{
- if(size() == 0) return;
-
- for(std::list<SaPPair>::const_iterator it = overlap_pairs.begin(), end = overlap_pairs.end(); it != end; ++it)
- {
- CollisionObject* obj1 = it->obj1;
- CollisionObject* obj2 = it->obj2;
-
- if(callback(obj1, obj2, cdata))
- return;
- }
-}
-
-void SaPCollisionManager::distance(void* cdata, DistanceCallBack callback) const
-{
- if(size() == 0) return;
-
- enable_tested_set_ = true;
- tested_set.clear();
-
- FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
-
- for(std::list<SaPAABB*>::const_iterator it = AABB_arr.begin(), end = AABB_arr.end(); it != end; ++it)
- {
- if(distance_((*it)->obj, cdata, callback, min_dist))
- break;
- }
-
- enable_tested_set_ = false;
- tested_set.clear();
-}
-
-void SaPCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const
-{
- SaPCollisionManager* other_manager = static_cast<SaPCollisionManager*>(other_manager_);
-
- if((size() == 0) || (other_manager->size() == 0)) return;
-
- if(this == other_manager)
- {
- collide(cdata, callback);
- return;
- }
-
- if(this->size() < other_manager->size())
- {
- for(std::list<SaPAABB*>::const_iterator it = AABB_arr.begin(); it != AABB_arr.end(); ++it)
- {
- if(other_manager->collide_((*it)->obj, cdata, callback))
- return;
- }
- }
- else
- {
- for(std::list<SaPAABB*>::const_iterator it = other_manager->AABB_arr.begin(), end = other_manager->AABB_arr.end(); it != end; ++it)
- {
- if(collide_((*it)->obj, cdata, callback))
- return;
- }
- }
-}
-
-void SaPCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const
-{
- SaPCollisionManager* other_manager = static_cast<SaPCollisionManager*>(other_manager_);
-
- if((size() == 0) || (other_manager->size() == 0)) return;
-
- if(this == other_manager)
- {
- distance(cdata, callback);
- return;
- }
-
- FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
-
- if(this->size() < other_manager->size())
- {
- for(std::list<SaPAABB*>::const_iterator it = AABB_arr.begin(), end = AABB_arr.end(); it != end; ++it)
- {
- if(other_manager->distance_((*it)->obj, cdata, callback, min_dist))
- return;
- }
- }
- else
- {
- for(std::list<SaPAABB*>::const_iterator it = other_manager->AABB_arr.begin(), end = other_manager->AABB_arr.end(); it != end; ++it)
- {
- if(distance_((*it)->obj, cdata, callback, min_dist))
- return;
- }
- }
-}
-
-bool SaPCollisionManager::empty() const
-{
- return AABB_arr.size();
-}
-
-
-
-void IntervalTreeCollisionManager::unregisterObject(CollisionObject* obj)
-{
- // must sorted before
- setup();
-
- EndPoint p;
- p.value = obj->getAABB().min_[0];
- std::vector<EndPoint>::iterator start1 = std::lower_bound(endpoints[0].begin(), endpoints[0].end(), p, boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2));
- p.value = obj->getAABB().max_[0];
- std::vector<EndPoint>::iterator end1 = std::upper_bound(start1, endpoints[0].end(), p, boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2));
-
- if(start1 < end1)
- {
- unsigned int start_id = start1 - endpoints[0].begin();
- unsigned int end_id = end1 - endpoints[0].begin();
- unsigned int cur_id = start_id;
- for(unsigned int i = start_id; i < end_id; ++i)
- {
- if(endpoints[0][i].obj != obj)
- {
- if(i == cur_id) cur_id++;
- else
- {
- endpoints[0][cur_id] = endpoints[0][i];
- cur_id++;
- }
- }
- }
- if(cur_id < end_id)
- endpoints[0].resize(endpoints[0].size() - 2);
- }
-
- p.value = obj->getAABB().min_[1];
- std::vector<EndPoint>::iterator start2 = std::lower_bound(endpoints[1].begin(), endpoints[1].end(), p, boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2));
- p.value = obj->getAABB().max_[1];
- std::vector<EndPoint>::iterator end2 = std::upper_bound(start2, endpoints[1].end(), p, boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2));
-
- if(start2 < end2)
- {
- unsigned int start_id = start2 - endpoints[1].begin();
- unsigned int end_id = end2 - endpoints[1].begin();
- unsigned int cur_id = start_id;
- for(unsigned int i = start_id; i < end_id; ++i)
- {
- if(endpoints[1][i].obj != obj)
- {
- if(i == cur_id) cur_id++;
- else
- {
- endpoints[1][cur_id] = endpoints[1][i];
- cur_id++;
- }
- }
- }
- if(cur_id < end_id)
- endpoints[1].resize(endpoints[1].size() - 2);
- }
-
-
- p.value = obj->getAABB().min_[2];
- std::vector<EndPoint>::iterator start3 = std::lower_bound(endpoints[2].begin(), endpoints[2].end(), p, boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2));
- p.value = obj->getAABB().max_[2];
- std::vector<EndPoint>::iterator end3 = std::upper_bound(start3, endpoints[2].end(), p, boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2));
-
- if(start3 < end3)
- {
- unsigned int start_id = start3 - endpoints[2].begin();
- unsigned int end_id = end3 - endpoints[2].begin();
- unsigned int cur_id = start_id;
- for(unsigned int i = start_id; i < end_id; ++i)
- {
- if(endpoints[2][i].obj != obj)
- {
- if(i == cur_id) cur_id++;
- else
- {
- endpoints[2][cur_id] = endpoints[2][i];
- cur_id++;
- }
- }
- }
- if(cur_id < end_id)
- endpoints[2].resize(endpoints[2].size() - 2);
- }
-
- // update the interval tree
- if(obj_interval_maps[0].find(obj) != obj_interval_maps[0].end())
- {
- SAPInterval* ivl1 = obj_interval_maps[0][obj];
- SAPInterval* ivl2 = obj_interval_maps[1][obj];
- SAPInterval* ivl3 = obj_interval_maps[2][obj];
-
- interval_trees[0]->deleteNode(ivl1);
- interval_trees[1]->deleteNode(ivl2);
- interval_trees[2]->deleteNode(ivl3);
-
- delete ivl1;
- delete ivl2;
- delete ivl3;
-
- obj_interval_maps[0].erase(obj);
- obj_interval_maps[1].erase(obj);
- obj_interval_maps[2].erase(obj);
- }
-}
-
-void IntervalTreeCollisionManager::registerObject(CollisionObject* obj)
-{
- EndPoint p, q;
-
- p.obj = obj;
- q.obj = obj;
- p.minmax = 0;
- q.minmax = 1;
- p.value = obj->getAABB().min_[0];
- q.value = obj->getAABB().max_[0];
- endpoints[0].push_back(p);
- endpoints[0].push_back(q);
-
- p.value = obj->getAABB().min_[1];
- q.value = obj->getAABB().max_[1];
- endpoints[1].push_back(p);
- endpoints[1].push_back(q);
-
- p.value = obj->getAABB().min_[2];
- q.value = obj->getAABB().max_[2];
- endpoints[2].push_back(p);
- endpoints[2].push_back(q);
- setup_ = false;
-}
-
-void IntervalTreeCollisionManager::setup()
-{
- if(!setup_)
- {
- std::sort(endpoints[0].begin(), endpoints[0].end(), boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2));
- std::sort(endpoints[1].begin(), endpoints[1].end(), boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2));
- std::sort(endpoints[2].begin(), endpoints[2].end(), boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2));
-
- for(int i = 0; i < 3; ++i)
- delete interval_trees[i];
-
- for(int i = 0; i < 3; ++i)
- interval_trees[i] = new IntervalTree;
-
- for(unsigned int i = 0, size = endpoints[0].size(); i < size; ++i)
- {
- EndPoint p = endpoints[0][i];
- CollisionObject* obj = p.obj;
- if(p.minmax == 0)
- {
- SAPInterval* ivl1 = new SAPInterval(obj->getAABB().min_[0], obj->getAABB().max_[0], obj);
- SAPInterval* ivl2 = new SAPInterval(obj->getAABB().min_[1], obj->getAABB().max_[1], obj);
- SAPInterval* ivl3 = new SAPInterval(obj->getAABB().min_[2], obj->getAABB().max_[2], obj);
-
- interval_trees[0]->insert(ivl1);
- interval_trees[1]->insert(ivl2);
- interval_trees[2]->insert(ivl3);
-
- obj_interval_maps[0][obj] = ivl1;
- obj_interval_maps[1][obj] = ivl2;
- obj_interval_maps[2][obj] = ivl3;
- }
- }
-
- setup_ = true;
- }
-}
-
-void IntervalTreeCollisionManager::update()
-{
- setup_ = false;
-
- for(unsigned int i = 0, size = endpoints[0].size(); i < size; ++i)
- {
- if(endpoints[0][i].minmax == 0)
- endpoints[0][i].value = endpoints[0][i].obj->getAABB().min_[0];
- else
- endpoints[0][i].value = endpoints[0][i].obj->getAABB().max_[0];
- }
-
- for(unsigned int i = 0, size = endpoints[1].size(); i < size; ++i)
- {
- if(endpoints[1][i].minmax == 0)
- endpoints[1][i].value = endpoints[1][i].obj->getAABB().min_[1];
- else
- endpoints[1][i].value = endpoints[1][i].obj->getAABB().max_[1];
- }
-
- for(unsigned int i = 0, size = endpoints[2].size(); i < size; ++i)
- {
- if(endpoints[2][i].minmax == 0)
- endpoints[2][i].value = endpoints[2][i].obj->getAABB().min_[2];
- else
- endpoints[2][i].value = endpoints[2][i].obj->getAABB().max_[2];
- }
-
- setup();
-
-}
-
-
-void IntervalTreeCollisionManager::update(CollisionObject* updated_obj)
-{
- AABB old_aabb;
- const AABB& new_aabb = updated_obj->getAABB();
- for(int i = 0; i < 3; ++i)
- {
- std::map<CollisionObject*, SAPInterval*>::const_iterator it = obj_interval_maps[i].find(updated_obj);
- interval_trees[i]->deleteNode(it->second);
- old_aabb.min_[i] = it->second->low;
- old_aabb.max_[i] = it->second->high;
- it->second->low = new_aabb.min_[i];
- it->second->high = new_aabb.max_[i];
- interval_trees[i]->insert(it->second);
- }
-
- EndPoint dummy;
- std::vector<EndPoint>::iterator it;
- for(int i = 0; i < 3; ++i)
- {
- dummy.value = old_aabb.min_[i];
- it = std::lower_bound(endpoints[i].begin(), endpoints[i].end(), dummy, boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2));
- for(; it != endpoints[i].end(); ++it)
- {
- if(it->obj == updated_obj && it->minmax == 0)
- {
- it->value = new_aabb.min_[i];
- break;
- }
- }
-
- dummy.value = old_aabb.max_[i];
- it = std::lower_bound(endpoints[i].begin(), endpoints[i].end(), dummy, boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2));
- for(; it != endpoints[i].end(); ++it)
- {
- if(it->obj == updated_obj && it->minmax == 0)
- {
- it->value = new_aabb.max_[i];
- break;
- }
- }
-
- std::sort(endpoints[i].begin(), endpoints[i].end(), boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2));
- }
-}
-
-void IntervalTreeCollisionManager::update(const std::vector<CollisionObject*>& updated_objs)
-{
- for(size_t i = 0; i < updated_objs.size(); ++i)
- update(updated_objs[i]);
-}
-
-void IntervalTreeCollisionManager::clear()
-{
- endpoints[0].clear();
- endpoints[1].clear();
- endpoints[2].clear();
-
- delete interval_trees[0]; interval_trees[0] = NULL;
- delete interval_trees[1]; interval_trees[1] = NULL;
- delete interval_trees[2]; interval_trees[2] = NULL;
-
- for(int i = 0; i < 3; ++i)
- {
- for(std::map<CollisionObject*, SAPInterval*>::const_iterator it = obj_interval_maps[i].begin(), end = obj_interval_maps[i].end();
- it != end; ++it)
- {
- delete it->second;
- }
- }
-
- for(int i = 0; i < 3; ++i)
- obj_interval_maps[i].clear();
-
- setup_ = false;
-}
-
-void IntervalTreeCollisionManager::getObjects(std::vector<CollisionObject*>& objs) const
-{
- objs.resize(endpoints[0].size() / 2);
- unsigned int j = 0;
- for(unsigned int i = 0, size = endpoints[0].size(); i < size; ++i)
- {
- if(endpoints[0][i].minmax == 0)
- {
- objs[j] = endpoints[0][i].obj; j++;
- }
- }
-}
-
-void IntervalTreeCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
-{
- if(size() == 0) return;
- collide_(obj, cdata, callback);
-}
-
-bool IntervalTreeCollisionManager::collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
-{
- static const unsigned int CUTOFF = 100;
-
- std::deque<SimpleInterval*> results0, results1, results2;
-
- results0 = interval_trees[0]->query(obj->getAABB().min_[0], obj->getAABB().max_[0]);
- if(results0.size() > CUTOFF)
- {
- results1 = interval_trees[1]->query(obj->getAABB().min_[1], obj->getAABB().max_[1]);
- if(results1.size() > CUTOFF)
- {
- results2 = interval_trees[2]->query(obj->getAABB().min_[2], obj->getAABB().max_[2]);
- if(results2.size() > CUTOFF)
- {
- int d1 = results0.size();
- int d2 = results1.size();
- int d3 = results2.size();
-
- if(d1 >= d2 && d1 >= d3)
- return checkColl(results0.begin(), results0.end(), obj, cdata, callback);
- else if(d2 >= d1 && d2 >= d3)
- return checkColl(results1.begin(), results1.end(), obj, cdata, callback);
- else
- return checkColl(results2.begin(), results2.end(), obj, cdata, callback);
- }
- else
- return checkColl(results2.begin(), results2.end(), obj, cdata, callback);
- }
- else
- return checkColl(results1.begin(), results1.end(), obj, cdata, callback);
- }
- else
- return checkColl(results0.begin(), results0.end(), obj, cdata, callback);
-}
-
-void IntervalTreeCollisionManager::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const
-{
- if(size() == 0) return;
- FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
- distance_(obj, cdata, callback, min_dist);
-}
-
-bool IntervalTreeCollisionManager::distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
-{
- static const unsigned int CUTOFF = 100;
-
- Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
- AABB aabb = obj->getAABB();
- if(min_dist < std::numeric_limits<FCL_REAL>::max())
- {
- Vec3f min_dist_delta(min_dist, min_dist, min_dist);
- aabb.expand(min_dist_delta);
- }
-
- int status = 1;
- FCL_REAL old_min_distance;
-
- while(1)
- {
- bool dist_res = false;
-
- old_min_distance = min_dist;
-
- std::deque<SimpleInterval*> results0, results1, results2;
-
- results0 = interval_trees[0]->query(aabb.min_[0], aabb.max_[0]);
- if(results0.size() > CUTOFF)
- {
- results1 = interval_trees[1]->query(aabb.min_[1], aabb.max_[1]);
- if(results1.size() > CUTOFF)
- {
- results2 = interval_trees[2]->query(aabb.min_[2], aabb.max_[2]);
- if(results2.size() > CUTOFF)
- {
- int d1 = results0.size();
- int d2 = results1.size();
- int d3 = results2.size();
-
- if(d1 >= d2 && d1 >= d3)
- dist_res = checkDist(results0.begin(), results0.end(), obj, cdata, callback, min_dist);
- else if(d2 >= d1 && d2 >= d3)
- dist_res = checkDist(results1.begin(), results1.end(), obj, cdata, callback, min_dist);
- else
- dist_res = checkDist(results2.begin(), results2.end(), obj, cdata, callback, min_dist);
- }
- else
- dist_res = checkDist(results2.begin(), results2.end(), obj, cdata, callback, min_dist);
- }
- else
- dist_res = checkDist(results1.begin(), results1.end(), obj, cdata, callback, min_dist);
- }
- else
- dist_res = checkDist(results0.begin(), results0.end(), obj, cdata, callback, min_dist);
-
- if(dist_res) return true;
-
- results0.clear();
- results1.clear();
- results2.clear();
-
- if(status == 1)
- {
- if(old_min_distance < std::numeric_limits<FCL_REAL>::max())
- break;
- else
- {
- if(min_dist < old_min_distance)
- {
- Vec3f min_dist_delta(min_dist, min_dist, min_dist);
- aabb = AABB(obj->getAABB(), min_dist_delta);
- status = 0;
- }
- else
- {
- if(aabb.equal(obj->getAABB()))
- aabb.expand(delta);
- else
- aabb.expand(obj->getAABB(), 2.0);
- }
- }
- }
- else if(status == 0)
- break;
- }
-
- return false;
-}
-
-void IntervalTreeCollisionManager::collide(void* cdata, CollisionCallBack callback) const
-{
- if(size() == 0) return;
-
- std::set<CollisionObject*> active;
- std::set<std::pair<CollisionObject*, CollisionObject*> > overlap;
- unsigned int n = endpoints[0].size();
- double diff_x = endpoints[0][0].value - endpoints[0][n-1].value;
- double diff_y = endpoints[1][0].value - endpoints[1][n-1].value;
- double diff_z = endpoints[2][0].value - endpoints[2][n-1].value;
-
- int axis = 0;
- if(diff_y > diff_x && diff_y > diff_z)
- axis = 1;
- else if(diff_z > diff_y && diff_z > diff_x)
- axis = 2;
-
- for(unsigned int i = 0; i < n; ++i)
- {
- const EndPoint& endpoint = endpoints[axis][i];
- CollisionObject* index = endpoint.obj;
- if(endpoint.minmax == 0)
- {
- std::set<CollisionObject*>::iterator iter = active.begin();
- std::set<CollisionObject*>::iterator end = active.end();
- for(; iter != end; ++iter)
- {
- CollisionObject* active_index = *iter;
- const AABB& b0 = active_index->getAABB();
- const AABB& b1 = index->getAABB();
-
- int axis2 = (axis + 1) % 3;
- int axis3 = (axis + 2) % 3;
-
- if(b0.axisOverlap(b1, axis2) && b0.axisOverlap(b1, axis3))
- {
- std::pair<std::set<std::pair<CollisionObject*, CollisionObject*> >::iterator, bool> insert_res;
- if(active_index < index)
- insert_res = overlap.insert(std::make_pair(active_index, index));
- else
- insert_res = overlap.insert(std::make_pair(index, active_index));
-
- if(insert_res.second)
- {
- if(callback(active_index, index, cdata))
- return;
- }
- }
- }
- active.insert(index);
- }
- else
- active.erase(index);
- }
-
-}
-
-void IntervalTreeCollisionManager::distance(void* cdata, DistanceCallBack callback) const
-{
- if(size() == 0) return;
-
- enable_tested_set_ = true;
- tested_set.clear();
- FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
-
- for(size_t i = 0; i < endpoints[0].size(); ++i)
- if(distance_(endpoints[0][i].obj, cdata, callback, min_dist)) break;
-
- enable_tested_set_ = false;
- tested_set.clear();
-}
-
-void IntervalTreeCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const
-{
- IntervalTreeCollisionManager* other_manager = static_cast<IntervalTreeCollisionManager*>(other_manager_);
-
- if((size() == 0) || (other_manager->size() == 0)) return;
-
- if(this == other_manager)
- {
- collide(cdata, callback);
- return;
- }
-
- if(this->size() < other_manager->size())
- {
- for(size_t i = 0, size = endpoints[0].size(); i < size; ++i)
- if(other_manager->collide_(endpoints[0][i].obj, cdata, callback)) return;
- }
- else
- {
- for(size_t i = 0, size = other_manager->endpoints[0].size(); i < size; ++i)
- if(collide_(other_manager->endpoints[0][i].obj, cdata, callback)) return;
- }
-}
-
-void IntervalTreeCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const
-{
- IntervalTreeCollisionManager* other_manager = static_cast<IntervalTreeCollisionManager*>(other_manager_);
-
- if((size() == 0) || (other_manager->size() == 0)) return;
-
- if(this == other_manager)
- {
- distance(cdata, callback);
- return;
- }
-
- FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
-
- if(this->size() < other_manager->size())
- {
- for(size_t i = 0, size = endpoints[0].size(); i < size; ++i)
- if(other_manager->distance_(endpoints[0][i].obj, cdata, callback, min_dist)) return;
- }
- else
- {
- for(size_t i = 0, size = other_manager->endpoints[0].size(); i < size; ++i)
- if(distance_(other_manager->endpoints[0][i].obj, cdata, callback, min_dist)) return;
- }
-}
-
-bool IntervalTreeCollisionManager::empty() const
-{
- return endpoints[0].empty();
-}
-
-bool IntervalTreeCollisionManager::checkColl(std::deque<SimpleInterval*>::const_iterator pos_start, std::deque<SimpleInterval*>::const_iterator pos_end, CollisionObject* obj, void* cdata, CollisionCallBack callback) const
-{
- while(pos_start < pos_end)
- {
- SAPInterval* ivl = static_cast<SAPInterval*>(*pos_start);
- if(ivl->obj != obj)
- {
- if(ivl->obj->getAABB().overlap(obj->getAABB()))
- {
- if(callback(ivl->obj, obj, cdata))
- return true;
- }
- }
-
- pos_start++;
- }
-
- return false;
-}
-
-bool IntervalTreeCollisionManager::checkDist(std::deque<SimpleInterval*>::const_iterator pos_start, std::deque<SimpleInterval*>::const_iterator pos_end, CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
-{
- while(pos_start < pos_end)
- {
- SAPInterval* ivl = static_cast<SAPInterval*>(*pos_start);
- if(ivl->obj != obj)
- {
- if(!enable_tested_set_)
- {
- if(ivl->obj->getAABB().distance(obj->getAABB()) < min_dist)
- {
- if(callback(ivl->obj, obj, cdata, min_dist))
- return true;
- }
- }
- else
- {
- if(!inTestedSet(ivl->obj, obj))
- {
- if(ivl->obj->getAABB().distance(obj->getAABB()) < min_dist)
- {
- if(callback(ivl->obj, obj, cdata, min_dist))
- return true;
- }
-
- insertTestedSet(ivl->obj, obj);
- }
- }
- }
-
- pos_start++;
- }
-
- return false;
-}
-
-void DynamicAABBTreeCollisionManager::registerObjects(const std::vector<CollisionObject*>& other_objs)
-{
- if(size() > 0)
- {
- BroadPhaseCollisionManager::registerObjects(other_objs);
- }
- else
- {
- std::vector<DynamicAABBNode*> leaves(other_objs.size());
- table.rehash(other_objs.size());
- for(size_t i = 0, size = other_objs.size(); i < size; ++i)
- {
- DynamicAABBNode* node = new DynamicAABBNode; // node will be managed by the dtree
- node->bv = other_objs[i]->getAABB();
- node->parent = NULL;
- node->childs[1] = NULL;
- node->data = other_objs[i];
- table[other_objs[i]] = node;
- leaves[i] = node;
- }
-
- dtree.init(leaves, tree_init_level);
-
- setup_ = true;
- }
-}
-
-void DynamicAABBTreeCollisionManager::registerObject(CollisionObject* obj)
-{
- DynamicAABBNode* node = dtree.insert(obj->getAABB(), obj);
- table[obj] = node;
-}
-
-void DynamicAABBTreeCollisionManager::unregisterObject(CollisionObject* obj)
-{
- DynamicAABBNode* node = table[obj];
- table.erase(obj);
- dtree.remove(node);
-}
-
-void DynamicAABBTreeCollisionManager::setup()
-{
- if(!setup_)
- {
- int num = dtree.size();
- if(num == 0)
- {
- setup_ = true;
- return;
- }
-
- int height = dtree.getMaxHeight();
-
-
- if(height - std::log((FCL_REAL)num) / std::log(2.0) < max_tree_nonbalanced_level)
- dtree.balanceIncremental(tree_incremental_balance_pass);
- else
- dtree.balanceTopdown();
-
- setup_ = true;
- }
-}
-
-
-void DynamicAABBTreeCollisionManager::update()
-{
- for(DynamicAABBTable::const_iterator it = table.begin(); it != table.end(); ++it)
- {
- CollisionObject* obj = it->first;
- DynamicAABBNode* node = it->second;
- node->bv = obj->getAABB();
- }
-
- dtree.refit();
- setup_ = false;
-
- setup();
-}
-
-void DynamicAABBTreeCollisionManager::update_(CollisionObject* updated_obj)
-{
- DynamicAABBTable::const_iterator it = table.find(updated_obj);
- if(it != table.end())
- {
- DynamicAABBNode* node = it->second;
- if(!node->bv.equal(updated_obj->getAABB()))
- dtree.update(node, updated_obj->getAABB());
- }
- setup_ = false;
-}
-
-void DynamicAABBTreeCollisionManager::update(CollisionObject* updated_obj)
-{
- update_(updated_obj);
- setup();
-}
-
-void DynamicAABBTreeCollisionManager::update(const std::vector<CollisionObject*>& updated_objs)
-{
- for(size_t i = 0, size = updated_objs.size(); i < size; ++i)
- update_(updated_objs[i]);
- setup();
-}
-
-bool DynamicAABBTreeCollisionManager::collisionRecurse(DynamicAABBNode* root1, DynamicAABBNode* root2, void* cdata, CollisionCallBack callback) const
-{
- if(root1->isLeaf() && root2->isLeaf())
- {
- if(!root1->bv.overlap(root2->bv)) return false;
- return callback(static_cast<CollisionObject*>(root1->data), static_cast<CollisionObject*>(root2->data), cdata);
- }
-
- if(!root1->bv.overlap(root2->bv)) return false;
-
- if(root2->isLeaf() || (!root1->isLeaf() && (root1->bv.size() > root2->bv.size())))
- {
- if(collisionRecurse(root1->childs[0], root2, cdata, callback))
- return true;
- if(collisionRecurse(root1->childs[1], root2, cdata, callback))
- return true;
- }
- else
- {
- if(collisionRecurse(root1, root2->childs[0], cdata, callback))
- return true;
- if(collisionRecurse(root1, root2->childs[1], cdata, callback))
- return true;
- }
- return false;
-}
-
-bool DynamicAABBTreeCollisionManager::collisionRecurse(DynamicAABBNode* root, CollisionObject* query, void* cdata, CollisionCallBack callback) const
-{
- if(root->isLeaf())
- {
- if(!root->bv.overlap(query->getAABB())) return false;
- return callback(static_cast<CollisionObject*>(root->data), query, cdata);
- }
-
- if(!root->bv.overlap(query->getAABB())) return false;
-
- int select_res = select(query->getAABB(), *(root->childs[0]), *(root->childs[1]));
-
- if(collisionRecurse(root->childs[select_res], query, cdata, callback))
- return true;
-
- if(collisionRecurse(root->childs[1-select_res], query, cdata, callback))
- return true;
-
- return false;
-}
-
-bool DynamicAABBTreeCollisionManager::selfCollisionRecurse(DynamicAABBNode* root, void* cdata, CollisionCallBack callback) const
-{
- if(root->isLeaf()) return false;
-
- if(selfCollisionRecurse(root->childs[0], cdata, callback))
- return true;
-
- if(selfCollisionRecurse(root->childs[1], cdata, callback))
- return true;
-
- if(collisionRecurse(root->childs[0], root->childs[1], cdata, callback))
- return true;
-
- return false;
-}
-
-bool DynamicAABBTreeCollisionManager::distanceRecurse(DynamicAABBNode* root1, DynamicAABBNode* root2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
-{
- if(root1->isLeaf() && root2->isLeaf())
- {
- CollisionObject* root1_obj = static_cast<CollisionObject*>(root1->data);
- CollisionObject* root2_obj = static_cast<CollisionObject*>(root2->data);
- return callback(root1_obj, root2_obj, cdata, min_dist);
- }
-
- if(root2->isLeaf() || (!root1->isLeaf() && (root1->bv.size() > root2->bv.size())))
- {
- FCL_REAL d1 = root2->bv.distance(root1->childs[0]->bv);
- FCL_REAL d2 = root2->bv.distance(root1->childs[1]->bv);
-
- if(d2 < d1)
- {
- if(d2 < min_dist)
- {
- if(distanceRecurse(root1->childs[1], root2, cdata, callback, min_dist))
- return true;
- }
-
- if(d1 < min_dist)
- {
- if(distanceRecurse(root1->childs[0], root2, cdata, callback, min_dist))
- return true;
- }
- }
- else
- {
- if(d1 < min_dist)
- {
- if(distanceRecurse(root1->childs[0], root2, cdata, callback, min_dist))
- return true;
- }
-
- if(d2 < min_dist)
- {
- if(distanceRecurse(root1->childs[1], root2, cdata, callback, min_dist))
- return true;
- }
- }
- }
- else
- {
- FCL_REAL d1 = root1->bv.distance(root2->childs[0]->bv);
- FCL_REAL d2 = root1->bv.distance(root2->childs[1]->bv);
-
- if(d2 < d1)
- {
- if(d2 < min_dist)
- {
- if(distanceRecurse(root1, root2->childs[1], cdata, callback, min_dist))
- return true;
- }
-
- if(d1 < min_dist)
- {
- if(distanceRecurse(root1, root2->childs[0], cdata, callback, min_dist))
- return true;
- }
- }
- else
- {
- if(d1 < min_dist)
- {
- if(distanceRecurse(root1, root2->childs[0], cdata, callback, min_dist))
- return true;
- }
-
- if(d2 < min_dist)
- {
- if(distanceRecurse(root1, root2->childs[1], cdata, callback, min_dist))
- return true;
- }
- }
- }
-
- return false;
-}
-
-bool DynamicAABBTreeCollisionManager::distanceRecurse(DynamicAABBNode* root, CollisionObject* query, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
-{
- if(root->isLeaf())
- {
- CollisionObject* root_obj = static_cast<CollisionObject*>(root->data);
- return callback(root_obj, query, cdata, min_dist);
- }
-
- FCL_REAL d1 = query->getAABB().distance(root->childs[0]->bv);
- FCL_REAL d2 = query->getAABB().distance(root->childs[1]->bv);
-
- if(d2 < d1)
- {
- if(d2 < min_dist)
- {
- if(distanceRecurse(root->childs[1], query, cdata, callback, min_dist))
- return true;
- }
-
- if(d1 < min_dist)
- {
- if(distanceRecurse(root->childs[0], query, cdata, callback, min_dist))
- return true;
- }
- }
- else
- {
- if(d1 < min_dist)
- {
- if(distanceRecurse(root->childs[0], query, cdata, callback, min_dist))
- return true;
- }
-
- if(d2 < min_dist)
- {
- if(distanceRecurse(root->childs[1], query, cdata, callback, min_dist))
- return true;
- }
- }
-
- return false;
-}
-
-bool DynamicAABBTreeCollisionManager::selfDistanceRecurse(DynamicAABBNode* root, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
-{
- if(root->isLeaf()) return false;
-
- if(selfDistanceRecurse(root->childs[0], cdata, callback, min_dist))
- return true;
-
- if(selfDistanceRecurse(root->childs[1], cdata, callback, min_dist))
- return true;
-
- if(distanceRecurse(root->childs[0], root->childs[1], cdata, callback, min_dist))
- return true;
-
- return false;
-}
-
-
-bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, CollisionCallBack callback)
-{
- if(root1->isLeaf() && !root2->hasChildren())
- {
- CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
-
- if(!tree2->isNodeFree(root2) && !obj1->isFree())
- {
- OBB obb1, obb2;
- convertBV(root1->bv, Transform3f(), obb1);
- convertBV(root2_bv, tf2, obb2);
-
- if(obb1.overlap(obb2))
- {
- Box* box = new Box();
- Transform3f box_tf;
- constructBox(root2_bv, tf2, *box, box_tf);
-
- box->cost_density = root2->getOccupancy();
- box->threshold_occupied = tree2->getOccupancyThres();
-
- CollisionObject obj2(boost::shared_ptr<CollisionGeometry>(box), box_tf);
- return callback(obj1, &obj2, cdata);
- }
- else return false;
- }
- else return false;
- }
-
- OBB obb1, obb2;
- convertBV(root1->bv, Transform3f(), obb1);
- convertBV(root2_bv, tf2, obb2);
-
- if(tree2->isNodeFree(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 collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Vec3f& tf2, void* cdata, CollisionCallBack callback)
-{
- if(root1->isLeaf() && !root2->hasChildren())
- {
- CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
-
- if(!tree2->isNodeFree(root2) && !obj1->isFree())
- {
- const AABB& root2_bv_t = translate(root2_bv, tf2);
- if(root1->bv.overlap(root2_bv_t))
- {
- Box* box = new Box();
- Transform3f box_tf;
- constructBox(root2_bv, tf2, *box, box_tf);
-
- box->cost_density = root2->getOccupancy();
- box->threshold_occupied = tree2->getOccupancyThres();
-
- CollisionObject obj2(boost::shared_ptr<CollisionGeometry>(box), box_tf);
- return callback(obj1, &obj2, cdata);
- }
- else return false;
- }
- else return false;
- }
-
- const AABB& root2_bv_t = translate(root2_bv, tf2);
- if(tree2->isNodeFree(root2) || !root1->bv.overlap(root2_bv_t)) 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::collisionRecurse(DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, CollisionCallBack callback) const
-{
- if(tf2.getQuatRotation().isIdentity())
- return collisionRecurse_(root1, tree2, root2, root2_bv, tf2.getTranslation(), cdata, callback);
- else // has rotation
- return collisionRecurse_(root1, tree2, root2, root2_bv, tf2, cdata, callback);
-}
-
-
-bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Vec3f& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist)
-{
- if(root1->isLeaf() && !root2->hasChildren())
- {
- if(tree2->isNodeOccupied(root2))
- {
- Box* box = new Box();
- Transform3f 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())))
- {
- const AABB& aabb2 = translate(root2_bv, tf2);
- 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);
- const AABB& aabb2 = translate(child_bv, tf2);
-
- 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;
-}
-
-
-bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist)
-{
- if(root1->isLeaf() && !root2->hasChildren())
- {
- if(tree2->isNodeOccupied(root2))
- {
- Box* box = new Box();
- Transform3f 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;
-}
-
-bool DynamicAABBTreeCollisionManager::distanceRecurse(DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
-{
- if(tf2.getQuatRotation().isIdentity())
- return distanceRecurse_(root1, tree2, root2, root2_bv, tf2.getTranslation(), cdata, callback, min_dist);
- else
- return distanceRecurse_(root1, tree2, root2, root2_bv, tf2, cdata, callback, min_dist);
-}
-
-
-
-
-void DynamicAABBTreeCollisionManager2::registerObjects(const std::vector<CollisionObject*>& other_objs)
-{
- if(size() > 0)
- {
- BroadPhaseCollisionManager::registerObjects(other_objs);
- }
- else
- {
- DynamicAABBNode* leaves = new DynamicAABBNode[other_objs.size()];
- table.rehash(other_objs.size());
- for(size_t i = 0, size = other_objs.size(); i < size; ++i)
- {
- leaves[i].bv = other_objs[i]->getAABB();
- leaves[i].parent = dtree.NULL_NODE;
- leaves[i].childs[1] = dtree.NULL_NODE;
- leaves[i].data = other_objs[i];
- table[other_objs[i]] = i;
- }
-
- int n_leaves = other_objs.size();
-
- dtree.init(leaves, n_leaves, tree_init_level);
-
- setup_ = true;
- }
-}
-
-void DynamicAABBTreeCollisionManager2::registerObject(CollisionObject* obj)
-{
- size_t node = dtree.insert(obj->getAABB(), obj);
- table[obj] = node;
-}
-
-void DynamicAABBTreeCollisionManager2::unregisterObject(CollisionObject* obj)
-{
- size_t node = table[obj];
- table.erase(obj);
- dtree.remove(node);
-}
-
-void DynamicAABBTreeCollisionManager2::setup()
-{
- if(!setup_)
- {
- int num = dtree.size();
- if(num == 0)
- {
- setup_ = true;
- return;
- }
-
- int height = dtree.getMaxHeight();
-
-
- if(height - std::log((FCL_REAL)num) / std::log(2.0) < max_tree_nonbalanced_level)
- dtree.balanceIncremental(tree_incremental_balance_pass);
- else
- dtree.balanceTopdown();
-
- setup_ = true;
- }
-}
-
-
-void DynamicAABBTreeCollisionManager2::update()
-{
- for(DynamicAABBTable::const_iterator it = table.begin(), end = table.end(); it != end; ++it)
- {
- CollisionObject* obj = it->first;
- size_t node = it->second;
- dtree.getNodes()[node].bv = obj->getAABB();
- }
-
- dtree.refit();
- setup_ = false;
-
- setup();
-}
-
-void DynamicAABBTreeCollisionManager2::update_(CollisionObject* updated_obj)
-{
- DynamicAABBTable::const_iterator it = table.find(updated_obj);
- if(it != table.end())
- {
- size_t node = it->second;
- if(!dtree.getNodes()[node].bv.equal(updated_obj->getAABB()))
- dtree.update(node, updated_obj->getAABB());
- }
- setup_ = false;
-}
-
-void DynamicAABBTreeCollisionManager2::update(CollisionObject* updated_obj)
-{
- update_(updated_obj);
- setup();
-}
-
-void DynamicAABBTreeCollisionManager2::update(const std::vector<CollisionObject*>& updated_objs)
-{
- for(size_t i = 0, size = updated_objs.size(); i < size; ++i)
- update_(updated_objs[i]);
- setup();
-}
-
-bool DynamicAABBTreeCollisionManager2::collisionRecurse(DynamicAABBNode* nodes1, size_t root1_id,
- DynamicAABBNode* nodes2, size_t root2_id,
- void* cdata, CollisionCallBack callback) const
-{
- DynamicAABBNode* root1 = nodes1 + root1_id;
- DynamicAABBNode* root2 = nodes2 + root2_id;
- if(root1->isLeaf() && root2->isLeaf())
- {
- if(!root1->bv.overlap(root2->bv)) return false;
- return callback(static_cast<CollisionObject*>(root1->data), static_cast<CollisionObject*>(root2->data), cdata);
- }
-
- if(!root1->bv.overlap(root2->bv)) return false;
-
- if(root2->isLeaf() || (!root1->isLeaf() && (root1->bv.size() > root2->bv.size())))
- {
- if(collisionRecurse(nodes1, root1->childs[0], nodes2, root2_id, cdata, callback))
- return true;
- if(collisionRecurse(nodes1, root1->childs[1], nodes2, root2_id, cdata, callback))
- return true;
- }
- else
- {
- if(collisionRecurse(nodes1, root1_id, nodes2, root2->childs[0], cdata, callback))
- return true;
- if(collisionRecurse(nodes1, root1_id, nodes2, root2->childs[1], cdata, callback))
- return true;
- }
- return false;
-}
-
-bool DynamicAABBTreeCollisionManager2::collisionRecurse(DynamicAABBNode* nodes, size_t root_id, CollisionObject* query, void* cdata, CollisionCallBack callback) const
-{
- DynamicAABBNode* root = nodes + root_id;
- if(root->isLeaf())
- {
- if(!root->bv.overlap(query->getAABB())) return false;
- return callback(static_cast<CollisionObject*>(root->data), query, cdata);
- }
-
- if(!root->bv.overlap(query->getAABB())) return false;
-
- int select_res = alternative::select(query->getAABB(), root->childs[0], root->childs[1], nodes);
-
- if(collisionRecurse(nodes, root->childs[select_res], query, cdata, callback))
- return true;
-
- if(collisionRecurse(nodes, root->childs[1-select_res], query, cdata, callback))
- return true;
-
- return false;
-}
-
-bool DynamicAABBTreeCollisionManager2::selfCollisionRecurse(DynamicAABBNode* nodes, size_t root_id, void* cdata, CollisionCallBack callback) const
-{
- DynamicAABBNode* root = nodes + root_id;
- if(root->isLeaf()) return false;
-
- if(selfCollisionRecurse(nodes, root->childs[0], cdata, callback))
- return true;
-
- if(selfCollisionRecurse(nodes, root->childs[1], cdata, callback))
- return true;
-
- if(collisionRecurse(nodes, root->childs[0], nodes, root->childs[1], cdata, callback))
- return true;
-
- return false;
-}
-
-bool DynamicAABBTreeCollisionManager2::distanceRecurse(DynamicAABBNode* nodes1, size_t root1_id,
- DynamicAABBNode* nodes2, size_t root2_id,
- void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
-{
- DynamicAABBNode* root1 = nodes1 + root1_id;
- DynamicAABBNode* root2 = nodes2 + root2_id;
- if(root1->isLeaf() && root2->isLeaf())
- {
- CollisionObject* root1_obj = static_cast<CollisionObject*>(root1->data);
- CollisionObject* root2_obj = static_cast<CollisionObject*>(root2->data);
- return callback(root1_obj, root2_obj, cdata, min_dist);
- }
-
- if(root2->isLeaf() || (!root1->isLeaf() && (root1->bv.size() > root2->bv.size())))
- {
- FCL_REAL d1 = root2->bv.distance((nodes1 + root1->childs[0])->bv);
- FCL_REAL d2 = root2->bv.distance((nodes1 + root1->childs[1])->bv);
-
- if(d2 < d1)
- {
- if(d2 < min_dist)
- {
- if(distanceRecurse(nodes1, root1->childs[1], nodes2, root2_id, cdata, callback, min_dist))
- return true;
- }
-
- if(d1 < min_dist)
- {
- if(distanceRecurse(nodes1, root1->childs[0], nodes2, root2_id, cdata, callback, min_dist))
- return true;
- }
- }
- else
- {
- if(d1 < min_dist)
- {
- if(distanceRecurse(nodes1, root1->childs[0], nodes2, root2_id, cdata, callback, min_dist))
- return true;
- }
-
- if(d2 < min_dist)
- {
- if(distanceRecurse(nodes1, root1->childs[1], nodes2, root2_id, cdata, callback, min_dist))
- return true;
- }
- }
- }
- else
- {
- FCL_REAL d1 = root1->bv.distance((nodes2 + root2->childs[0])->bv);
- FCL_REAL d2 = root1->bv.distance((nodes2 + root2->childs[1])->bv);
-
- if(d2 < d1)
- {
- if(d2 < min_dist)
- {
- if(distanceRecurse(nodes1, root1_id, nodes2, root2->childs[1], cdata, callback, min_dist))
- return true;
- }
-
- if(d1 < min_dist)
- {
- if(distanceRecurse(nodes1, root1_id, nodes2, root2->childs[0], cdata, callback, min_dist))
- return true;
- }
- }
- else
- {
- if(d1 < min_dist)
- {
- if(distanceRecurse(nodes1, root1_id, nodes2, root2->childs[0], cdata, callback, min_dist))
- return true;
- }
-
- if(d2 < min_dist)
- {
- if(distanceRecurse(nodes1, root1_id, nodes2, root2->childs[1], cdata, callback, min_dist))
- return true;
- }
- }
- }
-
- return false;
-}
-
-bool DynamicAABBTreeCollisionManager2::distanceRecurse(DynamicAABBNode* nodes, size_t root_id, CollisionObject* query, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
-{
- DynamicAABBNode* root = nodes + root_id;
- if(root->isLeaf())
- {
- CollisionObject* root_obj = static_cast<CollisionObject*>(root->data);
- return callback(root_obj, query, cdata, min_dist);
- }
-
- FCL_REAL d1 = query->getAABB().distance((nodes + root->childs[0])->bv);
- FCL_REAL d2 = query->getAABB().distance((nodes + root->childs[1])->bv);
-
- if(d2 < d1)
- {
- if(d2 < min_dist)
- {
- if(distanceRecurse(nodes, root->childs[1], query, cdata, callback, min_dist))
- return true;
- }
-
- if(d1 < min_dist)
- {
- if(distanceRecurse(nodes, root->childs[0], query, cdata, callback, min_dist))
- return true;
- }
- }
- else
- {
- if(d1 < min_dist)
- {
- if(distanceRecurse(nodes, root->childs[0], query, cdata, callback, min_dist))
- return true;
- }
-
- if(d2 < min_dist)
- {
- if(distanceRecurse(nodes, root->childs[1], query, cdata, callback, min_dist))
- return true;
- }
- }
-
- return false;
-}
-
-bool DynamicAABBTreeCollisionManager2::selfDistanceRecurse(DynamicAABBNode* nodes, size_t root_id, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
-{
- DynamicAABBNode* root = nodes + root_id;
- if(root->isLeaf()) return false;
-
- if(selfDistanceRecurse(nodes, root->childs[0], cdata, callback, min_dist))
- return true;
-
- if(selfDistanceRecurse(nodes, root->childs[1], cdata, callback, min_dist))
- return true;
-
- if(distanceRecurse(nodes, root->childs[0], nodes, root->childs[1], cdata, callback, min_dist))
- return true;
-
- return false;
-}
-
-bool collisionRecurse_(DynamicAABBTreeCollisionManager2::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, CollisionCallBack callback)
-{
- DynamicAABBTreeCollisionManager2::DynamicAABBNode* root1 = nodes1 + root1_id;
- if(root1->isLeaf() && !root2->hasChildren())
- {
- CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
- if(!tree2->isNodeFree(root2) && !obj1->isFree())
- {
- OBB obb1, obb2;
- convertBV(root1->bv, Transform3f(), obb1);
- convertBV(root2_bv, tf2, obb2);
-
- if(obb1.overlap(obb2))
- {
- Box* box = new Box();
- Transform3f box_tf;
- constructBox(root2_bv, tf2, *box, box_tf);
-
- box->cost_density = root2->getOccupancy();
- box->threshold_occupied = tree2->getOccupancyThres();
-
- CollisionObject obj2(boost::shared_ptr<CollisionGeometry>(box), box_tf);
- return callback(obj1, &obj2, cdata);
- }
- else return false;
- }
- else return false;
- }
-
- OBB obb1, obb2;
- convertBV(root1->bv, Transform3f(), obb1);
- convertBV(root2_bv, tf2, obb2);
-
- if(tree2->isNodeFree(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 collisionRecurse_(DynamicAABBTreeCollisionManager2::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Vec3f& tf2, void* cdata, CollisionCallBack callback)
-{
- DynamicAABBTreeCollisionManager2::DynamicAABBNode* root1 = nodes1 + root1_id;
- if(root1->isLeaf() && !root2->hasChildren())
- {
- CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
- if(!tree2->isNodeFree(root2))
- {
- const AABB& root_bv_t = translate(root2_bv, tf2);
- if(root1->bv.overlap(root_bv_t))
- {
- Box* box = new Box();
- Transform3f box_tf;
- constructBox(root2_bv, tf2, *box, box_tf);
-
- box->cost_density = root2->getOccupancy();
- box->threshold_occupied = tree2->getOccupancyThres();
-
- CollisionObject obj2(boost::shared_ptr<CollisionGeometry>(box), box_tf);
- return callback(obj1, &obj2, cdata);
- }
- else return false;
- }
- else return false;
- }
-
- const AABB& root_bv_t = translate(root2_bv, tf2);
- if(tree2->isNodeFree(root2) || !root1->bv.overlap(root_bv_t)) 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::collisionRecurse(DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, CollisionCallBack callback) const
-{
- if(tf2.getQuatRotation().isIdentity())
- return collisionRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2.getTranslation(), cdata, callback);
- else
- return collisionRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2, cdata, callback);
-}
-
-
-bool distanceRecurse_(DynamicAABBTreeCollisionManager2::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist)
-{
- DynamicAABBTreeCollisionManager2::DynamicAABBNode* root1 = nodes1 + root1_id;
- if(root1->isLeaf() && !root2->hasChildren())
- {
- if(tree2->isNodeOccupied(root2))
- {
- Box* box = new Box();
- Transform3f 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;
-}
-
-
-bool distanceRecurse_(DynamicAABBTreeCollisionManager2::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Vec3f& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist)
-{
- DynamicAABBTreeCollisionManager2::DynamicAABBNode* root1 = nodes1 + root1_id;
- if(root1->isLeaf() && !root2->hasChildren())
- {
- if(tree2->isNodeOccupied(root2))
- {
- Box* box = new Box();
- Transform3f 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())))
- {
- const AABB& aabb2 = translate(root2_bv, tf2);
-
- 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);
-
- const AABB& aabb2 = translate(child_bv, tf2);
- 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;
-}
-
-bool DynamicAABBTreeCollisionManager2::distanceRecurse(DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
-{
- if(tf2.getQuatRotation().isIdentity())
- return distanceRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2.getTranslation(), cdata, callback, min_dist);
- else
- return distanceRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2, cdata, callback, min_dist);
-}
-
-}
diff --git a/trunk/fcl/src/broadphase/broadphase_SSaP.cpp b/trunk/fcl/src/broadphase/broadphase_SSaP.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bc9d2df5af9780d5fd7303293e5356f9d543caf8
--- /dev/null
+++ b/trunk/fcl/src/broadphase/broadphase_SSaP.cpp
@@ -0,0 +1,504 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * 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 Jia Pan */
+
+#include "fcl/broadphase/broadphase_SSaP.h"
+#include <algorithm>
+#include <limits>
+
+namespace fcl
+{
+
+/** \brief Functor sorting objects according to the AABB lower x bound */
+struct SortByXLow
+{
+ bool operator()(const CollisionObject* a, const CollisionObject* b) const
+ {
+ if(a->getAABB().min_[0] < b->getAABB().min_[0])
+ return true;
+ return false;
+ }
+};
+
+/** \brief Functor sorting objects according to the AABB lower y bound */
+struct SortByYLow
+{
+ bool operator()(const CollisionObject* a, const CollisionObject* b) const
+ {
+ if(a->getAABB().min_[1] < b->getAABB().min_[1])
+ return true;
+ return false;
+ }
+};
+
+/** \brief Functor sorting objects according to the AABB lower z bound */
+struct SortByZLow
+{
+ bool operator()(const CollisionObject* a, const CollisionObject* b) const
+ {
+ if(a->getAABB().min_[2] < b->getAABB().min_[2])
+ return true;
+ return false;
+ }
+};
+
+/** \brief Dummy collision object with a point AABB */
+class DummyCollisionObject : public CollisionObject
+{
+public:
+ DummyCollisionObject(const AABB& aabb_) : CollisionObject()
+ {
+ aabb = aabb_;
+ }
+
+ void computeLocalAABB() {}
+};
+
+
+void SSaPCollisionManager::unregisterObject(CollisionObject* obj)
+{
+ setup();
+
+ DummyCollisionObject dummyHigh(AABB(obj->getAABB().max_));
+
+ std::vector<CollisionObject*>::iterator pos_start1 = objs_x.begin();
+ std::vector<CollisionObject*>::iterator pos_end1 = std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow());
+
+ while(pos_start1 < pos_end1)
+ {
+ if(*pos_start1 == obj)
+ {
+ objs_x.erase(pos_start1);
+ break;
+ }
+ ++pos_start1;
+ }
+
+ std::vector<CollisionObject*>::iterator pos_start2 = objs_y.begin();
+ std::vector<CollisionObject*>::iterator pos_end2 = std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow());
+
+ while(pos_start2 < pos_end2)
+ {
+ if(*pos_start2 == obj)
+ {
+ objs_y.erase(pos_start2);
+ break;
+ }
+ ++pos_start2;
+ }
+
+ std::vector<CollisionObject*>::iterator pos_start3 = objs_z.begin();
+ std::vector<CollisionObject*>::iterator pos_end3 = std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow());
+
+ while(pos_start3 < pos_end3)
+ {
+ if(*pos_start3 == obj)
+ {
+ objs_z.erase(pos_start3);
+ break;
+ }
+ ++pos_start3;
+ }
+}
+
+
+void SSaPCollisionManager::registerObject(CollisionObject* obj)
+{
+ objs_x.push_back(obj);
+ objs_y.push_back(obj);
+ objs_z.push_back(obj);
+ setup_ = false;
+}
+
+void SSaPCollisionManager::setup()
+{
+ if(!setup_)
+ {
+ std::sort(objs_x.begin(), objs_x.end(), SortByXLow());
+ std::sort(objs_y.begin(), objs_y.end(), SortByYLow());
+ std::sort(objs_z.begin(), objs_z.end(), SortByZLow());
+ setup_ = true;
+ }
+}
+
+void SSaPCollisionManager::update()
+{
+ setup_ = false;
+ setup();
+}
+
+void SSaPCollisionManager::clear()
+{
+ objs_x.clear();
+ objs_y.clear();
+ objs_z.clear();
+ setup_ = false;
+}
+
+void SSaPCollisionManager::getObjects(std::vector<CollisionObject*>& objs) const
+{
+ objs.resize(objs_x.size());
+ std::copy(objs_x.begin(), objs_x.end(), objs.begin());
+}
+
+bool SSaPCollisionManager::checkColl(std::vector<CollisionObject*>::const_iterator pos_start, std::vector<CollisionObject*>::const_iterator pos_end,
+ CollisionObject* obj, void* cdata, CollisionCallBack callback) const
+{
+ while(pos_start < pos_end)
+ {
+ if(*pos_start != obj) // no collision between the same object
+ {
+ if((*pos_start)->getAABB().overlap(obj->getAABB()))
+ {
+ if(callback(*pos_start, obj, cdata))
+ return true;
+ }
+ }
+ pos_start++;
+ }
+ return false;
+}
+
+bool SSaPCollisionManager::checkDis(std::vector<CollisionObject*>::const_iterator pos_start, std::vector<CollisionObject*>::const_iterator pos_end, CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
+{
+ while(pos_start < pos_end)
+ {
+ if(*pos_start != obj) // no distance between the same object
+ {
+ if((*pos_start)->getAABB().distance(obj->getAABB()) < min_dist)
+ {
+ if(callback(*pos_start, obj, cdata, min_dist))
+ return true;
+ }
+ }
+ pos_start++;
+ }
+
+ return false;
+}
+
+
+
+void SSaPCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
+{
+ if(size() == 0) return;
+
+ collide_(obj, cdata, callback);
+}
+
+bool SSaPCollisionManager::collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
+{
+ static const unsigned int CUTOFF = 100;
+
+ DummyCollisionObject dummyHigh(AABB(obj->getAABB().max_));
+ bool coll_res = false;
+
+ std::vector<CollisionObject*>::const_iterator pos_start1 = objs_x.begin();
+ std::vector<CollisionObject*>::const_iterator pos_end1 = std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow());
+ unsigned int d1 = pos_end1 - pos_start1;
+
+ if(d1 > CUTOFF)
+ {
+ std::vector<CollisionObject*>::const_iterator pos_start2 = objs_y.begin();
+ std::vector<CollisionObject*>::const_iterator pos_end2 = std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow());
+ unsigned int d2 = pos_end2 - pos_start2;
+
+ if(d2 > CUTOFF)
+ {
+ std::vector<CollisionObject*>::const_iterator pos_start3 = objs_z.begin();
+ std::vector<CollisionObject*>::const_iterator pos_end3 = std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow());
+ unsigned int d3 = pos_end3 - pos_start3;
+
+ if(d3 > CUTOFF)
+ {
+ if(d3 <= d2 && d3 <= d1)
+ coll_res = checkColl(pos_start3, pos_end3, obj, cdata, callback);
+ else
+ {
+ if(d2 <= d3 && d2 <= d1)
+ coll_res = checkColl(pos_start2, pos_end2, obj, cdata, callback);
+ else
+ coll_res = checkColl(pos_start1, pos_end1, obj, cdata, callback);
+ }
+ }
+ else
+ coll_res = checkColl(pos_start3, pos_end3, obj, cdata, callback);
+ }
+ else
+ coll_res = checkColl(pos_start2, pos_end2, obj, cdata, callback);
+ }
+ else
+ coll_res = checkColl(pos_start1, pos_end1, obj, cdata, callback);
+
+ return coll_res;
+}
+
+
+void SSaPCollisionManager::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const
+{
+ if(size() == 0) return;
+
+ FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
+ distance_(obj, cdata, callback, min_dist);
+}
+
+bool SSaPCollisionManager::distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
+{
+ static const unsigned int CUTOFF = 100;
+ Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
+ Vec3f dummy_vector = obj->getAABB().max_;
+ if(min_dist < std::numeric_limits<FCL_REAL>::max())
+ dummy_vector += Vec3f(min_dist, min_dist, min_dist);
+
+ std::vector<CollisionObject*>::const_iterator pos_start1 = objs_x.begin();
+ std::vector<CollisionObject*>::const_iterator pos_start2 = objs_y.begin();
+ std::vector<CollisionObject*>::const_iterator pos_start3 = objs_z.begin();
+ std::vector<CollisionObject*>::const_iterator pos_end1 = objs_x.end();
+ std::vector<CollisionObject*>::const_iterator pos_end2 = objs_y.end();
+ std::vector<CollisionObject*>::const_iterator pos_end3 = objs_z.end();
+
+ int status = 1;
+ FCL_REAL old_min_distance;
+
+ while(1)
+ {
+ old_min_distance = min_dist;
+ DummyCollisionObject dummyHigh((AABB(dummy_vector)));
+
+ pos_end1 = std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow());
+ unsigned int d1 = pos_end1 - pos_start1;
+
+ bool dist_res = false;
+
+ if(d1 > CUTOFF)
+ {
+ pos_end2 = std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow());
+ unsigned int d2 = pos_end2 - pos_start2;
+
+ if(d2 > CUTOFF)
+ {
+ pos_end3 = std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow());
+ unsigned int d3 = pos_end3 - pos_start3;
+
+ if(d3 > CUTOFF)
+ {
+ if(d3 <= d2 && d3 <= d1)
+ dist_res = checkDis(pos_start3, pos_end3, obj, cdata, callback, min_dist);
+ else
+ {
+ if(d2 <= d3 && d2 <= d1)
+ dist_res = checkDis(pos_start2, pos_end2, obj, cdata, callback, min_dist);
+ else
+ dist_res = checkDis(pos_start1, pos_end1, obj, cdata, callback, min_dist);
+ }
+ }
+ else
+ dist_res = checkDis(pos_start3, pos_end3, obj, cdata, callback, min_dist);
+ }
+ else
+ dist_res = checkDis(pos_start2, pos_end2, obj, cdata, callback, min_dist);
+ }
+ else
+ {
+ dist_res = checkDis(pos_start1, pos_end1, obj, cdata, callback, min_dist);
+ }
+
+ if(dist_res) return true;
+
+ if(status == 1)
+ {
+ if(old_min_distance < std::numeric_limits<FCL_REAL>::max())
+ break;
+ else
+ {
+ // from infinity to a finite one, only need one additional loop
+ // to check the possible missed ones to the right of the objs array
+ if(min_dist < old_min_distance)
+ {
+ dummy_vector = obj->getAABB().max_ + Vec3f(min_dist, min_dist, min_dist);
+ status = 0;
+ }
+ else // need more loop
+ {
+ if(dummy_vector.equal(obj->getAABB().max_))
+ dummy_vector = dummy_vector + delta;
+ else
+ dummy_vector = dummy_vector * 2 - obj->getAABB().max_;
+ }
+ }
+
+ // yes, following is wrong, will result in too large distance.
+ // if(pos_end1 != objs_x.end()) pos_start1 = pos_end1;
+ // if(pos_end2 != objs_y.end()) pos_start2 = pos_end2;
+ // if(pos_end3 != objs_z.end()) pos_start3 = pos_end3;
+ }
+ else if(status == 0)
+ break;
+ }
+
+ return false;
+}
+
+void SSaPCollisionManager::collide(void* cdata, CollisionCallBack callback) const
+{
+ if(size() == 0) return;
+
+ std::vector<CollisionObject*>::const_iterator pos, run_pos, pos_end;
+ size_t axis = selectOptimalAxis(objs_x, objs_y, objs_z,
+ pos, pos_end);
+ size_t axis2 = (axis + 1 > 2) ? 0 : (axis + 1);
+ size_t axis3 = (axis2 + 1 > 2) ? 0 : (axis2 + 1);
+
+ run_pos = pos;
+
+ while((run_pos < pos_end) && (pos < pos_end))
+ {
+ CollisionObject* obj = *(pos++);
+
+ while(1)
+ {
+ if((*run_pos)->getAABB().min_[axis] < obj->getAABB().min_[axis])
+ {
+ run_pos++;
+ if(run_pos == pos_end) break;
+ continue;
+ }
+ else
+ {
+ run_pos++;
+ break;
+ }
+ }
+
+ if(run_pos < pos_end)
+ {
+ std::vector<CollisionObject*>::const_iterator run_pos2 = run_pos;
+
+ while((*run_pos2)->getAABB().min_[axis] <= obj->getAABB().max_[axis])
+ {
+ CollisionObject* obj2 = *run_pos2;
+ run_pos2++;
+
+ if((obj->getAABB().max_[axis2] >= obj2->getAABB().min_[axis2]) && (obj2->getAABB().max_[axis2] >= obj->getAABB().min_[axis2]))
+ {
+ if((obj->getAABB().max_[axis3] >= obj2->getAABB().min_[axis3]) && (obj2->getAABB().max_[axis3] >= obj->getAABB().min_[axis3]))
+ {
+ if(callback(obj, obj2, cdata))
+ return;
+ }
+ }
+
+ if(run_pos2 == pos_end) break;
+ }
+ }
+ }
+}
+
+
+void SSaPCollisionManager::distance(void* cdata, DistanceCallBack callback) const
+{
+ if(size() == 0) return;
+
+ std::vector<CollisionObject*>::const_iterator it, it_end;
+ size_t axis = selectOptimalAxis(objs_x, objs_y, objs_z, it, it_end);
+
+ FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
+ for(; it != it_end; ++it)
+ {
+ if(distance_(*it, cdata, callback, min_dist))
+ return;
+ }
+}
+
+void SSaPCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const
+{
+ SSaPCollisionManager* other_manager = static_cast<SSaPCollisionManager*>(other_manager_);
+
+ if((size() == 0) || (other_manager->size() == 0)) return;
+
+ if(this == other_manager)
+ {
+ collide(cdata, callback);
+ return;
+ }
+
+
+ std::vector<CollisionObject*>::const_iterator it, end;
+ if(this->size() < other_manager->size())
+ {
+ for(it = objs_x.begin(), end = objs_x.end(); it != end; ++it)
+ if(other_manager->collide_(*it, cdata, callback)) return;
+ }
+ else
+ {
+ for(it = other_manager->objs_x.begin(), end != other_manager->objs_x.end(); it != end; ++it)
+ if(collide_(*it, cdata, callback)) return;
+ }
+}
+
+void SSaPCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const
+{
+ SSaPCollisionManager* other_manager = static_cast<SSaPCollisionManager*>(other_manager_);
+
+ if((size() == 0) || (other_manager->size() == 0)) return;
+
+ if(this == other_manager)
+ {
+ distance(cdata, callback);
+ return;
+ }
+
+ FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
+ std::vector<CollisionObject*>::const_iterator it, end;
+ if(this->size() < other_manager->size())
+ {
+ for(it = objs_x.begin(), end = objs_x.end(); it != end; ++it)
+ if(other_manager->distance_(*it, cdata, callback, min_dist)) return;
+ }
+ else
+ {
+ for(it = other_manager->objs_x.begin(), end = other_manager->objs_x.end(); it != end; ++it)
+ if(distance_(*it, cdata, callback, min_dist)) return;
+ }
+}
+
+bool SSaPCollisionManager::empty() const
+{
+ return objs_x.empty();
+}
+
+
+
+}
diff --git a/trunk/fcl/src/broadphase/broadphase_SaP.cpp b/trunk/fcl/src/broadphase/broadphase_SaP.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..432a94b333827f11c72deccb0f4ff2d2d0b23238
--- /dev/null
+++ b/trunk/fcl/src/broadphase/broadphase_SaP.cpp
@@ -0,0 +1,762 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * 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 Jia Pan */
+
+#include "fcl/broadphase/broadphase_SaP.h"
+#include <algorithm>
+#include <limits>
+#include <boost/bind.hpp>
+
+namespace fcl
+{
+
+void SaPCollisionManager::unregisterObject(CollisionObject* obj)
+{
+ std::list<SaPAABB*>::iterator it = AABB_arr.begin();
+ for(std::list<SaPAABB*>::iterator end = AABB_arr.end(); it != end; ++it)
+ {
+ if((*it)->obj == obj)
+ break;
+ }
+
+ AABB_arr.erase(it);
+ obj_aabb_map.erase(obj);
+
+ if(it == AABB_arr.end())
+ return;
+
+ SaPAABB* curr = *it;
+ *it = NULL;
+
+ for(int coord = 0; coord < 3; ++coord)
+ {
+ //first delete the lo endpoint of the interval.
+ if(curr->lo->prev[coord] == NULL)
+ elist[coord] = curr->lo->next[coord];
+ else
+ curr->lo->prev[coord]->next[coord] = curr->lo->next[coord];
+
+ curr->lo->next[coord]->prev[coord] = curr->lo->prev[coord];
+
+ //then, delete the "hi" endpoint.
+ if(curr->hi->prev[coord] == NULL)
+ elist[coord] = curr->hi->next[coord];
+ else
+ curr->hi->prev[coord]->next[coord] = curr->hi->next[coord];
+
+ if(curr->hi->next[coord] != NULL)
+ curr->hi->next[coord]->prev[coord] = curr->hi->prev[coord];
+ }
+
+ delete curr->lo;
+ delete curr->hi;
+ delete curr;
+
+ overlap_pairs.remove_if(isUnregistered(obj));
+}
+
+void SaPCollisionManager::registerObjects(const std::vector<CollisionObject*>& other_objs)
+{
+ if(size() > 0)
+ BroadPhaseCollisionManager::registerObjects(other_objs);
+ else
+ {
+ std::vector<EndPoint*> endpoints(2 * other_objs.size());
+
+ for(size_t i = 0; i < other_objs.size(); ++i)
+ {
+ SaPAABB* sapaabb = new SaPAABB();
+ sapaabb->obj = other_objs[i];
+ sapaabb->lo = new EndPoint();
+ sapaabb->hi = new EndPoint();
+ sapaabb->cached = other_objs[i]->getAABB();
+ endpoints[2 * i] = sapaabb->lo;
+ endpoints[2 * i + 1] = sapaabb->hi;
+ sapaabb->lo->minmax = 0;
+ sapaabb->hi->minmax = 1;
+ sapaabb->lo->aabb = sapaabb;
+ sapaabb->hi->aabb = sapaabb;
+ AABB_arr.push_back(sapaabb);
+ obj_aabb_map[other_objs[i]] = sapaabb;
+ }
+
+
+ FCL_REAL scale[3];
+ for(size_t coord = 0; coord < 3; ++coord)
+ {
+ std::sort(endpoints.begin(), endpoints.end(),
+ boost::bind(std::less<FCL_REAL>(),
+ boost::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const >(&EndPoint::getVal), _1, coord),
+ boost::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const >(&EndPoint::getVal), _2, coord)));
+
+ endpoints[0]->prev[coord] = NULL;
+ endpoints[0]->next[coord] = endpoints[1];
+ for(int i = 1; i < endpoints.size() - 1; ++i)
+ {
+ endpoints[i]->prev[coord] = endpoints[i-1];
+ endpoints[i]->next[coord] = endpoints[i+1];
+ }
+ endpoints[endpoints.size() - 1]->prev[coord] = endpoints[endpoints.size() - 2];
+ endpoints[endpoints.size() - 1]->next[coord] = NULL;
+
+ elist[coord] = endpoints[0];
+
+ scale[coord] = endpoints.back()->aabb->cached.max_[coord] - endpoints[0]->aabb->cached.min_[coord];
+ }
+
+ int axis = 0;
+ if(scale[axis] < scale[1]) axis = 1;
+ if(scale[axis] < scale[2]) axis = 2;
+
+ EndPoint* pos = elist[axis];
+
+ while(pos != NULL)
+ {
+ EndPoint* pos_next = NULL;
+ SaPAABB* aabb = pos->aabb;
+ EndPoint* pos_it = pos->next[axis];
+
+ while(pos_it != NULL)
+ {
+ if(pos_it->aabb == aabb)
+ {
+ if(pos_next == NULL) pos_next = pos_it;
+ break;
+ }
+
+ if(pos_it->minmax == 0)
+ {
+ if(pos_next == NULL) pos_next = pos_it;
+ if(pos_it->aabb->cached.overlap(aabb->cached))
+ overlap_pairs.push_back(SaPPair(pos_it->aabb->obj, aabb->obj));
+ }
+ pos_it = pos_it->next[axis];
+ }
+
+ pos = pos_next;
+ }
+ }
+
+ updateVelist();
+}
+
+void SaPCollisionManager::registerObject(CollisionObject* obj)
+{
+ SaPAABB* curr = new SaPAABB;
+ curr->cached = obj->getAABB();
+ curr->obj = obj;
+ curr->lo = new EndPoint;
+ curr->lo->minmax = 0;
+ curr->lo->aabb = curr;
+
+ curr->hi = new EndPoint;
+ curr->hi->minmax = 1;
+ curr->hi->aabb = curr;
+
+ for(int coord = 0; coord < 3; ++coord)
+ {
+ EndPoint* current = elist[coord];
+
+ // first insert the lo end point
+ if(current == NULL) // empty list
+ {
+ elist[coord] = curr->lo;
+ curr->lo->prev[coord] = curr->lo->next[coord] = NULL;
+ }
+ else // otherwise, find the correct location in the list and insert
+ {
+ EndPoint* curr_lo = curr->lo;
+ FCL_REAL curr_lo_val = curr_lo->getVal()[coord];
+ while((current->getVal()[coord] < curr_lo_val) && (current->next[coord] != NULL))
+ current = current->next[coord];
+
+ if(current->getVal()[coord] >= curr_lo_val)
+ {
+ curr_lo->prev[coord] = current->prev[coord];
+ curr_lo->next[coord] = current;
+ if(current->prev[coord] == NULL)
+ elist[coord] = curr_lo;
+ else
+ current->prev[coord]->next[coord] = curr_lo;
+
+ current->prev[coord] = curr_lo;
+ }
+ else
+ {
+ curr_lo->prev[coord] = current;
+ curr_lo->next[coord] = NULL;
+ current->next[coord] = curr_lo;
+ }
+ }
+
+ // now insert hi end point
+ current = curr->lo;
+
+ EndPoint* curr_hi = curr->hi;
+ FCL_REAL curr_hi_val = curr_hi->getVal()[coord];
+
+ if(coord == 0)
+ {
+ while((current->getVal()[coord] < curr_hi_val) && (current->next[coord] != NULL))
+ {
+ if(current != curr->lo)
+ if(current->aabb->cached.overlap(curr->cached))
+ overlap_pairs.push_back(SaPPair(current->aabb->obj, obj));
+
+ current = current->next[coord];
+ }
+ }
+ else
+ {
+ while((current->getVal()[coord] < curr_hi_val) && (current->next[coord] != NULL))
+ current = current->next[coord];
+ }
+
+ if(current->getVal()[coord] >= curr_hi_val)
+ {
+ curr_hi->prev[coord] = current->prev[coord];
+ curr_hi->next[coord] = current;
+ if(current->prev[coord] == NULL)
+ elist[coord] = curr_hi;
+ else
+ current->prev[coord]->next[coord] = curr_hi;
+
+ current->prev[coord] = curr_hi;
+ }
+ else
+ {
+ curr_hi->prev[coord] = current;
+ curr_hi->next[coord] = NULL;
+ current->next[coord] = curr_hi;
+ }
+ }
+
+ AABB_arr.push_back(curr);
+
+ obj_aabb_map[obj] = curr;
+
+ updateVelist();
+}
+
+void SaPCollisionManager::setup()
+{
+ FCL_REAL scale[3];
+ scale[0] = (velist[0].back())->getVal(0) - velist[0][0]->getVal(0);
+ scale[1] = (velist[1].back())->getVal(1) - velist[1][0]->getVal(1);;
+ scale[2] = (velist[2].back())->getVal(2) - velist[2][0]->getVal(2);
+ size_t axis = 0;
+ if(scale[axis] < scale[1]) axis = 1;
+ if(scale[axis] < scale[2]) axis = 2;
+ optimal_axis = axis;
+}
+
+void SaPCollisionManager::update_(SaPAABB* updated_aabb)
+{
+ if(updated_aabb->cached.equal(updated_aabb->obj->getAABB()))
+ return;
+
+ SaPAABB* current = updated_aabb;
+
+ Vec3f new_min = current->obj->getAABB().min_;
+ Vec3f new_max = current->obj->getAABB().max_;
+
+ SaPAABB dummy;
+ dummy.cached = current->obj->getAABB();
+
+ for(int coord = 0; coord < 3; ++coord)
+ {
+ int direction; // -1 reverse, 0 nochange, 1 forward
+ EndPoint* temp;
+
+ if(current->lo->getVal(coord) > new_min[coord])
+ direction = -1;
+ else if(current->lo->getVal(coord) < new_min[coord])
+ direction = 1;
+ else direction = 0;
+
+ if(direction == -1)
+ {
+ //first update the "lo" endpoint of the interval
+ if(current->lo->prev[coord] != NULL)
+ {
+ temp = current->lo;
+ while((temp != NULL) && (temp->getVal(coord) > new_min[coord]))
+ {
+ if(temp->minmax == 1)
+ if(temp->aabb->cached.overlap(dummy.cached))
+ addToOverlapPairs(SaPPair(temp->aabb->obj, current->obj));
+ temp = temp->prev[coord];
+ }
+
+ if(temp == NULL)
+ {
+ current->lo->prev[coord]->next[coord] = current->lo->next[coord];
+ current->lo->next[coord]->prev[coord] = current->lo->prev[coord];
+ current->lo->prev[coord] = NULL;
+ current->lo->next[coord] = elist[coord];
+ elist[coord]->prev[coord] = current->lo;
+ elist[coord] = current->lo;
+ }
+ else
+ {
+ current->lo->prev[coord]->next[coord] = current->lo->next[coord];
+ current->lo->next[coord]->prev[coord] = current->lo->prev[coord];
+ current->lo->prev[coord] = temp;
+ current->lo->next[coord] = temp->next[coord];
+ temp->next[coord]->prev[coord] = current->lo;
+ temp->next[coord] = current->lo;
+ }
+ }
+
+ current->lo->getVal(coord) = new_min[coord];
+
+ // update hi end point
+ temp = current->hi;
+ while(temp->getVal(coord) > new_max[coord])
+ {
+ if((temp->minmax == 0) && (temp->aabb->cached.overlap(current->cached)))
+ removeFromOverlapPairs(SaPPair(temp->aabb->obj, current->obj));
+ temp = temp->prev[coord];
+ }
+
+ current->hi->prev[coord]->next[coord] = current->hi->next[coord];
+ if(current->hi->next[coord] != NULL)
+ current->hi->next[coord]->prev[coord] = current->hi->prev[coord];
+ current->hi->prev[coord] = temp;
+ current->hi->next[coord] = temp->next[coord];
+ if(temp->next[coord] != NULL)
+ temp->next[coord]->prev[coord] = current->hi;
+ temp->next[coord] = current->hi;
+
+ current->hi->getVal(coord) = new_max[coord];
+ }
+ else if(direction == 1)
+ {
+ //here, we first update the "hi" endpoint.
+ if(current->hi->next[coord] != NULL)
+ {
+ temp = current->hi;
+ while((temp->next[coord] != NULL) && (temp->getVal(coord) < new_max[coord]))
+ {
+ if(temp->minmax == 0)
+ if(temp->aabb->cached.overlap(dummy.cached))
+ addToOverlapPairs(SaPPair(temp->aabb->obj, current->obj));
+ temp = temp->next[coord];
+ }
+
+ if(temp->getVal(coord) < new_max[coord])
+ {
+ current->hi->prev[coord]->next[coord] = current->hi->next[coord];
+ current->hi->next[coord]->prev[coord] = current->hi->prev[coord];
+ current->hi->prev[coord] = temp;
+ current->hi->next[coord] = NULL;
+ temp->next[coord] = current->hi;
+ }
+ else
+ {
+ current->hi->prev[coord]->next[coord] = current->hi->next[coord];
+ current->hi->next[coord]->prev[coord] = current->hi->prev[coord];
+ current->hi->prev[coord] = temp->prev[coord];
+ current->hi->next[coord] = temp;
+ temp->prev[coord]->next[coord] = current->hi;
+ temp->prev[coord] = current->hi;
+ }
+ }
+
+ current->hi->getVal(coord) = new_max[coord];
+
+ //then, update the "lo" endpoint of the interval.
+ temp = current->lo;
+
+ while(temp->getVal(coord) < new_min[coord])
+ {
+ if((temp->minmax == 1) && (temp->aabb->cached.overlap(current->cached)))
+ removeFromOverlapPairs(SaPPair(temp->aabb->obj, current->obj));
+ temp = temp->next[coord];
+ }
+
+ if(current->lo->prev[coord] != NULL)
+ current->lo->prev[coord]->next[coord] = current->lo->next[coord];
+ else
+ elist[coord] = current->lo->next[coord];
+ current->lo->next[coord]->prev[coord] = current->lo->prev[coord];
+ current->lo->prev[coord] = temp->prev[coord];
+ current->lo->next[coord] = temp;
+ if(temp->prev[coord] != NULL)
+ temp->prev[coord]->next[coord] = current->lo;
+ else
+ elist[coord] = current->lo;
+ temp->prev[coord] = current->lo;
+ current->lo->getVal(coord) = new_min[coord];
+ }
+ }
+}
+
+void SaPCollisionManager::update(CollisionObject* updated_obj)
+{
+ update_(obj_aabb_map[updated_obj]);
+
+ updateVelist();
+
+ setup();
+}
+
+void SaPCollisionManager::update(const std::vector<CollisionObject*>& updated_objs)
+{
+ for(size_t i = 0; i < updated_objs.size(); ++i)
+ update_(obj_aabb_map[updated_objs[i]]);
+
+ updateVelist();
+
+ setup();
+}
+
+void SaPCollisionManager::update()
+{
+ for(std::list<SaPAABB*>::const_iterator it = AABB_arr.begin(), end = AABB_arr.end(); it != end; ++it)
+ {
+ update_(*it);
+ }
+
+ updateVelist();
+
+ setup();
+}
+
+
+void SaPCollisionManager::clear()
+{
+ for(std::list<SaPAABB*>::iterator it = AABB_arr.begin(), end = AABB_arr.end();
+ it != end;
+ ++it)
+ {
+ delete (*it)->hi;
+ delete (*it)->lo;
+ delete *it;
+ *it = NULL;
+ }
+
+ AABB_arr.clear();
+ overlap_pairs.clear();
+
+ elist[0] = NULL;
+ elist[1] = NULL;
+ elist[2] = NULL;
+
+ velist[0].clear();
+ velist[1].clear();
+ velist[2].clear();
+
+ obj_aabb_map.clear();
+}
+
+void SaPCollisionManager::getObjects(std::vector<CollisionObject*>& objs) const
+{
+ objs.resize(AABB_arr.size());
+ int i = 0;
+ for(std::list<SaPAABB*>::const_iterator it = AABB_arr.begin(), end = AABB_arr.end(); it != end; ++it, ++i)
+ {
+ objs[i] = (*it)->obj;
+ }
+}
+
+bool SaPCollisionManager::collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
+{
+ size_t axis = optimal_axis;
+ const AABB& obj_aabb = obj->getAABB();
+
+ FCL_REAL min_val = obj_aabb.min_[axis];
+ FCL_REAL max_val = obj_aabb.max_[axis];
+
+ EndPoint dummy;
+ SaPAABB dummy_aabb;
+ dummy_aabb.cached = obj_aabb;
+ dummy.minmax = 1;
+ dummy.aabb = &dummy_aabb;
+
+ // compute stop_pos by binary search, this is cheaper than check it in while iteration linearly
+ std::vector<EndPoint*>::const_iterator res_it = std::upper_bound(velist[axis].begin(), velist[axis].end(), &dummy,
+ boost::bind(std::less<FCL_REAL>(),
+ boost::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const>(&EndPoint::getVal), _1, axis),
+ boost::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const>(&EndPoint::getVal), _2, axis)));
+
+ EndPoint* end_pos = NULL;
+ if(res_it != velist[axis].end())
+ end_pos = *res_it;
+
+ EndPoint* pos = elist[axis];
+
+ while(pos != end_pos)
+ {
+ if(pos->aabb->obj != obj)
+ {
+ if((pos->minmax == 0) && (pos->aabb->hi->getVal(axis) >= min_val))
+ {
+ if(pos->aabb->cached.overlap(obj->getAABB()))
+ if(callback(obj, pos->aabb->obj, cdata))
+ return true;
+ }
+ }
+ pos = pos->next[axis];
+ }
+
+ return false;
+}
+
+void SaPCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
+{
+ if(size() == 0) return;
+
+ collide_(obj, cdata, callback);
+}
+
+bool SaPCollisionManager::distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
+{
+ Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
+ AABB aabb = obj->getAABB();
+
+ if(min_dist < std::numeric_limits<FCL_REAL>::max())
+ {
+ Vec3f min_dist_delta(min_dist, min_dist, min_dist);
+ aabb.expand(min_dist_delta);
+ }
+
+ size_t axis = optimal_axis;
+
+ int status = 1;
+ FCL_REAL old_min_distance;
+
+ EndPoint* start_pos = elist[axis];
+
+ while(1)
+ {
+ old_min_distance = min_dist;
+ FCL_REAL min_val = aabb.min_[axis];
+ FCL_REAL max_val = aabb.max_[axis];
+
+ EndPoint dummy;
+ SaPAABB dummy_aabb;
+ dummy_aabb.cached = aabb;
+ dummy.minmax = 1;
+ dummy.aabb = &dummy_aabb;
+
+
+ std::vector<EndPoint*>::const_iterator res_it = std::upper_bound(velist[axis].begin(), velist[axis].end(), &dummy,
+ boost::bind(std::less<FCL_REAL>(),
+ boost::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const>(&EndPoint::getVal), _1, axis),
+ boost::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const>(&EndPoint::getVal), _2, axis)));
+
+ EndPoint* end_pos = NULL;
+ if(res_it != velist[axis].end())
+ end_pos = *res_it;
+
+ EndPoint* pos = start_pos;
+
+ while(pos != end_pos)
+ {
+ // can change to pos->aabb->hi->getVal(axis) >= min_val - min_dist, and then update start_pos to end_pos.
+ // but this seems slower.
+ if((pos->minmax == 0) && (pos->aabb->hi->getVal(axis) >= min_val))
+ {
+ CollisionObject* curr_obj = pos->aabb->obj;
+ if(curr_obj != obj)
+ {
+ if(!enable_tested_set_)
+ {
+ if(pos->aabb->cached.distance(obj->getAABB()) < min_dist)
+ {
+ if(callback(curr_obj, obj, cdata, min_dist))
+ return true;
+ }
+ }
+ else
+ {
+ if(!inTestedSet(curr_obj, obj))
+ {
+ if(pos->aabb->cached.distance(obj->getAABB()) < min_dist)
+ {
+ if(callback(curr_obj, obj, cdata, min_dist))
+ return true;
+ }
+
+ insertTestedSet(curr_obj, obj);
+ }
+ }
+ }
+ }
+
+ pos = pos->next[axis];
+ }
+
+ if(status == 1)
+ {
+ if(old_min_distance < std::numeric_limits<FCL_REAL>::max())
+ break;
+ else
+ {
+ if(min_dist < old_min_distance)
+ {
+ Vec3f min_dist_delta(min_dist, min_dist, min_dist);
+ aabb = AABB(obj->getAABB(), min_dist_delta);
+ status = 0;
+ }
+ else
+ {
+ if(aabb.equal(obj->getAABB()))
+ aabb.expand(delta);
+ else
+ aabb.expand(obj->getAABB(), 2.0);
+ }
+ }
+ }
+ else if(status == 0)
+ break;
+ }
+
+ return false;
+}
+
+void SaPCollisionManager::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const
+{
+ if(size() == 0) return;
+
+ FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
+
+ distance_(obj, cdata, callback, min_dist);
+}
+
+void SaPCollisionManager::collide(void* cdata, CollisionCallBack callback) const
+{
+ if(size() == 0) return;
+
+ for(std::list<SaPPair>::const_iterator it = overlap_pairs.begin(), end = overlap_pairs.end(); it != end; ++it)
+ {
+ CollisionObject* obj1 = it->obj1;
+ CollisionObject* obj2 = it->obj2;
+
+ if(callback(obj1, obj2, cdata))
+ return;
+ }
+}
+
+void SaPCollisionManager::distance(void* cdata, DistanceCallBack callback) const
+{
+ if(size() == 0) return;
+
+ enable_tested_set_ = true;
+ tested_set.clear();
+
+ FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
+
+ for(std::list<SaPAABB*>::const_iterator it = AABB_arr.begin(), end = AABB_arr.end(); it != end; ++it)
+ {
+ if(distance_((*it)->obj, cdata, callback, min_dist))
+ break;
+ }
+
+ enable_tested_set_ = false;
+ tested_set.clear();
+}
+
+void SaPCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const
+{
+ SaPCollisionManager* other_manager = static_cast<SaPCollisionManager*>(other_manager_);
+
+ if((size() == 0) || (other_manager->size() == 0)) return;
+
+ if(this == other_manager)
+ {
+ collide(cdata, callback);
+ return;
+ }
+
+ if(this->size() < other_manager->size())
+ {
+ for(std::list<SaPAABB*>::const_iterator it = AABB_arr.begin(); it != AABB_arr.end(); ++it)
+ {
+ if(other_manager->collide_((*it)->obj, cdata, callback))
+ return;
+ }
+ }
+ else
+ {
+ for(std::list<SaPAABB*>::const_iterator it = other_manager->AABB_arr.begin(), end = other_manager->AABB_arr.end(); it != end; ++it)
+ {
+ if(collide_((*it)->obj, cdata, callback))
+ return;
+ }
+ }
+}
+
+void SaPCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const
+{
+ SaPCollisionManager* other_manager = static_cast<SaPCollisionManager*>(other_manager_);
+
+ if((size() == 0) || (other_manager->size() == 0)) return;
+
+ if(this == other_manager)
+ {
+ distance(cdata, callback);
+ return;
+ }
+
+ FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
+
+ if(this->size() < other_manager->size())
+ {
+ for(std::list<SaPAABB*>::const_iterator it = AABB_arr.begin(), end = AABB_arr.end(); it != end; ++it)
+ {
+ if(other_manager->distance_((*it)->obj, cdata, callback, min_dist))
+ return;
+ }
+ }
+ else
+ {
+ for(std::list<SaPAABB*>::const_iterator it = other_manager->AABB_arr.begin(), end = other_manager->AABB_arr.end(); it != end; ++it)
+ {
+ if(distance_((*it)->obj, cdata, callback, min_dist))
+ return;
+ }
+ }
+}
+
+bool SaPCollisionManager::empty() const
+{
+ return AABB_arr.size();
+}
+
+
+
+}
diff --git a/trunk/fcl/src/broadphase/broadphase_bruteforce.cpp b/trunk/fcl/src/broadphase/broadphase_bruteforce.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b68508e99abcc982ac199c40667789ac090a69eb
--- /dev/null
+++ b/trunk/fcl/src/broadphase/broadphase_bruteforce.cpp
@@ -0,0 +1,197 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * 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 Jia Pan */
+
+#include "fcl/broadphase/broadphase_bruteforce.h"
+#include <limits>
+
+namespace fcl
+{
+
+void NaiveCollisionManager::registerObjects(const std::vector<CollisionObject*>& other_objs)
+{
+ std::copy(other_objs.begin(), other_objs.end(), std::back_inserter(objs));
+}
+
+void NaiveCollisionManager::unregisterObject(CollisionObject* obj)
+{
+ objs.remove(obj);
+}
+
+void NaiveCollisionManager::registerObject(CollisionObject* obj)
+{
+ objs.push_back(obj);
+}
+
+void NaiveCollisionManager::setup()
+{
+
+}
+
+void NaiveCollisionManager::update()
+{
+
+}
+
+void NaiveCollisionManager::clear()
+{
+ objs.clear();
+}
+
+void NaiveCollisionManager::getObjects(std::vector<CollisionObject*>& objs_) const
+{
+ objs_.resize(objs.size());
+ std::copy(objs.begin(), objs.end(), objs_.begin());
+}
+
+void NaiveCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
+{
+ if(size() == 0) return;
+
+ for(std::list<CollisionObject*>::const_iterator it = objs.begin(), end = objs.end(); it != end; ++it)
+ {
+ if(callback(obj, *it, cdata))
+ return;
+ }
+}
+
+void NaiveCollisionManager::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const
+{
+ if(size() == 0) return;
+
+ FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
+ for(std::list<CollisionObject*>::const_iterator it = objs.begin(), end = objs.end();
+ it != end; ++it)
+ {
+ if(obj->getAABB().distance((*it)->getAABB()) < min_dist)
+ {
+ if(callback(obj, *it, cdata, min_dist))
+ return;
+ }
+ }
+}
+
+void NaiveCollisionManager::collide(void* cdata, CollisionCallBack callback) const
+{
+ if(size() == 0) return;
+
+ for(std::list<CollisionObject*>::const_iterator it1 = objs.begin(), end = objs.end();
+ it1 != end; ++it1)
+ {
+ std::list<CollisionObject*>::const_iterator it2 = it1; it2++;
+ for(; it2 != end; ++it2)
+ {
+ if((*it1)->getAABB().overlap((*it2)->getAABB()))
+ if(callback(*it1, *it2, cdata))
+ return;
+ }
+ }
+}
+
+void NaiveCollisionManager::distance(void* cdata, DistanceCallBack callback) const
+{
+ if(size() == 0) return;
+
+ FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
+ for(std::list<CollisionObject*>::const_iterator it1 = objs.begin(), end = objs.end(); it1 != end; ++it1)
+ {
+ std::list<CollisionObject*>::const_iterator it2 = it1; it2++;
+ for(; it2 != end; ++it2)
+ {
+ if((*it1)->getAABB().distance((*it2)->getAABB()) < min_dist)
+ {
+ if(callback(*it1, *it2, cdata, min_dist))
+ return;
+ }
+ }
+ }
+}
+
+void NaiveCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const
+{
+ NaiveCollisionManager* other_manager = static_cast<NaiveCollisionManager*>(other_manager_);
+
+ if((size() == 0) || (other_manager->size() == 0)) return;
+
+ if(this == other_manager)
+ {
+ collide(cdata, callback);
+ return;
+ }
+
+ for(std::list<CollisionObject*>::const_iterator it1 = objs.begin(), end1 = objs.end(); it1 != end1; ++it1)
+ {
+ for(std::list<CollisionObject*>::const_iterator it2 = other_manager->objs.begin(), end2 = other_manager->objs.end(); it2 != end2; ++it2)
+ {
+ if((*it1)->getAABB().overlap((*it2)->getAABB()))
+ if(callback((*it1), (*it2), cdata))
+ return;
+ }
+ }
+}
+
+void NaiveCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const
+{
+ NaiveCollisionManager* other_manager = static_cast<NaiveCollisionManager*>(other_manager_);
+
+ if((size() == 0) || (other_manager->size() == 0)) return;
+
+ if(this == other_manager)
+ {
+ distance(cdata, callback);
+ return;
+ }
+
+ FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
+ for(std::list<CollisionObject*>::const_iterator it1 = objs.begin(), end1 = objs.end(); it1 != end1; ++it1)
+ {
+ for(std::list<CollisionObject*>::const_iterator it2 = other_manager->objs.begin(), end2 = other_manager->objs.end(); it2 != end2; ++it2)
+ {
+ if((*it1)->getAABB().distance((*it2)->getAABB()) < min_dist)
+ {
+ if(callback(*it1, *it2, cdata, min_dist))
+ return;
+ }
+ }
+ }
+}
+
+bool NaiveCollisionManager::empty() const
+{
+ return objs.empty();
+}
+
+
+}
diff --git a/trunk/fcl/src/broadphase/broadphase_dynamic_AABB_tree.cpp b/trunk/fcl/src/broadphase/broadphase_dynamic_AABB_tree.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cb106a5d8b92ec853ef3d306715cb12fb39e56f7
--- /dev/null
+++ b/trunk/fcl/src/broadphase/broadphase_dynamic_AABB_tree.cpp
@@ -0,0 +1,644 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * 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 Jia Pan */
+
+
+#include "fcl/broadphase/broadphase_dynamic_AABB_tree.h"
+
+namespace fcl
+{
+
+void DynamicAABBTreeCollisionManager::registerObjects(const std::vector<CollisionObject*>& other_objs)
+{
+ if(size() > 0)
+ {
+ BroadPhaseCollisionManager::registerObjects(other_objs);
+ }
+ else
+ {
+ std::vector<DynamicAABBNode*> leaves(other_objs.size());
+ table.rehash(other_objs.size());
+ for(size_t i = 0, size = other_objs.size(); i < size; ++i)
+ {
+ DynamicAABBNode* node = new DynamicAABBNode; // node will be managed by the dtree
+ node->bv = other_objs[i]->getAABB();
+ node->parent = NULL;
+ node->children[1] = NULL;
+ node->data = other_objs[i];
+ table[other_objs[i]] = node;
+ leaves[i] = node;
+ }
+
+ dtree.init(leaves, tree_init_level);
+
+ setup_ = true;
+ }
+}
+
+void DynamicAABBTreeCollisionManager::registerObject(CollisionObject* obj)
+{
+ DynamicAABBNode* node = dtree.insert(obj->getAABB(), obj);
+ table[obj] = node;
+}
+
+void DynamicAABBTreeCollisionManager::unregisterObject(CollisionObject* obj)
+{
+ DynamicAABBNode* node = table[obj];
+ table.erase(obj);
+ dtree.remove(node);
+}
+
+void DynamicAABBTreeCollisionManager::setup()
+{
+ if(!setup_)
+ {
+ int num = dtree.size();
+ if(num == 0)
+ {
+ setup_ = true;
+ return;
+ }
+
+ int height = dtree.getMaxHeight();
+
+
+ if(height - std::log((FCL_REAL)num) / std::log(2.0) < max_tree_nonbalanced_level)
+ dtree.balanceIncremental(tree_incremental_balance_pass);
+ else
+ dtree.balanceTopdown();
+
+ setup_ = true;
+ }
+}
+
+
+void DynamicAABBTreeCollisionManager::update()
+{
+ for(DynamicAABBTable::const_iterator it = table.begin(); it != table.end(); ++it)
+ {
+ CollisionObject* obj = it->first;
+ DynamicAABBNode* node = it->second;
+ node->bv = obj->getAABB();
+ }
+
+ dtree.refit();
+ setup_ = false;
+
+ setup();
+}
+
+void DynamicAABBTreeCollisionManager::update_(CollisionObject* updated_obj)
+{
+ DynamicAABBTable::const_iterator it = table.find(updated_obj);
+ if(it != table.end())
+ {
+ DynamicAABBNode* node = it->second;
+ if(!node->bv.equal(updated_obj->getAABB()))
+ dtree.update(node, updated_obj->getAABB());
+ }
+ setup_ = false;
+}
+
+void DynamicAABBTreeCollisionManager::update(CollisionObject* updated_obj)
+{
+ update_(updated_obj);
+ setup();
+}
+
+void DynamicAABBTreeCollisionManager::update(const std::vector<CollisionObject*>& updated_objs)
+{
+ for(size_t i = 0, size = updated_objs.size(); i < size; ++i)
+ update_(updated_objs[i]);
+ setup();
+}
+
+bool DynamicAABBTreeCollisionManager::collisionRecurse(DynamicAABBNode* root1, DynamicAABBNode* root2, void* cdata, CollisionCallBack callback) const
+{
+ if(root1->isLeaf() && root2->isLeaf())
+ {
+ if(!root1->bv.overlap(root2->bv)) return false;
+ return callback(static_cast<CollisionObject*>(root1->data), static_cast<CollisionObject*>(root2->data), cdata);
+ }
+
+ if(!root1->bv.overlap(root2->bv)) return false;
+
+ if(root2->isLeaf() || (!root1->isLeaf() && (root1->bv.size() > root2->bv.size())))
+ {
+ if(collisionRecurse(root1->children[0], root2, cdata, callback))
+ return true;
+ if(collisionRecurse(root1->children[1], root2, cdata, callback))
+ return true;
+ }
+ else
+ {
+ if(collisionRecurse(root1, root2->children[0], cdata, callback))
+ return true;
+ if(collisionRecurse(root1, root2->children[1], cdata, callback))
+ return true;
+ }
+ return false;
+}
+
+bool DynamicAABBTreeCollisionManager::collisionRecurse(DynamicAABBNode* root, CollisionObject* query, void* cdata, CollisionCallBack callback) const
+{
+ if(root->isLeaf())
+ {
+ if(!root->bv.overlap(query->getAABB())) return false;
+ return callback(static_cast<CollisionObject*>(root->data), query, cdata);
+ }
+
+ if(!root->bv.overlap(query->getAABB())) return false;
+
+ int select_res = select(query->getAABB(), *(root->children[0]), *(root->children[1]));
+
+ if(collisionRecurse(root->children[select_res], query, cdata, callback))
+ return true;
+
+ if(collisionRecurse(root->children[1-select_res], query, cdata, callback))
+ return true;
+
+ return false;
+}
+
+bool DynamicAABBTreeCollisionManager::selfCollisionRecurse(DynamicAABBNode* root, void* cdata, CollisionCallBack callback) const
+{
+ if(root->isLeaf()) return false;
+
+ if(selfCollisionRecurse(root->children[0], cdata, callback))
+ return true;
+
+ if(selfCollisionRecurse(root->children[1], cdata, callback))
+ return true;
+
+ if(collisionRecurse(root->children[0], root->children[1], cdata, callback))
+ return true;
+
+ return false;
+}
+
+bool DynamicAABBTreeCollisionManager::distanceRecurse(DynamicAABBNode* root1, DynamicAABBNode* root2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
+{
+ if(root1->isLeaf() && root2->isLeaf())
+ {
+ CollisionObject* root1_obj = static_cast<CollisionObject*>(root1->data);
+ CollisionObject* root2_obj = static_cast<CollisionObject*>(root2->data);
+ return callback(root1_obj, root2_obj, cdata, min_dist);
+ }
+
+ if(root2->isLeaf() || (!root1->isLeaf() && (root1->bv.size() > root2->bv.size())))
+ {
+ FCL_REAL d1 = root2->bv.distance(root1->children[0]->bv);
+ FCL_REAL d2 = root2->bv.distance(root1->children[1]->bv);
+
+ if(d2 < d1)
+ {
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse(root1->children[1], root2, cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse(root1->children[0], root2, cdata, callback, min_dist))
+ return true;
+ }
+ }
+ else
+ {
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse(root1->children[0], root2, cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse(root1->children[1], root2, cdata, callback, min_dist))
+ return true;
+ }
+ }
+ }
+ else
+ {
+ FCL_REAL d1 = root1->bv.distance(root2->children[0]->bv);
+ FCL_REAL d2 = root1->bv.distance(root2->children[1]->bv);
+
+ if(d2 < d1)
+ {
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse(root1, root2->children[1], cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse(root1, root2->children[0], cdata, callback, min_dist))
+ return true;
+ }
+ }
+ else
+ {
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse(root1, root2->children[0], cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse(root1, root2->children[1], cdata, callback, min_dist))
+ return true;
+ }
+ }
+ }
+
+ return false;
+}
+
+bool DynamicAABBTreeCollisionManager::distanceRecurse(DynamicAABBNode* root, CollisionObject* query, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
+{
+ if(root->isLeaf())
+ {
+ CollisionObject* root_obj = static_cast<CollisionObject*>(root->data);
+ return callback(root_obj, query, cdata, min_dist);
+ }
+
+ FCL_REAL d1 = query->getAABB().distance(root->children[0]->bv);
+ FCL_REAL d2 = query->getAABB().distance(root->children[1]->bv);
+
+ if(d2 < d1)
+ {
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse(root->children[1], query, cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse(root->children[0], query, cdata, callback, min_dist))
+ return true;
+ }
+ }
+ else
+ {
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse(root->children[0], query, cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse(root->children[1], query, cdata, callback, min_dist))
+ return true;
+ }
+ }
+
+ return false;
+}
+
+bool DynamicAABBTreeCollisionManager::selfDistanceRecurse(DynamicAABBNode* root, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
+{
+ if(root->isLeaf()) return false;
+
+ if(selfDistanceRecurse(root->children[0], cdata, callback, min_dist))
+ return true;
+
+ if(selfDistanceRecurse(root->children[1], cdata, callback, min_dist))
+ return true;
+
+ if(distanceRecurse(root->children[0], root->children[1], cdata, callback, min_dist))
+ return true;
+
+ return false;
+}
+
+
+bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, CollisionCallBack callback)
+{
+ if(root1->isLeaf() && !root2->hasChildren())
+ {
+ CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
+
+ if(!tree2->isNodeFree(root2) && !obj1->isFree())
+ {
+ OBB obb1, obb2;
+ convertBV(root1->bv, Transform3f(), obb1);
+ convertBV(root2_bv, tf2, obb2);
+
+ if(obb1.overlap(obb2))
+ {
+ Box* box = new Box();
+ Transform3f box_tf;
+ constructBox(root2_bv, tf2, *box, box_tf);
+
+ box->cost_density = root2->getOccupancy();
+ box->threshold_occupied = tree2->getOccupancyThres();
+
+ CollisionObject obj2(boost::shared_ptr<CollisionGeometry>(box), box_tf);
+ return callback(obj1, &obj2, cdata);
+ }
+ else return false;
+ }
+ else return false;
+ }
+
+ OBB obb1, obb2;
+ convertBV(root1->bv, Transform3f(), obb1);
+ convertBV(root2_bv, tf2, obb2);
+
+ if(tree2->isNodeFree(root2) || !obb1.overlap(obb2)) return false;
+
+ if(!root2->hasChildren() || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size())))
+ {
+ if(collisionRecurse_(root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback))
+ return true;
+ if(collisionRecurse_(root1->children[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 collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Vec3f& tf2, void* cdata, CollisionCallBack callback)
+{
+ if(root1->isLeaf() && !root2->hasChildren())
+ {
+ CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
+
+ if(!tree2->isNodeFree(root2) && !obj1->isFree())
+ {
+ const AABB& root2_bv_t = translate(root2_bv, tf2);
+ if(root1->bv.overlap(root2_bv_t))
+ {
+ Box* box = new Box();
+ Transform3f box_tf;
+ constructBox(root2_bv, tf2, *box, box_tf);
+
+ box->cost_density = root2->getOccupancy();
+ box->threshold_occupied = tree2->getOccupancyThres();
+
+ CollisionObject obj2(boost::shared_ptr<CollisionGeometry>(box), box_tf);
+ return callback(obj1, &obj2, cdata);
+ }
+ else return false;
+ }
+ else return false;
+ }
+
+ const AABB& root2_bv_t = translate(root2_bv, tf2);
+ if(tree2->isNodeFree(root2) || !root1->bv.overlap(root2_bv_t)) return false;
+
+ if(!root2->hasChildren() || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size())))
+ {
+ if(collisionRecurse_(root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback))
+ return true;
+ if(collisionRecurse_(root1->children[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::collisionRecurse(DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, CollisionCallBack callback) const
+{
+ if(tf2.getQuatRotation().isIdentity())
+ return collisionRecurse_(root1, tree2, root2, root2_bv, tf2.getTranslation(), cdata, callback);
+ else // has rotation
+ return collisionRecurse_(root1, tree2, root2, root2_bv, tf2, cdata, callback);
+}
+
+
+bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Vec3f& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist)
+{
+ if(root1->isLeaf() && !root2->hasChildren())
+ {
+ if(tree2->isNodeOccupied(root2))
+ {
+ Box* box = new Box();
+ Transform3f 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())))
+ {
+ const AABB& aabb2 = translate(root2_bv, tf2);
+ FCL_REAL d1 = aabb2.distance(root1->children[0]->bv);
+ FCL_REAL d2 = aabb2.distance(root1->children[1]->bv);
+
+ if(d2 < d1)
+ {
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse_(root1->children[1], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse_(root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
+ return true;
+ }
+ }
+ else
+ {
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse_(root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse_(root1->children[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);
+ const AABB& aabb2 = translate(child_bv, tf2);
+
+ 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;
+}
+
+
+bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist)
+{
+ if(root1->isLeaf() && !root2->hasChildren())
+ {
+ if(tree2->isNodeOccupied(root2))
+ {
+ Box* box = new Box();
+ Transform3f 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->children[0]->bv);
+ FCL_REAL d2 = aabb2.distance(root1->children[1]->bv);
+
+ if(d2 < d1)
+ {
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse_(root1->children[1], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse_(root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
+ return true;
+ }
+ }
+ else
+ {
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse_(root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse_(root1->children[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;
+}
+
+bool DynamicAABBTreeCollisionManager::distanceRecurse(DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
+{
+ if(tf2.getQuatRotation().isIdentity())
+ return distanceRecurse_(root1, tree2, root2, root2_bv, tf2.getTranslation(), cdata, callback, min_dist);
+ else
+ return distanceRecurse_(root1, tree2, root2, root2_bv, tf2, cdata, callback, min_dist);
+}
+
+
+}
diff --git a/trunk/fcl/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp b/trunk/fcl/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0d21379ed60bdd17322bb938df465c9fbd1a492c
--- /dev/null
+++ b/trunk/fcl/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp
@@ -0,0 +1,659 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * 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 Jia Pan */
+
+#include "fcl/broadphase/broadphase_dynamic_AABB_tree_array.h"
+
+namespace fcl
+{
+
+void DynamicAABBTreeCollisionManager_Array::registerObjects(const std::vector<CollisionObject*>& other_objs)
+{
+ if(size() > 0)
+ {
+ BroadPhaseCollisionManager::registerObjects(other_objs);
+ }
+ else
+ {
+ DynamicAABBNode* leaves = new DynamicAABBNode[other_objs.size()];
+ table.rehash(other_objs.size());
+ for(size_t i = 0, size = other_objs.size(); i < size; ++i)
+ {
+ leaves[i].bv = other_objs[i]->getAABB();
+ leaves[i].parent = dtree.NULL_NODE;
+ leaves[i].children[1] = dtree.NULL_NODE;
+ leaves[i].data = other_objs[i];
+ table[other_objs[i]] = i;
+ }
+
+ int n_leaves = other_objs.size();
+
+ dtree.init(leaves, n_leaves, tree_init_level);
+
+ setup_ = true;
+ }
+}
+
+void DynamicAABBTreeCollisionManager_Array::registerObject(CollisionObject* obj)
+{
+ size_t node = dtree.insert(obj->getAABB(), obj);
+ table[obj] = node;
+}
+
+void DynamicAABBTreeCollisionManager_Array::unregisterObject(CollisionObject* obj)
+{
+ size_t node = table[obj];
+ table.erase(obj);
+ dtree.remove(node);
+}
+
+void DynamicAABBTreeCollisionManager_Array::setup()
+{
+ if(!setup_)
+ {
+ int num = dtree.size();
+ if(num == 0)
+ {
+ setup_ = true;
+ return;
+ }
+
+ int height = dtree.getMaxHeight();
+
+
+ if(height - std::log((FCL_REAL)num) / std::log(2.0) < max_tree_nonbalanced_level)
+ dtree.balanceIncremental(tree_incremental_balance_pass);
+ else
+ dtree.balanceTopdown();
+
+ setup_ = true;
+ }
+}
+
+
+void DynamicAABBTreeCollisionManager_Array::update()
+{
+ for(DynamicAABBTable::const_iterator it = table.begin(), end = table.end(); it != end; ++it)
+ {
+ CollisionObject* obj = it->first;
+ size_t node = it->second;
+ dtree.getNodes()[node].bv = obj->getAABB();
+ }
+
+ dtree.refit();
+ setup_ = false;
+
+ setup();
+}
+
+void DynamicAABBTreeCollisionManager_Array::update_(CollisionObject* updated_obj)
+{
+ DynamicAABBTable::const_iterator it = table.find(updated_obj);
+ if(it != table.end())
+ {
+ size_t node = it->second;
+ if(!dtree.getNodes()[node].bv.equal(updated_obj->getAABB()))
+ dtree.update(node, updated_obj->getAABB());
+ }
+ setup_ = false;
+}
+
+void DynamicAABBTreeCollisionManager_Array::update(CollisionObject* updated_obj)
+{
+ update_(updated_obj);
+ setup();
+}
+
+void DynamicAABBTreeCollisionManager_Array::update(const std::vector<CollisionObject*>& updated_objs)
+{
+ for(size_t i = 0, size = updated_objs.size(); i < size; ++i)
+ update_(updated_objs[i]);
+ setup();
+}
+
+bool DynamicAABBTreeCollisionManager_Array::collisionRecurse(DynamicAABBNode* nodes1, size_t root1_id,
+ DynamicAABBNode* nodes2, size_t root2_id,
+ void* cdata, CollisionCallBack callback) const
+{
+ DynamicAABBNode* root1 = nodes1 + root1_id;
+ DynamicAABBNode* root2 = nodes2 + root2_id;
+ if(root1->isLeaf() && root2->isLeaf())
+ {
+ if(!root1->bv.overlap(root2->bv)) return false;
+ return callback(static_cast<CollisionObject*>(root1->data), static_cast<CollisionObject*>(root2->data), cdata);
+ }
+
+ if(!root1->bv.overlap(root2->bv)) return false;
+
+ if(root2->isLeaf() || (!root1->isLeaf() && (root1->bv.size() > root2->bv.size())))
+ {
+ if(collisionRecurse(nodes1, root1->children[0], nodes2, root2_id, cdata, callback))
+ return true;
+ if(collisionRecurse(nodes1, root1->children[1], nodes2, root2_id, cdata, callback))
+ return true;
+ }
+ else
+ {
+ if(collisionRecurse(nodes1, root1_id, nodes2, root2->children[0], cdata, callback))
+ return true;
+ if(collisionRecurse(nodes1, root1_id, nodes2, root2->children[1], cdata, callback))
+ return true;
+ }
+ return false;
+}
+
+bool DynamicAABBTreeCollisionManager_Array::collisionRecurse(DynamicAABBNode* nodes, size_t root_id, CollisionObject* query, void* cdata, CollisionCallBack callback) const
+{
+ DynamicAABBNode* root = nodes + root_id;
+ if(root->isLeaf())
+ {
+ if(!root->bv.overlap(query->getAABB())) return false;
+ return callback(static_cast<CollisionObject*>(root->data), query, cdata);
+ }
+
+ if(!root->bv.overlap(query->getAABB())) return false;
+
+ int select_res = implementation_array::select(query->getAABB(), root->children[0], root->children[1], nodes);
+
+ if(collisionRecurse(nodes, root->children[select_res], query, cdata, callback))
+ return true;
+
+ if(collisionRecurse(nodes, root->children[1-select_res], query, cdata, callback))
+ return true;
+
+ return false;
+}
+
+bool DynamicAABBTreeCollisionManager_Array::selfCollisionRecurse(DynamicAABBNode* nodes, size_t root_id, void* cdata, CollisionCallBack callback) const
+{
+ DynamicAABBNode* root = nodes + root_id;
+ if(root->isLeaf()) return false;
+
+ if(selfCollisionRecurse(nodes, root->children[0], cdata, callback))
+ return true;
+
+ if(selfCollisionRecurse(nodes, root->children[1], cdata, callback))
+ return true;
+
+ if(collisionRecurse(nodes, root->children[0], nodes, root->children[1], cdata, callback))
+ return true;
+
+ return false;
+}
+
+bool DynamicAABBTreeCollisionManager_Array::distanceRecurse(DynamicAABBNode* nodes1, size_t root1_id,
+ DynamicAABBNode* nodes2, size_t root2_id,
+ void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
+{
+ DynamicAABBNode* root1 = nodes1 + root1_id;
+ DynamicAABBNode* root2 = nodes2 + root2_id;
+ if(root1->isLeaf() && root2->isLeaf())
+ {
+ CollisionObject* root1_obj = static_cast<CollisionObject*>(root1->data);
+ CollisionObject* root2_obj = static_cast<CollisionObject*>(root2->data);
+ return callback(root1_obj, root2_obj, cdata, min_dist);
+ }
+
+ if(root2->isLeaf() || (!root1->isLeaf() && (root1->bv.size() > root2->bv.size())))
+ {
+ FCL_REAL d1 = root2->bv.distance((nodes1 + root1->children[0])->bv);
+ FCL_REAL d2 = root2->bv.distance((nodes1 + root1->children[1])->bv);
+
+ if(d2 < d1)
+ {
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse(nodes1, root1->children[1], nodes2, root2_id, cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse(nodes1, root1->children[0], nodes2, root2_id, cdata, callback, min_dist))
+ return true;
+ }
+ }
+ else
+ {
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse(nodes1, root1->children[0], nodes2, root2_id, cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse(nodes1, root1->children[1], nodes2, root2_id, cdata, callback, min_dist))
+ return true;
+ }
+ }
+ }
+ else
+ {
+ FCL_REAL d1 = root1->bv.distance((nodes2 + root2->children[0])->bv);
+ FCL_REAL d2 = root1->bv.distance((nodes2 + root2->children[1])->bv);
+
+ if(d2 < d1)
+ {
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse(nodes1, root1_id, nodes2, root2->children[1], cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse(nodes1, root1_id, nodes2, root2->children[0], cdata, callback, min_dist))
+ return true;
+ }
+ }
+ else
+ {
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse(nodes1, root1_id, nodes2, root2->children[0], cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse(nodes1, root1_id, nodes2, root2->children[1], cdata, callback, min_dist))
+ return true;
+ }
+ }
+ }
+
+ return false;
+}
+
+bool DynamicAABBTreeCollisionManager_Array::distanceRecurse(DynamicAABBNode* nodes, size_t root_id, CollisionObject* query, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
+{
+ DynamicAABBNode* root = nodes + root_id;
+ if(root->isLeaf())
+ {
+ CollisionObject* root_obj = static_cast<CollisionObject*>(root->data);
+ return callback(root_obj, query, cdata, min_dist);
+ }
+
+ FCL_REAL d1 = query->getAABB().distance((nodes + root->children[0])->bv);
+ FCL_REAL d2 = query->getAABB().distance((nodes + root->children[1])->bv);
+
+ if(d2 < d1)
+ {
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse(nodes, root->children[1], query, cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse(nodes, root->children[0], query, cdata, callback, min_dist))
+ return true;
+ }
+ }
+ else
+ {
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse(nodes, root->children[0], query, cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse(nodes, root->children[1], query, cdata, callback, min_dist))
+ return true;
+ }
+ }
+
+ return false;
+}
+
+bool DynamicAABBTreeCollisionManager_Array::selfDistanceRecurse(DynamicAABBNode* nodes, size_t root_id, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
+{
+ DynamicAABBNode* root = nodes + root_id;
+ if(root->isLeaf()) return false;
+
+ if(selfDistanceRecurse(nodes, root->children[0], cdata, callback, min_dist))
+ return true;
+
+ if(selfDistanceRecurse(nodes, root->children[1], cdata, callback, min_dist))
+ return true;
+
+ if(distanceRecurse(nodes, root->children[0], nodes, root->children[1], cdata, callback, min_dist))
+ return true;
+
+ return false;
+}
+
+bool collisionRecurse_(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, CollisionCallBack callback)
+{
+ DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root1 = nodes1 + root1_id;
+ if(root1->isLeaf() && !root2->hasChildren())
+ {
+ CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
+ if(!tree2->isNodeFree(root2) && !obj1->isFree())
+ {
+ OBB obb1, obb2;
+ convertBV(root1->bv, Transform3f(), obb1);
+ convertBV(root2_bv, tf2, obb2);
+
+ if(obb1.overlap(obb2))
+ {
+ Box* box = new Box();
+ Transform3f box_tf;
+ constructBox(root2_bv, tf2, *box, box_tf);
+
+ box->cost_density = root2->getOccupancy();
+ box->threshold_occupied = tree2->getOccupancyThres();
+
+ CollisionObject obj2(boost::shared_ptr<CollisionGeometry>(box), box_tf);
+ return callback(obj1, &obj2, cdata);
+ }
+ else return false;
+ }
+ else return false;
+ }
+
+ OBB obb1, obb2;
+ convertBV(root1->bv, Transform3f(), obb1);
+ convertBV(root2_bv, tf2, obb2);
+
+ if(tree2->isNodeFree(root2) || !obb1.overlap(obb2)) return false;
+
+ if(!root2->hasChildren() || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size())))
+ {
+ if(collisionRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback))
+ return true;
+ if(collisionRecurse_(nodes1, root1->children[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 collisionRecurse_(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Vec3f& tf2, void* cdata, CollisionCallBack callback)
+{
+ DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root1 = nodes1 + root1_id;
+ if(root1->isLeaf() && !root2->hasChildren())
+ {
+ CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
+ if(!tree2->isNodeFree(root2))
+ {
+ const AABB& root_bv_t = translate(root2_bv, tf2);
+ if(root1->bv.overlap(root_bv_t))
+ {
+ Box* box = new Box();
+ Transform3f box_tf;
+ constructBox(root2_bv, tf2, *box, box_tf);
+
+ box->cost_density = root2->getOccupancy();
+ box->threshold_occupied = tree2->getOccupancyThres();
+
+ CollisionObject obj2(boost::shared_ptr<CollisionGeometry>(box), box_tf);
+ return callback(obj1, &obj2, cdata);
+ }
+ else return false;
+ }
+ else return false;
+ }
+
+ const AABB& root_bv_t = translate(root2_bv, tf2);
+ if(tree2->isNodeFree(root2) || !root1->bv.overlap(root_bv_t)) return false;
+
+ if(!root2->hasChildren() || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size())))
+ {
+ if(collisionRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback))
+ return true;
+ if(collisionRecurse_(nodes1, root1->children[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 DynamicAABBTreeCollisionManager_Array::collisionRecurse(DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, CollisionCallBack callback) const
+{
+ if(tf2.getQuatRotation().isIdentity())
+ return collisionRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2.getTranslation(), cdata, callback);
+ else
+ return collisionRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2, cdata, callback);
+}
+
+
+bool distanceRecurse_(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist)
+{
+ DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root1 = nodes1 + root1_id;
+ if(root1->isLeaf() && !root2->hasChildren())
+ {
+ if(tree2->isNodeOccupied(root2))
+ {
+ Box* box = new Box();
+ Transform3f 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->children[0])->bv);
+ FCL_REAL d2 = aabb2.distance((nodes1 + root1->children[1])->bv);
+
+ if(d2 < d1)
+ {
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
+ return true;
+ }
+ }
+ else
+ {
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse_(nodes1, root1->children[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;
+}
+
+
+bool distanceRecurse_(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Vec3f& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist)
+{
+ DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root1 = nodes1 + root1_id;
+ if(root1->isLeaf() && !root2->hasChildren())
+ {
+ if(tree2->isNodeOccupied(root2))
+ {
+ Box* box = new Box();
+ Transform3f 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())))
+ {
+ const AABB& aabb2 = translate(root2_bv, tf2);
+
+ FCL_REAL d1 = aabb2.distance((nodes1 + root1->children[0])->bv);
+ FCL_REAL d2 = aabb2.distance((nodes1 + root1->children[1])->bv);
+
+ if(d2 < d1)
+ {
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
+ return true;
+ }
+ }
+ else
+ {
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse_(nodes1, root1->children[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);
+
+ const AABB& aabb2 = translate(child_bv, tf2);
+ 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;
+}
+
+bool DynamicAABBTreeCollisionManager_Array::distanceRecurse(DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
+{
+ if(tf2.getQuatRotation().isIdentity())
+ return distanceRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2.getTranslation(), cdata, callback, min_dist);
+ else
+ return distanceRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2, cdata, callback, min_dist);
+}
+
+
+}
diff --git a/trunk/fcl/src/broadphase/broadphase_interval_tree.cpp b/trunk/fcl/src/broadphase/broadphase_interval_tree.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..219086bc86dd1d35a35a61efd64c3d21ff658919
--- /dev/null
+++ b/trunk/fcl/src/broadphase/broadphase_interval_tree.cpp
@@ -0,0 +1,654 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * 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 Jia Pan */
+
+#include "fcl/broadphase/broadphase_interval_tree.h"
+#include <algorithm>
+#include <limits>
+#include <boost/bind.hpp>
+
+namespace fcl
+{
+
+void IntervalTreeCollisionManager::unregisterObject(CollisionObject* obj)
+{
+ // must sorted before
+ setup();
+
+ EndPoint p;
+ p.value = obj->getAABB().min_[0];
+ std::vector<EndPoint>::iterator start1 = std::lower_bound(endpoints[0].begin(), endpoints[0].end(), p, boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2));
+ p.value = obj->getAABB().max_[0];
+ std::vector<EndPoint>::iterator end1 = std::upper_bound(start1, endpoints[0].end(), p, boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2));
+
+ if(start1 < end1)
+ {
+ unsigned int start_id = start1 - endpoints[0].begin();
+ unsigned int end_id = end1 - endpoints[0].begin();
+ unsigned int cur_id = start_id;
+ for(unsigned int i = start_id; i < end_id; ++i)
+ {
+ if(endpoints[0][i].obj != obj)
+ {
+ if(i == cur_id) cur_id++;
+ else
+ {
+ endpoints[0][cur_id] = endpoints[0][i];
+ cur_id++;
+ }
+ }
+ }
+ if(cur_id < end_id)
+ endpoints[0].resize(endpoints[0].size() - 2);
+ }
+
+ p.value = obj->getAABB().min_[1];
+ std::vector<EndPoint>::iterator start2 = std::lower_bound(endpoints[1].begin(), endpoints[1].end(), p, boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2));
+ p.value = obj->getAABB().max_[1];
+ std::vector<EndPoint>::iterator end2 = std::upper_bound(start2, endpoints[1].end(), p, boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2));
+
+ if(start2 < end2)
+ {
+ unsigned int start_id = start2 - endpoints[1].begin();
+ unsigned int end_id = end2 - endpoints[1].begin();
+ unsigned int cur_id = start_id;
+ for(unsigned int i = start_id; i < end_id; ++i)
+ {
+ if(endpoints[1][i].obj != obj)
+ {
+ if(i == cur_id) cur_id++;
+ else
+ {
+ endpoints[1][cur_id] = endpoints[1][i];
+ cur_id++;
+ }
+ }
+ }
+ if(cur_id < end_id)
+ endpoints[1].resize(endpoints[1].size() - 2);
+ }
+
+
+ p.value = obj->getAABB().min_[2];
+ std::vector<EndPoint>::iterator start3 = std::lower_bound(endpoints[2].begin(), endpoints[2].end(), p, boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2));
+ p.value = obj->getAABB().max_[2];
+ std::vector<EndPoint>::iterator end3 = std::upper_bound(start3, endpoints[2].end(), p, boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2));
+
+ if(start3 < end3)
+ {
+ unsigned int start_id = start3 - endpoints[2].begin();
+ unsigned int end_id = end3 - endpoints[2].begin();
+ unsigned int cur_id = start_id;
+ for(unsigned int i = start_id; i < end_id; ++i)
+ {
+ if(endpoints[2][i].obj != obj)
+ {
+ if(i == cur_id) cur_id++;
+ else
+ {
+ endpoints[2][cur_id] = endpoints[2][i];
+ cur_id++;
+ }
+ }
+ }
+ if(cur_id < end_id)
+ endpoints[2].resize(endpoints[2].size() - 2);
+ }
+
+ // update the interval tree
+ if(obj_interval_maps[0].find(obj) != obj_interval_maps[0].end())
+ {
+ SAPInterval* ivl1 = obj_interval_maps[0][obj];
+ SAPInterval* ivl2 = obj_interval_maps[1][obj];
+ SAPInterval* ivl3 = obj_interval_maps[2][obj];
+
+ interval_trees[0]->deleteNode(ivl1);
+ interval_trees[1]->deleteNode(ivl2);
+ interval_trees[2]->deleteNode(ivl3);
+
+ delete ivl1;
+ delete ivl2;
+ delete ivl3;
+
+ obj_interval_maps[0].erase(obj);
+ obj_interval_maps[1].erase(obj);
+ obj_interval_maps[2].erase(obj);
+ }
+}
+
+void IntervalTreeCollisionManager::registerObject(CollisionObject* obj)
+{
+ EndPoint p, q;
+
+ p.obj = obj;
+ q.obj = obj;
+ p.minmax = 0;
+ q.minmax = 1;
+ p.value = obj->getAABB().min_[0];
+ q.value = obj->getAABB().max_[0];
+ endpoints[0].push_back(p);
+ endpoints[0].push_back(q);
+
+ p.value = obj->getAABB().min_[1];
+ q.value = obj->getAABB().max_[1];
+ endpoints[1].push_back(p);
+ endpoints[1].push_back(q);
+
+ p.value = obj->getAABB().min_[2];
+ q.value = obj->getAABB().max_[2];
+ endpoints[2].push_back(p);
+ endpoints[2].push_back(q);
+ setup_ = false;
+}
+
+void IntervalTreeCollisionManager::setup()
+{
+ if(!setup_)
+ {
+ std::sort(endpoints[0].begin(), endpoints[0].end(), boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2));
+ std::sort(endpoints[1].begin(), endpoints[1].end(), boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2));
+ std::sort(endpoints[2].begin(), endpoints[2].end(), boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2));
+
+ for(int i = 0; i < 3; ++i)
+ delete interval_trees[i];
+
+ for(int i = 0; i < 3; ++i)
+ interval_trees[i] = new IntervalTree;
+
+ for(unsigned int i = 0, size = endpoints[0].size(); i < size; ++i)
+ {
+ EndPoint p = endpoints[0][i];
+ CollisionObject* obj = p.obj;
+ if(p.minmax == 0)
+ {
+ SAPInterval* ivl1 = new SAPInterval(obj->getAABB().min_[0], obj->getAABB().max_[0], obj);
+ SAPInterval* ivl2 = new SAPInterval(obj->getAABB().min_[1], obj->getAABB().max_[1], obj);
+ SAPInterval* ivl3 = new SAPInterval(obj->getAABB().min_[2], obj->getAABB().max_[2], obj);
+
+ interval_trees[0]->insert(ivl1);
+ interval_trees[1]->insert(ivl2);
+ interval_trees[2]->insert(ivl3);
+
+ obj_interval_maps[0][obj] = ivl1;
+ obj_interval_maps[1][obj] = ivl2;
+ obj_interval_maps[2][obj] = ivl3;
+ }
+ }
+
+ setup_ = true;
+ }
+}
+
+void IntervalTreeCollisionManager::update()
+{
+ setup_ = false;
+
+ for(unsigned int i = 0, size = endpoints[0].size(); i < size; ++i)
+ {
+ if(endpoints[0][i].minmax == 0)
+ endpoints[0][i].value = endpoints[0][i].obj->getAABB().min_[0];
+ else
+ endpoints[0][i].value = endpoints[0][i].obj->getAABB().max_[0];
+ }
+
+ for(unsigned int i = 0, size = endpoints[1].size(); i < size; ++i)
+ {
+ if(endpoints[1][i].minmax == 0)
+ endpoints[1][i].value = endpoints[1][i].obj->getAABB().min_[1];
+ else
+ endpoints[1][i].value = endpoints[1][i].obj->getAABB().max_[1];
+ }
+
+ for(unsigned int i = 0, size = endpoints[2].size(); i < size; ++i)
+ {
+ if(endpoints[2][i].minmax == 0)
+ endpoints[2][i].value = endpoints[2][i].obj->getAABB().min_[2];
+ else
+ endpoints[2][i].value = endpoints[2][i].obj->getAABB().max_[2];
+ }
+
+ setup();
+
+}
+
+
+void IntervalTreeCollisionManager::update(CollisionObject* updated_obj)
+{
+ AABB old_aabb;
+ const AABB& new_aabb = updated_obj->getAABB();
+ for(int i = 0; i < 3; ++i)
+ {
+ std::map<CollisionObject*, SAPInterval*>::const_iterator it = obj_interval_maps[i].find(updated_obj);
+ interval_trees[i]->deleteNode(it->second);
+ old_aabb.min_[i] = it->second->low;
+ old_aabb.max_[i] = it->second->high;
+ it->second->low = new_aabb.min_[i];
+ it->second->high = new_aabb.max_[i];
+ interval_trees[i]->insert(it->second);
+ }
+
+ EndPoint dummy;
+ std::vector<EndPoint>::iterator it;
+ for(int i = 0; i < 3; ++i)
+ {
+ dummy.value = old_aabb.min_[i];
+ it = std::lower_bound(endpoints[i].begin(), endpoints[i].end(), dummy, boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2));
+ for(; it != endpoints[i].end(); ++it)
+ {
+ if(it->obj == updated_obj && it->minmax == 0)
+ {
+ it->value = new_aabb.min_[i];
+ break;
+ }
+ }
+
+ dummy.value = old_aabb.max_[i];
+ it = std::lower_bound(endpoints[i].begin(), endpoints[i].end(), dummy, boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2));
+ for(; it != endpoints[i].end(); ++it)
+ {
+ if(it->obj == updated_obj && it->minmax == 0)
+ {
+ it->value = new_aabb.max_[i];
+ break;
+ }
+ }
+
+ std::sort(endpoints[i].begin(), endpoints[i].end(), boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2));
+ }
+}
+
+void IntervalTreeCollisionManager::update(const std::vector<CollisionObject*>& updated_objs)
+{
+ for(size_t i = 0; i < updated_objs.size(); ++i)
+ update(updated_objs[i]);
+}
+
+void IntervalTreeCollisionManager::clear()
+{
+ endpoints[0].clear();
+ endpoints[1].clear();
+ endpoints[2].clear();
+
+ delete interval_trees[0]; interval_trees[0] = NULL;
+ delete interval_trees[1]; interval_trees[1] = NULL;
+ delete interval_trees[2]; interval_trees[2] = NULL;
+
+ for(int i = 0; i < 3; ++i)
+ {
+ for(std::map<CollisionObject*, SAPInterval*>::const_iterator it = obj_interval_maps[i].begin(), end = obj_interval_maps[i].end();
+ it != end; ++it)
+ {
+ delete it->second;
+ }
+ }
+
+ for(int i = 0; i < 3; ++i)
+ obj_interval_maps[i].clear();
+
+ setup_ = false;
+}
+
+void IntervalTreeCollisionManager::getObjects(std::vector<CollisionObject*>& objs) const
+{
+ objs.resize(endpoints[0].size() / 2);
+ unsigned int j = 0;
+ for(unsigned int i = 0, size = endpoints[0].size(); i < size; ++i)
+ {
+ if(endpoints[0][i].minmax == 0)
+ {
+ objs[j] = endpoints[0][i].obj; j++;
+ }
+ }
+}
+
+void IntervalTreeCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
+{
+ if(size() == 0) return;
+ collide_(obj, cdata, callback);
+}
+
+bool IntervalTreeCollisionManager::collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
+{
+ static const unsigned int CUTOFF = 100;
+
+ std::deque<SimpleInterval*> results0, results1, results2;
+
+ results0 = interval_trees[0]->query(obj->getAABB().min_[0], obj->getAABB().max_[0]);
+ if(results0.size() > CUTOFF)
+ {
+ results1 = interval_trees[1]->query(obj->getAABB().min_[1], obj->getAABB().max_[1]);
+ if(results1.size() > CUTOFF)
+ {
+ results2 = interval_trees[2]->query(obj->getAABB().min_[2], obj->getAABB().max_[2]);
+ if(results2.size() > CUTOFF)
+ {
+ int d1 = results0.size();
+ int d2 = results1.size();
+ int d3 = results2.size();
+
+ if(d1 >= d2 && d1 >= d3)
+ return checkColl(results0.begin(), results0.end(), obj, cdata, callback);
+ else if(d2 >= d1 && d2 >= d3)
+ return checkColl(results1.begin(), results1.end(), obj, cdata, callback);
+ else
+ return checkColl(results2.begin(), results2.end(), obj, cdata, callback);
+ }
+ else
+ return checkColl(results2.begin(), results2.end(), obj, cdata, callback);
+ }
+ else
+ return checkColl(results1.begin(), results1.end(), obj, cdata, callback);
+ }
+ else
+ return checkColl(results0.begin(), results0.end(), obj, cdata, callback);
+}
+
+void IntervalTreeCollisionManager::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const
+{
+ if(size() == 0) return;
+ FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
+ distance_(obj, cdata, callback, min_dist);
+}
+
+bool IntervalTreeCollisionManager::distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
+{
+ static const unsigned int CUTOFF = 100;
+
+ Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
+ AABB aabb = obj->getAABB();
+ if(min_dist < std::numeric_limits<FCL_REAL>::max())
+ {
+ Vec3f min_dist_delta(min_dist, min_dist, min_dist);
+ aabb.expand(min_dist_delta);
+ }
+
+ int status = 1;
+ FCL_REAL old_min_distance;
+
+ while(1)
+ {
+ bool dist_res = false;
+
+ old_min_distance = min_dist;
+
+ std::deque<SimpleInterval*> results0, results1, results2;
+
+ results0 = interval_trees[0]->query(aabb.min_[0], aabb.max_[0]);
+ if(results0.size() > CUTOFF)
+ {
+ results1 = interval_trees[1]->query(aabb.min_[1], aabb.max_[1]);
+ if(results1.size() > CUTOFF)
+ {
+ results2 = interval_trees[2]->query(aabb.min_[2], aabb.max_[2]);
+ if(results2.size() > CUTOFF)
+ {
+ int d1 = results0.size();
+ int d2 = results1.size();
+ int d3 = results2.size();
+
+ if(d1 >= d2 && d1 >= d3)
+ dist_res = checkDist(results0.begin(), results0.end(), obj, cdata, callback, min_dist);
+ else if(d2 >= d1 && d2 >= d3)
+ dist_res = checkDist(results1.begin(), results1.end(), obj, cdata, callback, min_dist);
+ else
+ dist_res = checkDist(results2.begin(), results2.end(), obj, cdata, callback, min_dist);
+ }
+ else
+ dist_res = checkDist(results2.begin(), results2.end(), obj, cdata, callback, min_dist);
+ }
+ else
+ dist_res = checkDist(results1.begin(), results1.end(), obj, cdata, callback, min_dist);
+ }
+ else
+ dist_res = checkDist(results0.begin(), results0.end(), obj, cdata, callback, min_dist);
+
+ if(dist_res) return true;
+
+ results0.clear();
+ results1.clear();
+ results2.clear();
+
+ if(status == 1)
+ {
+ if(old_min_distance < std::numeric_limits<FCL_REAL>::max())
+ break;
+ else
+ {
+ if(min_dist < old_min_distance)
+ {
+ Vec3f min_dist_delta(min_dist, min_dist, min_dist);
+ aabb = AABB(obj->getAABB(), min_dist_delta);
+ status = 0;
+ }
+ else
+ {
+ if(aabb.equal(obj->getAABB()))
+ aabb.expand(delta);
+ else
+ aabb.expand(obj->getAABB(), 2.0);
+ }
+ }
+ }
+ else if(status == 0)
+ break;
+ }
+
+ return false;
+}
+
+void IntervalTreeCollisionManager::collide(void* cdata, CollisionCallBack callback) const
+{
+ if(size() == 0) return;
+
+ std::set<CollisionObject*> active;
+ std::set<std::pair<CollisionObject*, CollisionObject*> > overlap;
+ unsigned int n = endpoints[0].size();
+ double diff_x = endpoints[0][0].value - endpoints[0][n-1].value;
+ double diff_y = endpoints[1][0].value - endpoints[1][n-1].value;
+ double diff_z = endpoints[2][0].value - endpoints[2][n-1].value;
+
+ int axis = 0;
+ if(diff_y > diff_x && diff_y > diff_z)
+ axis = 1;
+ else if(diff_z > diff_y && diff_z > diff_x)
+ axis = 2;
+
+ for(unsigned int i = 0; i < n; ++i)
+ {
+ const EndPoint& endpoint = endpoints[axis][i];
+ CollisionObject* index = endpoint.obj;
+ if(endpoint.minmax == 0)
+ {
+ std::set<CollisionObject*>::iterator iter = active.begin();
+ std::set<CollisionObject*>::iterator end = active.end();
+ for(; iter != end; ++iter)
+ {
+ CollisionObject* active_index = *iter;
+ const AABB& b0 = active_index->getAABB();
+ const AABB& b1 = index->getAABB();
+
+ int axis2 = (axis + 1) % 3;
+ int axis3 = (axis + 2) % 3;
+
+ if(b0.axisOverlap(b1, axis2) && b0.axisOverlap(b1, axis3))
+ {
+ std::pair<std::set<std::pair<CollisionObject*, CollisionObject*> >::iterator, bool> insert_res;
+ if(active_index < index)
+ insert_res = overlap.insert(std::make_pair(active_index, index));
+ else
+ insert_res = overlap.insert(std::make_pair(index, active_index));
+
+ if(insert_res.second)
+ {
+ if(callback(active_index, index, cdata))
+ return;
+ }
+ }
+ }
+ active.insert(index);
+ }
+ else
+ active.erase(index);
+ }
+
+}
+
+void IntervalTreeCollisionManager::distance(void* cdata, DistanceCallBack callback) const
+{
+ if(size() == 0) return;
+
+ enable_tested_set_ = true;
+ tested_set.clear();
+ FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
+
+ for(size_t i = 0; i < endpoints[0].size(); ++i)
+ if(distance_(endpoints[0][i].obj, cdata, callback, min_dist)) break;
+
+ enable_tested_set_ = false;
+ tested_set.clear();
+}
+
+void IntervalTreeCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const
+{
+ IntervalTreeCollisionManager* other_manager = static_cast<IntervalTreeCollisionManager*>(other_manager_);
+
+ if((size() == 0) || (other_manager->size() == 0)) return;
+
+ if(this == other_manager)
+ {
+ collide(cdata, callback);
+ return;
+ }
+
+ if(this->size() < other_manager->size())
+ {
+ for(size_t i = 0, size = endpoints[0].size(); i < size; ++i)
+ if(other_manager->collide_(endpoints[0][i].obj, cdata, callback)) return;
+ }
+ else
+ {
+ for(size_t i = 0, size = other_manager->endpoints[0].size(); i < size; ++i)
+ if(collide_(other_manager->endpoints[0][i].obj, cdata, callback)) return;
+ }
+}
+
+void IntervalTreeCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const
+{
+ IntervalTreeCollisionManager* other_manager = static_cast<IntervalTreeCollisionManager*>(other_manager_);
+
+ if((size() == 0) || (other_manager->size() == 0)) return;
+
+ if(this == other_manager)
+ {
+ distance(cdata, callback);
+ return;
+ }
+
+ FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
+
+ if(this->size() < other_manager->size())
+ {
+ for(size_t i = 0, size = endpoints[0].size(); i < size; ++i)
+ if(other_manager->distance_(endpoints[0][i].obj, cdata, callback, min_dist)) return;
+ }
+ else
+ {
+ for(size_t i = 0, size = other_manager->endpoints[0].size(); i < size; ++i)
+ if(distance_(other_manager->endpoints[0][i].obj, cdata, callback, min_dist)) return;
+ }
+}
+
+bool IntervalTreeCollisionManager::empty() const
+{
+ return endpoints[0].empty();
+}
+
+bool IntervalTreeCollisionManager::checkColl(std::deque<SimpleInterval*>::const_iterator pos_start, std::deque<SimpleInterval*>::const_iterator pos_end, CollisionObject* obj, void* cdata, CollisionCallBack callback) const
+{
+ while(pos_start < pos_end)
+ {
+ SAPInterval* ivl = static_cast<SAPInterval*>(*pos_start);
+ if(ivl->obj != obj)
+ {
+ if(ivl->obj->getAABB().overlap(obj->getAABB()))
+ {
+ if(callback(ivl->obj, obj, cdata))
+ return true;
+ }
+ }
+
+ pos_start++;
+ }
+
+ return false;
+}
+
+bool IntervalTreeCollisionManager::checkDist(std::deque<SimpleInterval*>::const_iterator pos_start, std::deque<SimpleInterval*>::const_iterator pos_end, CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
+{
+ while(pos_start < pos_end)
+ {
+ SAPInterval* ivl = static_cast<SAPInterval*>(*pos_start);
+ if(ivl->obj != obj)
+ {
+ if(!enable_tested_set_)
+ {
+ if(ivl->obj->getAABB().distance(obj->getAABB()) < min_dist)
+ {
+ if(callback(ivl->obj, obj, cdata, min_dist))
+ return true;
+ }
+ }
+ else
+ {
+ if(!inTestedSet(ivl->obj, obj))
+ {
+ if(ivl->obj->getAABB().distance(obj->getAABB()) < min_dist)
+ {
+ if(callback(ivl->obj, obj, cdata, min_dist))
+ return true;
+ }
+
+ insertTestedSet(ivl->obj, obj);
+ }
+ }
+ }
+
+ pos_start++;
+ }
+
+ return false;
+}
+
+}
diff --git a/trunk/fcl/src/broadphase/broadphase_spatialhash.cpp b/trunk/fcl/src/broadphase/broadphase_spatialhash.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e39cb3302dd0d3c04e53e06688e7498351ce030d
--- /dev/null
+++ b/trunk/fcl/src/broadphase/broadphase_spatialhash.cpp
@@ -0,0 +1,42 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * 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 Jia Pan */
+
+#include "fcl/broadphase/broadphase_spatialhash.h"
+
+namespace fcl
+{
+
+}
diff --git a/trunk/fcl/src/hierarchy_tree.cpp b/trunk/fcl/src/broadphase/hierarchy_tree.cpp
similarity index 98%
rename from trunk/fcl/src/hierarchy_tree.cpp
rename to trunk/fcl/src/broadphase/hierarchy_tree.cpp
index 015ce41d245a0fa1fbae40d487c2a2cc69657822..9c3ce96dad9140fe67b9b27761993886a8f614b8 100644
--- a/trunk/fcl/src/hierarchy_tree.cpp
+++ b/trunk/fcl/src/broadphase/hierarchy_tree.cpp
@@ -34,7 +34,7 @@
/** \author Jia Pan */
-#include "fcl/hierarchy_tree.h"
+#include "fcl/broadphase/hierarchy_tree.h"
namespace fcl
{
@@ -100,7 +100,7 @@ bool HierarchyTree<AABB>::update(NodeBase<AABB>* leaf, const AABB& bv_, const Ve
return true;
}
-namespace alternative
+namespace implementation_array
{
template<>
size_t select(size_t query, size_t node1, size_t node2, NodeBase<AABB>* nodes)
diff --git a/trunk/fcl/src/interval_tree.cpp b/trunk/fcl/src/broadphase/interval_tree.cpp
similarity index 94%
rename from trunk/fcl/src/interval_tree.cpp
rename to trunk/fcl/src/broadphase/interval_tree.cpp
index d96dc2fa6a2c41416dfe604d8772845eeb161d2f..1585b0145801b9cbefdeb613bd811df8b767b9db 100644
--- a/trunk/fcl/src/interval_tree.cpp
+++ b/trunk/fcl/src/broadphase/interval_tree.cpp
@@ -34,7 +34,7 @@
/** \author Jia Pan */
-#include "fcl/interval_tree.h"
+#include "fcl/broadphase/interval_tree.h"
#include <iostream>
#include <cstdlib>
@@ -45,13 +45,28 @@ namespace fcl
IntervalTreeNode::IntervalTreeNode(){}
IntervalTreeNode::IntervalTreeNode(SimpleInterval* new_interval) :
- stored_interval (new_interval),
- key(new_interval->low),
- high(new_interval->high),
- max_high(high) {}
+ stored_interval (new_interval),
+ key(new_interval->low),
+ high(new_interval->high),
+ max_high(high) {}
IntervalTreeNode::~IntervalTreeNode() {}
+
+/// @brief Class describes the information needed when we take the
+/// right branch in searching for intervals but possibly come back
+/// and check the left branch as well.
+struct it_recursion_node
+{
+public:
+ IntervalTreeNode* start_node;
+
+ unsigned int parent_index;
+
+ bool try_right_branch;
+};
+
+
IntervalTree::IntervalTree()
{
nil = new IntervalTreeNode;
@@ -66,7 +81,7 @@ IntervalTree::IntervalTree()
root->red = false;
root->stored_interval = NULL;
- /* the following are used for the query function */
+ /// the following are used for the query function
recursion_node_stack_size = 128;
recursion_node_stack = (it_recursion_node*)malloc(recursion_node_stack_size*sizeof(it_recursion_node));
recursion_node_stack_top = 1;
@@ -159,7 +174,7 @@ void IntervalTree::rightRotate(IntervalTreeNode* y)
-/** \brief Inserts z into the tree as if it were a regular binary tree */
+/// @brief Inserts z into the tree as if it were a regular binary tree
void IntervalTree::recursiveInsert(IntervalTreeNode* z)
{
IntervalTreeNode* x;
@@ -206,7 +221,7 @@ IntervalTreeNode* IntervalTree::insert(SimpleInterval* new_interval)
x->red = true;
while(x->parent->red)
{
- /* use sentinel instead of checking for root */
+ /// use sentinel instead of checking for root
if(x->parent == x->parent->parent->left)
{
y = x->parent->parent->right;
@@ -452,8 +467,8 @@ SimpleInterval* IntervalTree::deleteNode(IntervalTreeNode* z)
}
}
- /* y should not be nil in this case */
- /* y is the node to splice out and x is its child */
+ /// @brief y should not be nil in this case
+ /// y is the node to splice out and x is its child
if(y != z)
{
y->max_high = -std::numeric_limits<double>::max();
@@ -486,7 +501,7 @@ SimpleInterval* IntervalTree::deleteNode(IntervalTreeNode* z)
return node_to_delete;
}
-/** \brief returns 1 if the intervals overlap, and 0 otherwise */
+/// @brief returns 1 if the intervals overlap, and 0 otherwise
bool overlap(double a1, double a2, double b1, double b2)
{
if(a1 <= b1)