include/coal/BVH/BVH_utility.h

+/*
+ * 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 COAL_BVH_UTILITY_H
+#define COAL_BVH_UTILITY_H
+
+#include "coal/BVH/BVH_model.h"
+
+namespace coal {
+/// @brief Extract the part of the BVHModel that is inside an AABB.
+/// A triangle in collision with the AABB is considered inside.
+template <typename BV>
+COAL_DLLAPI BVHModel<BV>* BVHExtract(const BVHModel<BV>& model,
+                                     const Transform3s& pose, const AABB& aabb);
+
+template <>
+COAL_DLLAPI BVHModel<OBB>* BVHExtract(const BVHModel<OBB>& model,
+                                      const Transform3s& pose,
+                                      const AABB& aabb);
+template <>
+COAL_DLLAPI BVHModel<AABB>* BVHExtract(const BVHModel<AABB>& model,
+                                       const Transform3s& pose,
+                                       const AABB& aabb);
+template <>
+COAL_DLLAPI BVHModel<RSS>* BVHExtract(const BVHModel<RSS>& model,
+                                      const Transform3s& pose,
+                                      const AABB& aabb);
+template <>
+COAL_DLLAPI BVHModel<kIOS>* BVHExtract(const BVHModel<kIOS>& model,
+                                       const Transform3s& pose,
+                                       const AABB& aabb);
+template <>
+COAL_DLLAPI BVHModel<OBBRSS>* BVHExtract(const BVHModel<OBBRSS>& model,
+                                         const Transform3s& pose,
+                                         const AABB& aabb);
+template <>
+COAL_DLLAPI BVHModel<KDOP<16> >* BVHExtract(const BVHModel<KDOP<16> >& model,
+                                            const Transform3s& pose,
+                                            const AABB& aabb);
+template <>
+COAL_DLLAPI BVHModel<KDOP<18> >* BVHExtract(const BVHModel<KDOP<18> >& model,
+                                            const Transform3s& pose,
+                                            const AABB& aabb);
+template <>
+COAL_DLLAPI BVHModel<KDOP<24> >* BVHExtract(const BVHModel<KDOP<24> >& model,
+                                            const Transform3s& pose,
+                                            const AABB& aabb);
+
+/// @brief Compute the covariance matrix for a set or subset of points. if ts =
+/// null, then indices refer to points directly; otherwise refer to triangles
+COAL_DLLAPI void getCovariance(Vec3s* ps, Vec3s* ps2, Triangle* ts,
+                               unsigned int* indices, unsigned int n,
+                               Matrix3s& M);
+
+/// @brief Compute the RSS bounding volume parameters: radius, rectangle size
+/// and the origin, given the BV axises.
+COAL_DLLAPI void getRadiusAndOriginAndRectangleSize(
+    Vec3s* ps, Vec3s* ps2, Triangle* ts, unsigned int* indices, unsigned int n,
+    const Matrix3s& axes, Vec3s& origin, CoalScalar l[2], CoalScalar& r);
+
+/// @brief Compute the bounding volume extent and center for a set or subset of
+/// points, given the BV axises.
+COAL_DLLAPI void getExtentAndCenter(Vec3s* ps, Vec3s* ps2, Triangle* ts,
+                                    unsigned int* indices, unsigned int n,
+                                    Matrix3s& axes, Vec3s& center,
+                                    Vec3s& extent);
+
+/// @brief Compute the center and radius for a triangle's circumcircle
+COAL_DLLAPI void circumCircleComputation(const Vec3s& a, const Vec3s& b,
+                                         const Vec3s& c, Vec3s& center,
+                                         CoalScalar& radius);
+
+/// @brief Compute the maximum distance from a given center point to a point
+/// cloud
+COAL_DLLAPI CoalScalar maximumDistance(Vec3s* ps, Vec3s* ps2, Triangle* ts,
+                                       unsigned int* indices, unsigned int n,
+                                       const Vec3s& query);
+
+}  // namespace coal
+
+#endif

include/coal/broadphase/broadphase.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2022, Inria
+ *  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.
+ */
+
+#ifndef COAL_BROADPHASE_BROADPHASE_H
+#define COAL_BROADPHASE_BROADPHASE_H
+
+#include "coal/broadphase/broadphase_dynamic_AABB_tree.h"
+#include "coal/broadphase/broadphase_dynamic_AABB_tree_array.h"
+#include "coal/broadphase/broadphase_bruteforce.h"
+#include "coal/broadphase/broadphase_SaP.h"
+#include "coal/broadphase/broadphase_SSaP.h"
+#include "coal/broadphase/broadphase_interval_tree.h"
+#include "coal/broadphase/broadphase_spatialhash.h"
+
+#include "coal/broadphase/default_broadphase_callbacks.h"
+
+#endif  // ifndef COAL_BROADPHASE_BROADPHASE_H

include/coal/broadphase/broadphase_SSaP.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, 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 COAL_BROAD_PHASE_SSAP_H
+#define COAL_BROAD_PHASE_SSAP_H
+
+#include <vector>
+#include "coal/broadphase/broadphase_collision_manager.h"
+
+namespace coal {
+
+/// @brief Simple SAP collision manager
+class COAL_DLLAPI SSaPCollisionManager : public BroadPhaseCollisionManager {
+ public:
+  typedef BroadPhaseCollisionManager Base;
+  using Base::getObjects;
+
+  SSaPCollisionManager();
+
+  /// @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
+  virtual 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, CollisionCallBackBase* callback) const;
+
+  /// @brief perform distance computation between one object and all the objects
+  /// belonging to the manager
+  void distance(CollisionObject* obj, DistanceCallBackBase* callback) const;
+
+  /// @brief perform collision test for the objects belonging to the manager
+  /// (i.e., N^2 self collision)
+  void collide(CollisionCallBackBase* callback) const;
+
+  /// @brief perform distance test for the objects belonging to the manager
+  /// (i.e., N^2 self distance)
+  void distance(DistanceCallBackBase* callback) const;
+
+  /// @brief perform collision test with objects belonging to another manager
+  void collide(BroadPhaseCollisionManager* other_manager,
+               CollisionCallBackBase* callback) const;
+
+  /// @brief perform distance test with objects belonging to another manager
+  void distance(BroadPhaseCollisionManager* other_manager,
+                DistanceCallBackBase* callback) const;
+
+  /// @brief whether the manager is empty
+  bool empty() const;
+
+  /// @brief the number of objects managed by the manager
+  size_t size() const;
+
+ protected:
+  /// @brief check collision between one object and a list of objects, return
+  /// value is whether stop is possible
+  bool checkColl(
+      typename std::vector<CollisionObject*>::const_iterator pos_start,
+      typename std::vector<CollisionObject*>::const_iterator pos_end,
+      CollisionObject* obj, CollisionCallBackBase* callback) const;
+
+  /// @brief check distance between one object and a list of objects, return
+  /// value is whether stop is possible
+  bool checkDis(
+      typename std::vector<CollisionObject*>::const_iterator pos_start,
+      typename std::vector<CollisionObject*>::const_iterator pos_end,
+      CollisionObject* obj, DistanceCallBackBase* callback,
+      CoalScalar& min_dist) const;
+
+  bool collide_(CollisionObject* obj, CollisionCallBackBase* callback) const;
+
+  bool distance_(CollisionObject* obj, DistanceCallBackBase* callback,
+                 CoalScalar& min_dist) const;
+
+  static int selectOptimalAxis(
+      const std::vector<CollisionObject*>& objs_x,
+      const std::vector<CollisionObject*>& objs_y,
+      const std::vector<CollisionObject*>& objs_z,
+      typename std::vector<CollisionObject*>::const_iterator& it_beg,
+      typename std::vector<CollisionObject*>::const_iterator& it_end);
+
+  /// @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_;
+};
+
+}  // namespace coal
+
+#endif

include/coal/broadphase/broadphase_SaP.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, 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 COAL_BROAD_PHASE_SAP_H
+#define COAL_BROAD_PHASE_SAP_H
+
+#include <map>
+#include <list>
+
+#include "coal/broadphase/broadphase_collision_manager.h"
+
+namespace coal {
+
+/// @brief Rigorous SAP collision manager
+class COAL_DLLAPI SaPCollisionManager : public BroadPhaseCollisionManager {
+ public:
+  typedef BroadPhaseCollisionManager Base;
+  using Base::getObjects;
+
+  SaPCollisionManager();
+
+  ~SaPCollisionManager();
+
+  /// @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
+  virtual 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, CollisionCallBackBase* callback) const;
+
+  /// @brief perform distance computation between one object and all the objects
+  /// belonging to the manager
+  void distance(CollisionObject* obj, DistanceCallBackBase* callback) const;
+
+  /// @brief perform collision test for the objects belonging to the manager
+  /// (i.e., N^2 self collision)
+  void collide(CollisionCallBackBase* callback) const;
+
+  /// @brief perform distance test for the objects belonging to the manager
+  /// (i.e., N^2 self distance)
+  void distance(DistanceCallBackBase* callback) const;
+
+  /// @brief perform collision test with objects belonging to another manager
+  void collide(BroadPhaseCollisionManager* other_manager,
+               CollisionCallBackBase* callback) const;
+
+  /// @brief perform distance test with objects belonging to another manager
+  void distance(BroadPhaseCollisionManager* other_manager,
+                DistanceCallBackBase* callback) const;
+
+  /// @brief whether the manager is empty
+  bool empty() const;
+
+  /// @brief the number of objects managed by the manager
+  size_t size() const;
+
+ 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
+    const Vec3s& getVal() const;
+
+    /// @brief set the value of the end point
+    Vec3s& getVal();
+
+    CoalScalar getVal(int i) const;
+
+    CoalScalar& getVal(int i);
+  };
+
+  /// @brief A pair of objects that are not culling away and should further
+  /// check collision
+  struct SaPPair {
+    SaPPair(CollisionObject* a, CollisionObject* b);
+
+    CollisionObject* obj1;
+    CollisionObject* obj2;
+
+    bool operator==(const SaPPair& other) const;
+  };
+
+  /// @brief Functor to help unregister one object
+  class COAL_DLLAPI isUnregistered {
+    CollisionObject* obj;
+
+   public:
+    isUnregistered(CollisionObject* obj_);
+
+    bool operator()(const SaPPair& pair) const;
+  };
+
+  /// @brief Functor to help remove collision pairs no longer valid (i.e.,
+  /// should be culled away)
+  class COAL_DLLAPI isNotValidPair {
+    CollisionObject* obj1;
+    CollisionObject* obj2;
+
+   public:
+    isNotValidPair(CollisionObject* obj1_, CollisionObject* obj2_);
+
+    bool operator()(const SaPPair& pair);
+  };
+
+  void update_(SaPAABB* updated_aabb);
+
+  void updateVelist();
+
+  /// @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;
+
+  int optimal_axis;
+
+  std::map<CollisionObject*, SaPAABB*> obj_aabb_map;
+
+  bool distance_(CollisionObject* obj, DistanceCallBackBase* callback,
+                 CoalScalar& min_dist) const;
+
+  bool collide_(CollisionObject* obj, CollisionCallBackBase* callback) const;
+
+  void addToOverlapPairs(const SaPPair& p);
+
+  void removeFromOverlapPairs(const SaPPair& p);
+};
+
+}  // namespace coal
+
+#endif

include/coal/broadphase/broadphase_bruteforce.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, 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 COAL_BROAD_PHASE_BRUTE_FORCE_H
+#define COAL_BROAD_PHASE_BRUTE_FORCE_H
+
+#include <list>
+#include "coal/broadphase/broadphase_collision_manager.h"
+
+namespace coal {
+
+/// @brief Brute force N-body collision manager
+class COAL_DLLAPI NaiveCollisionManager : public BroadPhaseCollisionManager {
+ public:
+  typedef BroadPhaseCollisionManager Base;
+  using Base::getObjects;
+
+  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
+  virtual 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, CollisionCallBackBase* callback) const;
+
+  /// @brief perform distance computation between one object and all the objects
+  /// belonging to the manager
+  void distance(CollisionObject* obj, DistanceCallBackBase* callback) const;
+
+  /// @brief perform collision test for the objects belonging to the manager
+  /// (i.e., N^2 self collision)
+  void collide(CollisionCallBackBase* callback) const;
+
+  /// @brief perform distance test for the objects belonging to the manager
+  /// (i.e., N^2 self distance)
+  void distance(DistanceCallBackBase* callback) const;
+
+  /// @brief perform collision test with objects belonging to another manager
+  void collide(BroadPhaseCollisionManager* other_manager,
+               CollisionCallBackBase* callback) const;
+
+  /// @brief perform distance test with objects belonging to another manager
+  void distance(BroadPhaseCollisionManager* other_manager,
+                DistanceCallBackBase* callback) const;
+
+  /// @brief whether the manager is empty
+  bool empty() const;
+
+  /// @brief the number of objects managed by the manager
+  size_t size() const;
+
+ protected:
+  /// @brief objects belonging to the manager are stored in a list structure
+  std::list<CollisionObject*> objs;
+};
+
+}  // namespace coal
+
+#endif

src/distance_box_halfspace.cpp → include/coal/broadphase/broadphase_callbacks.h

 /*
  * Software License Agreement (BSD License)
  *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, Open Source Robotics Foundation
- *  Copyright (c) 2018-2019, Center National de la Recherche Scientifique
+ *  Copyright (c) 2022, INRIA
  *  All rights reserved.
  *
  *  Redistribution and use in source and binary forms, with or without
@@ -16,7 +14,7 @@
  *     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
+ *   * Neither the name of the copyright holder nor the names of its
  *     contributors may be used to endorse or promote products derived
  *     from this software without specific prior written permission.
  *
@@ -34,52 +32,65 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  */
 
-/** \author Florent Lamiraux */
+/** @author Justin Carpentier (justin.carpentier@inria.fr) */
 
-#include <cmath>
-#include <limits>
-#include <hpp/fcl/math/transform.h>
-#include <hpp/fcl/shape/geometric_shapes.h>
-#include <../src/distance_func_matrix.h>
-#include "narrowphase/details.h"
+#ifndef COAL_BROADPHASE_BROAD_PHASE_CALLBACKS_H
+#define COAL_BROADPHASE_BROAD_PHASE_CALLBACKS_H
 
-namespace hpp
-{
-namespace fcl {
-    class GJKSolver;
+#include "coal/fwd.hh"
+#include "coal/data_types.h"
 
-  template <>
-  FCL_REAL ShapeShapeDistance <Box, Halfspace>
-  (const CollisionGeometry* o1, const Transform3f& tf1,
-   const CollisionGeometry* o2, const Transform3f& tf2,
-   const GJKSolver*, const DistanceRequest&,
-   DistanceResult& result)
-  {
-    const Box& s1 = static_cast <const Box&> (*o1);
-    const Halfspace& s2 = static_cast <const Halfspace&> (*o2);
-    details::boxHalfspaceIntersect
-      (s1, tf1, s2, tf2, result.min_distance, result.nearest_points [0],
-       result.nearest_points [1], result.normal);
-    result.o1 = o1; result.o2 = o2; result.b1 = -1; result.b2 = -1;
-    return result.min_distance;
+namespace coal {
+
+/// @brief Base callback class for collision queries.
+/// This class can be supersed by child classes to provide desired behaviors
+/// according to the application (e.g, only listing the potential
+/// CollisionObjects in collision).
+struct COAL_DLLAPI CollisionCallBackBase {
+  /// @brief Initialization of the callback before running the collision
+  /// broadphase manager.
+  virtual void init() {};
+
+  /// @brief Collision evaluation between two objects in collision.
+  ///        This callback will cause the broadphase evaluation to stop if it
+  ///        returns true.
+  ///
+  /// @param[in] o1 Collision object #1.
+  /// @param[in] o2 Collision object #2.
+  virtual bool collide(CollisionObject* o1, CollisionObject* o2) = 0;
+
+  /// @brief Functor call associated to the collide operation.
+  virtual bool operator()(CollisionObject* o1, CollisionObject* o2) {
+    return collide(o1, o2);
   }
+};
 
-  template <>
-  FCL_REAL ShapeShapeDistance <Halfspace, Box>
-  (const CollisionGeometry* o1, const Transform3f& tf1,
-   const CollisionGeometry* o2, const Transform3f& tf2,
-   const GJKSolver*, const DistanceRequest&,
-   DistanceResult& result)
-  {
-    const Halfspace& s1 = static_cast <const Halfspace&> (*o1);
-    const Box& s2 = static_cast <const Box&> (*o2);
-    details::boxHalfspaceIntersect
-      (s2, tf2, s1, tf1, result.min_distance, result.nearest_points [1],
-       result.nearest_points [0], result.normal);
-    result.o1 = o1; result.o2 = o2; result.b1 = -1; result.b2 = -1;
-    result.normal = -result.normal;
-    return result.min_distance;
+/// @brief Base callback class for distance queries.
+/// This class can be supersed by child classes to provide desired behaviors
+/// according to the application (e.g, only listing the potential
+/// CollisionObjects in collision).
+struct COAL_DLLAPI DistanceCallBackBase {
+  /// @brief Initialization of the callback before running the collision
+  /// broadphase manager.
+  virtual void init() {};
+
+  /// @brief Distance evaluation between two objects in collision.
+  ///        This callback will cause the broadphase evaluation to stop if it
+  ///        returns true.
+  ///
+  /// @param[in] o1 Collision object #1.
+  /// @param[in] o2 Collision object #2.
+  /// @param[out] dist Distance between the two collision geometries.
+  virtual bool distance(CollisionObject* o1, CollisionObject* o2,
+                        CoalScalar& dist) = 0;
+
+  /// @brief Functor call associated to the distance operation.
+  virtual bool operator()(CollisionObject* o1, CollisionObject* o2,
+                          CoalScalar& dist) {
+    return distance(o1, o2, dist);
   }
-} // namespace fcl
+};
+
+}  // namespace coal
 
-} // namespace hpp
+#endif  // COAL_BROADPHASE_BROAD_PHASE_CALLBACKS_H

include/coal/broadphase/broadphase_collision_manager.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, 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 COAL_BROADPHASE_BROADPHASECOLLISIONMANAGER_H
+#define COAL_BROADPHASE_BROADPHASECOLLISIONMANAGER_H
+
+#include <set>
+#include <vector>
+#include <boost/function.hpp>
+
+#include "coal/collision_object.h"
+#include "coal/broadphase/broadphase_callbacks.h"
+
+namespace coal {
+
+/// @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 COAL_DLLAPI BroadPhaseCollisionManager {
+ public:
+  BroadPhaseCollisionManager();
+
+  virtual ~BroadPhaseCollisionManager();
+
+  /// @brief add objects to the manager
+  virtual void registerObjects(const std::vector<CollisionObject*>& other_objs);
+
+  /// @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);
+
+  /// @brief update the manager by explicitly given the set of objects update
+  virtual void update(const std::vector<CollisionObject*>& updated_objs);
+
+  /// @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 return the objects managed by the manager
+  virtual std::vector<CollisionObject*> getObjects() const {
+    std::vector<CollisionObject*> res(size());
+    getObjects(res);
+    return res;
+  };
+
+  /// @brief perform collision test between one object and all the objects
+  /// belonging to the manager
+  virtual void collide(CollisionObject* obj,
+                       CollisionCallBackBase* callback) const = 0;
+
+  /// @brief perform distance computation between one object and all the objects
+  /// belonging to the manager
+  virtual void distance(CollisionObject* obj,
+                        DistanceCallBackBase* callback) const = 0;
+
+  /// @brief perform collision test for the objects belonging to the manager
+  /// (i.e., N^2 self collision)
+  virtual void collide(CollisionCallBackBase* callback) const = 0;
+
+  /// @brief perform distance test for the objects belonging to the manager
+  /// (i.e., N^2 self distance)
+  virtual void distance(DistanceCallBackBase* callback) const = 0;
+
+  /// @brief perform collision test with objects belonging to another manager
+  virtual void collide(BroadPhaseCollisionManager* other_manager,
+                       CollisionCallBackBase* callback) const = 0;
+
+  /// @brief perform distance test with objects belonging to another manager
+  virtual void distance(BroadPhaseCollisionManager* other_manager,
+                        DistanceCallBackBase* 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;
+
+  void insertTestedSet(CollisionObject* a, CollisionObject* b) const;
+};
+
+}  // namespace coal
+
+#endif

include/coal/broadphase/broadphase_continuous_collision_manager-inl.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, 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 COAL_BROADPHASE_BROADPHASECONTINUOUSCOLLISIONMANAGER_INL_H
+#define COAL_BROADPHASE_BROADPHASECONTINUOUSCOLLISIONMANAGER_INL_H
+
+#include "coal/broadphase/broadphase_continuous_collision_manager.h"
+#include <algorithm>
+
+namespace coal {
+
+//==============================================================================
+BroadPhaseContinuousCollisionManager::BroadPhaseContinuousCollisionManager() {
+  // Do nothing
+}
+
+//==============================================================================
+
+BroadPhaseContinuousCollisionManager::~BroadPhaseContinuousCollisionManager() {
+  // Do nothing
+}
+
+//==============================================================================
+
+void BroadPhaseContinuousCollisionManager::registerObjects(
+    const std::vector<ContinuousCollisionObject*>& other_objs) {
+  for (size_t i = 0; i < other_objs.size(); ++i) registerObject(other_objs[i]);
+}
+
+//==============================================================================
+
+void BroadPhaseContinuousCollisionManager::update(
+    ContinuousCollisionObject* updated_obj) {
+  COAL_UNUSED_VARIABLE(updated_obj);
+
+  update();
+}
+
+//==============================================================================
+
+void BroadPhaseContinuousCollisionManager::update(
+    const std::vector<ContinuousCollisionObject*>& updated_objs) {
+  COAL_UNUSED_VARIABLE(updated_objs);
+
+  update();
+}
+
+}  // namespace coal
+
+#endif

include/coal/broadphase/broadphase_continuous_collision_manager.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, 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 COAL_BROADPHASE_BROADPHASECONTINUOUSCOLLISIONMANAGER_H
+#define COAL_BROADPHASE_BROADPHASECONTINUOUSCOLLISIONMANAGER_H
+
+#include "coal/broadphase/broadphase_collision_manager.h"
+#include "coal/collision_object.h"
+#include "coal/narrowphase/continuous_collision_object.h"
+
+namespace coal {
+
+/// @brief Callback for continuous collision between two objects. Return value
+/// is whether can stop now.
+template <typename S>
+using ContinuousCollisionCallBack = bool (*)(ContinuousCollisionObject* o1,
+                                             ContinuousCollisionObject* o2,
+                                             void* cdata);
+
+/// @brief Callback for continuous distance between two objects, Return value is
+/// whether can stop now, also return the minimum distance till now.
+template <typename S>
+using ContinuousDistanceCallBack = bool (*)(ContinuousCollisionObject* o1,
+                                            ContinuousCollisionObject* o2,
+                                            S& dist);
+
+/// @brief Base class for broad phase continuous collision. It helps to
+/// accelerate the continuous collision/distance between N objects. Also support
+/// self collision, self distance and collision/distance with another M objects.
+template <typename S>
+class COAL_DLLAPI BroadPhaseContinuousCollisionManager {
+ public:
+  BroadPhaseContinuousCollisionManager();
+
+  virtual ~BroadPhaseContinuousCollisionManager();
+
+  /// @brief add objects to the manager
+  virtual void registerObjects(
+      const std::vector<ContinuousCollisionObject*>& other_objs);
+
+  /// @brief add one object to the manager
+  virtual void registerObject(ContinuousCollisionObject* obj) = 0;
+
+  /// @brief remove one object from the manager
+  virtual void unregisterObject(ContinuousCollisionObject* 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(ContinuousCollisionObject* updated_obj);
+
+  /// @brief update the manager by explicitly given the set of objects update
+  virtual void update(
+      const std::vector<ContinuousCollisionObject*>& updated_objs);
+
+  /// @brief clear the manager
+  virtual void clear() = 0;
+
+  /// @brief return the objects managed by the manager
+  virtual void getObjects(
+      std::vector<ContinuousCollisionObject*>& objs) const = 0;
+
+  /// @brief perform collision test between one object and all the objects
+  /// belonging to the manager
+  virtual void collide(ContinuousCollisionObject* obj,
+                       CollisionCallBackBase* callback) const = 0;
+
+  /// @brief perform distance computation between one object and all the objects
+  /// belonging to the manager
+  virtual void distance(ContinuousCollisionObject* obj,
+                        DistanceCallBackBase* callback) const = 0;
+
+  /// @brief perform collision test for the objects belonging to the manager
+  /// (i.e., N^2 self collision)
+  virtual void collide(CollisionCallBackBase* callback) const = 0;
+
+  /// @brief perform distance test for the objects belonging to the manager
+  /// (i.e., N^2 self distance)
+  virtual void distance(DistanceCallBackBase* callback) const = 0;
+
+  /// @brief perform collision test with objects belonging to another manager
+  virtual void collide(BroadPhaseContinuousCollisionManager* other_manager,
+                       CollisionCallBackBase* callback) const = 0;
+
+  /// @brief perform distance test with objects belonging to another manager
+  virtual void distance(BroadPhaseContinuousCollisionManager* other_manager,
+                        DistanceCallBackBase* 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;
+};
+
+using BroadPhaseContinuousCollisionManagerf =
+    BroadPhaseContinuousCollisionManager<float>;
+using BroadPhaseContinuousCollisionManagerd =
+    BroadPhaseContinuousCollisionManager<CoalScalar>;
+
+}  // namespace coal
+
+#include "coal/broadphase/broadphase_continuous_collision_manager-inl.h"
+
+#endif

include/coal/broadphase/broadphase_dynamic_AABB_tree-inl.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, 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 COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_INL_H
+#define COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_INL_H
+
+#include "coal/broadphase/broadphase_dynamic_AABB_tree.h"
+
+#include <limits>
+
+#if COAL_HAVE_OCTOMAP
+#include "coal/octree.h"
+#endif
+
+#include "coal/BV/BV.h"
+#include "coal/shape/geometric_shapes_utility.h"
+
+namespace coal {
+namespace detail {
+
+namespace dynamic_AABB_tree {
+
+#if COAL_HAVE_OCTOMAP
+
+//==============================================================================
+template <typename Derived>
+bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
+                       const OcTree* tree2, const OcTree::OcTreeNode* root2,
+                       const AABB& root2_bv,
+                       const Eigen::MatrixBase<Derived>& translation2,
+                       CollisionCallBackBase* callback) {
+  if (!root2) {
+    if (root1->isLeaf()) {
+      CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
+
+      if (!obj1->collisionGeometry()->isFree()) {
+        const AABB& root2_bv_t = translate(root2_bv, translation2);
+        if (root1->bv.overlap(root2_bv_t)) {
+          Box* box = new Box();
+          Transform3s box_tf;
+          Transform3s tf2 = Transform3s::Identity();
+          tf2.translation() = translation2;
+          constructBox(root2_bv, tf2, *box, box_tf);
+
+          box->cost_density =
+              tree2->getOccupancyThres();  // thresholds are 0, 1, so uncertain
+
+          CollisionObject obj2(shared_ptr<CollisionGeometry>(box), box_tf);
+          return (*callback)(obj1, &obj2);
+        }
+      }
+    } else {
+      if (collisionRecurse_(root1->children[0], tree2, nullptr, root2_bv,
+                            translation2, callback))
+        return true;
+      if (collisionRecurse_(root1->children[1], tree2, nullptr, root2_bv,
+                            translation2, callback))
+        return true;
+    }
+
+    return false;
+  } else if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) {
+    CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
+
+    if (!tree2->isNodeFree(root2) && !obj1->collisionGeometry()->isFree()) {
+      const AABB& root2_bv_t = translate(root2_bv, translation2);
+      if (root1->bv.overlap(root2_bv_t)) {
+        Box* box = new Box();
+        Transform3s box_tf;
+        Transform3s tf2 = Transform3s::Identity();
+        tf2.translation() = translation2;
+        constructBox(root2_bv, tf2, *box, box_tf);
+
+        box->cost_density = root2->getOccupancy();
+        box->threshold_occupied = tree2->getOccupancyThres();
+
+        CollisionObject obj2(shared_ptr<CollisionGeometry>(box), box_tf);
+        return (*callback)(obj1, &obj2);
+      } else
+        return false;
+    } else
+      return false;
+  }
+
+  const AABB& root2_bv_t = translate(root2_bv, translation2);
+  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,
+                          translation2, callback))
+      return true;
+    if (collisionRecurse_(root1->children[1], tree2, root2, root2_bv,
+                          translation2, 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, translation2,
+                              callback))
+          return true;
+      } else {
+        AABB child_bv;
+        computeChildBV(root2_bv, i, child_bv);
+        if (collisionRecurse_(root1, tree2, nullptr, child_bv, translation2,
+                              callback))
+          return true;
+      }
+    }
+  }
+  return false;
+}
+
+//==============================================================================
+template <typename Derived>
+bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
+                      const OcTree* tree2, const OcTree::OcTreeNode* root2,
+                      const AABB& root2_bv,
+                      const Eigen::MatrixBase<Derived>& translation2,
+                      DistanceCallBackBase* callback, CoalScalar& min_dist) {
+  if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) {
+    if (tree2->isNodeOccupied(root2)) {
+      Box* box = new Box();
+      Transform3s box_tf;
+      Transform3s tf2 = Transform3s::Identity();
+      tf2.translation() = translation2;
+      constructBox(root2_bv, tf2, *box, box_tf);
+      CollisionObject obj(shared_ptr<CollisionGeometry>(box), box_tf);
+      return (*callback)(static_cast<CollisionObject*>(root1->data), &obj,
+                         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, translation2);
+    CoalScalar d1 = aabb2.distance(root1->children[0]->bv);
+    CoalScalar d2 = aabb2.distance(root1->children[1]->bv);
+
+    if (d2 < d1) {
+      if (d2 < min_dist) {
+        if (distanceRecurse_(root1->children[1], tree2, root2, root2_bv,
+                             translation2, callback, min_dist))
+          return true;
+      }
+
+      if (d1 < min_dist) {
+        if (distanceRecurse_(root1->children[0], tree2, root2, root2_bv,
+                             translation2, callback, min_dist))
+          return true;
+      }
+    } else {
+      if (d1 < min_dist) {
+        if (distanceRecurse_(root1->children[0], tree2, root2, root2_bv,
+                             translation2, callback, min_dist))
+          return true;
+      }
+
+      if (d2 < min_dist) {
+        if (distanceRecurse_(root1->children[1], tree2, root2, root2_bv,
+                             translation2, 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, translation2);
+
+        CoalScalar d = root1->bv.distance(aabb2);
+
+        if (d < min_dist) {
+          if (distanceRecurse_(root1, tree2, child, child_bv, translation2,
+                               callback, min_dist))
+            return true;
+        }
+      }
+    }
+  }
+
+  return false;
+}
+
+#endif
+
+}  // namespace dynamic_AABB_tree
+}  // namespace detail
+}  // namespace coal
+
+#endif

include/coal/broadphase/broadphase_dynamic_AABB_tree.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, 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 COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_H
+#define COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_H
+
+#include <unordered_map>
+#include <functional>
+
+#include "coal/fwd.hh"
+// #include "coal/BV/utility.h"
+#include "coal/shape/geometric_shapes.h"
+// #include "coal/geometry/shape/utility.h"
+#include "coal/broadphase/broadphase_collision_manager.h"
+#include "coal/broadphase/detail/hierarchy_tree.h"
+
+namespace coal {
+
+class COAL_DLLAPI DynamicAABBTreeCollisionManager
+    : public BroadPhaseCollisionManager {
+ public:
+  typedef BroadPhaseCollisionManager Base;
+  using Base::getObjects;
+
+  using DynamicAABBNode = detail::NodeBase<AABB>;
+  using DynamicAABBTable =
+      std::unordered_map<CollisionObject*, DynamicAABBNode*>;
+
+  int max_tree_nonbalanced_level;
+  int tree_incremental_balance_pass;
+  int* tree_topdown_balance_threshold{nullptr};
+  int* tree_topdown_level{nullptr};
+  int tree_init_level;
+
+  bool octree_as_geometry_collide;
+  bool octree_as_geometry_distance;
+
+  DynamicAABBTreeCollisionManager();
+
+  /// @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
+  virtual 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, CollisionCallBackBase* callback) const;
+
+  /// @brief perform distance computation between one object and all the objects
+  /// belonging to the manager
+  void distance(CollisionObject* obj, DistanceCallBackBase* callback) const;
+
+  /// @brief perform collision test for the objects belonging to the manager
+  /// (i.e., N^2 self collision)
+  void collide(CollisionCallBackBase* callback) const;
+
+  /// @brief perform distance test for the objects belonging to the manager
+  /// (i.e., N^2 self distance)
+  void distance(DistanceCallBackBase* callback) const;
+
+  /// @brief perform collision test with objects belonging to another manager
+  void collide(BroadPhaseCollisionManager* other_manager_,
+               CollisionCallBackBase* callback) const;
+
+  /// @brief perform distance test with objects belonging to another manager
+  void distance(BroadPhaseCollisionManager* other_manager_,
+                DistanceCallBackBase* 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 returns the AABB tree structure.
+  const detail::HierarchyTree<AABB>& getTree() const;
+
+  /// @brief returns the AABB tree structure.
+  detail::HierarchyTree<AABB>& getTree();
+
+ private:
+  detail::HierarchyTree<AABB> dtree{};
+  std::unordered_map<CollisionObject*, DynamicAABBNode*> table;
+
+  bool setup_;
+
+  void update_(CollisionObject* updated_obj);
+};
+
+}  // namespace coal
+
+#include "coal/broadphase/broadphase_dynamic_AABB_tree-inl.h"
+
+#endif

include/coal/broadphase/broadphase_dynamic_AABB_tree_array-inl.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, 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 COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_INL_H
+#define COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_INL_H
+
+#include "coal/broadphase/broadphase_dynamic_AABB_tree_array.h"
+#include "coal/shape/geometric_shapes_utility.h"
+
+#if COAL_HAVE_OCTOMAP
+#include "coal/octree.h"
+#endif
+
+namespace coal {
+namespace detail {
+namespace dynamic_AABB_tree_array {
+
+#if COAL_HAVE_OCTOMAP
+
+//==============================================================================
+template <typename Derived>
+bool collisionRecurse_(
+    DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1,
+    size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2,
+    const AABB& root2_bv, const Eigen::MatrixBase<Derived>& translation2,
+    CollisionCallBackBase* callback) {
+  DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 =
+      nodes1 + root1_id;
+  if (!root2) {
+    if (root1->isLeaf()) {
+      CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
+
+      if (!obj1->collisionGeometry()->isFree()) {
+        const AABB& root_bv_t = translate(root2_bv, translation2);
+        if (root1->bv.overlap(root_bv_t)) {
+          Box* box = new Box();
+          Transform3s box_tf;
+          Transform3s tf2 = Transform3s::Identity();
+          tf2.translation() = translation2;
+          constructBox(root2_bv, tf2, *box, box_tf);
+
+          box->cost_density = tree2->getDefaultOccupancy();
+
+          CollisionObject obj2(shared_ptr<CollisionGeometry>(box), box_tf);
+          return (*callback)(obj1, &obj2);
+        }
+      }
+    } else {
+      if (collisionRecurse_(nodes1, root1->children[0], tree2, nullptr,
+                            root2_bv, translation2, callback))
+        return true;
+      if (collisionRecurse_(nodes1, root1->children[1], tree2, nullptr,
+                            root2_bv, translation2, callback))
+        return true;
+    }
+
+    return false;
+  } else if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) {
+    CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
+    if (!tree2->isNodeFree(root2) && !obj1->collisionGeometry()->isFree()) {
+      const AABB& root_bv_t = translate(root2_bv, translation2);
+      if (root1->bv.overlap(root_bv_t)) {
+        Box* box = new Box();
+        Transform3s box_tf;
+        Transform3s tf2 = Transform3s::Identity();
+        tf2.translation() = translation2;
+        constructBox(root2_bv, tf2, *box, box_tf);
+
+        box->cost_density = root2->getOccupancy();
+        box->threshold_occupied = tree2->getOccupancyThres();
+
+        CollisionObject obj2(shared_ptr<CollisionGeometry>(box), box_tf);
+        return (*callback)(obj1, &obj2);
+      } else
+        return false;
+    } else
+      return false;
+  }
+
+  const AABB& root_bv_t = translate(root2_bv, translation2);
+  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,
+                          translation2, callback))
+      return true;
+    if (collisionRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv,
+                          translation2, 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,
+                              translation2, callback))
+          return true;
+      } else {
+        AABB child_bv;
+        computeChildBV(root2_bv, i, child_bv);
+        if (collisionRecurse_(nodes1, root1_id, tree2, nullptr, child_bv,
+                              translation2, callback))
+          return true;
+      }
+    }
+  }
+
+  return false;
+}
+
+//==============================================================================
+template <typename Derived>
+bool distanceRecurse_(
+    DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1,
+    size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2,
+    const AABB& root2_bv, const Eigen::MatrixBase<Derived>& translation2,
+    DistanceCallBackBase* callback, CoalScalar& min_dist) {
+  DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 =
+      nodes1 + root1_id;
+  if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) {
+    if (tree2->isNodeOccupied(root2)) {
+      Box* box = new Box();
+      Transform3s box_tf;
+      Transform3s tf2 = Transform3s::Identity();
+      tf2.translation() = translation2;
+      constructBox(root2_bv, tf2, *box, box_tf);
+      CollisionObject obj(shared_ptr<CollisionGeometry>(box), box_tf);
+      return (*callback)(static_cast<CollisionObject*>(root1->data), &obj,
+                         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, translation2);
+
+    CoalScalar d1 = aabb2.distance((nodes1 + root1->children[0])->bv);
+    CoalScalar 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,
+                             translation2, callback, min_dist))
+          return true;
+      }
+
+      if (d1 < min_dist) {
+        if (distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv,
+                             translation2, callback, min_dist))
+          return true;
+      }
+    } else {
+      if (d1 < min_dist) {
+        if (distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv,
+                             translation2, callback, min_dist))
+          return true;
+      }
+
+      if (d2 < min_dist) {
+        if (distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv,
+                             translation2, 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, translation2);
+        CoalScalar d = root1->bv.distance(aabb2);
+
+        if (d < min_dist) {
+          if (distanceRecurse_(nodes1, root1_id, tree2, child, child_bv,
+                               translation2, callback, min_dist))
+            return true;
+        }
+      }
+    }
+  }
+
+  return false;
+}
+
+#endif
+
+}  // namespace dynamic_AABB_tree_array
+}  // namespace detail
+}  // namespace coal
+
+#endif

include/coal/broadphase/broadphase_dynamic_AABB_tree_array.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, 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 COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_H
+#define COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_H
+
+#include <unordered_map>
+#include <functional>
+#include <limits>
+
+#include "coal/fwd.hh"
+// #include "coal/BV/utility.h"
+#include "coal/shape/geometric_shapes.h"
+// #include "coal/geometry/shape/utility.h"
+#include "coal/broadphase/broadphase_collision_manager.h"
+#include "coal/broadphase/detail/hierarchy_tree_array.h"
+
+namespace coal {
+
+class COAL_DLLAPI DynamicAABBTreeArrayCollisionManager
+    : public BroadPhaseCollisionManager {
+ public:
+  typedef BroadPhaseCollisionManager Base;
+  using Base::getObjects;
+
+  using DynamicAABBNode = detail::implementation_array::NodeBase<AABB>;
+  using DynamicAABBTable = std::unordered_map<CollisionObject*, size_t>;
+
+  int max_tree_nonbalanced_level;
+  int tree_incremental_balance_pass;
+  int* tree_topdown_balance_threshold{nullptr};
+  int* tree_topdown_level{nullptr};
+  int tree_init_level;
+
+  bool octree_as_geometry_collide;
+  bool octree_as_geometry_distance;
+
+  DynamicAABBTreeArrayCollisionManager();
+
+  /// @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
+  virtual 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, CollisionCallBackBase* callback) const;
+
+  /// @brief perform distance computation between one object and all the objects
+  /// belonging to the manager
+  void distance(CollisionObject* obj, DistanceCallBackBase* callback) const;
+
+  /// @brief perform collision test for the objects belonging to the manager
+  /// (i.e., N^2 self collision)
+  void collide(CollisionCallBackBase* callback) const;
+
+  /// @brief perform distance test for the objects belonging to the manager
+  /// (i.e., N^2 self distance)
+  void distance(DistanceCallBackBase* callback) const;
+
+  /// @brief perform collision test with objects belonging to another manager
+  void collide(BroadPhaseCollisionManager* other_manager_,
+               CollisionCallBackBase* callback) const;
+
+  /// @brief perform distance test with objects belonging to another manager
+  void distance(BroadPhaseCollisionManager* other_manager_,
+                DistanceCallBackBase* callback) const;
+
+  /// @brief whether the manager is empty
+  bool empty() const;
+
+  /// @brief the number of objects managed by the manager
+  size_t size() const;
+
+  const detail::implementation_array::HierarchyTree<AABB>& getTree() const;
+
+ private:
+  detail::implementation_array::HierarchyTree<AABB> dtree{};
+  std::unordered_map<CollisionObject*, size_t> table;
+
+  bool setup_;
+
+  void update_(CollisionObject* updated_obj);
+};
+
+}  // namespace coal
+
+#include "coal/broadphase/broadphase_dynamic_AABB_tree_array-inl.h"
+
+#endif

include/coal/broadphase/broadphase_interval_tree.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, 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 COAL_BROAD_PHASE_INTERVAL_TREE_H
+#define COAL_BROAD_PHASE_INTERVAL_TREE_H
+
+#include <deque>
+#include <map>
+
+#include "coal/broadphase/broadphase_collision_manager.h"
+#include "coal/broadphase/detail/interval_tree.h"
+
+namespace coal {
+
+/// @brief Collision manager based on interval tree
+class COAL_DLLAPI IntervalTreeCollisionManager
+    : public BroadPhaseCollisionManager {
+ public:
+  typedef BroadPhaseCollisionManager Base;
+  using Base::getObjects;
+
+  IntervalTreeCollisionManager();
+
+  ~IntervalTreeCollisionManager();
+
+  /// @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
+  virtual 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, CollisionCallBackBase* callback) const;
+
+  /// @brief perform distance computation between one object and all the objects
+  /// belonging to the manager
+  void distance(CollisionObject* obj, DistanceCallBackBase* callback) const;
+
+  /// @brief perform collision test for the objects belonging to the manager
+  /// (i.e., N^2 self collision)
+  void collide(CollisionCallBackBase* callback) const;
+
+  /// @brief perform distance test for the objects belonging to the manager
+  /// (i.e., N^2 self distance)
+  void distance(DistanceCallBackBase* callback) const;
+
+  /// @brief perform collision test with objects belonging to another manager
+  void collide(BroadPhaseCollisionManager* other_manager,
+               CollisionCallBackBase* callback) const;
+
+  /// @brief perform distance test with objects belonging to another manager
+  void distance(BroadPhaseCollisionManager* other_manager,
+                DistanceCallBackBase* callback) const;
+
+  /// @brief whether the manager is empty
+  bool empty() const;
+
+  /// @brief the number of objects managed by the manager
+  size_t size() const;
+
+ protected:
+  /// @brief SAP end point
+  /// @brief SAP end point
+  struct COAL_DLLAPI EndPoint {
+    /// @brief object related with the end point
+    CollisionObject* obj;
+
+    /// @brief end point value
+    CoalScalar 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;
+
+    bool operator<(const EndPoint& p) const;
+  };
+
+  /// @brief Extention interval tree's interval to SAP interval, adding more
+  /// information
+  struct COAL_DLLAPI SAPInterval : public detail::SimpleInterval {
+    CollisionObject* obj;
+
+    SAPInterval(CoalScalar low_, CoalScalar high_, CollisionObject* obj_);
+  };
+
+  bool checkColl(
+      typename std::deque<detail::SimpleInterval*>::const_iterator pos_start,
+      typename std::deque<detail::SimpleInterval*>::const_iterator pos_end,
+      CollisionObject* obj, CollisionCallBackBase* callback) const;
+
+  bool checkDist(
+      typename std::deque<detail::SimpleInterval*>::const_iterator pos_start,
+      typename std::deque<detail::SimpleInterval*>::const_iterator pos_end,
+      CollisionObject* obj, DistanceCallBackBase* callback,
+      CoalScalar& min_dist) const;
+
+  bool collide_(CollisionObject* obj, CollisionCallBackBase* callback) const;
+
+  bool distance_(CollisionObject* obj, DistanceCallBackBase* callback,
+                 CoalScalar& min_dist) const;
+
+  /// @brief vector stores all the end points
+  std::vector<EndPoint> endpoints[3];
+
+  /// @brief  interval tree manages the intervals
+  detail::IntervalTree* interval_trees[3];
+
+  std::map<CollisionObject*, SAPInterval*> obj_interval_maps[3];
+
+  /// @brief tag for whether the interval tree is maintained suitably
+  bool setup_;
+};
+
+}  // namespace coal
+
+#endif

include/coal/broadphase/broadphase_spatialhash-inl.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, 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 COAL_BROADPHASE_BROADPAHSESPATIALHASH_INL_H
+#define COAL_BROADPHASE_BROADPAHSESPATIALHASH_INL_H
+
+#include "coal/broadphase/broadphase_spatialhash.h"
+
+namespace coal {
+
+//==============================================================================
+template <typename HashTable>
+SpatialHashingCollisionManager<HashTable>::SpatialHashingCollisionManager(
+    CoalScalar cell_size, const Vec3s& scene_min, const Vec3s& scene_max,
+    unsigned int default_table_size)
+    : scene_limit(AABB(scene_min, scene_max)),
+      hash_table(new HashTable(detail::SpatialHash(scene_limit, cell_size))) {
+  hash_table->init(default_table_size);
+}
+
+//==============================================================================
+template <typename HashTable>
+SpatialHashingCollisionManager<HashTable>::~SpatialHashingCollisionManager() {
+  delete hash_table;
+}
+
+//==============================================================================
+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_partially_penetrating_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_partially_penetrating_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() {
+  // Do nothing
+}
+
+//==============================================================================
+template <typename HashTable>
+void SpatialHashingCollisionManager<HashTable>::update() {
+  hash_table->clear();
+  objs_partially_penetrating_scene_limit.clear();
+  objs_outside_scene_limit.clear();
+
+  for (auto it = objs.cbegin(), end = objs.cend(); 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_partially_penetrating_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];
+
+  AABB old_overlap_aabb;
+  const auto is_old_aabb_overlapping =
+      scene_limit.overlap(old_aabb, old_overlap_aabb);
+  if (is_old_aabb_overlapping)
+    hash_table->remove(old_overlap_aabb, updated_obj);
+
+  AABB new_overlap_aabb;
+  const auto is_new_aabb_overlapping =
+      scene_limit.overlap(new_aabb, new_overlap_aabb);
+  if (is_new_aabb_overlapping)
+    hash_table->insert(new_overlap_aabb, updated_obj);
+
+  ObjectStatus old_status;
+  if (is_old_aabb_overlapping) {
+    if (scene_limit.contain(old_aabb))
+      old_status = Inside;
+    else
+      old_status = PartiallyPenetrating;
+  } else {
+    old_status = Outside;
+  }
+
+  if (is_new_aabb_overlapping) {
+    if (scene_limit.contain(new_aabb)) {
+      if (old_status == PartiallyPenetrating) {
+        // Status change: PartiallyPenetrating --> Inside
+        // Required action(s):
+        // - remove object from "objs_partially_penetrating_scene_limit"
+
+        auto find_it = std::find(objs_partially_penetrating_scene_limit.begin(),
+                                 objs_partially_penetrating_scene_limit.end(),
+                                 updated_obj);
+        objs_partially_penetrating_scene_limit.erase(find_it);
+      } else if (old_status == Outside) {
+        // Status change: Outside --> Inside
+        // Required action(s):
+        // - remove object from "objs_outside_scene_limit"
+
+        auto 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 (old_status == Inside) {
+        // Status change: Inside --> PartiallyPenetrating
+        // Required action(s):
+        // - add object to "objs_partially_penetrating_scene_limit"
+
+        objs_partially_penetrating_scene_limit.push_back(updated_obj);
+      } else if (old_status == Outside) {
+        // Status change: Outside --> PartiallyPenetrating
+        // Required action(s):
+        // - remove object from "objs_outside_scene_limit"
+        // - add object to "objs_partially_penetrating_scene_limit"
+
+        auto find_it = std::find(objs_outside_scene_limit.begin(),
+                                 objs_outside_scene_limit.end(), updated_obj);
+        objs_outside_scene_limit.erase(find_it);
+
+        objs_partially_penetrating_scene_limit.push_back(updated_obj);
+      }
+    }
+  } else {
+    if (old_status == Inside) {
+      // Status change: Inside --> Outside
+      // Required action(s):
+      // - add object to "objs_outside_scene_limit"
+
+      objs_outside_scene_limit.push_back(updated_obj);
+    } else if (old_status == PartiallyPenetrating) {
+      // Status change: PartiallyPenetrating --> Outside
+      // Required action(s):
+      // - remove object from "objs_partially_penetrating_scene_limit"
+      // - add object to "objs_outside_scene_limit"
+
+      auto find_it =
+          std::find(objs_partially_penetrating_scene_limit.begin(),
+                    objs_partially_penetrating_scene_limit.end(), updated_obj);
+      objs_partially_penetrating_scene_limit.erase(find_it);
+
+      objs_outside_scene_limit.push_back(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, CollisionCallBackBase* callback) const {
+  if (size() == 0) return;
+  collide_(obj, callback);
+}
+
+//==============================================================================
+template <typename HashTable>
+void SpatialHashingCollisionManager<HashTable>::distance(
+    CollisionObject* obj, DistanceCallBackBase* callback) const {
+  if (size() == 0) return;
+  CoalScalar min_dist = (std::numeric_limits<CoalScalar>::max)();
+  distance_(obj, callback, min_dist);
+}
+
+//==============================================================================
+template <typename HashTable>
+bool SpatialHashingCollisionManager<HashTable>::collide_(
+    CollisionObject* obj, CollisionCallBackBase* callback) const {
+  const auto& obj_aabb = obj->getAABB();
+  AABB overlap_aabb;
+
+  if (scene_limit.overlap(obj_aabb, overlap_aabb)) {
+    const auto query_result = hash_table->query(overlap_aabb);
+    for (const auto& obj2 : query_result) {
+      if (obj == obj2) continue;
+
+      if ((*callback)(obj, obj2)) return true;
+    }
+
+    if (!scene_limit.contain(obj_aabb)) {
+      for (const auto& obj2 : objs_outside_scene_limit) {
+        if (obj == obj2) continue;
+
+        if ((*callback)(obj, obj2)) return true;
+      }
+    }
+  } else {
+    for (const auto& obj2 : objs_partially_penetrating_scene_limit) {
+      if (obj == obj2) continue;
+
+      if ((*callback)(obj, obj2)) return true;
+    }
+
+    for (const auto& obj2 : objs_outside_scene_limit) {
+      if (obj == obj2) continue;
+
+      if ((*callback)(obj, obj2)) return true;
+    }
+  }
+
+  return false;
+}
+
+//==============================================================================
+template <typename HashTable>
+bool SpatialHashingCollisionManager<HashTable>::distance_(
+    CollisionObject* obj, DistanceCallBackBase* callback,
+    CoalScalar& min_dist) const {
+  auto delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
+  auto aabb = obj->getAABB();
+  if (min_dist < (std::numeric_limits<CoalScalar>::max)()) {
+    Vec3s min_dist_delta(min_dist, min_dist, min_dist);
+    aabb.expand(min_dist_delta);
+  }
+
+  AABB overlap_aabb;
+
+  auto status = 1;
+  CoalScalar old_min_distance;
+
+  while (1) {
+    old_min_distance = min_dist;
+
+    if (scene_limit.overlap(aabb, overlap_aabb)) {
+      if (distanceObjectToObjects(obj, hash_table->query(overlap_aabb),
+                                  callback, min_dist)) {
+        return true;
+      }
+
+      if (!scene_limit.contain(aabb)) {
+        if (distanceObjectToObjects(obj, objs_outside_scene_limit, callback,
+                                    min_dist)) {
+          return true;
+        }
+      }
+    } else {
+      if (distanceObjectToObjects(obj, objs_partially_penetrating_scene_limit,
+                                  callback, min_dist)) {
+        return true;
+      }
+
+      if (distanceObjectToObjects(obj, objs_outside_scene_limit, callback,
+                                  min_dist)) {
+        return true;
+      }
+    }
+
+    if (status == 1) {
+      if (old_min_distance < (std::numeric_limits<CoalScalar>::max)()) {
+        break;
+      } else {
+        if (min_dist < old_min_distance) {
+          Vec3s min_dist_delta(min_dist, min_dist, min_dist);
+          aabb = AABB(obj->getAABB(), min_dist_delta);
+          status = 0;
+        } else {
+          if (aabb == 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(
+    CollisionCallBackBase* callback) const {
+  if (size() == 0) return;
+
+  for (const auto& obj1 : objs) {
+    const auto& obj_aabb = obj1->getAABB();
+    AABB overlap_aabb;
+
+    if (scene_limit.overlap(obj_aabb, overlap_aabb)) {
+      auto query_result = hash_table->query(overlap_aabb);
+      for (const auto& obj2 : query_result) {
+        if (obj1 < obj2) {
+          if ((*callback)(obj1, obj2)) return;
+        }
+      }
+
+      if (!scene_limit.contain(obj_aabb)) {
+        for (const auto& obj2 : objs_outside_scene_limit) {
+          if (obj1 < obj2) {
+            if ((*callback)(obj1, obj2)) return;
+          }
+        }
+      }
+    } else {
+      for (const auto& obj2 : objs_partially_penetrating_scene_limit) {
+        if (obj1 < obj2) {
+          if ((*callback)(obj1, obj2)) return;
+        }
+      }
+
+      for (const auto& obj2 : objs_outside_scene_limit) {
+        if (obj1 < obj2) {
+          if ((*callback)(obj1, obj2)) return;
+        }
+      }
+    }
+  }
+}
+
+//==============================================================================
+template <typename HashTable>
+void SpatialHashingCollisionManager<HashTable>::distance(
+    DistanceCallBackBase* callback) const {
+  if (size() == 0) return;
+
+  this->enable_tested_set_ = true;
+  this->tested_set.clear();
+
+  CoalScalar min_dist = (std::numeric_limits<CoalScalar>::max)();
+
+  for (const auto& obj : objs) {
+    if (distance_(obj, callback, min_dist)) break;
+  }
+
+  this->enable_tested_set_ = false;
+  this->tested_set.clear();
+}
+
+//==============================================================================
+template <typename HashTable>
+void SpatialHashingCollisionManager<HashTable>::collide(
+    BroadPhaseCollisionManager* other_manager_,
+    CollisionCallBackBase* callback) const {
+  auto* other_manager =
+      static_cast<SpatialHashingCollisionManager<HashTable>*>(other_manager_);
+
+  if ((size() == 0) || (other_manager->size() == 0)) return;
+
+  if (this == other_manager) {
+    collide(callback);
+    return;
+  }
+
+  if (this->size() < other_manager->size()) {
+    for (const auto& obj : objs) {
+      if (other_manager->collide_(obj, callback)) return;
+    }
+  } else {
+    for (const auto& obj : other_manager->objs) {
+      if (collide_(obj, callback)) return;
+    }
+  }
+}
+
+//==============================================================================
+template <typename HashTable>
+void SpatialHashingCollisionManager<HashTable>::distance(
+    BroadPhaseCollisionManager* other_manager_,
+    DistanceCallBackBase* callback) const {
+  auto* other_manager =
+      static_cast<SpatialHashingCollisionManager<HashTable>*>(other_manager_);
+
+  if ((size() == 0) || (other_manager->size() == 0)) return;
+
+  if (this == other_manager) {
+    distance(callback);
+    return;
+  }
+
+  CoalScalar min_dist = (std::numeric_limits<CoalScalar>::max)();
+
+  if (this->size() < other_manager->size()) {
+    for (const auto& obj : objs)
+      if (other_manager->distance_(obj, callback, min_dist)) return;
+  } else {
+    for (const auto& obj : other_manager->objs)
+      if (distance_(obj, 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();
+}
+
+//==============================================================================
+template <typename HashTable>
+void SpatialHashingCollisionManager<HashTable>::computeBound(
+    std::vector<CollisionObject*>& objs, Vec3s& l, Vec3s& u) {
+  AABB bound;
+  for (unsigned int i = 0; i < objs.size(); ++i) bound += objs[i]->getAABB();
+
+  l = bound.min_;
+  u = bound.max_;
+}
+
+//==============================================================================
+template <typename HashTable>
+template <typename Container>
+bool SpatialHashingCollisionManager<HashTable>::distanceObjectToObjects(
+    CollisionObject* obj, const Container& objs, DistanceCallBackBase* callback,
+    CoalScalar& min_dist) const {
+  for (auto& obj2 : objs) {
+    if (obj == obj2) continue;
+
+    if (!this->enable_tested_set_) {
+      if (obj->getAABB().distance(obj2->getAABB()) < min_dist) {
+        if ((*callback)(obj, obj2, min_dist)) return true;
+      }
+    } else {
+      if (!this->inTestedSet(obj, obj2)) {
+        if (obj->getAABB().distance(obj2->getAABB()) < min_dist) {
+          if ((*callback)(obj, obj2, min_dist)) return true;
+        }
+
+        this->insertTestedSet(obj, obj2);
+      }
+    }
+  }
+
+  return false;
+}
+
+}  // namespace coal
+
+#endif

include/coal/broadphase/broadphase_spatialhash.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, 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 COAL_BROADPHASE_BROADPAHSESPATIALHASH_H
+#define COAL_BROADPHASE_BROADPAHSESPATIALHASH_H
+
+#include <list>
+#include <map>
+#include "coal/BV/AABB.h"
+#include "coal/broadphase/broadphase_collision_manager.h"
+#include "coal/broadphase/detail/simple_hash_table.h"
+#include "coal/broadphase/detail/sparse_hash_table.h"
+#include "coal/broadphase/detail/spatial_hash.h"
+
+namespace coal {
+
+/// @brief spatial hashing collision mananger
+template <typename HashTable = detail::SimpleHashTable<AABB, CollisionObject*,
+                                                       detail::SpatialHash> >
+class SpatialHashingCollisionManager : public BroadPhaseCollisionManager {
+ public:
+  typedef BroadPhaseCollisionManager Base;
+  using Base::getObjects;
+
+  SpatialHashingCollisionManager(CoalScalar cell_size, const Vec3s& scene_min,
+                                 const Vec3s& scene_max,
+                                 unsigned int default_table_size = 1000);
+
+  ~SpatialHashingCollisionManager();
+
+  /// @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
+  virtual 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, CollisionCallBackBase* callback) const;
+
+  /// @brief perform distance computation between one object and all the objects
+  /// belonging ot the manager
+  void distance(CollisionObject* obj, DistanceCallBackBase* callback) const;
+
+  /// @brief perform collision test for the objects belonging to the manager
+  /// (i.e, N^2 self collision)
+  void collide(CollisionCallBackBase* callback) const;
+
+  /// @brief perform distance test for the objects belonging to the manager
+  /// (i.e., N^2 self distance)
+  void distance(DistanceCallBackBase* callback) const;
+
+  /// @brief perform collision test with objects belonging to another manager
+  void collide(BroadPhaseCollisionManager* other_manager,
+               CollisionCallBackBase* callback) const;
+
+  /// @brief perform distance test with objects belonging to another manager
+  void distance(BroadPhaseCollisionManager* other_manager,
+                DistanceCallBackBase* 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, Vec3s& l,
+                           Vec3s& u);
+
+ protected:
+  /// @brief perform collision test between one object and all the objects
+  /// belonging to the manager
+  bool collide_(CollisionObject* obj, CollisionCallBackBase* callback) const;
+
+  /// @brief perform distance computation between one object and all the objects
+  /// belonging ot the manager
+  bool distance_(CollisionObject* obj, DistanceCallBackBase* callback,
+                 CoalScalar& min_dist) const;
+
+  /// @brief all objects in the scene
+  std::list<CollisionObject*> objs;
+
+  /// @brief objects partially penetrating (not totally inside nor outside) the
+  /// scene limit are in another list
+  std::list<CollisionObject*> objs_partially_penetrating_scene_limit;
+
+  /// @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;
+
+ private:
+  enum ObjectStatus { Inside, PartiallyPenetrating, Outside };
+
+  template <typename Container>
+  bool distanceObjectToObjects(CollisionObject* obj, const Container& objs,
+                               DistanceCallBackBase* callback,
+                               CoalScalar& min_dist) const;
+};
+
+}  // namespace coal
+
+#include "coal/broadphase/broadphase_spatialhash-inl.h"
+
+#endif

include/coal/broadphase/default_broadphase_callbacks.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2020, Toyota Research Institute
+ *  Copyright (c) 2022-2023, INRIA
+ *  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 the copyright holder 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 Sean Curtis (sean@tri.global) */
+/** @author Justin Carpentier (justin.carpentier@inria.fr) */
+
+#ifndef COAL_BROADPHASE_DEFAULT_BROADPHASE_CALLBACKS_H
+#define COAL_BROADPHASE_DEFAULT_BROADPHASE_CALLBACKS_H
+
+#include "coal/broadphase/broadphase_callbacks.h"
+#include "coal/collision.h"
+#include "coal/distance.h"
+// #include "coal/narrowphase/continuous_collision.h"
+// #include "coal/narrowphase/continuous_collision_request.h"
+// #include "coal/narrowphase/continuous_collision_result.h"
+// #include "coal/narrowphase/distance_request.h"
+// #include "coal/narrowphase/distance_result.h"
+
+namespace coal {
+
+/// @brief Collision data stores the collision request and the result given by
+/// collision algorithm.
+struct CollisionData {
+  CollisionData() { done = false; }
+
+  /// @brief Collision request
+  CollisionRequest request;
+
+  /// @brief Collision result
+  CollisionResult result;
+
+  /// @brief Whether the collision iteration can stop
+  bool done;
+
+  /// @brief Clears the CollisionData
+  void clear() {
+    result.clear();
+    done = false;
+  }
+};
+
+/// @brief Distance data stores the distance request and the result given by
+/// distance algorithm.
+struct DistanceData {
+  DistanceData() { done = false; }
+
+  /// @brief Distance request
+  DistanceRequest request;
+
+  /// @brief Distance result
+  DistanceResult result;
+
+  /// @brief Whether the distance iteration can stop
+  bool done;
+
+  /// @brief Clears the DistanceData
+  void clear() {
+    result.clear();
+    done = false;
+  }
+};
+
+/// @brief Provides a simple callback for the collision query in the
+/// BroadPhaseCollisionManager. It assumes the `data` parameter is non-null and
+/// points to an instance of CollisionData. It simply invokes the
+/// `collide()` method on the culled pair of geometries and stores the results
+/// in the data's CollisionResult instance.
+///
+/// This callback will cause the broadphase evaluation to stop if:
+///   - the collision requests _disables_ cost _and_
+///   - the collide() reports a collision for the culled pair, _and_
+///   - we've reported the number of contacts requested in the CollisionRequest.
+///
+/// For a given instance of CollisionData, if broadphase evaluation has
+/// already terminated (i.e., defaultCollisionFunction() returned `true`),
+/// subsequent invocations with the same instance of CollisionData will
+/// return immediately, requesting termination of broadphase evaluation (i.e.,
+/// return `true`).
+///
+/// @param o1   The first object in the culled pair.
+/// @param o2   The second object in the culled pair.
+/// @param data A non-null pointer to a CollisionData instance.
+/// @return `true` if the broadphase evaluation should stop.
+/// @tparam S   The scalar type with which the computation will be performed.
+bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2,
+                              void* data);
+
+/// @brief Collision data for use with the DefaultContinuousCollisionFunction.
+/// It stores the collision request and the result given by the collision
+/// algorithm (and stores the conclusion of whether further evaluation of the
+/// broadphase collision manager has been deemed unnecessary).
+// struct DefaultContinuousCollisionData {
+//   ContinuousCollisionRequest request;
+//   ContinuousCollisionResult result;
+//
+//   /// If `true`, requests that the broadphase evaluation stop.
+//   bool done{false};
+// };
+
+/// @brief Provides a simple callback for the continuous collision query in the
+/// BroadPhaseCollisionManager. It assumes the `data` parameter is non-null and
+/// points to an instance of DefaultContinuousCollisionData. It simply invokes
+/// the `collide()` method on the culled pair of geometries and stores the
+/// results in the data's ContinuousCollisionResult instance.
+///
+/// This callback will never cause the broadphase evaluation to terminate early.
+/// However, if the `done` member of the DefaultContinuousCollisionData is set
+/// to true, this method will simply return without doing any computation.
+///
+/// For a given instance of DefaultContinuousCollisionData, if broadphase
+/// evaluation has already terminated (i.e.,
+/// DefaultContinuousCollisionFunction() returned `true`), subsequent
+/// invocations with the same instance of CollisionData will return
+/// immediately, requesting termination of broadphase evaluation (i.e., return
+/// `true`).
+///
+/// @param o1   The first object in the culled pair.
+/// @param o2   The second object in the culled pair.
+/// @param data A non-null pointer to a CollisionData instance.
+/// @return True if the broadphase evaluation should stop.
+/// @tparam S   The scalar type with which the computation will be performed.
+// bool DefaultContinuousCollisionFunction(ContinuousCollisionObject* o1,
+//                                         ContinuousCollisionObject* o2,
+//                                         void* data) {
+//   assert(data != nullptr);
+//   auto* cdata = static_cast<DefaultContinuousCollisionData*>(data);
+//
+//   if (cdata->done) return true;
+//
+//   const ContinuousCollisionRequest& request = cdata->request;
+//   ContinuousCollisionResult& result = cdata->result;
+//   collide(o1, o2, request, result);
+//
+//   return cdata->done;
+// }
+
+/// @brief Provides a simple callback for the distance query in the
+/// BroadPhaseCollisionManager. It assumes the `data` parameter is non-null and
+/// points to an instance of DistanceData. It simply invokes the
+/// `distance()` method on the culled pair of geometries and stores the results
+/// in the data's DistanceResult instance.
+///
+/// This callback will cause the broadphase evaluation to stop if:
+///   - The distance is less than or equal to zero (i.e., the pair is in
+///     contact).
+///
+/// For a given instance of DistanceData, if broadphase evaluation has
+/// already terminated (i.e., defaultDistanceFunction() returned `true`),
+/// subsequent invocations with the same instance of DistanceData will
+/// simply report the previously reported minimum distance and request
+/// termination of broadphase evaluation (i.e., return `true`).
+///
+/// @param o1     The first object in the culled pair.
+/// @param o2     The second object in the culled pair.
+/// @param data   A non-null pointer to a DistanceData instance.
+/// @param dist   The distance computed by distance().
+/// @return `true` if the broadphase evaluation should stop.
+/// @tparam S   The scalar type with which the computation will be performed.
+bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2,
+                             void* data, CoalScalar& dist);
+
+/// @brief Default collision callback to check collision between collision
+/// objects.
+struct COAL_DLLAPI CollisionCallBackDefault : CollisionCallBackBase {
+  /// @brief Initialize the callback.
+  /// Clears the collision result and sets the done boolean to false.
+  void init() { data.clear(); }
+
+  bool collide(CollisionObject* o1, CollisionObject* o2);
+
+  CollisionData data;
+
+  virtual ~CollisionCallBackDefault() {};
+};
+
+/// @brief Default distance callback to check collision between collision
+/// objects.
+struct COAL_DLLAPI DistanceCallBackDefault : DistanceCallBackBase {
+  /// @brief Initialize the callback.
+  /// Clears the distance result and sets the done boolean to false.
+  void init() { data.clear(); }
+
+  bool distance(CollisionObject* o1, CollisionObject* o2, CoalScalar& dist);
+
+  DistanceData data;
+
+  virtual ~DistanceCallBackDefault() {};
+};
+
+/// @brief Collision callback to collect collision pairs potentially in contacts
+struct COAL_DLLAPI CollisionCallBackCollect : CollisionCallBackBase {
+  typedef std::pair<CollisionObject*, CollisionObject*> CollisionPair;
+
+  /// @brief Default constructor.
+  CollisionCallBackCollect(const size_t max_size);
+
+  bool collide(CollisionObject* o1, CollisionObject* o2);
+
+  /// @brief Returns the number of registered collision pairs
+  size_t numCollisionPairs() const;
+
+  /// @brief Returns a const reference to the active collision_pairs to check
+  const std::vector<CollisionPair>& getCollisionPairs() const;
+
+  /// @brief Reset the callback
+  void init();
+
+  /// @brief Check whether a collision pair exists
+  bool exist(const CollisionPair& pair) const;
+
+  /// @brief Check whether a collision pair exists
+  bool exist(CollisionObject* o1, CollisionObject* o2) const;
+
+  virtual ~CollisionCallBackCollect() {};
+
+ protected:
+  std::vector<CollisionPair> collision_pairs;
+  size_t max_size;
+};
+
+}  // namespace coal
+
+#endif  // COAL_BROADPHASE_DEFAULT_BROADPHASE_CALLBACKS_H

include/coal/broadphase/detail/hierarchy_tree-inl.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, 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 COAL_HIERARCHY_TREE_INL_H
+#define COAL_HIERARCHY_TREE_INL_H
+
+#include "coal/broadphase/detail/hierarchy_tree.h"
+
+namespace coal {
+
+namespace detail {
+
+//==============================================================================
+template <typename BV>
+HierarchyTree<BV>::HierarchyTree(int bu_threshold_, int topdown_level_) {
+  root_node = nullptr;
+  n_leaves = 0;
+  free_node = nullptr;
+  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<Node*>& 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>::Node* HierarchyTree<BV>::insert(const BV& bv,
+                                                            void* data) {
+  Node* leaf = createNode(nullptr, bv, data);
+  insertLeaf(root_node, leaf);
+  ++n_leaves;
+  return leaf;
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::remove(Node* 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 = nullptr;
+  max_lookahead_level = -1;
+  opath = 0;
+}
+
+//==============================================================================
+template <typename BV>
+bool HierarchyTree<BV>::empty() const {
+  return (nullptr == root_node);
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::update(Node* leaf, int lookahead_level) {
+  // TODO(DamrongGuoy): Since we update a leaf node by removing and
+  //  inserting the same leaf node, it is likely to change the structure of
+  //  the tree even if no object's pose has changed. In the future,
+  //  find a way to preserve the structure of the tree to solve this issue:
+  //  https://github.com/flexible-collision-library/fcl/issues/368
+
+  // First we remove the leaf node pointed by `leaf` variable from the tree.
+  // The `sub_root` variable is the root of the subtree containing nodes
+  // whose BVs were adjusted due to node removal.
+  typename HierarchyTree<BV>::Node* sub_root = removeLeaf(leaf);
+  if (sub_root) {
+    if (lookahead_level > 0) {
+      // For positive `lookahead_level`, we move the `sub_root` variable up.
+      for (int i = 0; (i < lookahead_level) && sub_root->parent; ++i)
+        sub_root = sub_root->parent;
+    } else
+      // By default, lookahead_level = -1, and we reset the `sub_root` variable
+      // to the root of the entire tree.
+      sub_root = root_node;
+  }
+  // Then we insert the node pointed by `leaf` variable back into the
+  // subtree rooted at `sub_root` variable.
+  insertLeaf(sub_root, leaf);
+}
+
+//==============================================================================
+template <typename BV>
+bool HierarchyTree<BV>::update(Node* leaf, const BV& bv) {
+  if (leaf->bv.contain(bv)) return false;
+  update_(leaf, bv);
+  return true;
+}
+
+//==============================================================================
+template <typename S, typename BV>
+struct UpdateImpl {
+  static bool run(const HierarchyTree<BV>& tree,
+                  typename HierarchyTree<BV>::Node* leaf, const BV& bv,
+                  const Vec3s& /*vel*/, CoalScalar /*margin*/) {
+    if (leaf->bv.contain(bv)) return false;
+    tree.update_(leaf, bv);
+    return true;
+  }
+
+  static bool run(const HierarchyTree<BV>& tree,
+                  typename HierarchyTree<BV>::Node* leaf, const BV& bv,
+                  const Vec3s& /*vel*/) {
+    if (leaf->bv.contain(bv)) return false;
+    tree.update_(leaf, bv);
+    return true;
+  }
+};
+
+//==============================================================================
+template <typename BV>
+bool HierarchyTree<BV>::update(Node* leaf, const BV& bv, const Vec3s& vel,
+                               CoalScalar margin) {
+  return UpdateImpl<CoalScalar, BV>::run(*this, leaf, bv, vel, margin);
+}
+
+//==============================================================================
+template <typename BV>
+bool HierarchyTree<BV>::update(Node* leaf, const BV& bv, const Vec3s& vel) {
+  return UpdateImpl<CoalScalar, BV>::run(*this, leaf, bv, vel);
+}
+
+//==============================================================================
+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<Node*> 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<Node*> 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 = (int)n_leaves;
+  if (root_node && (iterations > 0)) {
+    for (int i = 0; i < iterations; ++i) {
+      Node* 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 Node* root,
+                                      std::vector<Node*>& 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>::Node* HierarchyTree<BV>::getRoot() const {
+  return root_node;
+}
+
+//==============================================================================
+template <typename BV>
+typename HierarchyTree<BV>::Node*& HierarchyTree<BV>::getRoot() {
+  return root_node;
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::print(Node* 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 = lbeg;
+    NodeVecIterator min_it2 = lbeg + 1;
+    CoalScalar min_size = (std::numeric_limits<CoalScalar>::max)();
+    for (NodeVecIterator it1 = lbeg; it1 < lcur_end; ++it1) {
+      for (NodeVecIterator it2 = it1 + 1; it2 < lcur_end; ++it2) {
+        CoalScalar cur_size = ((*it1)->bv + (*it2)->bv).size();
+        if (cur_size < min_size) {
+          min_size = cur_size;
+          min_it1 = it1;
+          min_it2 = it2;
+        }
+      }
+    }
+
+    Node* n[2] = {*min_it1, *min_it2};
+    Node* p = createNode(nullptr, n[0]->bv, n[1]->bv, nullptr);
+    p->children[0] = n[0];
+    p->children[1] = n[1];
+    n[0]->parent = p;
+    n[1]->parent = p;
+    *min_it1 = p;
+    Node* tmp = *min_it2;
+    lcur_end--;
+    *min_it2 = *lcur_end;
+    *lcur_end = tmp;
+  }
+}
+
+//==============================================================================
+template <typename BV>
+typename HierarchyTree<BV>::Node* 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(Node* 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(Node* 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>::Node* HierarchyTree<BV>::topdown_0(
+    const NodeVecIterator lbeg, const NodeVecIterator lend) {
+  long 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;
+      CoalScalar 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,
+                       std::bind(&nodeBaseLess<BV>, std::placeholders::_1,
+                                 std::placeholders::_2, std::ref(best_axis)));
+
+      Node* node = createNode(nullptr, vol, nullptr);
+      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>::Node* HierarchyTree<BV>::topdown_1(
+    const NodeVecIterator lbeg, const NodeVecIterator lend) {
+  long num_leaves = lend - lbeg;
+  if (num_leaves > 1) {
+    if (num_leaves > bu_threshold) {
+      Vec3s 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 /= static_cast<CoalScalar>(num_leaves);
+      int best_axis = -1;
+      long bestmidp = num_leaves;
+      int splitcount[3][2] = {{0, 0}, {0, 0}, {0, 0}};
+      for (it = lbeg; it < lend; ++it) {
+        Vec3s x = (*it)->bv.center() - split_p;
+        for (int j = 0; j < 3; ++j) ++splitcount[j][x[j] > 0 ? 1 : 0];
+      }
+
+      for (int i = 0; i < 3; ++i) {
+        if ((splitcount[i][0] > 0) && (splitcount[i][1] > 0)) {
+          long midp = std::abs(splitcount[i][0] - splitcount[i][1]);
+          if (midp < bestmidp) {
+            best_axis = i;
+            bestmidp = midp;
+          }
+        }
+      }
+
+      if (best_axis < 0) best_axis = 0;
+
+      CoalScalar split_value = split_p[best_axis];
+      NodeVecIterator lcenter = lbeg;
+      for (it = lbeg; it < lend; ++it) {
+        if ((*it)->bv.center()[best_axis] < split_value) {
+          Node* temp = *it;
+          *it = *lcenter;
+          *lcenter = temp;
+          ++lcenter;
+        }
+      }
+
+      Node* node = createNode(nullptr, vol, nullptr);
+      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<Node*>& 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<Node*>& 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<CoalScalar, uint32_t> 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<Node*>& 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<CoalScalar, uint32_t> 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<Node*>& 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<CoalScalar, uint32_t> 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>::Node* HierarchyTree<BV>::mortonRecurse_0(
+    const NodeVecIterator lbeg, const NodeVecIterator lend,
+    const uint32_t& split, int bits) {
+  long num_leaves = lend - lbeg;
+  if (num_leaves > 1) {
+    if (bits > 0) {
+      Node dummy;
+      dummy.code = split;
+      NodeVecIterator lcenter =
+          std::lower_bound(lbeg, lend, &dummy, SortByMorton());
+
+      if (lcenter == lbeg) {
+        uint32_t split2 = split | (1 << (bits - 1));
+        return mortonRecurse_0(lbeg, lend, split2, bits - 1);
+      } else if (lcenter == lend) {
+        uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
+        return mortonRecurse_0(lbeg, lend, split1, bits - 1);
+      } else {
+        uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
+        uint32_t split2 = split | (1 << (bits - 1));
+
+        Node* child1 = mortonRecurse_0(lbeg, lcenter, split1, bits - 1);
+        Node* child2 = mortonRecurse_0(lcenter, lend, split2, bits - 1);
+        Node* node = createNode(nullptr, nullptr);
+        node->children[0] = child1;
+        node->children[1] = child2;
+        child1->parent = node;
+        child2->parent = node;
+        return node;
+      }
+    } else {
+      Node* node = topdown(lbeg, lend);
+      return node;
+    }
+  } else
+    return *lbeg;
+}
+
+//==============================================================================
+template <typename BV>
+typename HierarchyTree<BV>::Node* HierarchyTree<BV>::mortonRecurse_1(
+    const NodeVecIterator lbeg, const NodeVecIterator lend,
+    const uint32_t& split, int bits) {
+  long num_leaves = lend - lbeg;
+  if (num_leaves > 1) {
+    if (bits > 0) {
+      Node dummy;
+      dummy.code = split;
+      NodeVecIterator lcenter =
+          std::lower_bound(lbeg, lend, &dummy, SortByMorton());
+
+      if (lcenter == lbeg) {
+        uint32_t split2 = split | (1 << (bits - 1));
+        return mortonRecurse_1(lbeg, lend, split2, bits - 1);
+      } else if (lcenter == lend) {
+        uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
+        return mortonRecurse_1(lbeg, lend, split1, bits - 1);
+      } else {
+        uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
+        uint32_t split2 = split | (1 << (bits - 1));
+
+        Node* child1 = mortonRecurse_1(lbeg, lcenter, split1, bits - 1);
+        Node* child2 = mortonRecurse_1(lcenter, lend, split2, bits - 1);
+        Node* node = createNode(nullptr, nullptr);
+        node->children[0] = child1;
+        node->children[1] = child2;
+        child1->parent = node;
+        child2->parent = node;
+        return node;
+      }
+    } else {
+      Node* child1 = mortonRecurse_1(lbeg, lbeg + num_leaves / 2, 0, bits - 1);
+      Node* child2 = mortonRecurse_1(lbeg + num_leaves / 2, lend, 0, bits - 1);
+      Node* node = createNode(nullptr, nullptr);
+      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>::Node* HierarchyTree<BV>::mortonRecurse_2(
+    const NodeVecIterator lbeg, const NodeVecIterator lend) {
+  long num_leaves = lend - lbeg;
+  if (num_leaves > 1) {
+    Node* child1 = mortonRecurse_2(lbeg, lbeg + num_leaves / 2);
+    Node* child2 = mortonRecurse_2(lbeg + num_leaves / 2, lend);
+    Node* node = createNode(nullptr, nullptr);
+    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_(Node* leaf, const BV& bv) {
+  Node* 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>::Node* HierarchyTree<BV>::sort(Node* n, Node*& r) {
+  Node* p = n->parent;
+  if (p > n) {
+    size_t i = indexOf(n);
+    size_t j = 1 - i;
+    Node* s = p->children[j];
+    Node* 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(Node* const sub_root, Node* const leaf)
+// Attempts to insert `leaf` into a subtree rooted at `sub_root`.
+// 1. If the whole tree is empty, then `leaf` simply becomes the tree.
+// 2. Otherwise, a leaf node called `sibling` of the subtree rooted at
+//    `sub_root` is selected (the selection criteria is a black box for this
+//    algorithm), and the `sibling` leaf is replaced by an internal node with
+//    two children, `sibling` and `leaf`. The bounding volumes are updated as
+//    necessary.
+{
+  if (!root_node) {
+    // If the entire tree is empty, the node pointed by `leaf` variable will
+    // become the root of the tree.
+    root_node = leaf;
+    leaf->parent = nullptr;
+    return;
+  }
+  // Traverse the tree from the given `sub_root` down to an existing leaf
+  // node that we call `sibling`. The `sibling` node will eventually become
+  // the sibling of the given `leaf` node.
+  Node* sibling = sub_root;
+  while (!sibling->isLeaf()) {
+    sibling = sibling->children[select(*leaf, *(sibling->children[0]),
+                                       *(sibling->children[1]))];
+  }
+  Node* prev = sibling->parent;
+  // Create a new `node` that later will become the new parent of both the
+  // `sibling` and the given `leaf`.
+  Node* node = createNode(prev, leaf->bv, sibling->bv, nullptr);
+  if (prev)
+  // If the parent `prev` of the `sibling` is an interior node, we will
+  // replace the `sibling` with the subtree {node {`sibling`, leaf}} like
+  // this:
+  //        Before                After
+  //
+  //          ╱                     ╱
+  //        prev                  prev
+  //        ╱  ╲        ─>        ╱  ╲
+  //  sibling  ...             node  ...
+  //                           ╱  ╲
+  //                     sibling  leaf
+  {
+    prev->children[indexOf(sibling)] = node;
+    node->children[0] = sibling;
+    sibling->parent = node;
+    node->children[1] = leaf;
+    leaf->parent = node;
+    // Now that we've inserted `leaf` some of the existing bounding
+    // volumes might not fully enclose their children. Walk up the tree
+    // looking for parents that don't already enclose their children
+    // and create a new tight-fitting bounding volume for those.
+    do {
+      if (!prev->bv.contain(node->bv))
+        prev->bv = prev->children[0]->bv + prev->children[1]->bv;
+      else
+        break;
+      node = prev;
+    } while (nullptr != (prev = node->parent));
+  } else
+  // If the `sibling` has no parent, i.e., the tree is a singleton,
+  // we will replace it with the 3-node tree {node {`sibling`, `leaf`}} like
+  // this:
+  //
+  //        node
+  //        ╱  ╲
+  //  sibling  leaf
+  {
+    node->children[0] = sibling;
+    sibling->parent = node;
+    node->children[1] = leaf;
+    leaf->parent = node;
+    root_node = node;
+  }
+
+  // Note that the above algorithm always adds the new `leaf` node as the right
+  // child, i.e., children[1].  Calling removeLeaf(l) followed by calling
+  // this function insertLeaf(l) where l is a left child will result in
+  // switching l and its sibling even if no object's pose has changed.
+}
+
+//==============================================================================
+template <typename BV>
+typename HierarchyTree<BV>::Node* HierarchyTree<BV>::removeLeaf(
+    Node* const leaf) {
+  // Deletes `leaf` by replacing the subtree consisting of `leaf`, its sibling,
+  // and its parent with just its sibling. It then "tightens" the ancestor
+  // bounding volumes. Returns a pointer to the parent of the highest node whose
+  // BV changed due to the removal.
+  if (leaf == root_node) {
+    // If the `leaf` node is the only node in the tree, the tree becomes empty.
+    root_node = nullptr;
+    return nullptr;
+  }
+  Node* parent = leaf->parent;
+  Node* prev = parent->parent;
+  Node* sibling = parent->children[1 - indexOf(leaf)];
+  if (prev) {
+    // If the parent node is not the root node, the sibling node will
+    // replace the parent node like this:
+    //
+    //            Before              After
+    //             ...                 ...
+    //             ╱                   ╱
+    //           prev                prev
+    //          ╱   ╲               ╱   ╲
+    //     parent   ...    ─>  sibling  ...
+    //      ╱  ╲                 ╱╲
+    //  leaf  sibling           ...
+    //           ╱╲
+    //          ...
+    //
+    // Step 1: replace the subtree {parent {leaf, sibling {...}}} with
+    // {sibling {...}}.
+    prev->children[indexOf(parent)] = sibling;
+    sibling->parent = prev;
+    deleteNode(parent);
+    // Step 2: tighten up the BVs of the ancestor nodes.
+    while (prev) {
+      BV new_bv = prev->children[0]->bv + prev->children[1]->bv;
+      if (!(new_bv == prev->bv)) {
+        prev->bv = new_bv;
+        prev = prev->parent;
+      } else
+        break;
+    }
+
+    return prev ? prev : root_node;
+  } else {
+    // If the parent node is the root node, the sibling node will become the
+    // root of the whole tree like this:
+    //
+    //     Before                   After
+    //
+    //     parent
+    //      ╱  ╲
+    //  leaf  sibling     ─>       sibling
+    //           ╱╲                  ╱╲
+    //          ...                 ...
+    root_node = sibling;
+    sibling->parent = nullptr;
+    deleteNode(parent);
+    return root_node;
+  }
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::fetchLeaves(Node* root, std::vector<Node*>& 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(Node* node) {
+  // node cannot be nullptr
+  return (node->parent->children[1] == node);
+}
+
+//==============================================================================
+template <typename BV>
+typename HierarchyTree<BV>::Node* HierarchyTree<BV>::createNode(Node* parent,
+                                                                const BV& bv,
+                                                                void* data) {
+  Node* node = createNode(parent, data);
+  node->bv = bv;
+  return node;
+}
+
+//==============================================================================
+template <typename BV>
+typename HierarchyTree<BV>::Node* HierarchyTree<BV>::createNode(Node* parent,
+                                                                const BV& bv1,
+                                                                const BV& bv2,
+                                                                void* data) {
+  Node* node = createNode(parent, data);
+  node->bv = bv1 + bv2;
+  return node;
+}
+
+//==============================================================================
+template <typename BV>
+typename HierarchyTree<BV>::Node* HierarchyTree<BV>::createNode(Node* parent,
+                                                                void* data) {
+  Node* node = nullptr;
+  if (free_node) {
+    node = free_node;
+    free_node = nullptr;
+  } else
+    node = new Node();
+  node->parent = parent;
+  node->data = data;
+  node->children[1] = 0;
+  return node;
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::deleteNode(Node* node) {
+  if (free_node != node) {
+    delete free_node;
+    free_node = node;
+  }
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::recurseDeleteNode(Node* node) {
+  if (!node->isLeaf()) {
+    recurseDeleteNode(node->children[0]);
+    recurseDeleteNode(node->children[1]);
+  }
+
+  if (node == root_node) root_node = nullptr;
+  deleteNode(node);
+}
+
+//==============================================================================
+template <typename BV>
+void HierarchyTree<BV>::recurseRefit(Node* 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>
+bool nodeBaseLess(NodeBase<BV>* a, NodeBase<BV>* b, int d) {
+  if (a->bv.center()[d] < b->bv.center()[d]) return true;
+  return false;
+}
+
+//==============================================================================
+template <typename S, typename BV>
+struct SelectImpl {
+  static std::size_t run(const NodeBase<BV>& /*query*/,
+                         const NodeBase<BV>& /*node1*/,
+                         const NodeBase<BV>& /*node2*/) {
+    return 0;
+  }
+
+  static std::size_t run(const BV& /*query*/, const NodeBase<BV>& /*node1*/,
+                         const NodeBase<BV>& /*node2*/) {
+    return 0;
+  }
+};
+
+//==============================================================================
+template <typename BV>
+size_t select(const NodeBase<BV>& query, const NodeBase<BV>& node1,
+              const NodeBase<BV>& node2) {
+  return SelectImpl<CoalScalar, BV>::run(query, node1, node2);
+}
+
+//==============================================================================
+template <typename BV>
+size_t select(const BV& query, const NodeBase<BV>& node1,
+              const NodeBase<BV>& node2) {
+  return SelectImpl<CoalScalar, BV>::run(query, node1, node2);
+}
+
+//==============================================================================
+template <typename S>
+struct SelectImpl<S, AABB> {
+  static std::size_t run(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;
+    Vec3s v = bv.min_ + bv.max_;
+    Vec3s v1 = v - (bv1.min_ + bv1.max_);
+    Vec3s v2 = v - (bv2.min_ + bv2.max_);
+    CoalScalar d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]);
+    CoalScalar d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]);
+    return (d1 < d2) ? 0 : 1;
+  }
+
+  static std::size_t run(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;
+    Vec3s v = bv.min_ + bv.max_;
+    Vec3s v1 = v - (bv1.min_ + bv1.max_);
+    Vec3s v2 = v - (bv2.min_ + bv2.max_);
+    CoalScalar d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]);
+    CoalScalar d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]);
+    return (d1 < d2) ? 0 : 1;
+  }
+};
+
+}  // namespace detail
+}  // namespace coal
+
+#endif

include/coal/broadphase/detail/hierarchy_tree.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, 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 COAL_HIERARCHY_TREE_H
+#define COAL_HIERARCHY_TREE_H
+
+#include <vector>
+#include <map>
+#include <functional>
+#include <iostream>
+#include "coal/warning.hh"
+#include "coal/BV/AABB.h"
+#include "coal/broadphase/detail/morton.h"
+#include "coal/broadphase/detail/node_base.h"
+
+namespace coal {
+
+namespace detail {
+
+/// @brief Class for hierarchy tree structure
+template <typename BV>
+class HierarchyTree {
+ public:
+  typedef NodeBase<BV> Node;
+
+  /// @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<Node*>& leaves, int level = 0);
+
+  /// @brief Insest a node
+  Node* insert(const BV& bv, void* data);
+
+  /// @brief Remove a leaf node
+  void remove(Node* leaf);
+
+  /// @brief Clear the tree
+  void clear();
+
+  /// @brief Whether the tree is empty
+  bool empty() const;
+
+  /// @brief Updates a `leaf` node. A use case is when the bounding volume
+  /// of an object changes. Ensure every parent node has its bounding volume
+  /// expand or shrink to fit the bounding volumes of its children.
+  /// @note Strangely the structure of the tree may change even if the
+  ///       bounding volume of the `leaf` node does not change. It is just
+  ///       another valid tree of the same set of objects.  This is because
+  ///       update() works by first removing `leaf` and then inserting `leaf`
+  ///       back. The structural change could be as simple as switching the
+  ///       order of two leaves if the sibling of the `leaf` is also a leaf.
+  ///       Or it could be more complicated if the sibling is an internal
+  ///       node.
+  void update(Node* leaf, int lookahead_level = -1);
+
+  /// @brief update the tree when the bounding volume of a given leaf has
+  /// changed
+  bool update(Node* leaf, const BV& bv);
+
+  /// @brief update one leaf's bounding volume, with prediction
+  bool update(Node* leaf, const BV& bv, const Vec3s& vel, CoalScalar margin);
+
+  /// @brief update one leaf's bounding volume, with prediction
+  bool update(Node* leaf, const BV& bv, const Vec3s& 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 Node* root, std::vector<Node*>& leaves) const;
+
+  /// @brief number of leaves in the tree
+  size_t size() const;
+
+  /// @brief get the root of the tree
+  Node* getRoot() const;
+
+  Node*& getRoot();
+
+  /// @brief print the tree in a recursive way
+  void print(Node* root, int depth);
+
+ private:
+  typedef typename std::vector<NodeBase<BV>*>::iterator NodeVecIterator;
+  typedef
+      typename std::vector<NodeBase<BV>*>::const_iterator NodeVecConstIterator;
+
+  struct SortByMorton {
+    bool operator()(const Node* a, const Node* b) const {
+      return a->code < b->code;
+    }
+  };
+
+  /// @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
+  Node* topdown(const NodeVecIterator lbeg, const NodeVecIterator lend);
+
+  /// @brief compute the maximum height of a subtree rooted from a given node
+  size_t getMaxHeight(Node* node) const;
+
+  /// @brief compute the maximum depth of a subtree rooted from a given node
+  void getMaxDepth(Node* 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.
+  Node* 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.
+  Node* 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<Node*>& 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<Node*>& 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<Node*>& 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<Node*>& leaves);
+
+  Node* mortonRecurse_0(const NodeVecIterator lbeg, const NodeVecIterator lend,
+                        const uint32_t& split, int bits);
+
+  Node* mortonRecurse_1(const NodeVecIterator lbeg, const NodeVecIterator lend,
+                        const uint32_t& split, int bits);
+
+  Node* mortonRecurse_2(const NodeVecIterator lbeg, const NodeVecIterator lend);
+
+  /// @brief update one leaf node's bounding volume
+  void update_(Node* leaf, const BV& bv);
+
+  /// @brief sort node n and its parent according to their memory position
+  Node* sort(Node* n, Node*& r);
+
+  /// @brief Insert a leaf node and also update its ancestors. Maintain the
+  /// tree as a full binary tree (every interior node has exactly two children).
+  /// Furthermore, adjust the BV of interior nodes so that each parent's BV
+  /// tightly fits its children's BVs.
+  /// @param sub_root The root of the subtree into which we will insert the
+  //                  leaf node.
+  void insertLeaf(Node* const sub_root, Node* const leaf);
+
+  /// @brief Remove a leaf. Maintain the tree as a full binary tree (every
+  /// interior node has exactly two children). Furthermore, adjust the BV of
+  /// interior nodes so that each parent's BV tightly fits its children's BVs.
+  /// @note The leaf node itself is not deleted yet, but all the unnecessary
+  ///       internal nodes are deleted.
+  /// @returns the root of the subtree containing the nodes whose BVs were
+  //           adjusted.
+  Node* removeLeaf(Node* const leaf);
+
+  /// @brief Delete all internal nodes and return all leaves nodes with given
+  /// depth from root
+  void fetchLeaves(Node* root, std::vector<Node*>& leaves, int depth = -1);
+
+  static size_t indexOf(Node* node);
+
+  /// @brief create one node (leaf or internal)
+  Node* createNode(Node* parent, const BV& bv, void* data);
+
+  Node* createNode(Node* parent, const BV& bv1, const BV& bv2, void* data);
+
+  Node* createNode(Node* parent, void* data);
+
+  void deleteNode(Node* node);
+
+  void recurseDeleteNode(Node* node);
+
+  void recurseRefit(Node* node);
+
+ protected:
+  Node* root_node;
+
+  size_t n_leaves;
+
+  unsigned int opath;
+
+  /// This is a one Node cache, the reason is that we need to remove a node and
+  /// then add it again frequently.
+  Node* 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;
+};
+
+/// @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);
+
+/// @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);
+
+/// @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);
+
+}  // namespace detail
+}  // namespace coal
+
+#include "coal/broadphase/detail/hierarchy_tree-inl.h"
+
+#endif

include/coal/broadphase/detail/hierarchy_tree_array-inl.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, 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 COAL_HIERARCHY_TREE_ARRAY_INL_H
+#define COAL_HIERARCHY_TREE_ARRAY_INL_H
+
+#include "coal/broadphase/detail/hierarchy_tree_array.h"
+
+#include <algorithm>
+#include <iostream>
+
+namespace coal {
+
+namespace detail {
+
+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 Node[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(Node* 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(Node* leaves, int n_leaves_) {
+  clear();
+
+  n_leaves = (size_t)n_leaves_;
+  root_node = NULL_NODE;
+  nodes = new Node[n_leaves * 2];
+  std::copy(leaves, leaves + n_leaves, nodes);
+  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(Node* leaves, int n_leaves_) {
+  clear();
+
+  n_leaves = (size_t)n_leaves_;
+  root_node = NULL_NODE;
+  nodes = new Node[n_leaves * 2];
+  std::copy(leaves, leaves + n_leaves, nodes);
+  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<CoalScalar, uint32_t> 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;
+
+  const SortByMorton comp{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(Node* leaves, int n_leaves_) {
+  clear();
+
+  n_leaves = (size_t)n_leaves_;
+  root_node = NULL_NODE;
+  nodes = new Node[n_leaves * 2];
+  std::copy(leaves, leaves + n_leaves, nodes);
+  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<CoalScalar, uint32_t> 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;
+
+  const SortByMorton comp{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(Node* leaves, int n_leaves_) {
+  clear();
+
+  n_leaves = (size_t)n_leaves_;
+  root_node = NULL_NODE;
+  nodes = new Node[n_leaves * 2];
+  std::copy(leaves, leaves + n_leaves, nodes);
+  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<CoalScalar, uint32_t> 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;
+
+  const SortByMorton comp{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 Node[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 Vec3s& vel,
+                               CoalScalar margin) {
+  COAL_UNUSED_VARIABLE(bv);
+  COAL_UNUSED_VARIABLE(vel);
+  COAL_UNUSED_VARIABLE(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 Vec3s& vel) {
+  COAL_UNUSED_VARIABLE(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) {
+    Node* leaves = new Node[n_leaves];
+    Node* leaves_ = leaves;
+    extractLeaves(root_node, leaves_);
+    root_node = NULL_NODE;
+    std::copy(leaves, leaves + n_leaves, nodes);
+    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) {
+    Node* leaves = new Node[n_leaves];
+    Node* leaves_ = leaves;
+    extractLeaves(root_node, leaves_);
+    root_node = NULL_NODE;
+    std::copy(leaves, leaves + n_leaves, nodes);
+    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 = (int)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, Node*& 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>::Node* 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 << " ";
+  Node* 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 = nullptr, *min_it2 = nullptr;
+    CoalScalar min_size = (std::numeric_limits<CoalScalar>::max)();
+    for (size_t* it1 = lbeg; it1 < lcur_end; ++it1) {
+      for (size_t* it2 = it1 + 1; it2 < lcur_end; ++it2) {
+        CoalScalar 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, nullptr);
+    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) {
+  long 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;
+
+      size_t best_axis = 0;
+      CoalScalar 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, nullptr);
+      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) {
+  long num_leaves = lend - lbeg;
+  if (num_leaves > 1) {
+    if (num_leaves > bu_threshold) {
+      Vec3s 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 /= static_cast<CoalScalar>(num_leaves);
+      int best_axis = -1;
+      int bestmidp = (int)num_leaves;
+      int splitcount[3][2] = {{0, 0}, {0, 0}, {0, 0}};
+      for (size_t* i = lbeg; i < lend; ++i) {
+        Vec3s x = nodes[*i].bv.center() - split_p;
+        for (int 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 = (int)i;
+            bestmidp = midp;
+          }
+        }
+      }
+
+      if (best_axis < 0) best_axis = 0;
+
+      CoalScalar 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, nullptr);
+      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 uint32_t& split, int bits) {
+  long num_leaves = lend - lbeg;
+  if (num_leaves > 1) {
+    if (bits > 0) {
+      const SortByMorton comp{nodes, split};
+      size_t* lcenter = std::lower_bound(lbeg, lend, NULL_NODE, comp);
+
+      if (lcenter == lbeg) {
+        uint32_t split2 = split | (1 << (bits - 1));
+        return mortonRecurse_0(lbeg, lend, split2, bits - 1);
+      } else if (lcenter == lend) {
+        uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
+        return mortonRecurse_0(lbeg, lend, split1, bits - 1);
+      } else {
+        uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
+        uint32_t 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, nullptr);
+        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 uint32_t& split, int bits) {
+  long num_leaves = lend - lbeg;
+  if (num_leaves > 1) {
+    if (bits > 0) {
+      const SortByMorton comp{nodes, split};
+      size_t* lcenter = std::lower_bound(lbeg, lend, NULL_NODE, comp);
+
+      if (lcenter == lbeg) {
+        uint32_t split2 = split | (1 << (bits - 1));
+        return mortonRecurse_1(lbeg, lend, split2, bits - 1);
+      } else if (lcenter == lend) {
+        uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
+        return mortonRecurse_1(lbeg, lend, split1, bits - 1);
+      } else {
+        uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
+        uint32_t 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, nullptr);
+        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, nullptr);
+      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) {
+  long 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, nullptr);
+    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, nullptr);
+    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 == 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) {
+    Node* old_nodes = nodes;
+    n_nodes_alloc *= 2;
+    nodes = new Node[n_nodes_alloc];
+    std::copy(old_nodes, old_nodes + n_nodes, nodes);
+    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, Node*& 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++;
+  }
+}
+
+//==============================================================================
+template <typename BV>
+nodeBaseLess<BV>::nodeBaseLess(const NodeBase<BV>* nodes_, size_t d_)
+    : nodes(nodes_), d(d_) {
+  // Do nothing
+}
+
+//==============================================================================
+template <typename BV>
+bool nodeBaseLess<BV>::operator()(size_t i, size_t j) const {
+  if (nodes[i].bv.center()[(int)d] < nodes[j].bv.center()[(int)d]) return true;
+
+  return false;
+}
+
+//==============================================================================
+template <typename S, typename BV>
+struct SelectImpl {
+  static bool run(size_t query, size_t node1, size_t node2,
+                  NodeBase<BV>* nodes) {
+    COAL_UNUSED_VARIABLE(query);
+    COAL_UNUSED_VARIABLE(node1);
+    COAL_UNUSED_VARIABLE(node2);
+    COAL_UNUSED_VARIABLE(nodes);
+
+    return 0;
+  }
+
+  static bool run(const BV& query, size_t node1, size_t node2,
+                  NodeBase<BV>* nodes) {
+    COAL_UNUSED_VARIABLE(query);
+    COAL_UNUSED_VARIABLE(node1);
+    COAL_UNUSED_VARIABLE(node2);
+    COAL_UNUSED_VARIABLE(nodes);
+
+    return 0;
+  }
+};
+
+//==============================================================================
+template <typename BV>
+size_t select(size_t query, size_t node1, size_t node2, NodeBase<BV>* nodes) {
+  return SelectImpl<CoalScalar, BV>::run(query, node1, node2, nodes);
+}
+
+//==============================================================================
+template <typename BV>
+size_t select(const BV& query, size_t node1, size_t node2,
+              NodeBase<BV>* nodes) {
+  return SelectImpl<CoalScalar, BV>::run(query, node1, node2, nodes);
+}
+
+//==============================================================================
+template <typename S>
+struct SelectImpl<S, AABB> {
+  static bool run(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;
+    Vec3s v = bv.min_ + bv.max_;
+    Vec3s v1 = v - (bv1.min_ + bv1.max_);
+    Vec3s v2 = v - (bv2.min_ + bv2.max_);
+    CoalScalar d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]);
+    CoalScalar d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]);
+    return (d1 < d2) ? 0 : 1;
+  }
+
+  static bool run(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;
+    Vec3s v = bv.min_ + bv.max_;
+    Vec3s v1 = v - (bv1.min_ + bv1.max_);
+    Vec3s v2 = v - (bv2.min_ + bv2.max_);
+    CoalScalar d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]);
+    CoalScalar d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]);
+    return (d1 < d2) ? 0 : 1;
+  }
+};
+
+}  // namespace implementation_array
+}  // namespace detail
+}  // namespace coal
+
+#endif