diff --git a/CMakeLists.txt b/CMakeLists.txt
index e2af8d7f47b528df992642649517e1eeb923a335..5ba08d8b8a34d7498dfb05b7657a3e9cacb15145 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -117,18 +117,6 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/fcl/BV/kDOP.h
include/hpp/fcl/narrowphase/narrowphase.h
include/hpp/fcl/narrowphase/gjk.h
- include/hpp/fcl/broadphase/interval_tree.h
- include/hpp/fcl/broadphase/broadphase_spatialhash.h
- include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h
- include/hpp/fcl/broadphase/broadphase_SSaP.h
- include/hpp/fcl/broadphase/broadphase_interval_tree.h
- include/hpp/fcl/broadphase/broadphase.h
- include/hpp/fcl/broadphase/hierarchy_tree.h
- include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h
- include/hpp/fcl/broadphase/broadphase_SaP.h
- include/hpp/fcl/broadphase/broadphase_bruteforce.h
- include/hpp/fcl/broadphase/morton.h
- include/hpp/fcl/broadphase/hash.h
include/hpp/fcl/shape/geometric_shapes_utility.h
include/hpp/fcl/shape/geometric_shape_to_BVH_model.h
include/hpp/fcl/shape/geometric_shapes.h
diff --git a/include/hpp/fcl/broadphase/broadphase.h b/include/hpp/fcl/broadphase/broadphase.h
deleted file mode 100644
index e34c2f723d637891a72cd7e64e39da718522d462..0000000000000000000000000000000000000000
--- a/include/hpp/fcl/broadphase/broadphase.h
+++ /dev/null
@@ -1,160 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011-2014, Willow Garage, Inc.
- * Copyright (c) 2014-2015, Open Source Robotics Foundation
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Open Source Robotics Foundation nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/** \author Jia Pan */
-
-
-
-#ifndef FCL_BROAD_PHASE_H
-#define FCL_BROAD_PHASE_H
-
-#include <hpp/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 <hpp/fcl/broadphase/broadphase_bruteforce.h>
-#include <hpp/fcl/broadphase/broadphase_spatialhash.h>
-#include <hpp/fcl/broadphase/broadphase_SaP.h>
-#include <hpp/fcl/broadphase/broadphase_SSaP.h>
-#include <hpp/fcl/broadphase/broadphase_interval_tree.h>
-#include <hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h>
-#include <hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h>
-
-#endif
diff --git a/include/hpp/fcl/broadphase/broadphase_SSaP.h b/include/hpp/fcl/broadphase/broadphase_SSaP.h
deleted file mode 100644
index 28e7107bd03f2e59b1fc5558763e2a990644f046..0000000000000000000000000000000000000000
--- a/include/hpp/fcl/broadphase/broadphase_SSaP.h
+++ /dev/null
@@ -1,158 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011-2014, Willow Garage, Inc.
- * Copyright (c) 2014-2015, Open Source Robotics Foundation
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Open Source Robotics Foundation nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/** \author Jia Pan */
-
-
-#ifndef FCL_BROAD_PHASE_SSAP_H
-#define FCL_BROAD_PHASE_SSAP_H
-
-#include <hpp/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/include/hpp/fcl/broadphase/broadphase_SaP.h b/include/hpp/fcl/broadphase/broadphase_SaP.h
deleted file mode 100644
index b0b230766bab0a03a81a0c742c3ddf41f4f7028a..0000000000000000000000000000000000000000
--- a/include/hpp/fcl/broadphase/broadphase_SaP.h
+++ /dev/null
@@ -1,317 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011-2014, Willow Garage, Inc.
- * Copyright (c) 2014-2015, Open Source Robotics Foundation
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Open Source Robotics Foundation nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/** \author Jia Pan */
-
-#ifndef FCL_BROAD_PHASE_SAP_H
-#define FCL_BROAD_PHASE_SAP_H
-
-#include <hpp/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 Vec3f::Scalar getVal(size_t i) const
- {
- if(minmax) return aabb->cached.max_[i];
- else return aabb->cached.min_[i];
- }
-
- inline Vec3f::Scalar& 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/include/hpp/fcl/broadphase/broadphase_bruteforce.h b/include/hpp/fcl/broadphase/broadphase_bruteforce.h
deleted file mode 100644
index 6e712107de72bdbaab0655109ec992a103a65fcc..0000000000000000000000000000000000000000
--- a/include/hpp/fcl/broadphase/broadphase_bruteforce.h
+++ /dev/null
@@ -1,108 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011-2014, Willow Garage, Inc.
- * Copyright (c) 2014-2015, Open Source Robotics Foundation
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Open Source Robotics Foundation nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/** \author Jia Pan */
-
-#ifndef FCL_BROAD_PHASE_BRUTE_FORCE_H
-#define FCL_BROAD_PHASE_BRUTE_FORCE_H
-
-#include <hpp/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/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h b/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h
deleted file mode 100644
index bd6a6063a00d25a796f73ccb8d3e2cfcf98228f3..0000000000000000000000000000000000000000
--- a/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h
+++ /dev/null
@@ -1,166 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011-2014, Willow Garage, Inc.
- * Copyright (c) 2014-2015, Open Source Robotics Foundation
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Open Source Robotics Foundation nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/** \author Jia Pan */
-
-
-#ifndef FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_H
-#define FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_H
-
-#include <hpp/fcl/broadphase/broadphase.h>
-#include <hpp/fcl/broadphase/hierarchy_tree.h>
-#include <hpp/fcl/BV/BV.h>
-#include <hpp/fcl/shape/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;
-
- /// @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
- {
- 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_;
-
- void update_(CollisionObject* updated_obj);
-};
-
-
-
-}
-
-#endif
diff --git a/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h b/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h
deleted file mode 100644
index 731010dd2573aa0088482d0cacb945bf3eeb6231..0000000000000000000000000000000000000000
--- a/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h
+++ /dev/null
@@ -1,163 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011-2014, Willow Garage, Inc.
- * Copyright (c) 2014-2015, Open Source Robotics Foundation
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Open Source Robotics Foundation nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/** \author Jia Pan */
-
-#ifndef FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_H
-#define FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_H
-
-#include <hpp/fcl/broadphase/broadphase.h>
-#include <hpp/fcl/broadphase/hierarchy_tree.h>
-#include <hpp/fcl/BV/BV.h>
-#include <hpp/fcl/shape/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;
-
- /// @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
- {
- 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_;
-
- void update_(CollisionObject* updated_obj);
-};
-
-
-}
-
-#endif
diff --git a/include/hpp/fcl/broadphase/broadphase_interval_tree.h b/include/hpp/fcl/broadphase/broadphase_interval_tree.h
deleted file mode 100644
index 53c105ef2fd6e3c36e1cff96899f2691f567a52a..0000000000000000000000000000000000000000
--- a/include/hpp/fcl/broadphase/broadphase_interval_tree.h
+++ /dev/null
@@ -1,164 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011-2014, Willow Garage, Inc.
- * Copyright (c) 2014-2015, Open Source Robotics Foundation
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Open Source Robotics Foundation nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/** \author Jia Pan */
-
-#ifndef FCL_BROAD_PHASE_INTERVAL_TREE_H
-#define FCL_BROAD_PHASE_INTERVAL_TREE_H
-
-#include <hpp/fcl/broadphase/broadphase.h>
-#include <hpp/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/include/hpp/fcl/broadphase/broadphase_spatialhash.h b/include/hpp/fcl/broadphase/broadphase_spatialhash.h
deleted file mode 100644
index fbcded166e2ba3bd5baea177b59f8fd016be8741..0000000000000000000000000000000000000000
--- a/include/hpp/fcl/broadphase/broadphase_spatialhash.h
+++ /dev/null
@@ -1,199 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011-2014, Willow Garage, Inc.
- * Copyright (c) 2014-2015, Open Source Robotics Foundation
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Open Source Robotics Foundation nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/** \author Jia Pan */
-
-#ifndef FCL_BROAD_PHASE_SPATIAL_HASH_H
-#define FCL_BROAD_PHASE_SPATIAL_HASH_H
-
-#include <hpp/fcl/broadphase/broadphase.h>
-#include <hpp/fcl/broadphase/hash.h>
-#include <hpp/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 <hpp/fcl/broadphase/broadphase_spatialhash.hxx>
-
-
-#endif
diff --git a/include/hpp/fcl/broadphase/broadphase_spatialhash.hxx b/include/hpp/fcl/broadphase/broadphase_spatialhash.hxx
deleted file mode 100644
index 2c1f62d56adc64dcb859bef276922ba81d0f0459..0000000000000000000000000000000000000000
--- a/include/hpp/fcl/broadphase/broadphase_spatialhash.hxx
+++ /dev/null
@@ -1,459 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011-2014, Willow Garage, Inc.
- * Copyright (c) 2014-2015, Open Source Robotics Foundation
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Open Source Robotics Foundation nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/** \author Jia Pan */
-
-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/include/hpp/fcl/broadphase/hash.h b/include/hpp/fcl/broadphase/hash.h
deleted file mode 100644
index 58686dbd7d91722a891f68fb41e2caecc4baa5be..0000000000000000000000000000000000000000
--- a/include/hpp/fcl/broadphase/hash.h
+++ /dev/null
@@ -1,189 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011-2014, Willow Garage, Inc.
- * Copyright (c) 2014-2015, Open Source Robotics Foundation
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Open Source Robotics Foundation nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/** \author Jia Pan */
-
-#ifndef FCL_HASH_H
-#define FCL_HASH_H
-
-#include <stdexcept>
-#include <set>
-#include <vector>
-#include <list>
-#include <boost/unordered_map.hpp>
-
-namespace fcl
-{
-
-/// @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)
- {
- throw std::logic_error("SimpleHashTable must have non-zero size.");
- }
-
- table_.resize(size);
- 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);
- size_t range = table_.size();
- for(size_t i = 0; i < indices.size(); ++i)
- 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();
- std::vector<unsigned int> indices = h_(key);
- std::set<Data> result;
- for(size_t i = 0; i < indices.size(); ++i)
- {
- unsigned int index = indices[i] % range;
- std::copy(table_[index].begin(), table_[index].end(), std::inserter(result, result.end()));
- }
-
- 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();
- std::vector<unsigned int> indices = h_(key);
- for(size_t i = 0; i < indices.size(); ++i)
- {
- unsigned int index = indices[i] % range;
- table_[index].remove(value);
- }
- }
-
- /// @brief clear the hash table
- void clear()
- {
- table_.clear();
- table_.resize(table_size_);
- }
-};
-
-
-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
-{
-protected:
- HashFnc h_;
- typedef std::list<Data> Bin;
- typedef TableT<size_t, Bin> Table;
-
- Table table_;
-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);
- for(size_t i = 0; i < indices.size(); ++i)
- 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);
- std::set<Data> result;
- for(size_t i = 0; i < indices.size(); ++i)
- {
- unsigned int index = indices[i];
- typename Table::const_iterator p = table_.find(index);
- if(p != table_.end())
- std::copy((*p).second.begin(), (*p).second.end(), std::inserter(result, result.end()));
- }
-
- 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);
- for(size_t i = 0; i < indices.size(); ++i)
- {
- unsigned int index = indices[i];
- table_[index].remove(value);
- }
- }
-
- /// @brief clear the hash table
- void clear()
- {
- table_.clear();
- }
-};
-
-
-}
-
-#endif
diff --git a/include/hpp/fcl/broadphase/hierarchy_tree.h b/include/hpp/fcl/broadphase/hierarchy_tree.h
deleted file mode 100644
index 347e90a37c4c18f02bc73a7f1d24a342e9dccf08..0000000000000000000000000000000000000000
--- a/include/hpp/fcl/broadphase/hierarchy_tree.h
+++ /dev/null
@@ -1,593 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011-2014, Willow Garage, Inc.
- * Copyright (c) 2014-2015, Open Source Robotics Foundation
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Open Source Robotics Foundation nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/** \author Jia Pan */
-
-#ifndef FCL_HIERARCHY_TREE_H
-#define FCL_HIERARCHY_TREE_H
-
-#include <vector>
-#include <map>
-#include <hpp/fcl/BV/AABB.h>
-#include <hpp/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() (const NodeType* a, const 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 <hpp/fcl/broadphase/hierarchy_tree.hxx>
-
-
-#endif
diff --git a/include/hpp/fcl/broadphase/hierarchy_tree.hxx b/include/hpp/fcl/broadphase/hierarchy_tree.hxx
deleted file mode 100644
index 10ce2f726aee6b51d61a109c3d49a132a97c6c82..0000000000000000000000000000000000000000
--- a/include/hpp/fcl/broadphase/hierarchy_tree.hxx
+++ /dev/null
@@ -1,1877 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011-2014, Willow Garage, Inc.
- * Copyright (c) 2014-2015, Open Source Robotics Foundation
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Open Source Robotics Foundation nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/** \author Jia Pan */
-
-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 = NULL, *min_it2 = NULL;
- 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/include/hpp/fcl/broadphase/interval_tree.h b/include/hpp/fcl/broadphase/interval_tree.h
deleted file mode 100644
index 9b32948bb5b64a6518fa7fa37b548679cd04e067..0000000000000000000000000000000000000000
--- a/include/hpp/fcl/broadphase/interval_tree.h
+++ /dev/null
@@ -1,167 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011-2014, Willow Garage, Inc.
- * Copyright (c) 2014-2015, Open Source Robotics Foundation
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Open Source Robotics Foundation nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/** \author Jia Pan */
-
-
-#ifndef FCL_INTERVAL_TREE_H
-#define FCL_INTERVAL_TREE_H
-
-#include <deque>
-#include <limits>
-
-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.
-struct SimpleInterval
-{
-public:
- 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
- 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;
-
- double high;
-
- double max_high;
-
- /// @brief red or black node: if red = false then the node is black
- bool red;
-
- IntervalTreeNode* left;
-
- IntervalTreeNode* right;
-
- IntervalTreeNode* parent;
-};
-
-struct it_recursion_node;
-
-/// @brief Interval tree
-class IntervalTree
-{
-public:
-
- IntervalTree();
-
- ~IntervalTree();
-
- /// @brief Print the whole interval tree
- void print() const;
-
- /// @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
- IntervalTreeNode* insert(SimpleInterval* new_interval);
-
- /// @brief get the predecessor of a given node
- IntervalTreeNode* getPredecessor(IntervalTreeNode* node) const;
-
- /// @brief Get the successor of a given node
- IntervalTreeNode* getSuccessor(IntervalTreeNode* node) const;
-
- /// @brief Return result for a given query
- std::deque<SimpleInterval*> query(double low, double high);
-
-protected:
-
- IntervalTreeNode* root;
-
- IntervalTreeNode* nil;
-
- /// @brief left rotation of tree node
- void leftRotate(IntervalTreeNode* node);
-
- /// @brief right rotation of tree node
- void rightRotate(IntervalTreeNode* node);
-
- /// @brief recursively insert a node
- void recursiveInsert(IntervalTreeNode* node);
-
- /// @brief recursively print a subtree
- void recursivePrint(IntervalTreeNode* node) const;
-
- /// @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
- void fixupMaxHigh(IntervalTreeNode* node);
-
- void deleteFixup(IntervalTreeNode* node);
-
-private:
- unsigned int recursion_node_stack_size;
- it_recursion_node* recursion_node_stack;
- unsigned int current_parent;
- unsigned int recursion_node_stack_top;
-};
-
-}
-
-#endif
-
-
diff --git a/include/hpp/fcl/broadphase/morton.h b/include/hpp/fcl/broadphase/morton.h
deleted file mode 100644
index f599a7cc53596598181f467fd7ed1bee9f16e3de..0000000000000000000000000000000000000000
--- a/include/hpp/fcl/broadphase/morton.h
+++ /dev/null
@@ -1,196 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011-2014, Willow Garage, Inc.
- * Copyright (c) 2014-2015, Open Source Robotics Foundation
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Open Source Robotics Foundation nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/** \author Jia Pan */
-
-#ifndef FCL_MORTON_H
-#define FCL_MORTON_H
-
-#include <boost/dynamic_bitset.hpp>
-#include <hpp/fcl/data_types.h>
-#include <hpp/fcl/BV/AABB.h>
-
-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;
- x = (x | (x << 8)) & 0x0300F00F;
- x = (x | (x << 4)) & 0x030C30C3;
- x = (x | (x << 2)) & 0x09249249;
-
- y = (y | (y << 16)) & 0x030000FF;
- y = (y | (y << 8)) & 0x0300F00F;
- y = (y | (y << 4)) & 0x030C30C3;
- y = (y | (y << 2)) & 0x09249249;
-
- z = (z | (z << 16)) & 0x030000FF;
- z = (z | (z << 8)) & 0x0300F00F;
- z = (z | (z << 4)) & 0x030C30C3;
- z = (z | (z << 2)) & 0x09249249;
-
- 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;
- FCL_UINT32 lo_y = y & 1023u;
- FCL_UINT32 lo_z = z & 1023u;
- FCL_UINT32 hi_x = x >> 10u;
- FCL_UINT32 hi_y = y >> 10u;
- FCL_UINT32 hi_z = z >> 10u;
-
- 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>
-{
- morton_functor(const AABB& bbox) : base(bbox.min_),
- inv(1.0 / (bbox.max_[0] - bbox.min_[0]),
- 1.0 / (bbox.max_[1] - bbox.min_[1]),
- 1.0 / (bbox.max_[2] - bbox.min_[2]))
- {}
-
- FCL_UINT32 operator() (const Vec3f& point) const
- {
- 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 details::morton_code(x, y, z);
- }
-
- const Vec3f base;
- const Vec3f inv;
-
- size_t bits() const { return 30; }
-};
-
-
-/// @brief Functor to compute 60 bit morton code for a given AABB
-template<>
-struct morton_functor<FCL_UINT64>
-{
- morton_functor(const AABB& bbox) : base(bbox.min_),
- inv(1.0 / (bbox.max_[0] - bbox.min_[0]),
- 1.0 / (bbox.max_[1] - bbox.min_[1]),
- 1.0 / (bbox.max_[2] - bbox.min_[2]))
- {}
-
- FCL_UINT64 operator() (const Vec3f& point) const
- {
- 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 details::morton_code60(x, y, z);
- }
-
- const Vec3f base;
- const Vec3f inv;
-
- 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<> >
-{
- morton_functor(const AABB& bbox, size_t bit_num_) : base(bbox.min_),
- inv(1.0 / (bbox.max_[0] - bbox.min_[0]),
- 1.0 / (bbox.max_[1] - bbox.min_[1]),
- 1.0 / (bbox.max_[2] - bbox.min_[2])),
- bit_num(bit_num_)
- {}
-
- boost::dynamic_bitset<> operator() (const Vec3f& point) const
- {
- FCL_REAL x = (point[0] - base[0]) * inv[0];
- FCL_REAL y = (point[1] - base[1]) * inv[1];
- FCL_REAL z = (point[2] - base[2]) * inv[2];
- int start_bit = bit_num * 3 - 1;
- boost::dynamic_bitset<> bits(bit_num * 3);
-
- x *= 2;
- y *= 2;
- z *= 2;
-
- for(size_t i = 0; i < bit_num; ++i)
- {
- bits[start_bit--] = ((z < 1) ? 0 : 1);
- bits[start_bit--] = ((y < 1) ? 0 : 1);
- bits[start_bit--] = ((x < 1) ? 0 : 1);
- x = ((x >= 1) ? 2*(x-1) : 2*x);
- y = ((y >= 1) ? 2*(y-1) : 2*y);
- z = ((z >= 1) ? 2*(z-1) : 2*z);
- }
-
- return bits;
- }
-
- const Vec3f base;
- const Vec3f inv;
- const size_t bit_num;
-
- size_t bits() const { return bit_num * 3; }
-};
-
-}
-
-#endif
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 15b893f9dfe1c570a4895f9cf12722377867fcac..fc3fcfa3c6b970b06ebb2551ba5aed03ef08f7f6 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -46,15 +46,6 @@ set(${LIBRARY_NAME}_SOURCES
BV/OBB.cpp
narrowphase/narrowphase.cpp
narrowphase/gjk.cpp
- broadphase/broadphase_bruteforce.cpp
- broadphase/interval_tree.cpp
- broadphase/broadphase_SaP.cpp
- broadphase/broadphase_dynamic_AABB_tree.cpp
- broadphase/broadphase_SSaP.cpp
- broadphase/broadphase_dynamic_AABB_tree_array.cpp
- broadphase/broadphase_interval_tree.cpp
- broadphase/broadphase_spatialhash.cpp
- broadphase/hierarchy_tree.cpp
shape/geometric_shapes.cpp
shape/geometric_shapes_utility.cpp
distance_capsule_capsule.cpp
diff --git a/src/broadphase/broadphase_SSaP.cpp b/src/broadphase/broadphase_SSaP.cpp
deleted file mode 100644
index 5595bde03f195d3cd1431abba66e8445c09158ce..0000000000000000000000000000000000000000
--- a/src/broadphase/broadphase_SSaP.cpp
+++ /dev/null
@@ -1,505 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011-2014, Willow Garage, Inc.
- * Copyright (c) 2014-2015, Open Source Robotics Foundation
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Open Source Robotics Foundation nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/** \author Jia Pan */
-
-#include <hpp/fcl/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(boost::shared_ptr<CollisionGeometry>())
- {
- 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(isEqual(dummy_vector, obj->getAABB().max_))
- dummy_vector.noalias() += 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;
- 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/src/broadphase/broadphase_SaP.cpp b/src/broadphase/broadphase_SaP.cpp
deleted file mode 100644
index 0fb886ea5fd8b7a1ad38770d6cab134a9e892f9b..0000000000000000000000000000000000000000
--- a/src/broadphase/broadphase_SaP.cpp
+++ /dev/null
@@ -1,767 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011-2014, Willow Garage, Inc.
- * Copyright (c) 2014-2015, Open Source Robotics Foundation
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Open Source Robotics Foundation nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/** \author Jia Pan */
-
-#include <hpp/fcl/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(other_objs.empty()) return;
-
- 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<Vec3f::Scalar>(),
- boost::bind(static_cast<Vec3f::Scalar (EndPoint::*)(size_t) const >(&EndPoint::getVal), _1, coord),
- boost::bind(static_cast<Vec3f::Scalar (EndPoint::*)(size_t) const >(&EndPoint::getVal), _2, coord)));
-
- endpoints[0]->prev[coord] = NULL;
- endpoints[0]->next[coord] = endpoints[1];
- for(size_t 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()
-{
- if(size() == 0) return;
-
- 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<Vec3f::Scalar>(),
- boost::bind(static_cast<Vec3f::Scalar (EndPoint::*)(size_t) const>(&EndPoint::getVal), _1, axis),
- boost::bind(static_cast<Vec3f::Scalar (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<Vec3f::Scalar>(),
- boost::bind(static_cast<Vec3f::Scalar (EndPoint::*)(size_t) const>(&EndPoint::getVal), _1, axis),
- boost::bind(static_cast<Vec3f::Scalar (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/src/broadphase/broadphase_bruteforce.cpp b/src/broadphase/broadphase_bruteforce.cpp
deleted file mode 100644
index f5eb1f6e8d1da8b6d94b0df147e86abe1c89d3d9..0000000000000000000000000000000000000000
--- a/src/broadphase/broadphase_bruteforce.cpp
+++ /dev/null
@@ -1,198 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011-2014, Willow Garage, Inc.
- * Copyright (c) 2014-2015, Open Source Robotics Foundation
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Open Source Robotics Foundation nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/** \author Jia Pan */
-
-#include <hpp/fcl/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/src/broadphase/broadphase_dynamic_AABB_tree.cpp b/src/broadphase/broadphase_dynamic_AABB_tree.cpp
deleted file mode 100644
index b4a6abafde65176c9ae5db5841fdb6e94e13ac33..0000000000000000000000000000000000000000
--- a/src/broadphase/broadphase_dynamic_AABB_tree.cpp
+++ /dev/null
@@ -1,817 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011-2014, Willow Garage, Inc.
- * Copyright (c) 2014-2015, Open Source Robotics Foundation
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Open Source Robotics Foundation nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/** \author Jia Pan */
-
-
-#include <hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h>
-
-#ifdef FCL_HAVE_OCTOMAP
-#include <hpp/fcl/octree.h>
-#endif
-
-namespace fcl
-{
-
-namespace details
-{
-
-namespace dynamic_AABB_tree
-{
-
-#ifdef FCL_HAVE_OCTOMAP
-bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, CollisionCallBack callback)
-{
- if(!root2)
- {
- if(root1->isLeaf())
- {
- CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
-
- if(!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 = tree2->getDefaultOccupancy();
-
- CollisionObject obj2(boost::shared_ptr<CollisionGeometry>(box), box_tf);
- return callback(obj1, &obj2, cdata);
- }
- }
- }
- else
- {
- if(collisionRecurse_(root1->children[0], tree2, NULL, root2_bv, tf2, cdata, callback))
- return true;
- if(collisionRecurse_(root1->children[1], tree2, NULL, root2_bv, tf2, cdata, callback))
- return true;
- }
-
- return false;
- }
- else if(root1->isLeaf() && !tree2->nodeHasChildren(root2))
- {
- 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(!tree2->nodeHasChildren(root2) || (!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(tree2->nodeChildExists(root2, i))
- {
- const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
- AABB child_bv;
- computeChildBV(root2_bv, i, child_bv);
-
- if(collisionRecurse_(root1, tree2, child, child_bv, tf2, cdata, callback))
- return true;
- }
- else
- {
- AABB child_bv;
- computeChildBV(root2_bv, i, child_bv);
- if(collisionRecurse_(root1, tree2, NULL, 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(!root2)
- {
- if(root1->isLeaf())
- {
- CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
-
- if(!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 = tree2->getOccupancyThres(); // thresholds are 0, 1, so uncertain
-
- CollisionObject obj2(boost::shared_ptr<CollisionGeometry>(box), box_tf);
- return callback(obj1, &obj2, cdata);
- }
- }
- }
- else
- {
- if(collisionRecurse_(root1->children[0], tree2, NULL, root2_bv, tf2, cdata, callback))
- return true;
- if(collisionRecurse_(root1->children[1], tree2, NULL, root2_bv, tf2, cdata, callback))
- return true;
- }
-
- return false;
- }
- else if(root1->isLeaf() && !tree2->nodeHasChildren(root2))
- {
- 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(!tree2->nodeHasChildren(root2) || (!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(tree2->nodeChildExists(root2, i))
- {
- const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
- AABB child_bv;
- computeChildBV(root2_bv, i, child_bv);
-
- if(collisionRecurse_(root1, tree2, child, child_bv, tf2, cdata, callback))
- return true;
- }
- else
- {
- AABB child_bv;
- computeChildBV(root2_bv, i, child_bv);
- if(collisionRecurse_(root1, tree2, NULL, child_bv, tf2, cdata, callback))
- 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() && !tree2->nodeHasChildren(root2))
- {
- 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(!tree2->nodeHasChildren(root2) || (!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(tree2->nodeChildExists(root2, i))
- {
- const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, 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 collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, CollisionCallBack callback)
-{
- if(isQuatIdentity(tf2.getQuatRotation()))
- 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() && !tree2->nodeHasChildren(root2))
- {
- 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(!tree2->nodeHasChildren(root2) || (!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(tree2->nodeChildExists(root2, i))
- {
- const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, 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(isQuatIdentity(tf2.getQuatRotation()))
- 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);
-}
-
-#endif
-
-bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, DynamicAABBTreeCollisionManager::DynamicAABBNode* root2, void* cdata, CollisionCallBack callback)
-{
- 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 collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root, CollisionObject* query, void* cdata, CollisionCallBack callback)
-{
- 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 selfCollisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root, void* cdata, CollisionCallBack callback)
-{
- 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 distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, DynamicAABBTreeCollisionManager::DynamicAABBNode* root2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist)
-{
- 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 distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root, CollisionObject* query, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist)
-{
- 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 selfDistanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist)
-{
- 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;
-}
-
-} // dynamic_AABB_tree
-
-} // details
-
-void DynamicAABBTreeCollisionManager::registerObjects(const std::vector<CollisionObject*>& other_objs)
-{
- if(other_objs.empty()) return;
-
- 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();
-}
-
-void DynamicAABBTreeCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
-{
- if(size() == 0) return;
- switch(obj->collisionGeometry()->getNodeType())
- {
-#ifdef FCL_HAVE_OCTOMAP
- case GEOM_OCTREE:
- {
- if(!octree_as_geometry_collide)
- {
- const OcTree* octree = static_cast<const OcTree*>(obj->getCollisionGeometry());
- details::dynamic_AABB_tree::collisionRecurse(dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback);
- }
- else
- details::dynamic_AABB_tree::collisionRecurse(dtree.getRoot(), obj, cdata, callback);
- }
- break;
-#endif
- default:
- details::dynamic_AABB_tree::collisionRecurse(dtree.getRoot(), obj, cdata, callback);
- }
-}
-
-void DynamicAABBTreeCollisionManager::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const
-{
- if(size() == 0) return;
- FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
- switch(obj->collisionGeometry()->getNodeType())
- {
-#ifdef FCL_HAVE_OCTOMAP
- case GEOM_OCTREE:
- {
- if(!octree_as_geometry_distance)
- {
- const OcTree* octree = static_cast<const OcTree*>(obj->getCollisionGeometry());
- details::dynamic_AABB_tree::distanceRecurse(dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback, min_dist);
- }
- else
- details::dynamic_AABB_tree::distanceRecurse(dtree.getRoot(), obj, cdata, callback, min_dist);
- }
- break;
-#endif
- default:
- details::dynamic_AABB_tree::distanceRecurse(dtree.getRoot(), obj, cdata, callback, min_dist);
- }
-}
-
-
-void DynamicAABBTreeCollisionManager::collide(void* cdata, CollisionCallBack callback) const
-{
- if(size() == 0) return;
- details::dynamic_AABB_tree::selfCollisionRecurse(dtree.getRoot(), cdata, callback);
-}
-
-void DynamicAABBTreeCollisionManager::distance(void* cdata, DistanceCallBack callback) const
-{
- if(size() == 0) return;
- FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
- details::dynamic_AABB_tree::selfDistanceRecurse(dtree.getRoot(), cdata, callback, min_dist);
-}
-
-void DynamicAABBTreeCollisionManager::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;
- details::dynamic_AABB_tree::collisionRecurse(dtree.getRoot(), other_manager->dtree.getRoot(), cdata, callback);
-}
-
-void DynamicAABBTreeCollisionManager::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();
- details::dynamic_AABB_tree::distanceRecurse(dtree.getRoot(), other_manager->dtree.getRoot(), cdata, callback, min_dist);
-}
-}
diff --git a/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp b/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp
deleted file mode 100644
index b994e34c3abeceef6aca7891db23f10b500b6e8f..0000000000000000000000000000000000000000
--- a/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp
+++ /dev/null
@@ -1,845 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011-2014, Willow Garage, Inc.
- * Copyright (c) 2014-2015, Open Source Robotics Foundation
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Open Source Robotics Foundation nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/** \author Jia Pan */
-
-#include <hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h>
-
-#ifdef FCL_HAVE_OCTOMAP
-#include <hpp/fcl/octree.h>
-#endif
-
-namespace fcl
-{
-
-namespace details
-{
-
-namespace dynamic_AABB_tree_array
-{
-
-#ifdef FCL_HAVE_OCTOMAP
-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(!root2)
- {
- if(root1->isLeaf())
- {
- CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
-
- if(!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 = tree2->getDefaultOccupancy();
-
- CollisionObject obj2(boost::shared_ptr<CollisionGeometry>(box), box_tf);
- return callback(obj1, &obj2, cdata);
- }
- }
- }
- else
- {
- if(collisionRecurse_(nodes1, root1->children[0], tree2, NULL, root2_bv, tf2, cdata, callback))
- return true;
- if(collisionRecurse_(nodes1, root1->children[1], tree2, NULL, root2_bv, tf2, cdata, callback))
- return true;
- }
-
- return false;
- }
- else if(root1->isLeaf() && !tree2->nodeHasChildren(root2))
- {
- 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(!tree2->nodeHasChildren(root2) || (!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(tree2->nodeChildExists(root2, i))
- {
- const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
- AABB child_bv;
- computeChildBV(root2_bv, i, child_bv);
-
- if(collisionRecurse_(nodes1, root1_id, tree2, child, child_bv, tf2, cdata, callback))
- return true;
- }
- else
- {
- AABB child_bv;
- computeChildBV(root2_bv, i, child_bv);
- if(collisionRecurse_(nodes1, root1_id, tree2, NULL, 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(!root2)
- {
- if(root1->isLeaf())
- {
- CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
-
- if(!obj1->isFree())
- {
- 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 = tree2->getDefaultOccupancy();
-
- CollisionObject obj2(boost::shared_ptr<CollisionGeometry>(box), box_tf);
- return callback(obj1, &obj2, cdata);
- }
- }
- }
- else
- {
- if(collisionRecurse_(nodes1, root1->children[0], tree2, NULL, root2_bv, tf2, cdata, callback))
- return true;
- if(collisionRecurse_(nodes1, root1->children[1], tree2, NULL, root2_bv, tf2, cdata, callback))
- return true;
- }
-
- return false;
- }
- else if(root1->isLeaf() && !tree2->nodeHasChildren(root2))
- {
- CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
- if(!tree2->isNodeFree(root2) && !obj1->isFree())
- {
- 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(!tree2->nodeHasChildren(root2) || (!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(tree2->nodeChildExists(root2, i))
- {
- const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
- AABB child_bv;
- computeChildBV(root2_bv, i, child_bv);
-
- if(collisionRecurse_(nodes1, root1_id, tree2, child, child_bv, tf2, cdata, callback))
- return true;
- }
- else
- {
- AABB child_bv;
- computeChildBV(root2_bv, i, child_bv);
- if(collisionRecurse_(nodes1, root1_id, tree2, NULL, child_bv, tf2, cdata, callback))
- 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 Transform3f& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist)
-{
- DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root1 = nodes1 + root1_id;
- if(root1->isLeaf() && !tree2->nodeHasChildren(root2))
- {
- 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(!tree2->nodeHasChildren(root2) || (!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(tree2->nodeChildExists(root2, i))
- {
- const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, 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() && !tree2->nodeHasChildren(root2))
- {
- 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(!tree2->nodeHasChildren(root2) || (!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(tree2->nodeChildExists(root2, i))
- {
- const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, 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;
-}
-
-
-#endif
-
-bool collisionRecurse(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes1, size_t root1_id,
- DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes2, size_t root2_id,
- void* cdata, CollisionCallBack callback)
-{
- DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root1 = nodes1 + root1_id;
- DynamicAABBTreeCollisionManager_Array::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 collisionRecurse(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes, size_t root_id, CollisionObject* query, void* cdata, CollisionCallBack callback)
-{
- DynamicAABBTreeCollisionManager_Array::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 selfCollisionRecurse(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes, size_t root_id, void* cdata, CollisionCallBack callback)
-{
- DynamicAABBTreeCollisionManager_Array::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 distanceRecurse(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes1, size_t root1_id,
- DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes2, size_t root2_id,
- void* cdata, DistanceCallBack callback, FCL_REAL& min_dist)
-{
- DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root1 = nodes1 + root1_id;
- DynamicAABBTreeCollisionManager_Array::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 distanceRecurse(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes, size_t root_id, CollisionObject* query, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist)
-{
- DynamicAABBTreeCollisionManager_Array::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 selfDistanceRecurse(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes, size_t root_id, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist)
-{
- DynamicAABBTreeCollisionManager_Array::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;
-}
-
-
-#ifdef FCL_HAVE_OCTOMAP
-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)
-{
- if(isQuatIdentity(tf2.getQuatRotation()))
- 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)
-{
- if(isQuatIdentity(tf2.getQuatRotation()))
- 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);
-}
-
-#endif
-
-} // dynamic_AABB_tree_array
-
-} // details
-
-
-void DynamicAABBTreeCollisionManager_Array::registerObjects(const std::vector<CollisionObject*>& other_objs)
-{
- if(other_objs.empty()) return;
-
- 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();
-}
-
-
-
-void DynamicAABBTreeCollisionManager_Array::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
-{
- if(size() == 0) return;
- switch(obj->collisionGeometry()->getNodeType())
- {
-#ifdef FCL_HAVE_OCTOMAP
- case GEOM_OCTREE:
- {
- if(!octree_as_geometry_collide)
- {
- const OcTree* octree = static_cast<const OcTree*>(obj->getCollisionGeometry());
- details::dynamic_AABB_tree_array::collisionRecurse(dtree.getNodes(), dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback);
- }
- else
- details::dynamic_AABB_tree_array::collisionRecurse(dtree.getNodes(), dtree.getRoot(), obj, cdata, callback);
- }
- break;
-#endif
- default:
- details::dynamic_AABB_tree_array::collisionRecurse(dtree.getNodes(), dtree.getRoot(), obj, cdata, callback);
- }
-}
-
-void DynamicAABBTreeCollisionManager_Array::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const
-{
- if(size() == 0) return;
- FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
- switch(obj->collisionGeometry()->getNodeType())
- {
-#ifdef FCL_HAVE_OCTOMAP
- case GEOM_OCTREE:
- {
- if(!octree_as_geometry_distance)
- {
- const OcTree* octree = static_cast<const OcTree*>(obj->getCollisionGeometry());
- details::dynamic_AABB_tree_array::distanceRecurse(dtree.getNodes(), dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback, min_dist);
- }
- else
- details::dynamic_AABB_tree_array::distanceRecurse(dtree.getNodes(), dtree.getRoot(), obj, cdata, callback, min_dist);
- }
- break;
-#endif
- default:
- details::dynamic_AABB_tree_array::distanceRecurse(dtree.getNodes(), dtree.getRoot(), obj, cdata, callback, min_dist);
- }
-}
-
-void DynamicAABBTreeCollisionManager_Array::collide(void* cdata, CollisionCallBack callback) const
-{
- if(size() == 0) return;
- details::dynamic_AABB_tree_array::selfCollisionRecurse(dtree.getNodes(), dtree.getRoot(), cdata, callback);
-}
-
-void DynamicAABBTreeCollisionManager_Array::distance(void* cdata, DistanceCallBack callback) const
-{
- if(size() == 0) return;
- FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
- details::dynamic_AABB_tree_array::selfDistanceRecurse(dtree.getNodes(), dtree.getRoot(), cdata, callback, min_dist);
-}
-
-void DynamicAABBTreeCollisionManager_Array::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;
- details::dynamic_AABB_tree_array::collisionRecurse(dtree.getNodes(), dtree.getRoot(), other_manager->dtree.getNodes(), other_manager->dtree.getRoot(), cdata, callback);
-}
-
-void DynamicAABBTreeCollisionManager_Array::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();
- details::dynamic_AABB_tree_array::distanceRecurse(dtree.getNodes(), dtree.getRoot(), other_manager->dtree.getNodes(), other_manager->dtree.getRoot(), cdata, callback, min_dist);
-}
-
-
-
-
-}
diff --git a/src/broadphase/broadphase_interval_tree.cpp b/src/broadphase/broadphase_interval_tree.cpp
deleted file mode 100644
index 7c66c1f20f85e284a62fe6412c49b46a9ea9ebd2..0000000000000000000000000000000000000000
--- a/src/broadphase/broadphase_interval_tree.cpp
+++ /dev/null
@@ -1,655 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011-2014, Willow Garage, Inc.
- * Copyright (c) 2014-2015, Open Source Robotics Foundation
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Open Source Robotics Foundation nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/** \author Jia Pan */
-
-#include <hpp/fcl/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/src/broadphase/broadphase_spatialhash.cpp b/src/broadphase/broadphase_spatialhash.cpp
deleted file mode 100644
index 90f272a54cba5b3ec1b34163f7e71de4209cc0c7..0000000000000000000000000000000000000000
--- a/src/broadphase/broadphase_spatialhash.cpp
+++ /dev/null
@@ -1,43 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011-2014, Willow Garage, Inc.
- * Copyright (c) 2014-2015, Open Source Robotics Foundation
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Open Source Robotics Foundation nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/** \author Jia Pan */
-
-#include <hpp/fcl/broadphase/broadphase_spatialhash.h>
-
-namespace fcl
-{
-
-}
diff --git a/src/broadphase/hierarchy_tree.cpp b/src/broadphase/hierarchy_tree.cpp
deleted file mode 100644
index c738be797e74cf21d12020dd068262cbee5c1d8a..0000000000000000000000000000000000000000
--- a/src/broadphase/hierarchy_tree.cpp
+++ /dev/null
@@ -1,136 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011-2014, Willow Garage, Inc.
- * Copyright (c) 2014-2015, Open Source Robotics Foundation
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Open Source Robotics Foundation nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/** \author Jia Pan */
-
-#include <hpp/fcl/broadphase/hierarchy_tree.h>
-
-namespace fcl
-{
-
-template<>
-size_t select(const NodeBase<AABB>& node, const NodeBase<AABB>& node1, const NodeBase<AABB>& node2)
-{
- const AABB& bv = node.bv;
- const AABB& bv1 = node1.bv;
- const AABB& bv2 = node2.bv;
- Vec3f v = bv.min_ + bv.max_;
- Vec3f v1 = v - (bv1.min_ + bv1.max_);
- Vec3f v2 = v - (bv2.min_ + bv2.max_);
- FCL_REAL d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]);
- FCL_REAL d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]);
- return (d1 < d2) ? 0 : 1;
-}
-
-template<>
-size_t select(const AABB& query, const NodeBase<AABB>& node1, const NodeBase<AABB>& node2)
-{
- const AABB& bv = query;
- const AABB& bv1 = node1.bv;
- const AABB& bv2 = node2.bv;
- Vec3f v = bv.min_ + bv.max_;
- Vec3f v1 = v - (bv1.min_ + bv1.max_);
- Vec3f v2 = v - (bv2.min_ + bv2.max_);
- FCL_REAL d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]);
- FCL_REAL d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]);
- return (d1 < d2) ? 0 : 1;
-}
-
-template<>
-bool HierarchyTree<AABB>::update(NodeBase<AABB>* leaf, const AABB& bv_, const Vec3f& vel, FCL_REAL margin)
-{
- AABB bv(bv_);
- if(leaf->bv.contain(bv)) return false;
- Vec3f marginv(Vec3f::Constant(margin));
- bv.min_ -= marginv;
- bv.max_ += marginv;
- if(vel[0] > 0) bv.max_[0] += vel[0];
- else bv.min_[0] += vel[0];
- if(vel[1] > 0) bv.max_[1] += vel[1];
- else bv.min_[1] += vel[1];
- if(vel[2] > 0) bv.max_[2] += vel[2];
- else bv.max_[2] += vel[2];
- update(leaf, bv);
- return true;
-}
-
-template<>
-bool HierarchyTree<AABB>::update(NodeBase<AABB>* leaf, const AABB& bv_, const Vec3f& vel)
-{
- AABB bv(bv_);
- if(leaf->bv.contain(bv)) return false;
- if(vel[0] > 0) bv.max_[0] += vel[0];
- else bv.min_[0] += vel[0];
- if(vel[1] > 0) bv.max_[1] += vel[1];
- else bv.min_[1] += vel[1];
- if(vel[2] > 0) bv.max_[2] += vel[2];
- else bv.max_[2] += vel[2];
- update(leaf, bv);
- return true;
-}
-
-namespace implementation_array
-{
-template<>
-size_t select(size_t query, size_t node1, size_t node2, NodeBase<AABB>* nodes)
-{
- const AABB& bv = nodes[query].bv;
- const AABB& bv1 = nodes[node1].bv;
- const AABB& bv2 = nodes[node2].bv;
- Vec3f v = bv.min_ + bv.max_;
- Vec3f v1 = v - (bv1.min_ + bv1.max_);
- Vec3f v2 = v - (bv2.min_ + bv2.max_);
- FCL_REAL d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]);
- FCL_REAL d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]);
- return (d1 < d2) ? 0 : 1;
-}
-
-template<>
-size_t select(const AABB& query, size_t node1, size_t node2, NodeBase<AABB>* nodes)
-{
- const AABB& bv = query;
- const AABB& bv1 = nodes[node1].bv;
- const AABB& bv2 = nodes[node2].bv;
- Vec3f v = bv.min_ + bv.max_;
- Vec3f v1 = v - (bv1.min_ + bv1.max_);
- Vec3f v2 = v - (bv2.min_ + bv2.max_);
- FCL_REAL d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]);
- FCL_REAL d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]);
- return (d1 < d2) ? 0 : 1;
-}
-
-}
-
-}
diff --git a/src/broadphase/interval_tree.cpp b/src/broadphase/interval_tree.cpp
deleted file mode 100644
index b5e84d45c7685f26e0523fc4902367027d1d935a..0000000000000000000000000000000000000000
--- a/src/broadphase/interval_tree.cpp
+++ /dev/null
@@ -1,566 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011-2014, Willow Garage, Inc.
- * Copyright (c) 2014-2015, Open Source Robotics Foundation
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Open Source Robotics Foundation nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/** \author Jia Pan */
-
-#include <hpp/fcl/broadphase/interval_tree.h>
-#include <iostream>
-#include <cstdlib>
-
-
-namespace fcl
-{
-
-IntervalTreeNode::IntervalTreeNode(){}
-
-IntervalTreeNode::IntervalTreeNode(SimpleInterval* new_interval) :
- 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;
- nil->left = nil->right = nil->parent = nil;
- nil->red = false;
- nil->key = nil->high = nil->max_high = -std::numeric_limits<double>::max();
- nil->stored_interval = NULL;
-
- root = new IntervalTreeNode;
- root->parent = root->left = root->right = nil;
- root->key = root->high = root->max_high = std::numeric_limits<double>::max();
- root->red = false;
- root->stored_interval = NULL;
-
- /// 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;
- recursion_node_stack[0].start_node = NULL;
-}
-
-IntervalTree::~IntervalTree()
-{
- IntervalTreeNode* x = root->left;
- std::deque<IntervalTreeNode*> nodes_to_free;
-
- if(x != nil)
- {
- if(x->left != nil)
- {
- nodes_to_free.push_back(x->left);
- }
- if(x->right != nil)
- {
- nodes_to_free.push_back(x->right);
- }
-
- delete x;
- while( nodes_to_free.size() > 0)
- {
- x = nodes_to_free.back();
- nodes_to_free.pop_back();
- if(x->left != nil)
- {
- nodes_to_free.push_back(x->left);
- }
- if(x->right != nil)
- {
- nodes_to_free.push_back(x->right);
- }
- delete x;
- }
- }
- delete nil;
- delete root;
- free(recursion_node_stack);
-}
-
-
-void IntervalTree::leftRotate(IntervalTreeNode* x)
-{
- IntervalTreeNode* y;
-
- y = x->right;
- x->right = y->left;
-
- if(y->left != nil) y->left->parent = x;
-
- y->parent = x->parent;
-
- if(x == x->parent->left)
- x->parent->left = y;
- else
- x->parent->right = y;
-
- y->left = x;
- x->parent = y;
-
- x->max_high = std::max(x->left->max_high, std::max(x->right->max_high, x->high));
- y->max_high = std::max(x->max_high, std::max(y->right->max_high, y->high));
-}
-
-
-void IntervalTree::rightRotate(IntervalTreeNode* y)
-{
- IntervalTreeNode* x;
-
- x = y->left;
- y->left = x->right;
-
- if(nil != x->right) x->right->parent = y;
-
- x->parent = y->parent;
- if(y == y->parent->left)
- y->parent->left = x;
- else
- y->parent->right = x;
-
- x->right = y;
- y->parent = x;
-
- y->max_high = std::max(y->left->max_high, std::max(y->right->max_high, y->high));
- x->max_high = std::max(x->left->max_high, std::max(y->max_high, x->high));
-}
-
-
-
-/// @brief Inserts z into the tree as if it were a regular binary tree
-void IntervalTree::recursiveInsert(IntervalTreeNode* z)
-{
- IntervalTreeNode* x;
- IntervalTreeNode* y;
-
- z->left = z->right = nil;
- y = root;
- x = root->left;
- while(x != nil)
- {
- y = x;
- if(x->key > z->key)
- x = x->left;
- else
- x = x->right;
- }
- z->parent = y;
- if((y == root) || (y->key > z->key))
- y->left = z;
- else
- y->right = z;
-}
-
-
-void IntervalTree::fixupMaxHigh(IntervalTreeNode* x)
-{
- while(x != root)
- {
- x->max_high = std::max(x->high, std::max(x->left->max_high, x->right->max_high));
- x = x->parent;
- }
-}
-
-IntervalTreeNode* IntervalTree::insert(SimpleInterval* new_interval)
-{
- IntervalTreeNode* y;
- IntervalTreeNode* x;
- IntervalTreeNode* new_node;
-
- x = new IntervalTreeNode(new_interval);
- recursiveInsert(x);
- fixupMaxHigh(x->parent);
- new_node = x;
- x->red = true;
- while(x->parent->red)
- {
- /// use sentinel instead of checking for root
- if(x->parent == x->parent->parent->left)
- {
- y = x->parent->parent->right;
- if(y->red)
- {
- x->parent->red = true;
- y->red = true;
- x->parent->parent->red = true;
- x = x->parent->parent;
- }
- else
- {
- if(x == x->parent->right)
- {
- x = x->parent;
- leftRotate(x);
- }
- x->parent->red = false;
- x->parent->parent->red = true;
- rightRotate(x->parent->parent);
- }
- }
- else
- {
- y = x->parent->parent->left;
- if(y->red)
- {
- x->parent->red = false;
- y->red = false;
- x->parent->parent->red = true;
- x = x->parent->parent;
- }
- else
- {
- if(x == x->parent->left)
- {
- x = x->parent;
- rightRotate(x);
- }
- x->parent->red = false;
- x->parent->parent->red = true;
- leftRotate(x->parent->parent);
- }
- }
- }
- root->left->red = false;
- return new_node;
-}
-
-IntervalTreeNode* IntervalTree::getSuccessor(IntervalTreeNode* x) const
-{
- IntervalTreeNode* y;
-
- if(nil != (y = x->right))
- {
- while(y->left != nil)
- y = y->left;
- return y;
- }
- else
- {
- y = x->parent;
- while(x == y->right)
- {
- x = y;
- y = y->parent;
- }
- if(y == root) return nil;
- return y;
- }
-}
-
-
-IntervalTreeNode* IntervalTree::getPredecessor(IntervalTreeNode* x) const
-{
- IntervalTreeNode* y;
-
- if(nil != (y = x->left))
- {
- while(y->right != nil)
- y = y->right;
- return y;
- }
- else
- {
- y = x->parent;
- while(x == y->left)
- {
- if(y == root) return nil;
- x = y;
- y = y->parent;
- }
- return y;
- }
-}
-
-void IntervalTreeNode::print(IntervalTreeNode* nil, IntervalTreeNode* root) const
-{
- stored_interval->print();
- std::cout << ", k = " << key << ", h = " << high << ", mH = " << max_high;
- std::cout << " l->key = ";
- if(left == nil) std::cout << "NULL"; else std::cout << left->key;
- std::cout << " r->key = ";
- if(right == nil) std::cout << "NULL"; else std::cout << right->key;
- std::cout << " p->key = ";
- if(parent == root) std::cout << "NULL"; else std::cout << parent->key;
- std::cout << " red = " << (int)red << std::endl;
-}
-
-void IntervalTree::recursivePrint(IntervalTreeNode* x) const
-{
- if(x != nil)
- {
- recursivePrint(x->left);
- x->print(nil,root);
- recursivePrint(x->right);
- }
-}
-
-
-void IntervalTree::print() const
-{
- recursivePrint(root->left);
-}
-
-void IntervalTree::deleteFixup(IntervalTreeNode* x)
-{
- IntervalTreeNode* w;
- IntervalTreeNode* root_left_node = root->left;
-
- while((!x->red) && (root_left_node != x))
- {
- if(x == x->parent->left)
- {
- w = x->parent->right;
- if(w->red)
- {
- w->red = false;
- x->parent->red = true;
- leftRotate(x->parent);
- w = x->parent->right;
- }
- if((!w->right->red) && (!w->left->red))
- {
- w->red = true;
- x = x->parent;
- }
- else
- {
- if(!w->right->red)
- {
- w->left->red = false;
- w->red = true;
- rightRotate(w);
- w = x->parent->right;
- }
- w->red = x->parent->red;
- x->parent->red = false;
- w->right->red = false;
- leftRotate(x->parent);
- x = root_left_node;
- }
- }
- else
- {
- w = x->parent->left;
- if(w->red)
- {
- w->red = false;
- x->parent->red = true;
- rightRotate(x->parent);
- w = x->parent->left;
- }
- if((!w->right->red) && (!w->left->red))
- {
- w->red = true;
- x = x->parent;
- }
- else
- {
- if(!w->left->red)
- {
- w->right->red = false;
- w->red = true;
- leftRotate(w);
- w = x->parent->left;
- }
- w->red = x->parent->red;
- x->parent->red = false;
- w->left->red = false;
- rightRotate(x->parent);
- x = root_left_node;
- }
- }
- }
- x->red = false;
-}
-
-void IntervalTree::deleteNode(SimpleInterval* ivl)
-{
- IntervalTreeNode* node = recursiveSearch(root, ivl);
- if(node)
- deleteNode(node);
-}
-
-IntervalTreeNode* IntervalTree::recursiveSearch(IntervalTreeNode* node, SimpleInterval* ivl) const
-{
- if(node != nil)
- {
- if(node->stored_interval == ivl)
- return node;
-
- IntervalTreeNode* left = recursiveSearch(node->left, ivl);
- if(left != nil) return left;
- IntervalTreeNode* right = recursiveSearch(node->right, ivl);
- if(right != nil) return right;
- }
-
- return nil;
-}
-
-SimpleInterval* IntervalTree::deleteNode(IntervalTreeNode* z)
-{
- IntervalTreeNode* y;
- IntervalTreeNode* x;
- SimpleInterval* node_to_delete = z->stored_interval;
-
- y= ((z->left == nil) || (z->right == nil)) ? z : getSuccessor(z);
- x= (y->left == nil) ? y->right : y->left;
- if(root == (x->parent = y->parent))
- {
- root->left = x;
- }
- else
- {
- if(y == y->parent->left)
- {
- y->parent->left = x;
- }
- else
- {
- y->parent->right = x;
- }
- }
-
- /// @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();
- y->left = z->left;
- y->right = z->right;
- y->parent = z->parent;
- z->left->parent = z->right->parent = y;
- if(z == z->parent->left)
- z->parent->left = y;
- else
- z->parent->right = y;
-
- fixupMaxHigh(x->parent);
- if(!(y->red))
- {
- y->red = z->red;
- deleteFixup(x);
- }
- else
- y->red = z->red;
- delete z;
- }
- else
- {
- fixupMaxHigh(x->parent);
- if(!(y->red)) deleteFixup(x);
- delete y;
- }
-
- return node_to_delete;
-}
-
-/// @brief returns 1 if the intervals overlap, and 0 otherwise
-bool overlap(double a1, double a2, double b1, double b2)
-{
- if(a1 <= b1)
- {
- return (b1 <= a2);
- }
- else
- {
- return (a1 <= b2);
- }
-}
-
-std::deque<SimpleInterval*> IntervalTree::query(double low, double high)
-{
- std::deque<SimpleInterval*> result_stack;
- IntervalTreeNode* x = root->left;
- bool run = (x != nil);
-
- current_parent = 0;
-
- while(run)
- {
- if(overlap(low,high,x->key,x->high))
- {
- result_stack.push_back(x->stored_interval);
- recursion_node_stack[current_parent].try_right_branch = true;
- }
- if(x->left->max_high >= low)
- {
- if(recursion_node_stack_top == recursion_node_stack_size)
- {
- recursion_node_stack_size *= 2;
- recursion_node_stack = (it_recursion_node *)realloc(recursion_node_stack, recursion_node_stack_size * sizeof(it_recursion_node));
- if(recursion_node_stack == NULL)
- exit(1);
- }
- recursion_node_stack[recursion_node_stack_top].start_node = x;
- recursion_node_stack[recursion_node_stack_top].try_right_branch = false;
- recursion_node_stack[recursion_node_stack_top].parent_index = current_parent;
- current_parent = recursion_node_stack_top++;
- x = x->left;
- }
- else
- x = x->right;
-
- run = (x != nil);
- while((!run) && (recursion_node_stack_top > 1))
- {
- if(recursion_node_stack[--recursion_node_stack_top].try_right_branch)
- {
- x=recursion_node_stack[recursion_node_stack_top].start_node->right;
- current_parent=recursion_node_stack[recursion_node_stack_top].parent_index;
- recursion_node_stack[current_parent].try_right_branch = true;
- run = (x != nil);
- }
- }
- }
- return result_stack;
-}
-
-}