include/fcl/distance_func_matrix.h → include/coal/distance_func_matrix.h

@@ -35,32 +35,40 @@
 
 /** \author Jia Pan */
 
-#ifndef FCL_DISTANCE_FUNC_MATRIX_H
-#define FCL_DISTANCE_FUNC_MATRIX_H
+#ifndef COAL_DISTANCE_FUNC_MATRIX_H
+#define COAL_DISTANCE_FUNC_MATRIX_H
 
-#include "fcl/collision_object.h"
-#include "fcl/collision_data.h"
+#include "coal/collision_object.h"
+#include "coal/collision_data.h"
+#include "coal/narrowphase/narrowphase.h"
 
-namespace fcl
-{
+namespace coal {
 
-/// @brief distance matrix stores the functions for distance between different types of objects and provides a uniform call interface
-template<typename NarrowPhaseSolver>
-struct DistanceFunctionMatrix
-{
+/// @brief distance matrix stores the functions for distance between different
+/// types of objects and provides a uniform call interface
+struct COAL_DLLAPI DistanceFunctionMatrix {
   /// @brief the uniform call interface for distance: for distance, we need know
-  /// 1. two objects o1 and o2 and their configuration in world coordinate tf1 and tf2;
-  /// 2. the solver for narrow phase collision, this is for distance computation between geometric shapes;
-  /// 3. the request setting for distance (e.g., whether need to return nearest points);
-  typedef FCL_REAL (*DistanceFunc)(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
-                                   const DistanceRequest& request, DistanceResult& result);
-  
-  /// @brief each item in the distance matrix is a function to handle distance between objects of type1 and type2
+  /// 1. two objects o1 and o2 and their configuration in world coordinate tf1
+  /// and tf2;
+  /// 2. the solver for narrow phase collision, this is for distance computation
+  /// between geometric shapes;
+  /// 3. the request setting for distance (e.g., whether need to return nearest
+  /// points);
+  typedef CoalScalar (*DistanceFunc)(const CollisionGeometry* o1,
+                                     const Transform3s& tf1,
+                                     const CollisionGeometry* o2,
+                                     const Transform3s& tf2,
+                                     const GJKSolver* nsolver,
+                                     const DistanceRequest& request,
+                                     DistanceResult& result);
+
+  /// @brief each item in the distance matrix is a function to handle distance
+  /// between objects of type1 and type2
   DistanceFunc distance_matrix[NODE_COUNT][NODE_COUNT];
 
   DistanceFunctionMatrix();
 };
 
-}
+}  // namespace coal
 
 #endif

include/fcl/doc.hh → include/coal/doc.hh

@@ -32,16 +32,47 @@
 //  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 //  POSSIBILITY OF SUCH DAMAGE.
 
+namespace coal {
+
 /// \mainpage
-/// \section fcl_introduction Introduction 
+/// \anchor coal_documentation
+///
+/// \section coal_introduction Introduction
 ///
-/// Fcl is a library for collision detection and distance computation between
+/// Coal is a modified version the FCL libraries.
+///
+/// It is a library for collision detection and distance computation between
 /// various types of geometric shapes reprensented either by
-/// \li basic shapes (fcl::ShapeBase) like box, sphere, cylinders, ...
-/// \li or by bounding volume hierarchies of various types (fcl::BVHModel)
+/// \li basic shapes (coal::ShapeBase) like box, sphere, cylinders, ...
+/// \li or by bounding volume hierarchies of various types (coal::BVHModel)
 ///
-/// \section fcl_howto Using fcl
+/// \par Using Coal
 ///
 /// The main entry points to the library are functions
-/// \li fcl::collide and
-/// \li fcl::distance.
+/// \li coal::collide(const CollisionObject*, const CollisionObject*, const
+/// CollisionRequest&, CollisionResult&) \li coal::distance(const
+/// CollisionObject*, const CollisionObject*, const DistanceRequest&,
+/// DistanceResult&)
+///
+/// \section coal_collision_and_distance_lower_bound_computation Collision
+/// detection and distance lower bound
+///
+/// Collision queries can return a distance lower bound between the two objects,
+/// which is computationally cheaper than computing the real distance.
+/// The following figure shows the returned result in
+/// CollisionResult::distance_lower_bound and DistanceResult::min_distance,
+/// with respect to the objects real distance.
+///
+/// \image html distance_computation.png
+///
+/// The two parameters refer to CollisionRequest::security_margin and
+/// CollisionRequest::break_distance.
+/// \note In the green hatched area, the distance lower bound is not known. It
+/// is only guaranted that it will be inferior to
+/// <em>distance - security_margin</em> and superior to \em break_distance.
+/// \note If CollisionRequest::security_margin is set to -inf, no collision test
+/// is performed by function coal::collide or class
+/// coal::ComputeCollision and the objects are considered as not
+/// colliding.
+
+}  // namespace coal

include/coal/fwd.hh

+//
+// Software License Agreement (BSD License)
+//
+//  Copyright (c) 2014, CNRS-LAAS
+//  Copyright (c) 2022-2024, Inria
+//  Author: Florent Lamiraux
+//
+//  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 CNRS-LAAS 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_FWD_HH
+#define COAL_FWD_HH
+
+#include <cassert>
+#include <memory>
+#include <sstream>
+#include <stdexcept>
+
+#include "coal/config.hh"
+#include "coal/deprecated.hh"
+#include "coal/warning.hh"
+
+#if _WIN32
+#define COAL_PRETTY_FUNCTION __FUNCSIG__
+#else
+#define COAL_PRETTY_FUNCTION __PRETTY_FUNCTION__
+#endif
+
+#define COAL_UNUSED_VARIABLE(var) (void)(var)
+
+#ifdef NDEBUG
+#define COAL_ONLY_USED_FOR_DEBUG(var) COAL_UNUSED_VARIABLE(var)
+#else
+#define COAL_ONLY_USED_FOR_DEBUG(var)
+#endif
+
+#define COAL_THROW_PRETTY(message, exception)              \
+  {                                                        \
+    std::stringstream ss;                                  \
+    ss << "From file: " << __FILE__ << "\n";               \
+    ss << "in function: " << COAL_PRETTY_FUNCTION << "\n"; \
+    ss << "at line: " << __LINE__ << "\n";                 \
+    ss << "message: " << message << "\n";                  \
+    throw exception(ss.str());                             \
+  }
+
+#ifdef COAL_TURN_ASSERT_INTO_EXCEPTION
+#define COAL_ASSERT(check, message, exception) \
+  do {                                         \
+    if (!(check)) {                            \
+      COAL_THROW_PRETTY(message, exception);   \
+    }                                          \
+  } while (0)
+#else
+#define COAL_ASSERT(check, message, exception) \
+  {                                            \
+    COAL_UNUSED_VARIABLE(exception(message));  \
+    assert((check) && message);                \
+  }
+#endif
+
+#if (__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1600))
+#define COAL_WITH_CXX11_SUPPORT
+#endif
+
+#if defined(__GNUC__) || defined(__clang__)
+#define COAL_COMPILER_DIAGNOSTIC_PUSH _Pragma("GCC diagnostic push")
+#define COAL_COMPILER_DIAGNOSTIC_POP _Pragma("GCC diagnostic pop")
+#define COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS \
+  _Pragma("GCC diagnostic ignored \"-Wdeprecated-declarations\"")
+
+// GCC version 4.6 and higher supports -Wmaybe-uninitialized
+#if (defined(__GNUC__) && \
+     ((__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 6)))
+#define COAL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED \
+  _Pragma("GCC diagnostic ignored \"-Wmaybe-uninitialized\"")
+// Use __has_warning with clang. Clang introduced it in 2024 (3.5+)
+#elif (defined(__clang__) && defined(__has_warning) && \
+       __has_warning("-Wmaybe-uninitialized"))
+#define COAL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED \
+  _Pragma("clang diagnostic ignored \"-Wmaybe-uninitialized\"")
+#else
+#define COAL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
+#endif
+#elif defined(WIN32)
+#define COAL_COMPILER_DIAGNOSTIC_PUSH _Pragma("warning(push)")
+#define COAL_COMPILER_DIAGNOSTIC_POP _Pragma("warning(pop)")
+#define COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS \
+  _Pragma("warning(disable : 4996)")
+#define COAL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED \
+  _Pragma("warning(disable : 4700)")
+#else
+#define COAL_COMPILER_DIAGNOSTIC_PUSH
+#define COAL_COMPILER_DIAGNOSTIC_POP
+#define COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+#define COAL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
+#endif  // __GNUC__
+
+namespace coal {
+using std::dynamic_pointer_cast;
+using std::make_shared;
+using std::shared_ptr;
+
+class CollisionObject;
+typedef shared_ptr<CollisionObject> CollisionObjectPtr_t;
+typedef shared_ptr<const CollisionObject> CollisionObjectConstPtr_t;
+class CollisionGeometry;
+typedef shared_ptr<CollisionGeometry> CollisionGeometryPtr_t;
+typedef shared_ptr<const CollisionGeometry> CollisionGeometryConstPtr_t;
+class Transform3s;
+
+class AABB;
+
+class BVHModelBase;
+typedef shared_ptr<BVHModelBase> BVHModelPtr_t;
+
+class OcTree;
+typedef shared_ptr<OcTree> OcTreePtr_t;
+typedef shared_ptr<const OcTree> OcTreeConstPtr_t;
+}  // namespace coal
+
+#ifdef COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL
+namespace hpp {
+namespace fcl {
+using namespace coal;
+using Transform3f = Transform3s;  // For backward compatibility
+}  // namespace fcl
+}  // namespace hpp
+#endif
+
+#endif  // COAL_FWD_HH

include/coal/hfield.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2021-2024, 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.
+ */
+
+/** \author Justin Carpentier */
+
+#ifndef COAL_HEIGHT_FIELD_H
+#define COAL_HEIGHT_FIELD_H
+
+#include "coal/fwd.hh"
+#include "coal/data_types.h"
+#include "coal/collision_object.h"
+#include "coal/BV/BV_node.h"
+#include "coal/BVH/BVH_internal.h"
+
+#include <vector>
+
+namespace coal {
+
+/// @addtogroup Construction_Of_HeightField
+/// @{
+
+struct COAL_DLLAPI HFNodeBase {
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  enum class FaceOrientation {
+    TOP = 1,
+    BOTTOM = 1,
+    NORTH = 2,
+    EAST = 4,
+    SOUTH = 8,
+    WEST = 16
+  };
+
+  /// @brief An index for first child node or primitive
+  /// If the value is positive, it is the index of the first child bv node
+  /// If the value is negative, it is -(primitive index + 1)
+  /// Zero is not used.
+  size_t first_child;
+
+  Eigen::DenseIndex x_id, x_size;
+  Eigen::DenseIndex y_id, y_size;
+
+  CoalScalar max_height;
+  int contact_active_faces;
+
+  /// @brief Default constructor
+  HFNodeBase()
+      : first_child(0),
+        x_id(-1),
+        x_size(0),
+        y_id(-1),
+        y_size(0),
+        max_height(std::numeric_limits<CoalScalar>::lowest()),
+        contact_active_faces(0) {}
+
+  /// @brief Comparison operator
+  bool operator==(const HFNodeBase& other) const {
+    return first_child == other.first_child && x_id == other.x_id &&
+           x_size == other.x_size && y_id == other.y_id &&
+           y_size == other.y_size && max_height == other.max_height &&
+           contact_active_faces == other.contact_active_faces;
+  }
+
+  /// @brief Difference operator
+  bool operator!=(const HFNodeBase& other) const { return !(*this == other); }
+
+  /// @brief Whether current node is a leaf node (i.e. contains a primitive
+  /// index)
+  inline bool isLeaf() const { return x_size == 1 && y_size == 1; }
+
+  /// @brief Return the index of the first child. The index is referred to the
+  /// bounding volume array (i.e. bvs) in BVHModel
+  inline size_t leftChild() const { return first_child; }
+
+  /// @brief Return the index of the second child. The index is referred to the
+  /// bounding volume array (i.e. bvs) in BVHModel
+  inline size_t rightChild() const { return first_child + 1; }
+
+  inline Eigen::Vector2i leftChildIndexes() const {
+    return Eigen::Vector2i(x_id, y_id);
+  }
+  inline Eigen::Vector2i rightChildIndexes() const {
+    return Eigen::Vector2i(x_id + x_size / 2, y_id + y_size / 2);
+  }
+};
+
+inline HFNodeBase::FaceOrientation operator&(HFNodeBase::FaceOrientation a,
+                                             HFNodeBase::FaceOrientation b) {
+  return HFNodeBase::FaceOrientation(int(a) & int(b));
+}
+
+inline int operator&(int a, HFNodeBase::FaceOrientation b) {
+  return a & int(b);
+}
+
+template <typename BV>
+struct COAL_DLLAPI HFNode : public HFNodeBase {
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  typedef HFNodeBase Base;
+
+  /// @brief bounding volume storing the geometry
+  BV bv;
+
+  /// @brief Equality operator
+  bool operator==(const HFNode& other) const {
+    return Base::operator==(other) && bv == other.bv;
+  }
+
+  /// @brief Difference operator
+  bool operator!=(const HFNode& other) const { return !(*this == other); }
+
+  /// @brief Check whether two BVNode collide
+  bool overlap(const HFNode& other) const { return bv.overlap(other.bv); }
+  /// @brief Check whether two BVNode collide
+  bool overlap(const HFNode& other, const CollisionRequest& request,
+               CoalScalar& sqrDistLowerBound) const {
+    return bv.overlap(other.bv, request, sqrDistLowerBound);
+  }
+
+  /// @brief Compute the distance between two BVNode. P1 and P2, if not NULL and
+  /// the underlying BV supports distance, return the nearest points.
+  CoalScalar distance(const HFNode& other, Vec3s* P1 = NULL,
+                      Vec3s* P2 = NULL) const {
+    return bv.distance(other.bv, P1, P2);
+  }
+
+  /// @brief Access to the center of the BV
+  Vec3s getCenter() const { return bv.center(); }
+
+  /// @brief Access to the orientation of the BV
+  coal::Matrix3s::IdentityReturnType getOrientation() const {
+    return Matrix3s::Identity();
+  }
+
+  virtual ~HFNode() {}
+};
+
+namespace details {
+
+template <typename BV>
+struct UpdateBoundingVolume {
+  static void run(const Vec3s& pointA, const Vec3s& pointB, BV& bv) {
+    AABB bv_aabb(pointA, pointB);
+    //      AABB bv_aabb;
+    //      bv_aabb.update(pointA,pointB);
+    convertBV(bv_aabb, bv);
+  }
+};
+
+template <>
+struct UpdateBoundingVolume<AABB> {
+  static void run(const Vec3s& pointA, const Vec3s& pointB, AABB& bv) {
+    AABB bv_aabb(pointA, pointB);
+    convertBV(bv_aabb, bv);
+    //      bv.update(pointA,pointB);
+  }
+};
+
+}  // namespace details
+
+/// @brief Data structure depicting a height field given by the base grid
+/// dimensions and the elevation along the grid. \tparam BV one of the bounding
+/// volume class in \ref Bounding_Volume.
+///
+/// An height field is defined by its base dimensions along the X and Y axes and
+/// a set ofpoints defined by their altitude, regularly dispatched on the grid.
+/// The height field is centered at the origin and the corners of the geometry
+/// correspond to the following coordinates [± x_dim/2; ± y_dim/2].
+template <typename BV>
+class COAL_DLLAPI HeightField : public CollisionGeometry {
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  typedef CollisionGeometry Base;
+
+  typedef HFNode<BV> Node;
+  typedef std::vector<Node, Eigen::aligned_allocator<Node>> BVS;
+
+  /// @brief Constructing an empty HeightField
+  HeightField()
+      : CollisionGeometry(),
+        min_height((std::numeric_limits<CoalScalar>::min)()),
+        max_height((std::numeric_limits<CoalScalar>::max)()) {}
+
+  /// @brief Constructing an HeightField from its base dimensions and the set of
+  /// heights points.
+  ///        The granularity of the height field along X and Y direction is
+  ///        extraded from the Z grid.
+  ///
+  /// \param[in] x_dim Dimension along the X axis
+  /// \param[in] y_dim Dimension along the Y axis
+  /// \param[in] heights Matrix containing the altitude of each point compositng
+  /// the height field
+  /// \param[in] min_height Minimal height of the height field
+  ///
+  HeightField(const CoalScalar x_dim, const CoalScalar y_dim,
+              const MatrixXs& heights,
+              const CoalScalar min_height = (CoalScalar)0)
+      : CollisionGeometry() {
+    init(x_dim, y_dim, heights, min_height);
+  }
+
+  /// @brief Copy contructor from another HeightField
+  ///
+  /// \param[in] other to copy.
+  ///
+  HeightField(const HeightField& other)
+      : CollisionGeometry(other),
+        x_dim(other.x_dim),
+        y_dim(other.y_dim),
+        heights(other.heights),
+        min_height(other.min_height),
+        max_height(other.max_height),
+        x_grid(other.x_grid),
+        y_grid(other.y_grid),
+        bvs(other.bvs),
+        num_bvs(other.num_bvs) {}
+
+  /// @brief Returns a const reference of the grid along the X direction.
+  const VecXs& getXGrid() const { return x_grid; }
+  /// @brief Returns a const reference of the grid along the Y direction.
+  const VecXs& getYGrid() const { return y_grid; }
+
+  /// @brief Returns a const reference of the heights
+  const MatrixXs& getHeights() const { return heights; }
+
+  /// @brief Returns the dimension of the Height Field along the X direction.
+  CoalScalar getXDim() const { return x_dim; }
+  /// @brief Returns the dimension of the Height Field along the Y direction.
+  CoalScalar getYDim() const { return y_dim; }
+
+  /// @brief Returns the minimal height value of the Height Field.
+  CoalScalar getMinHeight() const { return min_height; }
+  /// @brief Returns the maximal height value of the Height Field.
+  CoalScalar getMaxHeight() const { return max_height; }
+
+  virtual HeightField<BV>* clone() const { return new HeightField(*this); }
+
+  const BVS& getNodes() const { return bvs; }
+
+  /// @brief deconstruction, delete mesh data related.
+  virtual ~HeightField() {}
+
+  /// @brief Compute the AABB for the HeightField, used for broad-phase
+  /// collision
+  void computeLocalAABB() {
+    const Vec3s A(x_grid[0], y_grid[0], min_height);
+    const Vec3s B(x_grid[x_grid.size() - 1], y_grid[y_grid.size() - 1],
+                  max_height);
+    const AABB aabb_(A, B);
+
+    aabb_radius = (A - B).norm() / 2.;
+    aabb_local = aabb_;
+    aabb_center = aabb_.center();
+  }
+
+  /// @brief Update Height Field height
+  void updateHeights(const MatrixXs& new_heights) {
+    if (new_heights.rows() != heights.rows() ||
+        new_heights.cols() != heights.cols())
+      COAL_THROW_PRETTY(
+          "The matrix containing the new heights values does not have the same "
+          "matrix size as the original one.\n"
+          "\tinput values - rows: "
+              << new_heights.rows() << " - cols: " << new_heights.cols() << "\n"
+              << "\texpected values - rows: " << heights.rows()
+              << " - cols: " << heights.cols() << "\n",
+          std::invalid_argument);
+
+    heights = new_heights.cwiseMax(min_height);
+    this->max_height = recursiveUpdateHeight(0);
+    assert(this->max_height == heights.maxCoeff());
+  }
+
+ protected:
+  void init(const CoalScalar x_dim, const CoalScalar y_dim,
+            const MatrixXs& heights, const CoalScalar min_height) {
+    this->x_dim = x_dim;
+    this->y_dim = y_dim;
+    this->heights = heights.cwiseMax(min_height);
+    this->min_height = min_height;
+    this->max_height = heights.maxCoeff();
+
+    const Eigen::DenseIndex NX = heights.cols(), NY = heights.rows();
+    assert(NX >= 2 && "The number of columns is too small.");
+    assert(NY >= 2 && "The number of rows is too small.");
+
+    x_grid = VecXs::LinSpaced(NX, -0.5 * x_dim, 0.5 * x_dim);
+    y_grid = VecXs::LinSpaced(NY, 0.5 * y_dim, -0.5 * y_dim);
+
+    // Allocate BVS
+    const size_t num_tot_bvs =
+        (size_t)(NX * NY) - 1 + (size_t)((NX - 1) * (NY - 1));
+    bvs.resize(num_tot_bvs);
+    num_bvs = 0;
+
+    // Build tree
+    buildTree();
+  }
+
+  /// @brief Get the object type: it is a HFIELD
+  OBJECT_TYPE getObjectType() const { return OT_HFIELD; }
+
+  Vec3s computeCOM() const { return Vec3s::Zero(); }
+
+  CoalScalar computeVolume() const { return 0; }
+
+  Matrix3s computeMomentofInertia() const { return Matrix3s::Zero(); }
+
+ protected:
+  /// @brief Dimensions in meters along X and Y directions
+  CoalScalar x_dim, y_dim;
+
+  /// @brief Elevation values in meters of the Height Field
+  MatrixXs heights;
+
+  /// @brief Minimal height of the Height Field: all values bellow min_height
+  /// will be discarded.
+  CoalScalar min_height, max_height;
+
+  /// @brief Grids along the X and Y directions. Useful for plotting or other
+  /// related things.
+  VecXs x_grid, y_grid;
+
+  /// @brief Bounding volume hierarchy
+  BVS bvs;
+  unsigned int num_bvs;
+
+  /// @brief Build the bounding volume hierarchy
+  int buildTree() {
+    num_bvs = 1;
+    const CoalScalar max_recursive_height =
+        recursiveBuildTree(0, 0, heights.cols() - 1, 0, heights.rows() - 1);
+    assert(max_recursive_height == max_height &&
+           "the maximal height is not correct");
+    COAL_UNUSED_VARIABLE(max_recursive_height);
+
+    bvs.resize(num_bvs);
+    return BVH_OK;
+  }
+
+  CoalScalar recursiveUpdateHeight(const size_t bv_id) {
+    HFNode<BV>& bv_node = bvs[bv_id];
+
+    CoalScalar max_height;
+    if (bv_node.isLeaf()) {
+      max_height = heights.block<2, 2>(bv_node.y_id, bv_node.x_id).maxCoeff();
+    } else {
+      CoalScalar max_left_height = recursiveUpdateHeight(bv_node.leftChild()),
+                 max_right_height = recursiveUpdateHeight(bv_node.rightChild());
+
+      max_height = (std::max)(max_left_height, max_right_height);
+    }
+
+    bv_node.max_height = max_height;
+
+    const Vec3s pointA(x_grid[bv_node.x_id], y_grid[bv_node.y_id], min_height);
+    const Vec3s pointB(x_grid[bv_node.x_id + bv_node.x_size],
+                       y_grid[bv_node.y_id + bv_node.y_size], max_height);
+
+    details::UpdateBoundingVolume<BV>::run(pointA, pointB, bv_node.bv);
+
+    return max_height;
+  }
+
+  CoalScalar recursiveBuildTree(const size_t bv_id,
+                                const Eigen::DenseIndex x_id,
+                                const Eigen::DenseIndex x_size,
+                                const Eigen::DenseIndex y_id,
+                                const Eigen::DenseIndex y_size) {
+    assert(x_id < heights.cols() && "x_id is out of bounds");
+    assert(y_id < heights.rows() && "y_id is out of bounds");
+    assert(x_size >= 0 && y_size >= 0 &&
+           "x_size or y_size are not of correct value");
+    assert(bv_id < bvs.size() && "bv_id exceeds the vector dimension");
+
+    HFNode<BV>& bv_node = bvs[bv_id];
+    CoalScalar max_height;
+    if (x_size == 1 &&
+        y_size == 1)  // don't build any BV for the current child node
+    {
+      max_height = heights.block<2, 2>(y_id, x_id).maxCoeff();
+    } else {
+      bv_node.first_child = num_bvs;
+      num_bvs += 2;
+
+      CoalScalar max_left_height = min_height, max_right_height = min_height;
+      if (x_size >= y_size)  // splitting along the X axis
+      {
+        Eigen::DenseIndex x_size_half = x_size / 2;
+        if (x_size == 1) x_size_half = 1;
+        max_left_height = recursiveBuildTree(bv_node.leftChild(), x_id,
+                                             x_size_half, y_id, y_size);
+
+        max_right_height =
+            recursiveBuildTree(bv_node.rightChild(), x_id + x_size_half,
+                               x_size - x_size_half, y_id, y_size);
+      } else  // splitting along the Y axis
+      {
+        Eigen::DenseIndex y_size_half = y_size / 2;
+        if (y_size == 1) y_size_half = 1;
+        max_left_height = recursiveBuildTree(bv_node.leftChild(), x_id, x_size,
+                                             y_id, y_size_half);
+
+        max_right_height =
+            recursiveBuildTree(bv_node.rightChild(), x_id, x_size,
+                               y_id + y_size_half, y_size - y_size_half);
+      }
+
+      max_height = (std::max)(max_left_height, max_right_height);
+    }
+
+    bv_node.max_height = max_height;
+    //    max_height = std::max(max_height,min_height);
+
+    const Vec3s pointA(x_grid[x_id], y_grid[y_id], min_height);
+    assert(x_id + x_size < x_grid.size());
+    assert(y_id + y_size < y_grid.size());
+    const Vec3s pointB(x_grid[x_id + x_size], y_grid[y_id + y_size],
+                       max_height);
+
+    details::UpdateBoundingVolume<BV>::run(pointA, pointB, bv_node.bv);
+    bv_node.x_id = x_id;
+    bv_node.y_id = y_id;
+    bv_node.x_size = x_size;
+    bv_node.y_size = y_size;
+
+    if (bv_node.isLeaf()) {
+      int& contact_active_faces = bv_node.contact_active_faces;
+      contact_active_faces |= int(HFNodeBase::FaceOrientation::TOP);
+      contact_active_faces |= int(HFNodeBase::FaceOrientation::BOTTOM);
+
+      if (bv_node.x_id == 0)  // first col
+        contact_active_faces |= int(HFNodeBase::FaceOrientation::WEST);
+
+      if (bv_node.y_id == 0)  // first row (TOP)
+        contact_active_faces |= int(HFNodeBase::FaceOrientation::NORTH);
+
+      if (bv_node.x_id + 1 == heights.cols() - 1)  // last col
+        contact_active_faces |= int(HFNodeBase::FaceOrientation::EAST);
+
+      if (bv_node.y_id + 1 == heights.rows() - 1)  // last row (BOTTOM)
+        contact_active_faces |= int(HFNodeBase::FaceOrientation::SOUTH);
+    }
+
+    return max_height;
+  }
+
+ public:
+  /// @brief Access the bv giving the its index
+  const HFNode<BV>& getBV(unsigned int i) const {
+    if (i >= num_bvs)
+      COAL_THROW_PRETTY("Index out of bounds", std::invalid_argument);
+    return bvs[i];
+  }
+
+  /// @brief Access the bv giving the its index
+  HFNode<BV>& getBV(unsigned int i) {
+    if (i >= num_bvs)
+      COAL_THROW_PRETTY("Index out of bounds", std::invalid_argument);
+    return bvs[i];
+  }
+
+  /// @brief Get the BV type: default is unknown
+  NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
+
+ private:
+  virtual bool isEqual(const CollisionGeometry& _other) const {
+    const HeightField* other_ptr = dynamic_cast<const HeightField*>(&_other);
+    if (other_ptr == nullptr) return false;
+    const HeightField& other = *other_ptr;
+
+    return x_dim == other.x_dim && y_dim == other.y_dim &&
+           heights == other.heights && min_height == other.min_height &&
+           max_height == other.max_height && x_grid == other.x_grid &&
+           y_grid == other.y_grid && bvs == other.bvs &&
+           num_bvs == other.num_bvs;
+  }
+};
+
+/// @brief Specialization of getNodeType() for HeightField with different BV
+/// types
+template <>
+NODE_TYPE HeightField<AABB>::getNodeType() const;
+
+template <>
+NODE_TYPE HeightField<OBB>::getNodeType() const;
+
+template <>
+NODE_TYPE HeightField<RSS>::getNodeType() const;
+
+template <>
+NODE_TYPE HeightField<kIOS>::getNodeType() const;
+
+template <>
+NODE_TYPE HeightField<OBBRSS>::getNodeType() const;
+
+template <>
+NODE_TYPE HeightField<KDOP<16>>::getNodeType() const;
+
+template <>
+NODE_TYPE HeightField<KDOP<18>>::getNodeType() const;
+
+template <>
+NODE_TYPE HeightField<KDOP<24>>::getNodeType() const;
+
+/// @}
+
+}  // namespace coal
+
+#endif

include/fcl/BVH/BV_fitter.h → include/coal/internal/BV_fitter.h

@@ -35,114 +35,120 @@
 
 /** \author Jia Pan */
 
-#ifndef FCL_BV_FITTER_H
-#define FCL_BV_FITTER_H
+#ifndef COAL_BV_FITTER_H
+#define COAL_BV_FITTER_H
 
-#include "fcl/BVH/BVH_internal.h"
-#include "fcl/BV/kIOS.h"
-#include "fcl/BV/OBBRSS.h"
+#include "coal/BVH/BVH_internal.h"
+#include "coal/BV/kIOS.h"
+#include "coal/BV/OBBRSS.h"
+#include "coal/BV/AABB.h"
 #include <iostream>
 
-namespace fcl
-{
+namespace coal {
 
 /// @brief Compute a bounding volume that fits a set of n points.
-template<typename BV>
-void fit(Vec3f* ps, int n, BV& bv)
-{
-  for(int i = 0; i < n; ++i)
+template <typename BV>
+void fit(Vec3s* ps, unsigned int n, BV& bv) {
+  for (unsigned int i = 0; i < n; ++i)  // TODO(jcarpent): vectorize
   {
     bv += ps[i];
   }
 }
 
-template<>
-void fit<OBB>(Vec3f* ps, int n, OBB& bv);
+template <>
+void fit<OBB>(Vec3s* ps, unsigned int n, OBB& bv);
 
-template<>
-void fit<RSS>(Vec3f* ps, int n, RSS& bv);
+template <>
+void fit<RSS>(Vec3s* ps, unsigned int n, RSS& bv);
 
-template<>
-void fit<kIOS>(Vec3f* ps, int n, kIOS& bv);
+template <>
+void fit<kIOS>(Vec3s* ps, unsigned int n, kIOS& bv);
 
-template<>
-void fit<OBBRSS>(Vec3f* ps, int n, OBBRSS& bv);
+template <>
+void fit<OBBRSS>(Vec3s* ps, unsigned int n, OBBRSS& bv);
 
+template <>
+void fit<AABB>(Vec3s* ps, unsigned int n, AABB& bv);
 
-/// @brief Interface for fitting a bv given the triangles or points inside it.
-template<typename BV>
-class BVFitterBase
-{
-public:
-  /// @brief Set the primitives to be processed by the fitter
-  virtual void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_) = 0;
-
-  /// @brief Set the primitives to be processed by the fitter, for deformable mesh.
-  virtual void set(Vec3f* vertices_, Vec3f* prev_vertices_, Triangle* tri_indices_, BVHModelType type_) = 0;
-
-  /// @brief Compute the fitting BV
-  virtual BV fit(unsigned int* primitive_indices, int num_primitives) = 0;
-
-  /// @brief clear the temporary data generated.
-  virtual void clear() = 0;
-};
-
-/// @brief The class for the default algorithm fitting a bounding volume to a set of points
-template<typename BV>
-class BVFitter : public BVFitterBase<BV>
-{
-public:
+/// @brief The class for the default algorithm fitting a bounding volume to a
+/// set of points
+template <typename BV>
+class COAL_DLLAPI BVFitterTpl {
+ public:
   /// @brief default deconstructor
-  virtual ~BVFitter() {}
+  virtual ~BVFitterTpl() {}
 
   /// @brief Prepare the geometry primitive data for fitting
-  void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_)
-  {
+  void set(Vec3s* vertices_, Triangle* tri_indices_, BVHModelType type_) {
     vertices = vertices_;
     prev_vertices = NULL;
     tri_indices = tri_indices_;
     type = type_;
   }
 
-  /// @brief Prepare the geometry primitive data for fitting, for deformable mesh
-  void set(Vec3f* vertices_, Vec3f* prev_vertices_, Triangle* tri_indices_, BVHModelType type_)
-  {
+  /// @brief Prepare the geometry primitive data for fitting, for deformable
+  /// mesh
+  void set(Vec3s* vertices_, Vec3s* prev_vertices_, Triangle* tri_indices_,
+           BVHModelType type_) {
     vertices = vertices_;
     prev_vertices = prev_vertices_;
     tri_indices = tri_indices_;
     type = type_;
   }
 
-  /// @brief Compute a bounding volume that fits a set of primitives (points or triangles).
-  /// The primitive data was set by set function and primitive_indices is the primitive index relative to the data
-  BV fit(unsigned int* primitive_indices, int num_primitives)
-  {
+  /// @brief Compute the fitting BV
+  virtual BV fit(unsigned int* primitive_indices,
+                 unsigned int num_primitives) = 0;
+
+  /// @brief Clear the geometry primitive data
+  void clear() {
+    vertices = NULL;
+    prev_vertices = NULL;
+    tri_indices = NULL;
+    type = BVH_MODEL_UNKNOWN;
+  }
+
+ protected:
+  Vec3s* vertices;
+  Vec3s* prev_vertices;
+  Triangle* tri_indices;
+  BVHModelType type;
+};
+
+/// @brief The class for the default algorithm fitting a bounding volume to a
+/// set of points
+template <typename BV>
+class COAL_DLLAPI BVFitter : public BVFitterTpl<BV> {
+  typedef BVFitterTpl<BV> Base;
+
+ public:
+  /// @brief Compute a bounding volume that fits a set of primitives (points or
+  /// triangles). The primitive data was set by set function and
+  /// primitive_indices is the primitive index relative to the data
+  BV fit(unsigned int* primitive_indices, unsigned int num_primitives) {
     BV bv;
 
-    if(type == BVH_MODEL_TRIANGLES)             /// The primitive is triangle
+    if (type == BVH_MODEL_TRIANGLES)  /// The primitive is triangle
     {
-      for(int i = 0; i < num_primitives; ++i)
-      {
+      for (unsigned int i = 0; i < num_primitives; ++i) {
         Triangle t = tri_indices[primitive_indices[i]];
         bv += vertices[t[0]];
         bv += vertices[t[1]];
         bv += vertices[t[2]];
 
-        if(prev_vertices)                      /// can fitting both current and previous frame
+        if (prev_vertices)  /// can fitting both current and previous frame
         {
           bv += prev_vertices[t[0]];
           bv += prev_vertices[t[1]];
           bv += prev_vertices[t[2]];
         }
       }
-    }
-    else if(type == BVH_MODEL_POINTCLOUD)       /// The primitive is point
+    } else if (type == BVH_MODEL_POINTCLOUD)  /// The primitive is point
     {
-      for(int i = 0; i < num_primitives; ++i)
-      {
+      for (unsigned int i = 0; i < num_primitives; ++i) {
         bv += vertices[primitive_indices[i]];
 
-        if(prev_vertices)                       /// can fitting both current and previous frame
+        if (prev_vertices)  /// can fitting both current and previous frame
         {
           bv += prev_vertices[primitive_indices[i]];
         }
@@ -152,202 +158,63 @@ public:
     return bv;
   }
 
-  /// @brief Clear the geometry primitive data
-  void clear()
-  {
-    vertices = NULL;
-    prev_vertices = NULL;
-    tri_indices = NULL;
-    type = BVH_MODEL_UNKNOWN;
-  }
-
-private:
-
-  Vec3f* vertices;
-  Vec3f* prev_vertices;
-  Triangle* tri_indices;
-  BVHModelType type;
+ protected:
+  using Base::prev_vertices;
+  using Base::tri_indices;
+  using Base::type;
+  using Base::vertices;
 };
 
-
 /// @brief Specification of BVFitter for OBB bounding volume
-template<>
-class BVFitter<OBB> : public BVFitterBase<OBB>
-{
-public:
-  /// @brief Prepare the geometry primitive data for fitting
-  void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_)
-  {
-    vertices = vertices_;
-    prev_vertices = NULL;
-    tri_indices = tri_indices_;
-    type = type_;
-  }
-
-  /// @brief Prepare the geometry primitive data for fitting, for deformable mesh
-  void set(Vec3f* vertices_, Vec3f* prev_vertices_, Triangle* tri_indices_, BVHModelType type_)
-  {
-    vertices = vertices_;
-    prev_vertices = prev_vertices_;
-    tri_indices = tri_indices_;
-    type = type_;
-  }
-
-  /// @brief Compute a bounding volume that fits a set of primitives (points or triangles).
-  /// The primitive data was set by set function and primitive_indices is the primitive index relative to the data.
-  OBB fit(unsigned int* primitive_indices, int num_primitives);
-
-  /// brief Clear the geometry primitive data
-  void clear()
-  {
-    vertices = NULL;
-    prev_vertices = NULL;
-    tri_indices = NULL;
-    type = BVH_MODEL_UNKNOWN;
-  }
-
-private:
-
-  Vec3f* vertices;
-  Vec3f* prev_vertices;
-  Triangle* tri_indices;
-  BVHModelType type;
+template <>
+class COAL_DLLAPI BVFitter<OBB> : public BVFitterTpl<OBB> {
+ public:
+  /// @brief Compute a bounding volume that fits a set of primitives (points or
+  /// triangles). The primitive data was set by set function and
+  /// primitive_indices is the primitive index relative to the data.
+  OBB fit(unsigned int* primitive_indices, unsigned int num_primitives);
 };
 
-
 /// @brief Specification of BVFitter for RSS bounding volume
-template<>
-class BVFitter<RSS> : public BVFitterBase<RSS>
-{
-public:
-  /// brief Prepare the geometry primitive data for fitting
-  void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_)
-  {
-    vertices = vertices_;
-    prev_vertices = NULL;
-    tri_indices = tri_indices_;
-    type = type_;
-  }
-
-  /// @brief Prepare the geometry primitive data for fitting, for deformable mesh
-  void set(Vec3f* vertices_, Vec3f* prev_vertices_, Triangle* tri_indices_, BVHModelType type_)
-  {
-    vertices = vertices_;
-    prev_vertices = prev_vertices_;
-    tri_indices = tri_indices_;
-    type = type_;
-  }
-
-  /// @brief Compute a bounding volume that fits a set of primitives (points or triangles).
-  /// The primitive data was set by set function and primitive_indices is the primitive index relative to the data.
-  RSS fit(unsigned int* primitive_indices, int num_primitives);
-
-  /// @brief Clear the geometry primitive data
-  void clear()
-  {
-    vertices = NULL;
-    prev_vertices = NULL;
-    tri_indices = NULL;
-    type = BVH_MODEL_UNKNOWN;
-  }
-
-private:
-
-  Vec3f* vertices;
-  Vec3f* prev_vertices;
-  Triangle* tri_indices;
-  BVHModelType type;
+template <>
+class COAL_DLLAPI BVFitter<RSS> : public BVFitterTpl<RSS> {
+ public:
+  /// @brief Compute a bounding volume that fits a set of primitives (points or
+  /// triangles). The primitive data was set by set function and
+  /// primitive_indices is the primitive index relative to the data.
+  RSS fit(unsigned int* primitive_indices, unsigned int num_primitives);
 };
 
-
 /// @brief Specification of BVFitter for kIOS bounding volume
-template<>
-class BVFitter<kIOS> : public BVFitterBase<kIOS>
-{
-public:
-  /// @brief Prepare the geometry primitive data for fitting
-  void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_)
-  {
-    vertices = vertices_;
-    prev_vertices = NULL;
-    tri_indices = tri_indices_;
-    type = type_;
-  }
-
-  /// @brief Prepare the geometry primitive data for fitting
-  void set(Vec3f* vertices_, Vec3f* prev_vertices_, Triangle* tri_indices_, BVHModelType type_)
-  {
-    vertices = vertices_;
-    prev_vertices = prev_vertices_;
-    tri_indices = tri_indices_;
-    type = type_;
-  }
-
-  /// @brief Compute a bounding volume that fits a set of primitives (points or triangles).
-  /// The primitive data was set by set function and primitive_indices is the primitive index relative to the data.
-  kIOS fit(unsigned int* primitive_indices, int num_primitives);
-
-  /// @brief Clear the geometry primitive data
-  void clear()
-  {
-    vertices = NULL;
-    prev_vertices = NULL;
-    tri_indices = NULL;
-    type = BVH_MODEL_UNKNOWN;
-  }
-
-private:
-  Vec3f* vertices;
-  Vec3f* prev_vertices;
-  Triangle* tri_indices;
-  BVHModelType type;
+template <>
+class COAL_DLLAPI BVFitter<kIOS> : public BVFitterTpl<kIOS> {
+ public:
+  /// @brief Compute a bounding volume that fits a set of primitives (points or
+  /// triangles). The primitive data was set by set function and
+  /// primitive_indices is the primitive index relative to the data.
+  kIOS fit(unsigned int* primitive_indices, unsigned int num_primitives);
 };
 
-
 /// @brief Specification of BVFitter for OBBRSS bounding volume
-template<>
-class BVFitter<OBBRSS> : public BVFitterBase<OBBRSS>
-{
-public:
-  /// @brief Prepare the geometry primitive data for fitting
-  void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_)
-  {
-    vertices = vertices_;
-    prev_vertices = NULL;
-    tri_indices = tri_indices_;
-    type = type_;
-  }
-
-  /// @brief Prepare the geometry primitive data for fitting
-  void set(Vec3f* vertices_, Vec3f* prev_vertices_, Triangle* tri_indices_, BVHModelType type_)
-  {
-    vertices = vertices_;
-    prev_vertices = prev_vertices_;
-    tri_indices = tri_indices_;
-    type = type_;
-  }
-
-  /// @brief Compute a bounding volume that fits a set of primitives (points or triangles).
-  /// The primitive data was set by set function and primitive_indices is the primitive index relative to the data.
-  OBBRSS fit(unsigned int* primitive_indices, int num_primitives);
-
-  /// @brief Clear the geometry primitive data
-  void clear()
-  {
-    vertices = NULL;
-    prev_vertices = NULL;
-    tri_indices = NULL;
-    type = BVH_MODEL_UNKNOWN;
-  }
-
-private:
+template <>
+class COAL_DLLAPI BVFitter<OBBRSS> : public BVFitterTpl<OBBRSS> {
+ public:
+  /// @brief Compute a bounding volume that fits a set of primitives (points or
+  /// triangles). The primitive data was set by set function and
+  /// primitive_indices is the primitive index relative to the data.
+  OBBRSS fit(unsigned int* primitive_indices, unsigned int num_primitives);
+};
 
-  Vec3f* vertices;
-  Vec3f* prev_vertices;
-  Triangle* tri_indices;
-  BVHModelType type;
+/// @brief Specification of BVFitter for AABB bounding volume
+template <>
+class COAL_DLLAPI BVFitter<AABB> : public BVFitterTpl<AABB> {
+ public:
+  /// @brief Compute a bounding volume that fits a set of primitives (points or
+  /// triangles). The primitive data was set by set function and
+  /// primitive_indices is the primitive index relative to the data.
+  AABB fit(unsigned int* primitive_indices, unsigned int num_primitives);
 };
 
-}
+}  // namespace coal
 
 #endif

include/fcl/BVH/BV_splitter.h → include/coal/internal/BV_splitter.h

@@ -35,107 +35,85 @@
 
 /** \author Jia Pan */
 
-#ifndef FCL_BV_SPLITTER_H
-#define FCL_BV_SPLITTER_H
+#ifndef COAL_BV_SPLITTER_H
+#define COAL_BV_SPLITTER_H
 
-#include "fcl/BVH/BVH_internal.h"
-#include "fcl/BV/kIOS.h"
-#include "fcl/BV/OBBRSS.h"
+#include "coal/BVH/BVH_internal.h"
+#include "coal/BV/kIOS.h"
+#include "coal/BV/OBBRSS.h"
 #include <vector>
 #include <iostream>
 
-namespace fcl
-{
-
-/// @brief Base interface for BV splitting algorithm
-template<typename BV>
-class BVSplitterBase
-{
-public:
-  /// @brief Set the geometry data needed by the split rule
-  virtual void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_) = 0;
-
-  /// @brief Compute the split rule according to a subset of geometry and the corresponding BV node
-  virtual void computeRule(const BV& bv, unsigned int* primitive_indices, int num_primitives) = 0;
-
-  /// @brief Apply the split rule on a given point
-  virtual bool apply(const Vec3f& q) const = 0;
-
-  /// @brief Clear the geometry data set before
-  virtual void clear() = 0;
-};
-
+namespace coal {
 
 /// @brief Three types of split algorithms are provided in FCL as default
-enum SplitMethodType {SPLIT_METHOD_MEAN, SPLIT_METHOD_MEDIAN, SPLIT_METHOD_BV_CENTER};
-
+enum SplitMethodType {
+  SPLIT_METHOD_MEAN,
+  SPLIT_METHOD_MEDIAN,
+  SPLIT_METHOD_BV_CENTER
+};
 
 /// @brief A class describing the split rule that splits each BV node
-template<typename BV>
-class BVSplitter : public BVSplitterBase<BV>
-{
-public:
-
-  BVSplitter(SplitMethodType method) : split_method(method)
-  {
-  }
+template <typename BV>
+class BVSplitter {
+ public:
+  BVSplitter(SplitMethodType method)
+      : split_vector(0, 0, 0), split_method(method) {}
 
   /// @brief Default deconstructor
   virtual ~BVSplitter() {}
 
   /// @brief Set the geometry data needed by the split rule
-  void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_)
-  {
+  void set(Vec3s* vertices_, Triangle* tri_indices_, BVHModelType type_) {
     vertices = vertices_;
     tri_indices = tri_indices_;
     type = type_;
   }
 
-  /// @brief Compute the split rule according to a subset of geometry and the corresponding BV node
-  void computeRule(const BV& bv, unsigned int* primitive_indices, int num_primitives)
-  {
-    switch(split_method)
-    {
-    case SPLIT_METHOD_MEAN:
-      computeRule_mean(bv, primitive_indices, num_primitives);
-      break;
-    case SPLIT_METHOD_MEDIAN:
-      computeRule_median(bv, primitive_indices, num_primitives);
-      break;
-    case SPLIT_METHOD_BV_CENTER:
-      computeRule_bvcenter(bv, primitive_indices, num_primitives);
-      break;
-    default:
-      std::cerr << "Split method not supported" << std::endl;
+  /// @brief Compute the split rule according to a subset of geometry and the
+  /// corresponding BV node
+  void computeRule(const BV& bv, unsigned int* primitive_indices,
+                   unsigned int num_primitives) {
+    switch (split_method) {
+      case SPLIT_METHOD_MEAN:
+        computeRule_mean(bv, primitive_indices, num_primitives);
+        break;
+      case SPLIT_METHOD_MEDIAN:
+        computeRule_median(bv, primitive_indices, num_primitives);
+        break;
+      case SPLIT_METHOD_BV_CENTER:
+        computeRule_bvcenter(bv, primitive_indices, num_primitives);
+        break;
+      default:
+        std::cerr << "Split method not supported" << std::endl;
     }
   }
 
   /// @brief Apply the split rule on a given point
-  bool apply(const Vec3f& q) const
-  {
-    return q[split_axis] > split_value;
-  }
+  bool apply(const Vec3s& q) const { return q[split_axis] > split_value; }
 
   /// @brief Clear the geometry data set before
-  void clear()
-  {
+  void clear() {
     vertices = NULL;
     tri_indices = NULL;
     type = BVH_MODEL_UNKNOWN;
   }
 
-private:
-
-  /// @brief The axis based on which the split decision is made. For most BV, the axis is aligned with one of the world coordinate, so only split_axis is needed.
-  /// For oriented node, we can use a vector to make a better split decision.
+ protected:
+  /// @brief The axis based on which the split decision is made. For most BV,
+  /// the axis is aligned with one of the world coordinate, so only split_axis
+  /// is needed. For oriented node, we can use a vector to make a better split
+  /// decision.
   int split_axis;
-  Vec3f split_vector;
+  Vec3s split_vector;
 
-  /// @brief The split threshold, different primitives are splitted according whether their projection on the split_axis is larger or smaller than the threshold
-  FCL_REAL split_value;
+  /// @brief The split threshold, different primitives are splitted according
+  /// whether their projection on the split_axis is larger or smaller than the
+  /// threshold
+  CoalScalar split_value;
 
   /// @brief The mesh vertices or points handled by the splitter
-  Vec3f* vertices;
+  Vec3s* vertices;
 
   /// @brief The triangles handled by the splitter
   Triangle* tri_indices;
@@ -147,47 +125,43 @@ private:
   SplitMethodType split_method;
 
   /// @brief Split algorithm 1: Split the node from center
-  void computeRule_bvcenter(const BV& bv, unsigned int* primitive_indices, int num_primitives)
-  {
-    Vec3f center = bv.center();
+  void computeRule_bvcenter(const BV& bv, unsigned int*, unsigned int) {
+    Vec3s center = bv.center();
     int axis = 2;
 
-    if(bv.width() >= bv.height() && bv.width() >= bv.depth())
+    if (bv.width() >= bv.height() && bv.width() >= bv.depth())
       axis = 0;
-    else if(bv.height() >= bv.width() && bv.height() >= bv.depth())
+    else if (bv.height() >= bv.width() && bv.height() >= bv.depth())
       axis = 1;
 
     split_axis = axis;
     split_value = center[axis];
   }
 
-  /// @brief Split algorithm 2: Split the node according to the mean of the data contained
-  void computeRule_mean(const BV& bv, unsigned int* primitive_indices, int num_primitives)
-  {
+  /// @brief Split algorithm 2: Split the node according to the mean of the data
+  /// contained
+  void computeRule_mean(const BV& bv, unsigned int* primitive_indices,
+                        unsigned int num_primitives) {
     int axis = 2;
 
-    if(bv.width() >= bv.height() && bv.width() >= bv.depth())
+    if (bv.width() >= bv.height() && bv.width() >= bv.depth())
       axis = 0;
-    else if(bv.height() >= bv.width() && bv.height() >= bv.depth())
+    else if (bv.height() >= bv.width() && bv.height() >= bv.depth())
       axis = 1;
 
     split_axis = axis;
-    FCL_REAL sum = 0;
+    CoalScalar sum = 0;
 
-    if(type == BVH_MODEL_TRIANGLES)
-    {
-      for(int i = 0; i < num_primitives; ++i)
-      {
+    if (type == BVH_MODEL_TRIANGLES) {
+      for (unsigned int i = 0; i < num_primitives; ++i) {
         const Triangle& t = tri_indices[primitive_indices[i]];
-        sum += (vertices[t[0]][split_axis] + vertices[t[1]][split_axis] + vertices[t[2]][split_axis]);
+        sum += (vertices[t[0]][split_axis] + vertices[t[1]][split_axis] +
+                vertices[t[2]][split_axis]);
       }
 
       sum /= 3;
-    }
-    else if(type == BVH_MODEL_POINTCLOUD)
-    {
-      for(int i = 0; i < num_primitives; ++i)
-      {
+    } else if (type == BVH_MODEL_POINTCLOUD) {
+      for (unsigned int i = 0; i < num_primitives; ++i) {
         sum += vertices[primitive_indices[i]][split_axis];
       }
     }
@@ -195,95 +169,115 @@ private:
     split_value = sum / num_primitives;
   }
 
-  /// @brief Split algorithm 3: Split the node according to the median of the data contained
-  void computeRule_median(const BV& bv, unsigned int* primitive_indices, int num_primitives)
-  {
+  /// @brief Split algorithm 3: Split the node according to the median of the
+  /// data contained
+  void computeRule_median(const BV& bv, unsigned int* primitive_indices,
+                          unsigned int num_primitives) {
     int axis = 2;
 
-    if(bv.width() >= bv.height() && bv.width() >= bv.depth())
+    if (bv.width() >= bv.height() && bv.width() >= bv.depth())
       axis = 0;
-    else if(bv.height() >= bv.width() && bv.height() >= bv.depth())
+    else if (bv.height() >= bv.width() && bv.height() >= bv.depth())
       axis = 1;
 
     split_axis = axis;
-    std::vector<FCL_REAL> proj(num_primitives);
+    std::vector<CoalScalar> proj((size_t)num_primitives);
 
-    if(type == BVH_MODEL_TRIANGLES)
-    {
-      for(int i = 0; i < num_primitives; ++i)
-      {
+    if (type == BVH_MODEL_TRIANGLES) {
+      for (unsigned int i = 0; i < num_primitives; ++i) {
         const Triangle& t = tri_indices[primitive_indices[i]];
-        proj[i] = (vertices[t[0]][split_axis] + vertices[t[1]][split_axis] + vertices[t[2]][split_axis]) / 3;
+        proj[i] = (vertices[t[0]][split_axis] + vertices[t[1]][split_axis] +
+                   vertices[t[2]][split_axis]) /
+                  3;
       }
-    }
-    else if(type == BVH_MODEL_POINTCLOUD)
-    {
-      for(int i = 0; i < num_primitives; ++i)
+    } else if (type == BVH_MODEL_POINTCLOUD) {
+      for (unsigned int i = 0; i < num_primitives; ++i)
         proj[i] = vertices[primitive_indices[i]][split_axis];
     }
 
     std::sort(proj.begin(), proj.end());
 
-    if(num_primitives % 2 == 1)
-    {
+    if (num_primitives % 2 == 1) {
       split_value = proj[(num_primitives - 1) / 2];
-    }
-    else
-    {
-      split_value = (proj[num_primitives / 2] + proj[num_primitives / 2 - 1]) / 2;
+    } else {
+      split_value =
+          (proj[num_primitives / 2] + proj[num_primitives / 2 - 1]) / 2;
     }
   }
 };
 
-
-template<>
-bool BVSplitter<OBB>::apply(const Vec3f& q) const;
-
-template<>
-bool BVSplitter<RSS>::apply(const Vec3f& q) const;
-
-template<>
-bool BVSplitter<kIOS>::apply(const Vec3f& q) const;
-
-template<>
-bool BVSplitter<OBBRSS>::apply(const Vec3f& q) const;
-
-template<>
-void BVSplitter<OBB>::computeRule_bvcenter(const OBB& bv, unsigned int* primitive_indices, int num_primitives);
-
-template<>
-void BVSplitter<OBB>::computeRule_mean(const OBB& bv, unsigned int* primitive_indices, int num_primitives);
-
-template<>
-void BVSplitter<OBB>::computeRule_median(const OBB& bv, unsigned int* primitive_indices, int num_primitives);
-
-template<>
-void BVSplitter<RSS>::computeRule_bvcenter(const RSS& bv, unsigned int* primitive_indices, int num_primitives);
-          
-template<>                        
-void BVSplitter<RSS>::computeRule_mean(const RSS& bv, unsigned int* primitive_indices, int num_primitives);
-
-template<>
-void BVSplitter<RSS>::computeRule_median(const RSS& bv, unsigned int* primitive_indices, int num_primitives);
-
-template<>
-void BVSplitter<kIOS>::computeRule_bvcenter(const kIOS& bv, unsigned int* primitive_indices, int num_primitives);
-
-template<>
-void BVSplitter<kIOS>::computeRule_mean(const kIOS& bv, unsigned int* primitive_indices, int num_primitives);
-
-template<>
-void BVSplitter<kIOS>::computeRule_median(const kIOS& bv, unsigned int* primitive_indices, int num_primitives);
-
-template<>
-void BVSplitter<OBBRSS>::computeRule_bvcenter(const OBBRSS& bv, unsigned int* primitive_indices, int num_primitives);
-
-template<>
-void BVSplitter<OBBRSS>::computeRule_mean(const OBBRSS& bv, unsigned int* primitive_indices, int num_primitives);
-
-template<>
-void BVSplitter<OBBRSS>::computeRule_median(const OBBRSS& bv, unsigned int* primitive_indices, int num_primitives);
-
-}
+template <>
+bool COAL_DLLAPI BVSplitter<OBB>::apply(const Vec3s& q) const;
+
+template <>
+bool COAL_DLLAPI BVSplitter<RSS>::apply(const Vec3s& q) const;
+
+template <>
+bool COAL_DLLAPI BVSplitter<kIOS>::apply(const Vec3s& q) const;
+
+template <>
+bool COAL_DLLAPI BVSplitter<OBBRSS>::apply(const Vec3s& q) const;
+
+template <>
+void COAL_DLLAPI BVSplitter<OBB>::computeRule_bvcenter(
+    const OBB& bv, unsigned int* primitive_indices,
+    unsigned int num_primitives);
+
+template <>
+void COAL_DLLAPI BVSplitter<OBB>::computeRule_mean(
+    const OBB& bv, unsigned int* primitive_indices,
+    unsigned int num_primitives);
+
+template <>
+void COAL_DLLAPI BVSplitter<OBB>::computeRule_median(
+    const OBB& bv, unsigned int* primitive_indices,
+    unsigned int num_primitives);
+
+template <>
+void COAL_DLLAPI BVSplitter<RSS>::computeRule_bvcenter(
+    const RSS& bv, unsigned int* primitive_indices,
+    unsigned int num_primitives);
+
+template <>
+void COAL_DLLAPI BVSplitter<RSS>::computeRule_mean(
+    const RSS& bv, unsigned int* primitive_indices,
+    unsigned int num_primitives);
+
+template <>
+void COAL_DLLAPI BVSplitter<RSS>::computeRule_median(
+    const RSS& bv, unsigned int* primitive_indices,
+    unsigned int num_primitives);
+
+template <>
+void COAL_DLLAPI BVSplitter<kIOS>::computeRule_bvcenter(
+    const kIOS& bv, unsigned int* primitive_indices,
+    unsigned int num_primitives);
+
+template <>
+void COAL_DLLAPI BVSplitter<kIOS>::computeRule_mean(
+    const kIOS& bv, unsigned int* primitive_indices,
+    unsigned int num_primitives);
+
+template <>
+void COAL_DLLAPI BVSplitter<kIOS>::computeRule_median(
+    const kIOS& bv, unsigned int* primitive_indices,
+    unsigned int num_primitives);
+
+template <>
+void COAL_DLLAPI BVSplitter<OBBRSS>::computeRule_bvcenter(
+    const OBBRSS& bv, unsigned int* primitive_indices,
+    unsigned int num_primitives);
+
+template <>
+void COAL_DLLAPI BVSplitter<OBBRSS>::computeRule_mean(
+    const OBBRSS& bv, unsigned int* primitive_indices,
+    unsigned int num_primitives);
+
+template <>
+void COAL_DLLAPI BVSplitter<OBBRSS>::computeRule_median(
+    const OBBRSS& bv, unsigned int* primitive_indices,
+    unsigned int num_primitives);
+
+}  // namespace coal
 
 #endif

include/coal/internal/intersect.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_INTERSECT_H
+#define COAL_INTERSECT_H
+
+/// @cond INTERNAL
+
+#include "coal/math/transform.h"
+
+namespace coal {
+
+/// @brief CCD intersect kernel among primitives
+class COAL_DLLAPI Intersect {
+ public:
+  static bool buildTrianglePlane(const Vec3s& v1, const Vec3s& v2,
+                                 const Vec3s& v3, Vec3s* n, CoalScalar* t);
+};  // class Intersect
+
+/// @brief Project functions
+class COAL_DLLAPI Project {
+ public:
+  struct COAL_DLLAPI ProjectResult {
+    /// @brief Parameterization of the projected point (based on the simplex to
+    /// be projected, use 2 or 3 or 4 of the array)
+    CoalScalar parameterization[4];
+
+    /// @brief square distance from the query point to the projected simplex
+    CoalScalar sqr_distance;
+
+    /// @brief the code of the projection type
+    unsigned int encode;
+
+    ProjectResult() : sqr_distance(-1), encode(0) {}
+  };
+
+  /// @brief Project point p onto line a-b
+  static ProjectResult projectLine(const Vec3s& a, const Vec3s& b,
+                                   const Vec3s& p);
+
+  /// @brief Project point p onto triangle a-b-c
+  static ProjectResult projectTriangle(const Vec3s& a, const Vec3s& b,
+                                       const Vec3s& c, const Vec3s& p);
+
+  /// @brief Project point p onto tetrahedra a-b-c-d
+  static ProjectResult projectTetrahedra(const Vec3s& a, const Vec3s& b,
+                                         const Vec3s& c, const Vec3s& d,
+                                         const Vec3s& p);
+
+  /// @brief Project origin (0) onto line a-b
+  static ProjectResult projectLineOrigin(const Vec3s& a, const Vec3s& b);
+
+  /// @brief Project origin (0) onto triangle a-b-c
+  static ProjectResult projectTriangleOrigin(const Vec3s& a, const Vec3s& b,
+                                             const Vec3s& c);
+
+  /// @brief Project origin (0) onto tetrahedran a-b-c-d
+  static ProjectResult projectTetrahedraOrigin(const Vec3s& a, const Vec3s& b,
+                                               const Vec3s& c, const Vec3s& d);
+};
+
+/// @brief Triangle distance functions
+class COAL_DLLAPI TriangleDistance {
+ public:
+  /// @brief Returns closest points between an segment pair.
+  /// The first segment is P + t * A
+  /// The second segment is Q + t * B
+  /// X, Y are the closest points on the two segments
+  /// VEC is the vector between X and Y
+  static void segPoints(const Vec3s& P, const Vec3s& A, const Vec3s& Q,
+                        const Vec3s& B, Vec3s& VEC, Vec3s& X, Vec3s& Y);
+
+  /// Compute squared distance between triangles
+  /// @param S and T are two triangles
+  /// @retval P, Q closest points if triangles do not intersect.
+  /// @return squared distance if triangles do not intersect, 0 otherwise.
+  /// If the triangles are disjoint, P and Q give the closet points of
+  /// S and T respectively. However,
+  /// if the triangles overlap, P and Q are basically a random pair of points
+  /// from the triangles, not coincident points on the intersection of the
+  /// triangles, as might be expected.
+  static CoalScalar sqrTriDistance(const Vec3s S[3], const Vec3s T[3], Vec3s& P,
+                                   Vec3s& Q);
+
+  static CoalScalar sqrTriDistance(const Vec3s& S1, const Vec3s& S2,
+                                   const Vec3s& S3, const Vec3s& T1,
+                                   const Vec3s& T2, const Vec3s& T3, Vec3s& P,
+                                   Vec3s& Q);
+
+  /// Compute squared distance between triangles
+  /// @param S and T are two triangles
+  /// @param R, Tl, rotation and translation applied to T,
+  /// @retval P, Q closest points if triangles do not intersect.
+  /// @return squared distance if triangles do not intersect, 0 otherwise.
+  /// If the triangles are disjoint, P and Q give the closet points of
+  /// S and T respectively. However,
+  /// if the triangles overlap, P and Q are basically a random pair of points
+  /// from the triangles, not coincident points on the intersection of the
+  /// triangles, as might be expected.
+  static CoalScalar sqrTriDistance(const Vec3s S[3], const Vec3s T[3],
+                                   const Matrix3s& R, const Vec3s& Tl, Vec3s& P,
+                                   Vec3s& Q);
+
+  /// Compute squared distance between triangles
+  /// @param S and T are two triangles
+  /// @param tf, rotation and translation applied to T,
+  /// @retval P, Q closest points if triangles do not intersect.
+  /// @return squared distance if triangles do not intersect, 0 otherwise.
+  /// If the triangles are disjoint, P and Q give the closet points of
+  /// S and T respectively. However,
+  /// if the triangles overlap, P and Q are basically a random pair of points
+  /// from the triangles, not coincident points on the intersection of the
+  /// triangles, as might be expected.
+  static CoalScalar sqrTriDistance(const Vec3s S[3], const Vec3s T[3],
+                                   const Transform3s& tf, Vec3s& P, Vec3s& Q);
+
+  /// Compute squared distance between triangles
+  /// @param S1, S2, S3 and T1, T2, T3 are triangle vertices
+  /// @param R, Tl, rotation and translation applied to T1, T2, T3,
+  /// @retval P, Q closest points if triangles do not intersect.
+  /// @return squared distance if triangles do not intersect, 0 otherwise.
+  /// If the triangles are disjoint, P and Q give the closet points of
+  /// S and T respectively. However,
+  /// if the triangles overlap, P and Q are basically a random pair of points
+  /// from the triangles, not coincident points on the intersection of the
+  /// triangles, as might be expected.
+  static CoalScalar sqrTriDistance(const Vec3s& S1, const Vec3s& S2,
+                                   const Vec3s& S3, const Vec3s& T1,
+                                   const Vec3s& T2, const Vec3s& T3,
+                                   const Matrix3s& R, const Vec3s& Tl, Vec3s& P,
+                                   Vec3s& Q);
+
+  /// Compute squared distance between triangles
+  /// @param S1, S2, S3 and T1, T2, T3 are triangle vertices
+  /// @param tf, rotation and translation applied to T1, T2, T3,
+  /// @retval P, Q closest points if triangles do not intersect.
+  /// @return squared distance if triangles do not intersect, 0 otherwise.
+  /// If the triangles are disjoint, P and Q give the closet points of
+  /// S and T respectively. However,
+  /// if the triangles overlap, P and Q are basically a random pair of points
+  /// from the triangles, not coincident points on the intersection of the
+  /// triangles, as might be expected.
+  static CoalScalar sqrTriDistance(const Vec3s& S1, const Vec3s& S2,
+                                   const Vec3s& S3, const Vec3s& T1,
+                                   const Vec3s& T2, const Vec3s& T3,
+                                   const Transform3s& tf, Vec3s& P, Vec3s& Q);
+};
+
+}  // namespace coal
+
+/// @endcond
+
+#endif

include/coal/internal/shape_shape_contact_patch_func.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2024, 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 Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Louis Montaut */
+
+#ifndef COAL_INTERNAL_SHAPE_SHAPE_CONTACT_PATCH_FUNC_H
+#define COAL_INTERNAL_SHAPE_SHAPE_CONTACT_PATCH_FUNC_H
+
+#include "coal/collision_data.h"
+#include "coal/collision_utility.h"
+#include "coal/narrowphase/narrowphase.h"
+#include "coal/contact_patch/contact_patch_solver.h"
+#include "coal/shape/geometric_shapes_traits.h"
+
+namespace coal {
+
+/// @brief Shape-shape contact patch computation.
+/// Assumes that `csolver` and the `ContactPatchResult` have already been set up
+/// by the `ContactPatchRequest`.
+template <typename ShapeType1, typename ShapeType2>
+struct ComputeShapeShapeContactPatch {
+  static void run(const CollisionGeometry* o1, const Transform3s& tf1,
+                  const CollisionGeometry* o2, const Transform3s& tf2,
+                  const CollisionResult& collision_result,
+                  const ContactPatchSolver* csolver,
+                  const ContactPatchRequest& request,
+                  ContactPatchResult& result) {
+    // Note: see specializations for Plane and Halfspace below.
+    if (!collision_result.isCollision()) {
+      return;
+    }
+    COAL_ASSERT(
+        result.check(request),
+        "The contact patch result and request are incompatible (issue of "
+        "contact patch size or maximum number of contact patches). Make sure "
+        "result is initialized with request.",
+        std::logic_error);
+
+    const ShapeType1& s1 = static_cast<const ShapeType1&>(*o1);
+    const ShapeType2& s2 = static_cast<const ShapeType2&>(*o2);
+    for (size_t i = 0; i < collision_result.numContacts(); ++i) {
+      if (i >= request.max_num_patch) {
+        break;
+      }
+      csolver->setSupportGuess(collision_result.cached_support_func_guess);
+      const Contact& contact = collision_result.getContact(i);
+      ContactPatch& contact_patch = result.getUnusedContactPatch();
+      csolver->computePatch(s1, tf1, s2, tf2, contact, contact_patch);
+    }
+  }
+};
+
+/// @brief Computes the contact patch between a Plane/Halfspace and another
+/// shape.
+/// @tparam InvertShapes set to true if the first shape of the collision pair
+/// is s2 and not s1 (if you had to invert (s1, tf1) and (s2, tf2) when calling
+/// this function).
+template <bool InvertShapes, typename OtherShapeType, typename PlaneOrHalfspace>
+void computePatchPlaneOrHalfspace(const OtherShapeType& s1,
+                                  const Transform3s& tf1,
+                                  const PlaneOrHalfspace& s2,
+                                  const Transform3s& tf2,
+                                  const ContactPatchSolver* csolver,
+                                  const Contact& contact,
+                                  ContactPatch& contact_patch) {
+  COAL_UNUSED_VARIABLE(s2);
+  COAL_UNUSED_VARIABLE(tf2);
+  constructContactPatchFrameFromContact(contact, contact_patch);
+  if ((bool)(shape_traits<OtherShapeType>::IsStrictlyConvex)) {
+    // Only one point of contact; it has already been computed.
+    contact_patch.addPoint(contact.pos);
+    return;
+  }
+
+  // We only need to compute the support set in the direction of the normal.
+  // We need to temporarily express the patch in the local frame of shape1.
+  SupportSet& support_set = csolver->support_set_shape1;
+  support_set.tf.rotation().noalias() =
+      tf1.rotation().transpose() * contact_patch.tf.rotation();
+  support_set.tf.translation().noalias() =
+      tf1.rotation().transpose() *
+      (contact_patch.tf.translation() - tf1.translation());
+
+  // Note: for now, taking into account swept-sphere radius does not change
+  // anything to the support set computations. However it will be used in the
+  // future if we want to store the offsets to the support plane for each point
+  // in a support set.
+  using SupportOptions = details::SupportOptions;
+  if (InvertShapes) {
+    support_set.direction = ContactPatch::PatchDirection::INVERTED;
+    details::getShapeSupportSet<SupportOptions::WithSweptSphere>(
+        &s1, support_set, csolver->support_guess[1], csolver->supports_data[1],
+        csolver->num_samples_curved_shapes, csolver->patch_tolerance);
+  } else {
+    support_set.direction = ContactPatch::PatchDirection::DEFAULT;
+    details::getShapeSupportSet<SupportOptions::WithSweptSphere>(
+        &s1, support_set, csolver->support_guess[0], csolver->supports_data[0],
+        csolver->num_samples_curved_shapes, csolver->patch_tolerance);
+  }
+  csolver->getResult(contact, &(support_set.points()), contact_patch);
+}
+
+#define PLANE_OR_HSPACE_AND_OTHER_SHAPE_CONTACT_PATCH(PlaneOrHspace)          \
+  template <typename OtherShapeType>                                          \
+  struct ComputeShapeShapeContactPatch<OtherShapeType, PlaneOrHspace> {       \
+    static void run(const CollisionGeometry* o1, const Transform3s& tf1,      \
+                    const CollisionGeometry* o2, const Transform3s& tf2,      \
+                    const CollisionResult& collision_result,                  \
+                    const ContactPatchSolver* csolver,                        \
+                    const ContactPatchRequest& request,                       \
+                    ContactPatchResult& result) {                             \
+      if (!collision_result.isCollision()) {                                  \
+        return;                                                               \
+      }                                                                       \
+      COAL_ASSERT(                                                            \
+          result.check(request),                                              \
+          "The contact patch result and request are incompatible (issue of "  \
+          "contact patch size or maximum number of contact patches). Make "   \
+          "sure "                                                             \
+          "result is initialized with request.",                              \
+          std::logic_error);                                                  \
+                                                                              \
+      const OtherShapeType& s1 = static_cast<const OtherShapeType&>(*o1);     \
+      const PlaneOrHspace& s2 = static_cast<const PlaneOrHspace&>(*o2);       \
+      for (size_t i = 0; i < collision_result.numContacts(); ++i) {           \
+        if (i >= request.max_num_patch) {                                     \
+          break;                                                              \
+        }                                                                     \
+        csolver->setSupportGuess(collision_result.cached_support_func_guess); \
+        const Contact& contact = collision_result.getContact(i);              \
+        ContactPatch& contact_patch = result.getUnusedContactPatch();         \
+        computePatchPlaneOrHalfspace<false, OtherShapeType, PlaneOrHspace>(   \
+            s1, tf1, s2, tf2, csolver, contact, contact_patch);               \
+      }                                                                       \
+    }                                                                         \
+  };                                                                          \
+                                                                              \
+  template <typename OtherShapeType>                                          \
+  struct ComputeShapeShapeContactPatch<PlaneOrHspace, OtherShapeType> {       \
+    static void run(const CollisionGeometry* o1, const Transform3s& tf1,      \
+                    const CollisionGeometry* o2, const Transform3s& tf2,      \
+                    const CollisionResult& collision_result,                  \
+                    const ContactPatchSolver* csolver,                        \
+                    const ContactPatchRequest& request,                       \
+                    ContactPatchResult& result) {                             \
+      if (!collision_result.isCollision()) {                                  \
+        return;                                                               \
+      }                                                                       \
+      COAL_ASSERT(                                                            \
+          result.check(request),                                              \
+          "The contact patch result and request are incompatible (issue of "  \
+          "contact patch size or maximum number of contact patches). Make "   \
+          "sure "                                                             \
+          "result is initialized with request.",                              \
+          std::logic_error);                                                  \
+                                                                              \
+      const PlaneOrHspace& s1 = static_cast<const PlaneOrHspace&>(*o1);       \
+      const OtherShapeType& s2 = static_cast<const OtherShapeType&>(*o2);     \
+      for (size_t i = 0; i < collision_result.numContacts(); ++i) {           \
+        if (i >= request.max_num_patch) {                                     \
+          break;                                                              \
+        }                                                                     \
+        csolver->setSupportGuess(collision_result.cached_support_func_guess); \
+        const Contact& contact = collision_result.getContact(i);              \
+        ContactPatch& contact_patch = result.getUnusedContactPatch();         \
+        computePatchPlaneOrHalfspace<true, OtherShapeType, PlaneOrHspace>(    \
+            s2, tf2, s1, tf1, csolver, contact, contact_patch);               \
+      }                                                                       \
+    }                                                                         \
+  };
+
+PLANE_OR_HSPACE_AND_OTHER_SHAPE_CONTACT_PATCH(Plane)
+PLANE_OR_HSPACE_AND_OTHER_SHAPE_CONTACT_PATCH(Halfspace)
+
+#define PLANE_HSPACE_CONTACT_PATCH(PlaneOrHspace1, PlaneOrHspace2)           \
+  template <>                                                                \
+  struct ComputeShapeShapeContactPatch<PlaneOrHspace1, PlaneOrHspace2> {     \
+    static void run(const CollisionGeometry* o1, const Transform3s& tf1,     \
+                    const CollisionGeometry* o2, const Transform3s& tf2,     \
+                    const CollisionResult& collision_result,                 \
+                    const ContactPatchSolver* csolver,                       \
+                    const ContactPatchRequest& request,                      \
+                    ContactPatchResult& result) {                            \
+      COAL_UNUSED_VARIABLE(o1);                                              \
+      COAL_UNUSED_VARIABLE(tf1);                                             \
+      COAL_UNUSED_VARIABLE(o2);                                              \
+      COAL_UNUSED_VARIABLE(tf2);                                             \
+      COAL_UNUSED_VARIABLE(csolver);                                         \
+      if (!collision_result.isCollision()) {                                 \
+        return;                                                              \
+      }                                                                      \
+      COAL_ASSERT(                                                           \
+          result.check(request),                                             \
+          "The contact patch result and request are incompatible (issue of " \
+          "contact patch size or maximum number of contact patches). Make "  \
+          "sure "                                                            \
+          "result is initialized with request.",                             \
+          std::logic_error);                                                 \
+                                                                             \
+      for (size_t i = 0; i < collision_result.numContacts(); ++i) {          \
+        if (i >= request.max_num_patch) {                                    \
+          break;                                                             \
+        }                                                                    \
+        const Contact& contact = collision_result.getContact(i);             \
+        ContactPatch& contact_patch = result.getUnusedContactPatch();        \
+        constructContactPatchFrameFromContact(contact, contact_patch);       \
+        contact_patch.addPoint(contact.pos);                                 \
+      }                                                                      \
+    }                                                                        \
+  };
+
+PLANE_HSPACE_CONTACT_PATCH(Plane, Plane)
+PLANE_HSPACE_CONTACT_PATCH(Plane, Halfspace)
+PLANE_HSPACE_CONTACT_PATCH(Halfspace, Plane)
+PLANE_HSPACE_CONTACT_PATCH(Halfspace, Halfspace)
+
+#undef PLANE_OR_HSPACE_AND_OTHER_SHAPE_CONTACT_PATCH
+#undef PLANE_HSPACE_CONTACT_PATCH
+
+template <typename ShapeType1, typename ShapeType2>
+void ShapeShapeContactPatch(const CollisionGeometry* o1, const Transform3s& tf1,
+                            const CollisionGeometry* o2, const Transform3s& tf2,
+                            const CollisionResult& collision_result,
+                            const ContactPatchSolver* csolver,
+                            const ContactPatchRequest& request,
+                            ContactPatchResult& result) {
+  return ComputeShapeShapeContactPatch<ShapeType1, ShapeType2>::run(
+      o1, tf1, o2, tf2, collision_result, csolver, request, result);
+}
+
+}  // namespace coal
+
+#endif

include/coal/internal/shape_shape_func.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2014, CNRS-LAAS
+ *  Copyright (c) 2024, 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 Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Florent Lamiraux */
+
+#ifndef COAL_INTERNAL_SHAPE_SHAPE_FUNC_H
+#define COAL_INTERNAL_SHAPE_SHAPE_FUNC_H
+
+/// @cond INTERNAL
+
+#include "coal/collision_data.h"
+#include "coal/collision_utility.h"
+#include "coal/narrowphase/narrowphase.h"
+#include "coal/shape/geometric_shapes_traits.h"
+
+namespace coal {
+
+template <typename ShapeType1, typename ShapeType2>
+struct ShapeShapeDistancer {
+  static CoalScalar run(const CollisionGeometry* o1, const Transform3s& tf1,
+                        const CollisionGeometry* o2, const Transform3s& tf2,
+                        const GJKSolver* nsolver,
+                        const DistanceRequest& request,
+                        DistanceResult& result) {
+    if (request.isSatisfied(result)) return result.min_distance;
+
+    // Witness points on shape1 and shape2, normal pointing from shape1 to
+    // shape2.
+    Vec3s p1, p2, normal;
+    const CoalScalar distance =
+        ShapeShapeDistancer<ShapeType1, ShapeType2>::run(
+            o1, tf1, o2, tf2, nsolver, request.enable_signed_distance, p1, p2,
+            normal);
+
+    result.update(distance, o1, o2, DistanceResult::NONE, DistanceResult::NONE,
+                  p1, p2, normal);
+
+    return distance;
+  }
+
+  static CoalScalar run(const CollisionGeometry* o1, const Transform3s& tf1,
+                        const CollisionGeometry* o2, const Transform3s& tf2,
+                        const GJKSolver* nsolver,
+                        const bool compute_signed_distance, Vec3s& p1,
+                        Vec3s& p2, Vec3s& normal) {
+    const ShapeType1* obj1 = static_cast<const ShapeType1*>(o1);
+    const ShapeType2* obj2 = static_cast<const ShapeType2*>(o2);
+    return nsolver->shapeDistance(*obj1, tf1, *obj2, tf2,
+                                  compute_signed_distance, p1, p2, normal);
+  }
+};
+
+/// @brief Shape-shape distance computation.
+/// Assumes that `nsolver` has already been set up by a `DistanceRequest` or
+/// a `CollisionRequest`.
+///
+/// @note This function is typically used for collision pairs containing two
+/// primitive shapes.
+/// @note This function might be specialized for some pairs of shapes.
+template <typename ShapeType1, typename ShapeType2>
+CoalScalar ShapeShapeDistance(const CollisionGeometry* o1,
+                              const Transform3s& tf1,
+                              const CollisionGeometry* o2,
+                              const Transform3s& tf2, const GJKSolver* nsolver,
+                              const DistanceRequest& request,
+                              DistanceResult& result) {
+  return ShapeShapeDistancer<ShapeType1, ShapeType2>::run(
+      o1, tf1, o2, tf2, nsolver, request, result);
+}
+
+namespace internal {
+/// @brief Shape-shape distance computation.
+/// Assumes that `nsolver` has already been set up by a `DistanceRequest` or
+/// a `CollisionRequest`.
+///
+/// @note This function is typically used for collision pairs complex structures
+/// like BVHs, HeightFields or Octrees. These structures contain sets of
+/// primitive shapes.
+/// This function is meant to be called on the pairs of primitive shapes of
+/// these structures.
+/// @note This function might be specialized for some pairs of shapes.
+template <typename ShapeType1, typename ShapeType2>
+CoalScalar ShapeShapeDistance(const CollisionGeometry* o1,
+                              const Transform3s& tf1,
+                              const CollisionGeometry* o2,
+                              const Transform3s& tf2, const GJKSolver* nsolver,
+                              const bool compute_signed_distance, Vec3s& p1,
+                              Vec3s& p2, Vec3s& normal) {
+  return ::coal::ShapeShapeDistancer<ShapeType1, ShapeType2>::run(
+      o1, tf1, o2, tf2, nsolver, compute_signed_distance, p1, p2, normal);
+}
+}  // namespace internal
+
+/// @brief Shape-shape collision detection.
+/// Assumes that `nsolver` has already been set up by a `DistanceRequest` or
+/// a `CollisionRequest`.
+///
+/// @note This function is typically used for collision pairs containing two
+/// primitive shapes.
+/// Complex structures like BVHs, HeightFields or Octrees contain sets of
+/// primitive shapes should use the `ShapeShapeDistance` function to do their
+/// internal collision detection checks.
+template <typename ShapeType1, typename ShapeType2>
+struct ShapeShapeCollider {
+  static std::size_t run(const CollisionGeometry* o1, const Transform3s& tf1,
+                         const CollisionGeometry* o2, const Transform3s& tf2,
+                         const GJKSolver* nsolver,
+                         const CollisionRequest& request,
+                         CollisionResult& result) {
+    if (request.isSatisfied(result)) return result.numContacts();
+
+    const bool compute_penetration =
+        request.enable_contact || (request.security_margin < 0);
+    Vec3s p1, p2, normal;
+    CoalScalar distance = internal::ShapeShapeDistance<ShapeType1, ShapeType2>(
+        o1, tf1, o2, tf2, nsolver, compute_penetration, p1, p2, normal);
+
+    size_t num_contacts = 0;
+    const CoalScalar distToCollision = distance - request.security_margin;
+
+    internal::updateDistanceLowerBoundFromLeaf(request, result, distToCollision,
+                                               p1, p2, normal);
+    if (distToCollision <= request.collision_distance_threshold &&
+        result.numContacts() < request.num_max_contacts) {
+      if (result.numContacts() < request.num_max_contacts) {
+        Contact contact(o1, o2, Contact::NONE, Contact::NONE, p1, p2, normal,
+                        distance);
+        result.addContact(contact);
+      }
+      num_contacts = result.numContacts();
+    }
+
+    return num_contacts;
+  }
+};
+
+template <typename ShapeType1, typename ShapeType2>
+std::size_t ShapeShapeCollide(const CollisionGeometry* o1,
+                              const Transform3s& tf1,
+                              const CollisionGeometry* o2,
+                              const Transform3s& tf2, const GJKSolver* nsolver,
+                              const CollisionRequest& request,
+                              CollisionResult& result) {
+  return ShapeShapeCollider<ShapeType1, ShapeType2>::run(
+      o1, tf1, o2, tf2, nsolver, request, result);
+}
+
+// clang-format off
+// ==============================================================================================================
+// ==============================================================================================================
+// ==============================================================================================================
+// Shape distance algorithms based on:
+// - built-in function: 0
+// - GJK:               1
+//
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
+// |            | box | sphere | capsule | cone | cylinder | plane | half-space | triangle | ellipsoid | convex |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
+// | box        |  1  |   0    |    1    |   1  |    1     |   0   |      0     |    1     |    1      |    1   |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
+// | sphere     |/////|   0    |    0    |   1  |    0     |   0   |      0     |    0     |    1      |    1   |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
+// | capsule    |/////|////////|    0    |   1  |    1     |   0   |      0     |    1     |    1      |    1   |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
+// | cone       |/////|////////|/////////|   1  |    1     |   0   |      0     |    1     |    1      |    1   |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
+// | cylinder   |/////|////////|/////////|//////|    1     |   0   |      0     |    1     |    1      |    1   |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
+// | plane      |/////|////////|/////////|//////|//////////|   ?   |      ?     |    0     |    0      |    0   |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
+// | half-space |/////|////////|/////////|//////|//////////|///////|      ?     |    0     |    0      |    0   |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
+// | triangle   |/////|////////|/////////|//////|//////////|///////|////////////|    0     |    1      |    1   |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
+// | ellipsoid  |/////|////////|/////////|//////|//////////|///////|////////////|//////////|    1      |    1   |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
+// | convex     |/////|////////|/////////|//////|//////////|///////|////////////|//////////|///////////|    1   |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
+//
+// Number of pairs: 55
+//   - Specialized: 26
+//   - GJK:         29
+// clang-format on
+
+#define SHAPE_SHAPE_DISTANCE_SPECIALIZATION(T1, T2)                            \
+  template <>                                                                  \
+  COAL_DLLAPI CoalScalar internal::ShapeShapeDistance<T1, T2>(                 \
+      const CollisionGeometry* o1, const Transform3s& tf1,                     \
+      const CollisionGeometry* o2, const Transform3s& tf2,                     \
+      const GJKSolver* nsolver, const bool compute_signed_distance, Vec3s& p1, \
+      Vec3s& p2, Vec3s& normal);                                               \
+  template <>                                                                  \
+  COAL_DLLAPI CoalScalar internal::ShapeShapeDistance<T2, T1>(                 \
+      const CollisionGeometry* o1, const Transform3s& tf1,                     \
+      const CollisionGeometry* o2, const Transform3s& tf2,                     \
+      const GJKSolver* nsolver, const bool compute_signed_distance, Vec3s& p1, \
+      Vec3s& p2, Vec3s& normal);                                               \
+  template <>                                                                  \
+  inline COAL_DLLAPI CoalScalar ShapeShapeDistance<T1, T2>(                    \
+      const CollisionGeometry* o1, const Transform3s& tf1,                     \
+      const CollisionGeometry* o2, const Transform3s& tf2,                     \
+      const GJKSolver* nsolver, const DistanceRequest& request,                \
+      DistanceResult& result) {                                                \
+    result.o1 = o1;                                                            \
+    result.o2 = o2;                                                            \
+    result.b1 = DistanceResult::NONE;                                          \
+    result.b2 = DistanceResult::NONE;                                          \
+    result.min_distance = internal::ShapeShapeDistance<T1, T2>(                \
+        o1, tf1, o2, tf2, nsolver, request.enable_signed_distance,             \
+        result.nearest_points[0], result.nearest_points[1], result.normal);    \
+    return result.min_distance;                                                \
+  }                                                                            \
+  template <>                                                                  \
+  inline COAL_DLLAPI CoalScalar ShapeShapeDistance<T2, T1>(                    \
+      const CollisionGeometry* o1, const Transform3s& tf1,                     \
+      const CollisionGeometry* o2, const Transform3s& tf2,                     \
+      const GJKSolver* nsolver, const DistanceRequest& request,                \
+      DistanceResult& result) {                                                \
+    result.o1 = o1;                                                            \
+    result.o2 = o2;                                                            \
+    result.b1 = DistanceResult::NONE;                                          \
+    result.b2 = DistanceResult::NONE;                                          \
+    result.min_distance = internal::ShapeShapeDistance<T2, T1>(                \
+        o1, tf1, o2, tf2, nsolver, request.enable_signed_distance,             \
+        result.nearest_points[0], result.nearest_points[1], result.normal);    \
+    return result.min_distance;                                                \
+  }
+
+#define SHAPE_SELF_DISTANCE_SPECIALIZATION(T)                                  \
+  template <>                                                                  \
+  COAL_DLLAPI CoalScalar internal::ShapeShapeDistance<T, T>(                   \
+      const CollisionGeometry* o1, const Transform3s& tf1,                     \
+      const CollisionGeometry* o2, const Transform3s& tf2,                     \
+      const GJKSolver* nsolver, const bool compute_signed_distance, Vec3s& p1, \
+      Vec3s& p2, Vec3s& normal);                                               \
+  template <>                                                                  \
+  inline COAL_DLLAPI CoalScalar ShapeShapeDistance<T, T>(                      \
+      const CollisionGeometry* o1, const Transform3s& tf1,                     \
+      const CollisionGeometry* o2, const Transform3s& tf2,                     \
+      const GJKSolver* nsolver, const DistanceRequest& request,                \
+      DistanceResult& result) {                                                \
+    result.o1 = o1;                                                            \
+    result.o2 = o2;                                                            \
+    result.b1 = DistanceResult::NONE;                                          \
+    result.b2 = DistanceResult::NONE;                                          \
+    result.min_distance = internal::ShapeShapeDistance<T, T>(                  \
+        o1, tf1, o2, tf2, nsolver, request.enable_signed_distance,             \
+        result.nearest_points[0], result.nearest_points[1], result.normal);    \
+    return result.min_distance;                                                \
+  }
+
+SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Box, Halfspace)
+SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Box, Plane)
+SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Box, Sphere)
+SHAPE_SELF_DISTANCE_SPECIALIZATION(Capsule)
+SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Capsule, Halfspace)
+SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Capsule, Plane)
+SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cone, Halfspace)
+SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cone, Plane)
+SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cylinder, Halfspace)
+SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cylinder, Plane)
+SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Halfspace)
+SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Plane)
+SHAPE_SELF_DISTANCE_SPECIALIZATION(Sphere)
+SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Cylinder)
+SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Capsule)
+SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Ellipsoid, Halfspace)
+SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Ellipsoid, Plane)
+SHAPE_SHAPE_DISTANCE_SPECIALIZATION(ConvexBase, Halfspace)
+SHAPE_SHAPE_DISTANCE_SPECIALIZATION(ConvexBase, Plane)
+SHAPE_SHAPE_DISTANCE_SPECIALIZATION(TriangleP, Halfspace)
+SHAPE_SHAPE_DISTANCE_SPECIALIZATION(TriangleP, Plane)
+SHAPE_SELF_DISTANCE_SPECIALIZATION(TriangleP)
+SHAPE_SHAPE_DISTANCE_SPECIALIZATION(TriangleP, Sphere)
+SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Plane, Halfspace)
+SHAPE_SELF_DISTANCE_SPECIALIZATION(Plane)
+SHAPE_SELF_DISTANCE_SPECIALIZATION(Halfspace)
+
+#undef SHAPE_SHAPE_DISTANCE_SPECIALIZATION
+#undef SHAPE_SELF_DISTANCE_SPECIALIZATION
+
+}  // namespace coal
+
+/// @endcond
+
+#endif

include/coal/internal/tools.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 Joseph Mirabel */
+
+#ifndef COAL_INTERNAL_TOOLS_H
+#define COAL_INTERNAL_TOOLS_H
+
+#include "coal/fwd.hh"
+
+#include <cmath>
+#include <iostream>
+#include <limits>
+
+#include "coal/data_types.h"
+
+namespace coal {
+
+template <typename Derived>
+static inline typename Derived::Scalar triple(
+    const Eigen::MatrixBase<Derived>& x, const Eigen::MatrixBase<Derived>& y,
+    const Eigen::MatrixBase<Derived>& z) {
+  return x.derived().dot(y.derived().cross(z.derived()));
+}
+
+template <typename Derived1, typename Derived2, typename Derived3>
+void generateCoordinateSystem(const Eigen::MatrixBase<Derived1>& _w,
+                              const Eigen::MatrixBase<Derived2>& _u,
+                              const Eigen::MatrixBase<Derived3>& _v) {
+  typedef typename Derived1::Scalar T;
+
+  Eigen::MatrixBase<Derived1>& w = const_cast<Eigen::MatrixBase<Derived1>&>(_w);
+  Eigen::MatrixBase<Derived2>& u = const_cast<Eigen::MatrixBase<Derived2>&>(_u);
+  Eigen::MatrixBase<Derived3>& v = const_cast<Eigen::MatrixBase<Derived3>&>(_v);
+
+  T inv_length;
+  if (std::abs(w[0]) >= std::abs(w[1])) {
+    inv_length = (T)1.0 / sqrt(w[0] * w[0] + w[2] * w[2]);
+    u[0] = -w[2] * inv_length;
+    u[1] = (T)0;
+    u[2] = w[0] * inv_length;
+    v[0] = w[1] * u[2];
+    v[1] = w[2] * u[0] - w[0] * u[2];
+    v[2] = -w[1] * u[0];
+  } else {
+    inv_length = (T)1.0 / sqrt(w[1] * w[1] + w[2] * w[2]);
+    u[0] = (T)0;
+    u[1] = w[2] * inv_length;
+    u[2] = -w[1] * inv_length;
+    v[0] = w[1] * u[2] - w[2] * u[1];
+    v[1] = -w[0] * u[2];
+    v[2] = w[0] * u[1];
+  }
+}
+
+/* ----- Start Matrices ------ */
+template <typename Derived, typename OtherDerived>
+void relativeTransform(const Eigen::MatrixBase<Derived>& R1,
+                       const Eigen::MatrixBase<OtherDerived>& t1,
+                       const Eigen::MatrixBase<Derived>& R2,
+                       const Eigen::MatrixBase<OtherDerived>& t2,
+                       const Eigen::MatrixBase<Derived>& R,
+                       const Eigen::MatrixBase<OtherDerived>& t) {
+  const_cast<Eigen::MatrixBase<Derived>&>(R) = R1.transpose() * R2;
+  const_cast<Eigen::MatrixBase<OtherDerived>&>(t) = R1.transpose() * (t2 - t1);
+}
+
+/// @brief compute the eigen vector and eigen vector of a matrix. dout is the
+/// eigen values, vout is the eigen vectors
+template <typename Derived, typename Vector>
+void eigen(const Eigen::MatrixBase<Derived>& m,
+           typename Derived::Scalar dout[3], Vector* vout) {
+  typedef typename Derived::Scalar S;
+  Derived R(m.derived());
+  int n = 3;
+  int j, iq, ip, i;
+  S tresh, theta, tau, t, sm, s, h, g, c;
+
+  S b[3];
+  S z[3];
+  S v[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
+  S d[3];
+
+  for (ip = 0; ip < n; ++ip) {
+    b[ip] = d[ip] = R(ip, ip);
+    z[ip] = 0;
+  }
+
+  for (i = 0; i < 50; ++i) {
+    sm = 0;
+    for (ip = 0; ip < n; ++ip)
+      for (iq = ip + 1; iq < n; ++iq) sm += std::abs(R(ip, iq));
+    if (sm == 0.0) {
+      vout[0] << v[0][0], v[0][1], v[0][2];
+      vout[1] << v[1][0], v[1][1], v[1][2];
+      vout[2] << v[2][0], v[2][1], v[2][2];
+      dout[0] = d[0];
+      dout[1] = d[1];
+      dout[2] = d[2];
+      return;
+    }
+
+    if (i < 3)
+      tresh = 0.2 * sm / (n * n);
+    else
+      tresh = 0.0;
+
+    for (ip = 0; ip < n; ++ip) {
+      for (iq = ip + 1; iq < n; ++iq) {
+        g = 100.0 * std::abs(R(ip, iq));
+        if (i > 3 && std::abs(d[ip]) + g == std::abs(d[ip]) &&
+            std::abs(d[iq]) + g == std::abs(d[iq]))
+          R(ip, iq) = 0.0;
+        else if (std::abs(R(ip, iq)) > tresh) {
+          h = d[iq] - d[ip];
+          if (std::abs(h) + g == std::abs(h))
+            t = (R(ip, iq)) / h;
+          else {
+            theta = 0.5 * h / (R(ip, iq));
+            t = 1.0 / (std::abs(theta) + std::sqrt(1.0 + theta * theta));
+            if (theta < 0.0) t = -t;
+          }
+          c = 1.0 / std::sqrt(1 + t * t);
+          s = t * c;
+          tau = s / (1.0 + c);
+          h = t * R(ip, iq);
+          z[ip] -= h;
+          z[iq] += h;
+          d[ip] -= h;
+          d[iq] += h;
+          R(ip, iq) = 0.0;
+          for (j = 0; j < ip; ++j) {
+            g = R(j, ip);
+            h = R(j, iq);
+            R(j, ip) = g - s * (h + g * tau);
+            R(j, iq) = h + s * (g - h * tau);
+          }
+          for (j = ip + 1; j < iq; ++j) {
+            g = R(ip, j);
+            h = R(j, iq);
+            R(ip, j) = g - s * (h + g * tau);
+            R(j, iq) = h + s * (g - h * tau);
+          }
+          for (j = iq + 1; j < n; ++j) {
+            g = R(ip, j);
+            h = R(iq, j);
+            R(ip, j) = g - s * (h + g * tau);
+            R(iq, j) = h + s * (g - h * tau);
+          }
+          for (j = 0; j < n; ++j) {
+            g = v[j][ip];
+            h = v[j][iq];
+            v[j][ip] = g - s * (h + g * tau);
+            v[j][iq] = h + s * (g - h * tau);
+          }
+        }
+      }
+    }
+    for (ip = 0; ip < n; ++ip) {
+      b[ip] += z[ip];
+      d[ip] = b[ip];
+      z[ip] = 0.0;
+    }
+  }
+
+  std::cerr << "eigen: too many iterations in Jacobi transform." << std::endl;
+
+  return;
+}
+
+template <typename Derived, typename OtherDerived>
+bool isEqual(const Eigen::MatrixBase<Derived>& lhs,
+             const Eigen::MatrixBase<OtherDerived>& rhs,
+             const CoalScalar tol = std::numeric_limits<CoalScalar>::epsilon() *
+                                    100) {
+  return ((lhs - rhs).array().abs() < tol).all();
+}
+
+}  // namespace coal
+
+#endif

include/coal/internal/traversal.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2019, LAAS CNRS
+ *  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 Joseph Mirabel */
+
+#ifndef COAL_TRAVERSAL_DETAILS_TRAVERSAL_H
+#define COAL_TRAVERSAL_DETAILS_TRAVERSAL_H
+
+/// @cond INTERNAL
+
+namespace coal {
+
+enum { RelativeTransformationIsIdentity = 1 };
+
+namespace details {
+template <bool enabled>
+struct COAL_DLLAPI RelativeTransformation {
+  RelativeTransformation() : R(Matrix3s::Identity()) {}
+
+  const Matrix3s& _R() const { return R; }
+  const Vec3s& _T() const { return T; }
+
+  Matrix3s R;
+  Vec3s T;
+};
+
+template <>
+struct COAL_DLLAPI RelativeTransformation<false> {
+  static const Matrix3s& _R() {
+    COAL_THROW_PRETTY("should never reach this point", std::logic_error);
+  }
+  static const Vec3s& _T() {
+    COAL_THROW_PRETTY("should never reach this point", std::logic_error);
+  }
+};
+}  // namespace details
+
+}  // namespace coal
+
+/// @endcond
+
+#endif

include/fcl/traversal/traversal_node_base.h → include/coal/internal/traversal_node_base.h

@@ -35,145 +35,138 @@
 
 /** \author Jia Pan */
 
-#ifndef FCL_TRAVERSAL_NODE_BASE_H
-#define FCL_TRAVERSAL_NODE_BASE_H
+#ifndef COAL_TRAVERSAL_NODE_BASE_H
+#define COAL_TRAVERSAL_NODE_BASE_H
 
-#include "fcl/data_types.h"
-#include "fcl/math/transform.h"
-#include "fcl/collision_data.h"
+/// @cond INTERNAL
 
-namespace fcl
-{
+#include "coal/data_types.h"
+#include "coal/math/transform.h"
+#include "coal/collision_data.h"
+
+namespace coal {
 
 /// @brief Node structure encoding the information required for traversal.
-class TraversalNodeBase
-{
-public:
-  virtual ~TraversalNodeBase();
+
+class TraversalNodeBase {
+ public:
+  TraversalNodeBase() : enable_statistics(false) {}
+
+  virtual ~TraversalNodeBase() {}
 
   virtual void preprocess() {}
-  
+
   virtual void postprocess() {}
 
-  /// @brief Whether b is a leaf node in the first BVH tree 
-  virtual bool isFirstNodeLeaf(int b) const;
+  /// @brief Whether b is a leaf node in the first BVH tree
+  virtual bool isFirstNodeLeaf(unsigned int /*b*/) const { return true; }
 
   /// @brief Whether b is a leaf node in the second BVH tree
-  virtual bool isSecondNodeLeaf(int b) const;
+  virtual bool isSecondNodeLeaf(unsigned int /*b*/) const { return true; }
 
   /// @brief Traverse the subtree of the node in the first tree first
-  virtual bool firstOverSecond(int b1, int b2) const;
+  virtual bool firstOverSecond(unsigned int /*b1*/, unsigned int /*b2*/) const {
+    return true;
+  }
 
   /// @brief Get the left child of the node b in the first tree
-  virtual int getFirstLeftChild(int b) const;
+  virtual int getFirstLeftChild(unsigned int b) const { return (int)b; }
 
   /// @brief Get the right child of the node b in the first tree
-  virtual int getFirstRightChild(int b) const;
+  virtual int getFirstRightChild(unsigned int b) const { return (int)b; }
 
   /// @brief Get the left child of the node b in the second tree
-  virtual int getSecondLeftChild(int b) const;
+  virtual int getSecondLeftChild(unsigned int b) const { return (int)b; }
 
   /// @brief Get the right child of the node b in the second tree
-  virtual int getSecondRightChild(int b) const;
+  virtual int getSecondRightChild(unsigned int b) const { return (int)b; }
 
-  /// @brief Enable statistics (verbose mode)
-  virtual void enableStatistics(bool enable) = 0;
+  /// @brief Whether store some statistics information during traversal
+  void enableStatistics(bool enable) { enable_statistics = enable; }
 
   /// @brief configuation of first object
-  Transform3f tf1;
+  Transform3s tf1;
 
   /// @brief configuration of second object
-  Transform3f tf2;
+  Transform3s tf2;
+
+  /// @brief Whether stores statistics
+  bool enable_statistics;
 };
 
-/// @brief Node structure encoding the information required for collision traversal.
-class CollisionTraversalNodeBase : public TraversalNodeBase
-{
-public:
-  CollisionTraversalNodeBase(bool enable_distance_lower_bound_ = false) :
-  result(NULL), enable_statistics(false),
-    enable_distance_lower_bound (enable_distance_lower_bound_){}
+/// @defgroup Traversal_For_Collision
+/// regroup class about traversal for distance.
+/// @{
 
-  virtual ~CollisionTraversalNodeBase();
+/// @brief Node structure encoding the information required for collision
+/// traversal.
+class CollisionTraversalNodeBase : public TraversalNodeBase {
+ public:
+  CollisionTraversalNodeBase(const CollisionRequest& request_)
+      : request(request_), result(NULL) {}
 
-  /// @brief BV test between b1 and b2
-  virtual bool BVTesting(int b1, int b2) const = 0;
+  virtual ~CollisionTraversalNodeBase() {}
 
   /// BV test between b1 and b2
-  /// \param b1, b2 Bounding volumes to test,
-  /// \retval sqrDistLowerBound square of a lower bound of the minimal
+  /// @param b1, b2 Bounding volumes to test,
+  /// @retval sqrDistLowerBound square of a lower bound of the minimal
   ///         distance between bounding volumes.
-  virtual bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const = 0;
+  virtual bool BVDisjoints(unsigned int b1, unsigned int b2,
+                           CoalScalar& sqrDistLowerBound) const = 0;
 
   /// @brief Leaf test between node b1 and b2, if they are both leafs
-  virtual void leafTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
-  {
-    throw std::runtime_error ("Not implemented");
-  }
+  virtual void leafCollides(unsigned int /*b1*/, unsigned int /*b2*/,
+                            CoalScalar& /*sqrDistLowerBound*/) const = 0;
 
   /// @brief Check whether the traversal can stop
-  virtual bool canStop() const;
-
-  /// @brief Whether store some statistics information during traversal
-  void enableStatistics(bool enable) { enable_statistics = enable; }
+  bool canStop() const { return this->request.isSatisfied(*(this->result)); }
 
   /// @brief request setting for collision
-  CollisionRequest request;
+  const CollisionRequest& request;
 
   /// @brief collision result kept during the traversal iteration
   CollisionResult* result;
+};
 
-  /// @brief Whether stores statistics 
-  bool enable_statistics;
+/// @}
 
-  /// Whether to compute a lower bound on distance between bounding volumes
-  bool enable_distance_lower_bound;
-};
+/// @defgroup Traversal_For_Distance
+/// regroup class about traversal for distance.
+/// @{
 
-/// @brief Node structure encoding the information required for distance traversal.
-class DistanceTraversalNodeBase : public TraversalNodeBase
-{
-public:
-  DistanceTraversalNodeBase() : result(NULL), enable_statistics(false) {}
+/// @brief Node structure encoding the information required for distance
+/// traversal.
+class DistanceTraversalNodeBase : public TraversalNodeBase {
+ public:
+  DistanceTraversalNodeBase() : result(NULL) {}
 
-  virtual ~DistanceTraversalNodeBase();
+  virtual ~DistanceTraversalNodeBase() {}
 
   /// @brief BV test between b1 and b2
-  virtual FCL_REAL BVTesting(int b1, int b2) const;
+  /// @return a lower bound of the distance between the two BV.
+  /// @note except for OBB, this method returns the distance.
+  virtual CoalScalar BVDistanceLowerBound(unsigned int /*b1*/,
+                                          unsigned int /*b2*/) const {
+    return (std::numeric_limits<CoalScalar>::max)();
+  }
 
   /// @brief Leaf test between node b1 and b2, if they are both leafs
-  virtual void leafTesting(int b1, int b2) const = 0;
+  virtual void leafComputeDistance(unsigned int b1, unsigned int b2) const = 0;
 
   /// @brief Check whether the traversal can stop
-  virtual bool canStop(FCL_REAL c) const;
-
-  /// @brief Whether store some statistics information during traversal
-  void enableStatistics(bool enable) { enable_statistics = enable; }
+  virtual bool canStop(CoalScalar /*c*/) const { return false; }
 
   /// @brief request setting for distance
   DistanceRequest request;
 
   /// @brief distance result kept during the traversal iteration
   DistanceResult* result;
-
-  /// @brief Whether stores statistics 
-  bool enable_statistics;
 };
 
+///@}
 
-struct ConservativeAdvancementStackData
-{
-  ConservativeAdvancementStackData(const Vec3f& P1_, const Vec3f& P2_, int c1_, int c2_, FCL_REAL d_)
-    : P1(P1_), P2(P2_), c1(c1_), c2(c2_), d(d_) {}
-
-  Vec3f P1;
-  Vec3f P2;
-  int c1;
-  int c2;
-  FCL_REAL d;
-};
-
+}  // namespace coal
 
-}
+/// @endcond
 
 #endif

include/coal/internal/traversal_node_bvh_hfield.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2021, 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.
+ */
+
+/** \author Justin Carpentier */
+
+#ifndef COAL_TRAVERSAL_NODE_BVH_HFIELD_H
+#define COAL_TRAVERSAL_NODE_BVH_HFIELD_H
+
+/// @cond INTERNAL
+
+#include "coal/collision_data.h"
+#include "coal/internal/traversal_node_base.h"
+#include "coal/internal/traversal_node_hfield_shape.h"
+#include "coal/BV/BV_node.h"
+#include "coal/BV/BV.h"
+#include "coal/BVH/BVH_model.h"
+#include "coal/hfield.h"
+#include "coal/internal/intersect.h"
+#include "coal/shape/geometric_shapes.h"
+#include "coal/narrowphase/narrowphase.h"
+#include "coal/internal/traversal.h"
+
+#include <limits>
+#include <vector>
+#include <cassert>
+
+namespace coal {
+
+/// @addtogroup Traversal_For_Collision
+/// @{
+
+/// @brief Traversal node for collision between one BVH model and one
+/// HeightField
+template <typename BV1, typename BV2,
+          int _Options = RelativeTransformationIsIdentity>
+class MeshHeightFieldCollisionTraversalNode
+    : public CollisionTraversalNodeBase {
+ public:
+  enum {
+    Options = _Options,
+    RTIsIdentity = _Options & RelativeTransformationIsIdentity
+  };
+
+  MeshHeightFieldCollisionTraversalNode(const CollisionRequest& request)
+      : CollisionTraversalNodeBase(request) {
+    model1 = NULL;
+    model2 = NULL;
+
+    num_bv_tests = 0;
+    num_leaf_tests = 0;
+    query_time_seconds = 0.0;
+
+    vertices1 = NULL;
+    tri_indices1 = NULL;
+  }
+
+  /// @brief Whether the BV node in the first BVH tree is leaf
+  bool isFirstNodeLeaf(unsigned int b) const {
+    assert(model1 != NULL && "model1 is NULL");
+    return model1->getBV(b).isLeaf();
+  }
+
+  /// @brief Whether the BV node in the second BVH tree is leaf
+  bool isSecondNodeLeaf(unsigned int b) const {
+    assert(model2 != NULL && "model2 is NULL");
+    return model2->getBV(b).isLeaf();
+  }
+
+  /// @brief Determine the traversal order, is the first BVTT subtree better
+  bool firstOverSecond(unsigned int b1, unsigned int b2) const {
+    CoalScalar sz1 = model1->getBV(b1).bv.size();
+    CoalScalar sz2 = model2->getBV(b2).bv.size();
+
+    bool l1 = model1->getBV(b1).isLeaf();
+    bool l2 = model2->getBV(b2).isLeaf();
+
+    if (l2 || (!l1 && (sz1 > sz2))) return true;
+    return false;
+  }
+
+  /// @brief Obtain the left child of BV node in the first BVH
+  int getFirstLeftChild(unsigned int b) const {
+    return model1->getBV(b).leftChild();
+  }
+
+  /// @brief Obtain the right child of BV node in the first BVH
+  int getFirstRightChild(unsigned int b) const {
+    return model1->getBV(b).rightChild();
+  }
+
+  /// @brief Obtain the left child of BV node in the second BVH
+  int getSecondLeftChild(unsigned int b) const {
+    return model2->getBV(b).leftChild();
+  }
+
+  /// @brief Obtain the right child of BV node in the second BVH
+  int getSecondRightChild(unsigned int b) const {
+    return model2->getBV(b).rightChild();
+  }
+
+  /// @brief BV culling test in one BVTT node
+  bool BVDisjoints(unsigned int b1, unsigned int b2) const {
+    if (this->enable_statistics) this->num_bv_tests++;
+    if (RTIsIdentity)
+      return !this->model1->getBV(b1).overlap(this->model2->getBV(b2));
+    else
+      return !overlap(RT._R(), RT._T(), this->model1->getBV(b1).bv,
+                      this->model2->getBV(b2).bv);
+  }
+
+  /// BV test between b1 and b2
+  /// @param b1, b2 Bounding volumes to test,
+  /// @retval sqrDistLowerBound square of a lower bound of the minimal
+  ///         distance between bounding volumes.
+  bool BVDisjoints(unsigned int b1, unsigned int b2,
+                   CoalScalar& sqrDistLowerBound) const {
+    if (this->enable_statistics) this->num_bv_tests++;
+    if (RTIsIdentity)
+      return !this->model1->getBV(b1).overlap(this->model2->getBV(b2),
+                                              this->request, sqrDistLowerBound);
+    else {
+      bool res = !overlap(RT._R(), RT._T(), this->model1->getBV(b1).bv,
+                          this->model2->getBV(b2).bv, this->request,
+                          sqrDistLowerBound);
+      assert(!res || sqrDistLowerBound > 0);
+      return res;
+    }
+  }
+
+  /// Intersection testing between leaves (two triangles)
+  ///
+  /// @param b1, b2 id of primitive in bounding volume hierarchy
+  /// @retval sqrDistLowerBound squared lower bound of distance between
+  ///         primitives if they are not in collision.
+  ///
+  /// This method supports a security margin. If the distance between
+  /// the primitives is less than the security margin, the objects are
+  /// considered as in collision. in this case a contact point is
+  /// returned in the CollisionResult.
+  ///
+  /// @note If the distance between objects is less than the security margin,
+  ///       and the object are not colliding, the penetration depth is
+  ///       negative.
+  void leafCollides(unsigned int b1, unsigned int b2,
+                    CoalScalar& sqrDistLowerBound) const {
+    if (this->enable_statistics) this->num_leaf_tests++;
+
+    const BVNode<BV1>& node1 = this->model1->getBV(b1);
+    const HeightFieldNode<BV2>& node2 = this->model2->getBV(b2);
+
+    int primitive_id1 = node1.primitiveId();
+    const Triangle& tri_id1 = tri_indices1[primitive_id1];
+
+    const Vec3s& P1 = vertices1[tri_id1[0]];
+    const Vec3s& P2 = vertices1[tri_id1[1]];
+    const Vec3s& P3 = vertices1[tri_id1[2]];
+
+    TriangleP tri1(P1, P2, P3);
+
+    typedef Convex<Triangle> ConvexTriangle;
+    ConvexTriangle convex1, convex2;
+    details::buildConvexTriangles(node2, *this->model2, convex2, convex2);
+
+    GJKSolver solver;
+    Vec3s p1,
+        p2;  // closest points if no collision contact points if collision.
+    Vec3s normal;
+    CoalScalar distance;
+    solver.shapeDistance(tri1, this->tf1, tri2, this->tf2, distance, p1, p2,
+                         normal);
+    CoalScalar distToCollision = distance - this->request.security_margin;
+    sqrDistLowerBound = distance * distance;
+    if (distToCollision <= 0) {  // collision
+      Vec3s p(p1);               // contact point
+      CoalScalar penetrationDepth(0);
+      if (this->result->numContacts() < this->request.num_max_contacts) {
+        // How much (Q1, Q2, Q3) should be moved so that all vertices are
+        // above (P1, P2, P3).
+        penetrationDepth = -distance;
+        if (distance > 0) {
+          normal = (p2 - p1).normalized();
+          p = .5 * (p1 + p2);
+        }
+        this->result->addContact(Contact(this->model1, this->model2,
+                                         primitive_id1, primitive_id2, p,
+                                         normal, penetrationDepth));
+      }
+    }
+  }
+
+  /// @brief The first BVH model
+  const BVHModel<BV1>* model1;
+  /// @brief The second HeightField model
+  const HeightField<BV2>* model2;
+
+  /// @brief statistical information
+  mutable int num_bv_tests;
+  mutable int num_leaf_tests;
+  mutable CoalScalar query_time_seconds;
+
+  Vec3s* vertices1 Triangle* tri_indices1;
+
+  details::RelativeTransformation<!bool(RTIsIdentity)> RT;
+};
+
+/// @brief Traversal node for collision between two meshes if their underlying
+/// BVH node is oriented node (OBB, RSS, OBBRSS, kIOS)
+typedef MeshHeightFieldCollisionTraversalNode<OBB, 0>
+    MeshHeightFieldCollisionTraversalNodeOBB;
+typedef MeshHeightFieldCollisionTraversalNode<RSS, 0>
+    MeshHeightFieldCollisionTraversalNodeRSS;
+typedef MeshHeightFieldCollisionTraversalNode<kIOS, 0>
+    MeshHeightFieldCollisionTraversalNodekIOS;
+typedef MeshHeightFieldCollisionTraversalNode<OBBRSS, 0>
+    MeshHeightFieldCollisionTraversalNodeOBBRSS;
+
+/// @}
+
+namespace details {
+template <typename BV>
+struct DistanceTraversalBVDistanceLowerBound_impl {
+  static CoalScalar run(const BVNode<BV>& b1, const BVNode<BV>& b2) {
+    return b1.distance(b2);
+  }
+  static CoalScalar run(const Matrix3s& R, const Vec3s& T, const BVNode<BV>& b1,
+                        const BVNode<BV>& b2) {
+    return distance(R, T, b1.bv, b2.bv);
+  }
+};
+
+template <>
+struct DistanceTraversalBVDistanceLowerBound_impl<OBB> {
+  static CoalScalar run(const BVNode<OBB>& b1, const BVNode<OBB>& b2) {
+    CoalScalar sqrDistLowerBound;
+    CollisionRequest request(DISTANCE_LOWER_BOUND, 0);
+    // request.break_distance = ?
+    if (b1.overlap(b2, request, sqrDistLowerBound)) {
+      // TODO A penetration upper bound should be computed.
+      return -1;
+    }
+    return sqrt(sqrDistLowerBound);
+  }
+  static CoalScalar run(const Matrix3s& R, const Vec3s& T,
+                        const BVNode<OBB>& b1, const BVNode<OBB>& b2) {
+    CoalScalar sqrDistLowerBound;
+    CollisionRequest request(DISTANCE_LOWER_BOUND, 0);
+    // request.break_distance = ?
+    if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) {
+      // TODO A penetration upper bound should be computed.
+      return -1;
+    }
+    return sqrt(sqrDistLowerBound);
+  }
+};
+
+template <>
+struct DistanceTraversalBVDistanceLowerBound_impl<AABB> {
+  static CoalScalar run(const BVNode<AABB>& b1, const BVNode<AABB>& b2) {
+    CoalScalar sqrDistLowerBound;
+    CollisionRequest request(DISTANCE_LOWER_BOUND, 0);
+    // request.break_distance = ?
+    if (b1.overlap(b2, request, sqrDistLowerBound)) {
+      // TODO A penetration upper bound should be computed.
+      return -1;
+    }
+    return sqrt(sqrDistLowerBound);
+  }
+  static CoalScalar run(const Matrix3s& R, const Vec3s& T,
+                        const BVNode<AABB>& b1, const BVNode<AABB>& b2) {
+    CoalScalar sqrDistLowerBound;
+    CollisionRequest request(DISTANCE_LOWER_BOUND, 0);
+    // request.break_distance = ?
+    if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) {
+      // TODO A penetration upper bound should be computed.
+      return -1;
+    }
+    return sqrt(sqrDistLowerBound);
+  }
+};
+}  // namespace details
+
+/// @addtogroup Traversal_For_Distance
+/// @{
+
+/// @brief Traversal node for distance computation between BVH models
+template <typename BV>
+class BVHDistanceTraversalNode : public DistanceTraversalNodeBase {
+ public:
+  BVHDistanceTraversalNode() : DistanceTraversalNodeBase() {
+    model1 = NULL;
+    model2 = NULL;
+
+    num_bv_tests = 0;
+    num_leaf_tests = 0;
+    query_time_seconds = 0.0;
+  }
+
+  /// @brief Whether the BV node in the first BVH tree is leaf
+  bool isFirstNodeLeaf(unsigned int b) const {
+    return model1->getBV(b).isLeaf();
+  }
+
+  /// @brief Whether the BV node in the second BVH tree is leaf
+  bool isSecondNodeLeaf(unsigned int b) const {
+    return model2->getBV(b).isLeaf();
+  }
+
+  /// @brief Determine the traversal order, is the first BVTT subtree better
+  bool firstOverSecond(unsigned int b1, unsigned int b2) const {
+    CoalScalar sz1 = model1->getBV(b1).bv.size();
+    CoalScalar sz2 = model2->getBV(b2).bv.size();
+
+    bool l1 = model1->getBV(b1).isLeaf();
+    bool l2 = model2->getBV(b2).isLeaf();
+
+    if (l2 || (!l1 && (sz1 > sz2))) return true;
+    return false;
+  }
+
+  /// @brief Obtain the left child of BV node in the first BVH
+  int getFirstLeftChild(unsigned int b) const {
+    return model1->getBV(b).leftChild();
+  }
+
+  /// @brief Obtain the right child of BV node in the first BVH
+  int getFirstRightChild(unsigned int b) const {
+    return model1->getBV(b).rightChild();
+  }
+
+  /// @brief Obtain the left child of BV node in the second BVH
+  int getSecondLeftChild(unsigned int b) const {
+    return model2->getBV(b).leftChild();
+  }
+
+  /// @brief Obtain the right child of BV node in the second BVH
+  int getSecondRightChild(unsigned int b) const {
+    return model2->getBV(b).rightChild();
+  }
+
+  /// @brief The first BVH model
+  const BVHModel<BV>* model1;
+  /// @brief The second BVH model
+  const BVHModel<BV>* model2;
+
+  /// @brief statistical information
+  mutable int num_bv_tests;
+  mutable int num_leaf_tests;
+  mutable CoalScalar query_time_seconds;
+};
+
+/// @brief Traversal node for distance computation between two meshes
+template <typename BV, int _Options = RelativeTransformationIsIdentity>
+class MeshDistanceTraversalNode : public BVHDistanceTraversalNode<BV> {
+ public:
+  enum {
+    Options = _Options,
+    RTIsIdentity = _Options & RelativeTransformationIsIdentity
+  };
+
+  using BVHDistanceTraversalNode<BV>::enable_statistics;
+  using BVHDistanceTraversalNode<BV>::request;
+  using BVHDistanceTraversalNode<BV>::result;
+  using BVHDistanceTraversalNode<BV>::tf1;
+  using BVHDistanceTraversalNode<BV>::model1;
+  using BVHDistanceTraversalNode<BV>::model2;
+  using BVHDistanceTraversalNode<BV>::num_bv_tests;
+  using BVHDistanceTraversalNode<BV>::num_leaf_tests;
+
+  MeshDistanceTraversalNode() : BVHDistanceTraversalNode<BV>() {
+    vertices1 = NULL;
+    vertices2 = NULL;
+    tri_indices1 = NULL;
+    tri_indices2 = NULL;
+
+    rel_err = this->request.rel_err;
+    abs_err = this->request.abs_err;
+  }
+
+  void preprocess() {
+    if (!RTIsIdentity) preprocessOrientedNode();
+  }
+
+  void postprocess() {
+    if (!RTIsIdentity) postprocessOrientedNode();
+  }
+
+  /// @brief BV culling test in one BVTT node
+  CoalScalar BVDistanceLowerBound(unsigned int b1, unsigned int b2) const {
+    if (enable_statistics) num_bv_tests++;
+    if (RTIsIdentity)
+      return details::DistanceTraversalBVDistanceLowerBound_impl<BV>::run(
+          model1->getBV(b1), model2->getBV(b2));
+    else
+      return details::DistanceTraversalBVDistanceLowerBound_impl<BV>::run(
+          RT._R(), RT._T(), model1->getBV(b1), model2->getBV(b2));
+  }
+
+  /// @brief Distance testing between leaves (two triangles)
+  void leafComputeDistance(unsigned int b1, unsigned int b2) const {
+    if (this->enable_statistics) this->num_leaf_tests++;
+
+    const BVNode<BV>& node1 = this->model1->getBV(b1);
+    const BVNode<BV>& node2 = this->model2->getBV(b2);
+
+    int primitive_id1 = node1.primitiveId();
+    int primitive_id2 = node2.primitiveId();
+
+    const Triangle& tri_id1 = tri_indices1[primitive_id1];
+    const Triangle& tri_id2 = tri_indices2[primitive_id2];
+
+    const Vec3s& t11 = vertices1[tri_id1[0]];
+    const Vec3s& t12 = vertices1[tri_id1[1]];
+    const Vec3s& t13 = vertices1[tri_id1[2]];
+
+    const Vec3s& t21 = vertices2[tri_id2[0]];
+    const Vec3s& t22 = vertices2[tri_id2[1]];
+    const Vec3s& t23 = vertices2[tri_id2[2]];
+
+    // nearest point pair
+    Vec3s P1, P2, normal;
+
+    CoalScalar d2;
+    if (RTIsIdentity)
+      d2 = TriangleDistance::sqrTriDistance(t11, t12, t13, t21, t22, t23, P1,
+                                            P2);
+    else
+      d2 = TriangleDistance::sqrTriDistance(t11, t12, t13, t21, t22, t23,
+                                            RT._R(), RT._T(), P1, P2);
+    CoalScalar d = sqrt(d2);
+
+    this->result->update(d, this->model1, this->model2, primitive_id1,
+                         primitive_id2, P1, P2, normal);
+  }
+
+  /// @brief Whether the traversal process can stop early
+  bool canStop(CoalScalar c) const {
+    if ((c >= this->result->min_distance - abs_err) &&
+        (c * (1 + rel_err) >= this->result->min_distance))
+      return true;
+    return false;
+  }
+
+  Vec3s* vertices1;
+  Vec3s* vertices2;
+
+  Triangle* tri_indices1;
+  Triangle* tri_indices2;
+
+  /// @brief relative and absolute error, default value is 0.01 for both terms
+  CoalScalar rel_err;
+  CoalScalar abs_err;
+
+  details::RelativeTransformation<!bool(RTIsIdentity)> RT;
+
+ private:
+  void preprocessOrientedNode() {
+    const int init_tri_id1 = 0, init_tri_id2 = 0;
+    const Triangle& init_tri1 = tri_indices1[init_tri_id1];
+    const Triangle& init_tri2 = tri_indices2[init_tri_id2];
+
+    Vec3s init_tri1_points[3];
+    Vec3s init_tri2_points[3];
+
+    init_tri1_points[0] = vertices1[init_tri1[0]];
+    init_tri1_points[1] = vertices1[init_tri1[1]];
+    init_tri1_points[2] = vertices1[init_tri1[2]];
+
+    init_tri2_points[0] = vertices2[init_tri2[0]];
+    init_tri2_points[1] = vertices2[init_tri2[1]];
+    init_tri2_points[2] = vertices2[init_tri2[2]];
+
+    Vec3s p1, p2, normal;
+    CoalScalar distance = sqrt(TriangleDistance::sqrTriDistance(
+        init_tri1_points[0], init_tri1_points[1], init_tri1_points[2],
+        init_tri2_points[0], init_tri2_points[1], init_tri2_points[2], RT._R(),
+        RT._T(), p1, p2));
+
+    result->update(distance, model1, model2, init_tri_id1, init_tri_id2, p1, p2,
+                   normal);
+  }
+  void postprocessOrientedNode() {
+    /// the points obtained by triDistance are not in world space: both are in
+    /// object1's local coordinate system, so we need to convert them into the
+    /// world space.
+    if (request.enable_nearest_points && (result->o1 == model1) &&
+        (result->o2 == model2)) {
+      result->nearest_points[0] = tf1.transform(result->nearest_points[0]);
+      result->nearest_points[1] = tf1.transform(result->nearest_points[1]);
+    }
+  }
+};
+
+/// @brief Traversal node for distance computation between two meshes if their
+/// underlying BVH node is oriented node (RSS, OBBRSS, kIOS)
+typedef MeshDistanceTraversalNode<RSS, 0> MeshDistanceTraversalNodeRSS;
+typedef MeshDistanceTraversalNode<kIOS, 0> MeshDistanceTraversalNodekIOS;
+typedef MeshDistanceTraversalNode<OBBRSS, 0> MeshDistanceTraversalNodeOBBRSS;
+
+/// @}
+
+/// @brief for OBB and RSS, there is local coordinate of BV, so normal need to
+/// be transformed
+namespace details {
+
+template <typename BV>
+inline const Matrix3s& getBVAxes(const BV& bv) {
+  return bv.axes;
+}
+
+template <>
+inline const Matrix3s& getBVAxes<OBBRSS>(const OBBRSS& bv) {
+  return bv.obb.axes;
+}
+
+}  // namespace details
+
+}  // namespace coal
+
+/// @endcond
+
+#endif

include/coal/internal/traversal_node_bvh_shape.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_TRAVERSAL_NODE_MESH_SHAPE_H
+#define COAL_TRAVERSAL_NODE_MESH_SHAPE_H
+
+/// @cond INTERNAL
+
+#include "coal/collision_data.h"
+#include "coal/shape/geometric_shapes.h"
+#include "coal/narrowphase/narrowphase.h"
+#include "coal/shape/geometric_shapes_utility.h"
+#include "coal/internal/traversal_node_base.h"
+#include "coal/internal/traversal.h"
+#include "coal/BVH/BVH_model.h"
+#include "coal/internal/shape_shape_func.h"
+
+namespace coal {
+
+/// @addtogroup Traversal_For_Collision
+/// @{
+
+/// @brief Traversal node for collision between BVH and shape
+template <typename BV, typename S>
+class BVHShapeCollisionTraversalNode : public CollisionTraversalNodeBase {
+ public:
+  BVHShapeCollisionTraversalNode(const CollisionRequest& request)
+      : CollisionTraversalNodeBase(request) {
+    model1 = NULL;
+    model2 = NULL;
+
+    num_bv_tests = 0;
+    num_leaf_tests = 0;
+    query_time_seconds = 0.0;
+  }
+
+  /// @brief Whether the BV node in the first BVH tree is leaf
+  bool isFirstNodeLeaf(unsigned int b) const {
+    return model1->getBV(b).isLeaf();
+  }
+
+  /// @brief Obtain the left child of BV node in the first BVH
+  int getFirstLeftChild(unsigned int b) const {
+    return model1->getBV(b).leftChild();
+  }
+
+  /// @brief Obtain the right child of BV node in the first BVH
+  int getFirstRightChild(unsigned int b) const {
+    return model1->getBV(b).rightChild();
+  }
+
+  const BVHModel<BV>* model1;
+  const S* model2;
+  BV model2_bv;
+
+  mutable int num_bv_tests;
+  mutable int num_leaf_tests;
+  mutable CoalScalar query_time_seconds;
+};
+
+/// @brief Traversal node for collision between mesh and shape
+template <typename BV, typename S,
+          int _Options = RelativeTransformationIsIdentity>
+class MeshShapeCollisionTraversalNode
+    : public BVHShapeCollisionTraversalNode<BV, S> {
+ public:
+  enum {
+    Options = _Options,
+    RTIsIdentity = _Options & RelativeTransformationIsIdentity
+  };
+
+  MeshShapeCollisionTraversalNode(const CollisionRequest& request)
+      : BVHShapeCollisionTraversalNode<BV, S>(request) {
+    vertices = NULL;
+    tri_indices = NULL;
+
+    nsolver = NULL;
+  }
+
+  /// test between BV b1 and shape
+  /// @param b1 BV to test,
+  /// @retval sqrDistLowerBound square of a lower bound of the minimal
+  ///         distance between bounding volumes.
+  /// @brief BV culling test in one BVTT node
+  bool BVDisjoints(unsigned int b1, unsigned int /*b2*/,
+                   CoalScalar& sqrDistLowerBound) const {
+    if (this->enable_statistics) this->num_bv_tests++;
+    bool disjoint;
+    if (RTIsIdentity)
+      disjoint = !this->model1->getBV(b1).bv.overlap(
+          this->model2_bv, this->request, sqrDistLowerBound);
+    else
+      disjoint = !overlap(this->tf1.getRotation(), this->tf1.getTranslation(),
+                          this->model1->getBV(b1).bv, this->model2_bv,
+                          this->request, sqrDistLowerBound);
+    if (disjoint)
+      internal::updateDistanceLowerBoundFromBV(this->request, *this->result,
+                                               sqrDistLowerBound);
+    assert(!disjoint || sqrDistLowerBound > 0);
+    return disjoint;
+  }
+
+  /// @brief Intersection testing between leaves (one triangle and one shape)
+  void leafCollides(unsigned int b1, unsigned int /*b2*/,
+                    CoalScalar& sqrDistLowerBound) const {
+    if (this->enable_statistics) this->num_leaf_tests++;
+    const BVNode<BV>& node = this->model1->getBV(b1);
+
+    int primitive_id = node.primitiveId();
+
+    const Triangle& tri_id = this->tri_indices[primitive_id];
+    const TriangleP tri(this->vertices[tri_id[0]], this->vertices[tri_id[1]],
+                        this->vertices[tri_id[2]]);
+
+    // When reaching this point, `this->solver` has already been set up
+    // by the CollisionRequest `this->request`.
+    // The only thing we need to (and can) pass to `ShapeShapeDistance` is
+    // whether or not penetration information is should be computed in case of
+    // collision.
+    const bool compute_penetration =
+        this->request.enable_contact || (this->request.security_margin < 0);
+    Vec3s c1, c2, normal;
+    CoalScalar distance;
+
+    if (RTIsIdentity) {
+      static const Transform3s Id;
+      distance = internal::ShapeShapeDistance<TriangleP, S>(
+          &tri, Id, this->model2, this->tf2, this->nsolver, compute_penetration,
+          c1, c2, normal);
+    } else {
+      distance = internal::ShapeShapeDistance<TriangleP, S>(
+          &tri, this->tf1, this->model2, this->tf2, this->nsolver,
+          compute_penetration, c1, c2, normal);
+    }
+    const CoalScalar distToCollision = distance - this->request.security_margin;
+
+    internal::updateDistanceLowerBoundFromLeaf(this->request, *(this->result),
+                                               distToCollision, c1, c2, normal);
+
+    if (distToCollision <= this->request.collision_distance_threshold) {
+      sqrDistLowerBound = 0;
+      if (this->result->numContacts() < this->request.num_max_contacts) {
+        this->result->addContact(Contact(this->model1, this->model2,
+                                         primitive_id, Contact::NONE, c1, c2,
+                                         normal, distance));
+        assert(this->result->isCollision());
+      }
+    } else {
+      sqrDistLowerBound = distToCollision * distToCollision;
+    }
+
+    assert(this->result->isCollision() || sqrDistLowerBound > 0);
+  }  // leafCollides
+
+  Vec3s* vertices;
+  Triangle* tri_indices;
+
+  const GJKSolver* nsolver;
+};
+
+/// @}
+
+/// @addtogroup Traversal_For_Distance
+/// @{
+
+/// @brief Traversal node for distance computation between BVH and shape
+template <typename BV, typename S>
+class BVHShapeDistanceTraversalNode : public DistanceTraversalNodeBase {
+ public:
+  BVHShapeDistanceTraversalNode() : DistanceTraversalNodeBase() {
+    model1 = NULL;
+    model2 = NULL;
+
+    num_bv_tests = 0;
+    num_leaf_tests = 0;
+    query_time_seconds = 0.0;
+  }
+
+  /// @brief Whether the BV node in the first BVH tree is leaf
+  bool isFirstNodeLeaf(unsigned int b) const {
+    return model1->getBV(b).isLeaf();
+  }
+
+  /// @brief Obtain the left child of BV node in the first BVH
+  int getFirstLeftChild(unsigned int b) const {
+    return model1->getBV(b).leftChild();
+  }
+
+  /// @brief Obtain the right child of BV node in the first BVH
+  int getFirstRightChild(unsigned int b) const {
+    return model1->getBV(b).rightChild();
+  }
+
+  /// @brief BV culling test in one BVTT node
+  CoalScalar BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const {
+    return model1->getBV(b1).bv.distance(model2_bv);
+  }
+
+  const BVHModel<BV>* model1;
+  const S* model2;
+  BV model2_bv;
+
+  mutable int num_bv_tests;
+  mutable int num_leaf_tests;
+  mutable CoalScalar query_time_seconds;
+};
+
+/// @brief Traversal node for distance computation between shape and BVH
+template <typename S, typename BV>
+class ShapeBVHDistanceTraversalNode : public DistanceTraversalNodeBase {
+ public:
+  ShapeBVHDistanceTraversalNode() : DistanceTraversalNodeBase() {
+    model1 = NULL;
+    model2 = NULL;
+
+    num_bv_tests = 0;
+    num_leaf_tests = 0;
+    query_time_seconds = 0.0;
+  }
+
+  /// @brief Whether the BV node in the second BVH tree is leaf
+  bool isSecondNodeLeaf(unsigned int b) const {
+    return model2->getBV(b).isLeaf();
+  }
+
+  /// @brief Obtain the left child of BV node in the second BVH
+  int getSecondLeftChild(unsigned int b) const {
+    return model2->getBV(b).leftChild();
+  }
+
+  /// @brief Obtain the right child of BV node in the second BVH
+  int getSecondRightChild(unsigned int b) const {
+    return model2->getBV(b).rightChild();
+  }
+
+  /// @brief BV culling test in one BVTT node
+  CoalScalar BVDistanceLowerBound(unsigned int /*b1*/, unsigned int b2) const {
+    return model1_bv.distance(model2->getBV(b2).bv);
+  }
+
+  const S* model1;
+  const BVHModel<BV>* model2;
+  BV model1_bv;
+
+  mutable int num_bv_tests;
+  mutable int num_leaf_tests;
+  mutable CoalScalar query_time_seconds;
+};
+
+/// @brief Traversal node for distance between mesh and shape
+template <typename BV, typename S>
+class MeshShapeDistanceTraversalNode
+    : public BVHShapeDistanceTraversalNode<BV, S> {
+ public:
+  MeshShapeDistanceTraversalNode() : BVHShapeDistanceTraversalNode<BV, S>() {
+    vertices = NULL;
+    tri_indices = NULL;
+
+    rel_err = 0;
+    abs_err = 0;
+
+    nsolver = NULL;
+  }
+
+  /// @brief Distance testing between leaves (one triangle and one shape)
+  void leafComputeDistance(unsigned int b1, unsigned int /*b2*/) const {
+    if (this->enable_statistics) this->num_leaf_tests++;
+
+    const BVNode<BV>& node = this->model1->getBV(b1);
+
+    int primitive_id = node.primitiveId();
+
+    const Triangle& tri_id = tri_indices[primitive_id];
+    const TriangleP tri(this->vertices[tri_id[0]], this->vertices[tri_id[1]],
+                        this->vertices[tri_id[2]]);
+
+    Vec3s p1, p2, normal;
+    const CoalScalar distance = internal::ShapeShapeDistance<TriangleP, S>(
+        &tri, this->tf1, this->model2, this->tf2, this->nsolver,
+        this->request.enable_signed_distance, p1, p2, normal);
+
+    this->result->update(distance, this->model1, this->model2, primitive_id,
+                         DistanceResult::NONE, p1, p2, normal);
+  }
+
+  /// @brief Whether the traversal process can stop early
+  bool canStop(CoalScalar c) const {
+    if ((c >= this->result->min_distance - abs_err) &&
+        (c * (1 + rel_err) >= this->result->min_distance))
+      return true;
+    return false;
+  }
+
+  Vec3s* vertices;
+  Triangle* tri_indices;
+
+  CoalScalar rel_err;
+  CoalScalar abs_err;
+
+  const GJKSolver* nsolver;
+};
+
+/// @cond IGNORE
+namespace details {
+
+template <typename BV, typename S>
+void meshShapeDistanceOrientedNodeleafComputeDistance(
+    unsigned int b1, unsigned int /* b2 */, const BVHModel<BV>* model1,
+    const S& model2, Vec3s* vertices, Triangle* tri_indices,
+    const Transform3s& tf1, const Transform3s& tf2, const GJKSolver* nsolver,
+    bool enable_statistics, int& num_leaf_tests, const DistanceRequest& request,
+    DistanceResult& result) {
+  if (enable_statistics) num_leaf_tests++;
+
+  const BVNode<BV>& node = model1->getBV(b1);
+  int primitive_id = node.primitiveId();
+
+  const Triangle& tri_id = tri_indices[primitive_id];
+  const TriangleP tri(vertices[tri_id[0]], vertices[tri_id[1]],
+                      vertices[tri_id[2]]);
+
+  Vec3s p1, p2, normal;
+  const CoalScalar distance = internal::ShapeShapeDistance<TriangleP, S>(
+      &tri, tf1, &model2, tf2, nsolver, request.enable_signed_distance, p1, p2,
+      normal);
+
+  result.update(distance, model1, &model2, primitive_id, DistanceResult::NONE,
+                p1, p2, normal);
+}
+
+template <typename BV, typename S>
+static inline void distancePreprocessOrientedNode(
+    const BVHModel<BV>* model1, Vec3s* vertices, Triangle* tri_indices,
+    int init_tri_id, const S& model2, const Transform3s& tf1,
+    const Transform3s& tf2, const GJKSolver* nsolver,
+    const DistanceRequest& request, DistanceResult& result) {
+  const Triangle& tri_id = tri_indices[init_tri_id];
+  const TriangleP tri(vertices[tri_id[0]], vertices[tri_id[1]],
+                      vertices[tri_id[2]]);
+
+  Vec3s p1, p2, normal;
+  const CoalScalar distance = internal::ShapeShapeDistance<TriangleP, S>(
+      &tri, tf1, &model2, tf2, nsolver, request.enable_signed_distance, p1, p2,
+      normal);
+  result.update(distance, model1, &model2, init_tri_id, DistanceResult::NONE,
+                p1, p2, normal);
+}
+
+}  // namespace details
+
+/// @endcond
+
+/// @brief Traversal node for distance between mesh and shape, when mesh BVH is
+/// one of the oriented node (RSS, kIOS, OBBRSS)
+template <typename S>
+class MeshShapeDistanceTraversalNodeRSS
+    : public MeshShapeDistanceTraversalNode<RSS, S> {
+ public:
+  MeshShapeDistanceTraversalNodeRSS()
+      : MeshShapeDistanceTraversalNode<RSS, S>() {}
+
+  void preprocess() {
+    details::distancePreprocessOrientedNode(
+        this->model1, this->vertices, this->tri_indices, 0, *(this->model2),
+        this->tf1, this->tf2, this->nsolver, this->request, *(this->result));
+  }
+
+  void postprocess() {}
+
+  CoalScalar BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const {
+    if (this->enable_statistics) this->num_bv_tests++;
+    return distance(this->tf1.getRotation(), this->tf1.getTranslation(),
+                    this->model2_bv, this->model1->getBV(b1).bv);
+  }
+
+  void leafComputeDistance(unsigned int b1, unsigned int b2) const {
+    details::meshShapeDistanceOrientedNodeleafComputeDistance(
+        b1, b2, this->model1, *(this->model2), this->vertices,
+        this->tri_indices, this->tf1, this->tf2, this->nsolver,
+        this->enable_statistics, this->num_leaf_tests, this->request,
+        *(this->result));
+  }
+};
+
+template <typename S>
+class MeshShapeDistanceTraversalNodekIOS
+    : public MeshShapeDistanceTraversalNode<kIOS, S> {
+ public:
+  MeshShapeDistanceTraversalNodekIOS()
+      : MeshShapeDistanceTraversalNode<kIOS, S>() {}
+
+  void preprocess() {
+    details::distancePreprocessOrientedNode(
+        this->model1, this->vertices, this->tri_indices, 0, *(this->model2),
+        this->tf1, this->tf2, this->nsolver, this->request, *(this->result));
+  }
+
+  void postprocess() {}
+
+  CoalScalar BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const {
+    if (this->enable_statistics) this->num_bv_tests++;
+    return distance(this->tf1.getRotation(), this->tf1.getTranslation(),
+                    this->model2_bv, this->model1->getBV(b1).bv);
+  }
+
+  void leafComputeDistance(unsigned int b1, unsigned int b2) const {
+    details::meshShapeDistanceOrientedNodeleafComputeDistance(
+        b1, b2, this->model1, *(this->model2), this->vertices,
+        this->tri_indices, this->tf1, this->tf2, this->nsolver,
+        this->enable_statistics, this->num_leaf_tests, this->request,
+        *(this->result));
+  }
+};
+
+template <typename S>
+class MeshShapeDistanceTraversalNodeOBBRSS
+    : public MeshShapeDistanceTraversalNode<OBBRSS, S> {
+ public:
+  MeshShapeDistanceTraversalNodeOBBRSS()
+      : MeshShapeDistanceTraversalNode<OBBRSS, S>() {}
+
+  void preprocess() {
+    details::distancePreprocessOrientedNode(
+        this->model1, this->vertices, this->tri_indices, 0, *(this->model2),
+        this->tf1, this->tf2, this->nsolver, this->request, *(this->result));
+  }
+
+  void postprocess() {}
+
+  CoalScalar BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const {
+    if (this->enable_statistics) this->num_bv_tests++;
+    return distance(this->tf1.getRotation(), this->tf1.getTranslation(),
+                    this->model2_bv, this->model1->getBV(b1).bv);
+  }
+
+  void leafComputeDistance(unsigned int b1, unsigned int b2) const {
+    details::meshShapeDistanceOrientedNodeleafComputeDistance(
+        b1, b2, this->model1, *(this->model2), this->vertices,
+        this->tri_indices, this->tf1, this->tf2, this->nsolver,
+        this->enable_statistics, this->num_leaf_tests, this->request,
+        *(this->result));
+  }
+};
+
+/// @}
+
+}  // namespace coal
+
+/// @endcond
+
+#endif

include/coal/internal/traversal_node_bvhs.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_TRAVERSAL_NODE_MESHES_H
+#define COAL_TRAVERSAL_NODE_MESHES_H
+
+/// @cond INTERNAL
+
+#include "coal/collision_data.h"
+#include "coal/internal/traversal_node_base.h"
+#include "coal/BV/BV_node.h"
+#include "coal/BV/BV.h"
+#include "coal/BVH/BVH_model.h"
+#include "coal/internal/intersect.h"
+#include "coal/shape/geometric_shapes.h"
+#include "coal/narrowphase/narrowphase.h"
+#include "coal/internal/traversal.h"
+#include "coal/internal/shape_shape_func.h"
+
+#include <cassert>
+
+namespace coal {
+
+/// @addtogroup Traversal_For_Collision
+/// @{
+
+/// @brief Traversal node for collision between BVH models
+template <typename BV>
+class BVHCollisionTraversalNode : public CollisionTraversalNodeBase {
+ public:
+  BVHCollisionTraversalNode(const CollisionRequest& request)
+      : CollisionTraversalNodeBase(request) {
+    model1 = NULL;
+    model2 = NULL;
+
+    num_bv_tests = 0;
+    num_leaf_tests = 0;
+    query_time_seconds = 0.0;
+  }
+
+  /// @brief Whether the BV node in the first BVH tree is leaf
+  bool isFirstNodeLeaf(unsigned int b) const {
+    assert(model1 != NULL && "model1 is NULL");
+    return model1->getBV(b).isLeaf();
+  }
+
+  /// @brief Whether the BV node in the second BVH tree is leaf
+  bool isSecondNodeLeaf(unsigned int b) const {
+    assert(model2 != NULL && "model2 is NULL");
+    return model2->getBV(b).isLeaf();
+  }
+
+  /// @brief Determine the traversal order, is the first BVTT subtree better
+  bool firstOverSecond(unsigned int b1, unsigned int b2) const {
+    CoalScalar sz1 = model1->getBV(b1).bv.size();
+    CoalScalar sz2 = model2->getBV(b2).bv.size();
+
+    bool l1 = model1->getBV(b1).isLeaf();
+    bool l2 = model2->getBV(b2).isLeaf();
+
+    if (l2 || (!l1 && (sz1 > sz2))) return true;
+    return false;
+  }
+
+  /// @brief Obtain the left child of BV node in the first BVH
+  int getFirstLeftChild(unsigned int b) const {
+    return model1->getBV(b).leftChild();
+  }
+
+  /// @brief Obtain the right child of BV node in the first BVH
+  int getFirstRightChild(unsigned int b) const {
+    return model1->getBV(b).rightChild();
+  }
+
+  /// @brief Obtain the left child of BV node in the second BVH
+  int getSecondLeftChild(unsigned int b) const {
+    return model2->getBV(b).leftChild();
+  }
+
+  /// @brief Obtain the right child of BV node in the second BVH
+  int getSecondRightChild(unsigned int b) const {
+    return model2->getBV(b).rightChild();
+  }
+
+  /// @brief The first BVH model
+  const BVHModel<BV>* model1;
+  /// @brief The second BVH model
+  const BVHModel<BV>* model2;
+
+  /// @brief statistical information
+  mutable int num_bv_tests;
+  mutable int num_leaf_tests;
+  mutable CoalScalar query_time_seconds;
+};
+
+/// @brief Traversal node for collision between two meshes
+template <typename BV, int _Options = RelativeTransformationIsIdentity>
+class MeshCollisionTraversalNode : public BVHCollisionTraversalNode<BV> {
+ public:
+  enum {
+    Options = _Options,
+    RTIsIdentity = _Options & RelativeTransformationIsIdentity
+  };
+
+  MeshCollisionTraversalNode(const CollisionRequest& request)
+      : BVHCollisionTraversalNode<BV>(request) {
+    vertices1 = NULL;
+    vertices2 = NULL;
+    tri_indices1 = NULL;
+    tri_indices2 = NULL;
+  }
+
+  /// BV test between b1 and b2
+  /// @param b1, b2 Bounding volumes to test,
+  /// @retval sqrDistLowerBound square of a lower bound of the minimal
+  ///         distance between bounding volumes.
+  bool BVDisjoints(unsigned int b1, unsigned int b2,
+                   CoalScalar& sqrDistLowerBound) const {
+    if (this->enable_statistics) this->num_bv_tests++;
+    bool disjoint;
+    if (RTIsIdentity)
+      disjoint = !this->model1->getBV(b1).overlap(
+          this->model2->getBV(b2), this->request, sqrDistLowerBound);
+    else {
+      disjoint = !overlap(RT._R(), RT._T(), this->model2->getBV(b2).bv,
+                          this->model1->getBV(b1).bv, this->request,
+                          sqrDistLowerBound);
+    }
+    if (disjoint)
+      internal::updateDistanceLowerBoundFromBV(this->request, *this->result,
+                                               sqrDistLowerBound);
+    return disjoint;
+  }
+
+  /// Intersection testing between leaves (two triangles)
+  ///
+  /// @param b1, b2 id of primitive in bounding volume hierarchy
+  /// @retval sqrDistLowerBound squared lower bound of distance between
+  ///         primitives if they are not in collision.
+  ///
+  /// This method supports a security margin. If the distance between
+  /// the primitives is less than the security margin, the objects are
+  /// considered as in collision. in this case a contact point is
+  /// returned in the CollisionResult.
+  ///
+  /// @note If the distance between objects is less than the security margin,
+  ///       and the object are not colliding, the penetration depth is
+  ///       negative.
+  void leafCollides(unsigned int b1, unsigned int b2,
+                    CoalScalar& sqrDistLowerBound) const {
+    if (this->enable_statistics) this->num_leaf_tests++;
+
+    const BVNode<BV>& node1 = this->model1->getBV(b1);
+    const BVNode<BV>& node2 = this->model2->getBV(b2);
+
+    int primitive_id1 = node1.primitiveId();
+    int primitive_id2 = node2.primitiveId();
+
+    const Triangle& tri_id1 = tri_indices1[primitive_id1];
+    const Triangle& tri_id2 = tri_indices2[primitive_id2];
+
+    const Vec3s& P1 = vertices1[tri_id1[0]];
+    const Vec3s& P2 = vertices1[tri_id1[1]];
+    const Vec3s& P3 = vertices1[tri_id1[2]];
+    const Vec3s& Q1 = vertices2[tri_id2[0]];
+    const Vec3s& Q2 = vertices2[tri_id2[1]];
+    const Vec3s& Q3 = vertices2[tri_id2[2]];
+
+    TriangleP tri1(P1, P2, P3);
+    TriangleP tri2(Q1, Q2, Q3);
+
+    // TODO(louis): MeshCollisionTraversalNode should have its own GJKSolver.
+    GJKSolver solver(this->request);
+
+    const bool compute_penetration =
+        this->request.enable_contact || (this->request.security_margin < 0);
+    Vec3s p1, p2, normal;
+    DistanceResult distanceResult;
+    CoalScalar distance = internal::ShapeShapeDistance<TriangleP, TriangleP>(
+        &tri1, this->tf1, &tri2, this->tf2, &solver, compute_penetration, p1,
+        p2, normal);
+
+    const CoalScalar distToCollision = distance - this->request.security_margin;
+
+    internal::updateDistanceLowerBoundFromLeaf(this->request, *(this->result),
+                                               distToCollision, p1, p2, normal);
+
+    if (distToCollision <=
+        this->request.collision_distance_threshold) {  // collision
+      sqrDistLowerBound = 0;
+      if (this->result->numContacts() < this->request.num_max_contacts) {
+        this->result->addContact(Contact(this->model1, this->model2,
+                                         primitive_id1, primitive_id2, p1, p2,
+                                         normal, distance));
+      }
+    } else
+      sqrDistLowerBound = distToCollision * distToCollision;
+  }
+
+  Vec3s* vertices1;
+  Vec3s* vertices2;
+
+  Triangle* tri_indices1;
+  Triangle* tri_indices2;
+
+  details::RelativeTransformation<!bool(RTIsIdentity)> RT;
+};
+
+/// @brief Traversal node for collision between two meshes if their underlying
+/// BVH node is oriented node (OBB, RSS, OBBRSS, kIOS)
+typedef MeshCollisionTraversalNode<OBB, 0> MeshCollisionTraversalNodeOBB;
+typedef MeshCollisionTraversalNode<RSS, 0> MeshCollisionTraversalNodeRSS;
+typedef MeshCollisionTraversalNode<kIOS, 0> MeshCollisionTraversalNodekIOS;
+typedef MeshCollisionTraversalNode<OBBRSS, 0> MeshCollisionTraversalNodeOBBRSS;
+
+/// @}
+
+namespace details {
+template <typename BV>
+struct DistanceTraversalBVDistanceLowerBound_impl {
+  static CoalScalar run(const BVNode<BV>& b1, const BVNode<BV>& b2) {
+    return b1.distance(b2);
+  }
+  static CoalScalar run(const Matrix3s& R, const Vec3s& T, const BVNode<BV>& b1,
+                        const BVNode<BV>& b2) {
+    return distance(R, T, b1.bv, b2.bv);
+  }
+};
+
+template <>
+struct DistanceTraversalBVDistanceLowerBound_impl<OBB> {
+  static CoalScalar run(const BVNode<OBB>& b1, const BVNode<OBB>& b2) {
+    CoalScalar sqrDistLowerBound;
+    CollisionRequest request(DISTANCE_LOWER_BOUND, 0);
+    // request.break_distance = ?
+    if (b1.overlap(b2, request, sqrDistLowerBound)) {
+      // TODO A penetration upper bound should be computed.
+      return -1;
+    }
+    return sqrt(sqrDistLowerBound);
+  }
+  static CoalScalar run(const Matrix3s& R, const Vec3s& T,
+                        const BVNode<OBB>& b1, const BVNode<OBB>& b2) {
+    CoalScalar sqrDistLowerBound;
+    CollisionRequest request(DISTANCE_LOWER_BOUND, 0);
+    // request.break_distance = ?
+    if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) {
+      // TODO A penetration upper bound should be computed.
+      return -1;
+    }
+    return sqrt(sqrDistLowerBound);
+  }
+};
+
+template <>
+struct DistanceTraversalBVDistanceLowerBound_impl<AABB> {
+  static CoalScalar run(const BVNode<AABB>& b1, const BVNode<AABB>& b2) {
+    CoalScalar sqrDistLowerBound;
+    CollisionRequest request(DISTANCE_LOWER_BOUND, 0);
+    // request.break_distance = ?
+    if (b1.overlap(b2, request, sqrDistLowerBound)) {
+      // TODO A penetration upper bound should be computed.
+      return -1;
+    }
+    return sqrt(sqrDistLowerBound);
+  }
+  static CoalScalar run(const Matrix3s& R, const Vec3s& T,
+                        const BVNode<AABB>& b1, const BVNode<AABB>& b2) {
+    CoalScalar sqrDistLowerBound;
+    CollisionRequest request(DISTANCE_LOWER_BOUND, 0);
+    // request.break_distance = ?
+    if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) {
+      // TODO A penetration upper bound should be computed.
+      return -1;
+    }
+    return sqrt(sqrDistLowerBound);
+  }
+};
+}  // namespace details
+
+/// @addtogroup Traversal_For_Distance
+/// @{
+
+/// @brief Traversal node for distance computation between BVH models
+template <typename BV>
+class BVHDistanceTraversalNode : public DistanceTraversalNodeBase {
+ public:
+  BVHDistanceTraversalNode() : DistanceTraversalNodeBase() {
+    model1 = NULL;
+    model2 = NULL;
+
+    num_bv_tests = 0;
+    num_leaf_tests = 0;
+    query_time_seconds = 0.0;
+  }
+
+  /// @brief Whether the BV node in the first BVH tree is leaf
+  bool isFirstNodeLeaf(unsigned int b) const {
+    return model1->getBV(b).isLeaf();
+  }
+
+  /// @brief Whether the BV node in the second BVH tree is leaf
+  bool isSecondNodeLeaf(unsigned int b) const {
+    return model2->getBV(b).isLeaf();
+  }
+
+  /// @brief Determine the traversal order, is the first BVTT subtree better
+  bool firstOverSecond(unsigned int b1, unsigned int b2) const {
+    CoalScalar sz1 = model1->getBV(b1).bv.size();
+    CoalScalar sz2 = model2->getBV(b2).bv.size();
+
+    bool l1 = model1->getBV(b1).isLeaf();
+    bool l2 = model2->getBV(b2).isLeaf();
+
+    if (l2 || (!l1 && (sz1 > sz2))) return true;
+    return false;
+  }
+
+  /// @brief Obtain the left child of BV node in the first BVH
+  int getFirstLeftChild(unsigned int b) const {
+    return model1->getBV(b).leftChild();
+  }
+
+  /// @brief Obtain the right child of BV node in the first BVH
+  int getFirstRightChild(unsigned int b) const {
+    return model1->getBV(b).rightChild();
+  }
+
+  /// @brief Obtain the left child of BV node in the second BVH
+  int getSecondLeftChild(unsigned int b) const {
+    return model2->getBV(b).leftChild();
+  }
+
+  /// @brief Obtain the right child of BV node in the second BVH
+  int getSecondRightChild(unsigned int b) const {
+    return model2->getBV(b).rightChild();
+  }
+
+  /// @brief The first BVH model
+  const BVHModel<BV>* model1;
+  /// @brief The second BVH model
+  const BVHModel<BV>* model2;
+
+  /// @brief statistical information
+  mutable int num_bv_tests;
+  mutable int num_leaf_tests;
+  mutable CoalScalar query_time_seconds;
+};
+
+/// @brief Traversal node for distance computation between two meshes
+template <typename BV, int _Options = RelativeTransformationIsIdentity>
+class MeshDistanceTraversalNode : public BVHDistanceTraversalNode<BV> {
+ public:
+  enum {
+    Options = _Options,
+    RTIsIdentity = _Options & RelativeTransformationIsIdentity
+  };
+
+  using BVHDistanceTraversalNode<BV>::enable_statistics;
+  using BVHDistanceTraversalNode<BV>::request;
+  using BVHDistanceTraversalNode<BV>::result;
+  using BVHDistanceTraversalNode<BV>::tf1;
+  using BVHDistanceTraversalNode<BV>::model1;
+  using BVHDistanceTraversalNode<BV>::model2;
+  using BVHDistanceTraversalNode<BV>::num_bv_tests;
+  using BVHDistanceTraversalNode<BV>::num_leaf_tests;
+
+  MeshDistanceTraversalNode() : BVHDistanceTraversalNode<BV>() {
+    vertices1 = NULL;
+    vertices2 = NULL;
+    tri_indices1 = NULL;
+    tri_indices2 = NULL;
+
+    rel_err = this->request.rel_err;
+    abs_err = this->request.abs_err;
+  }
+
+  void preprocess() {
+    if (!RTIsIdentity) preprocessOrientedNode();
+  }
+
+  void postprocess() {
+    if (!RTIsIdentity) postprocessOrientedNode();
+  }
+
+  /// @brief BV culling test in one BVTT node
+  CoalScalar BVDistanceLowerBound(unsigned int b1, unsigned int b2) const {
+    if (enable_statistics) num_bv_tests++;
+    if (RTIsIdentity)
+      return details::DistanceTraversalBVDistanceLowerBound_impl<BV>::run(
+          model1->getBV(b1), model2->getBV(b2));
+    else
+      return details::DistanceTraversalBVDistanceLowerBound_impl<BV>::run(
+          RT._R(), RT._T(), model1->getBV(b1), model2->getBV(b2));
+  }
+
+  /// @brief Distance testing between leaves (two triangles)
+  void leafComputeDistance(unsigned int b1, unsigned int b2) const {
+    if (this->enable_statistics) this->num_leaf_tests++;
+
+    const BVNode<BV>& node1 = this->model1->getBV(b1);
+    const BVNode<BV>& node2 = this->model2->getBV(b2);
+
+    int primitive_id1 = node1.primitiveId();
+    int primitive_id2 = node2.primitiveId();
+
+    const Triangle& tri_id1 = tri_indices1[primitive_id1];
+    const Triangle& tri_id2 = tri_indices2[primitive_id2];
+
+    const Vec3s& t11 = vertices1[tri_id1[0]];
+    const Vec3s& t12 = vertices1[tri_id1[1]];
+    const Vec3s& t13 = vertices1[tri_id1[2]];
+
+    const Vec3s& t21 = vertices2[tri_id2[0]];
+    const Vec3s& t22 = vertices2[tri_id2[1]];
+    const Vec3s& t23 = vertices2[tri_id2[2]];
+
+    // nearest point pair
+    Vec3s P1, P2, normal;
+
+    CoalScalar d2;
+    if (RTIsIdentity)
+      d2 = TriangleDistance::sqrTriDistance(t11, t12, t13, t21, t22, t23, P1,
+                                            P2);
+    else
+      d2 = TriangleDistance::sqrTriDistance(t11, t12, t13, t21, t22, t23,
+                                            RT._R(), RT._T(), P1, P2);
+    CoalScalar d = sqrt(d2);
+
+    this->result->update(d, this->model1, this->model2, primitive_id1,
+                         primitive_id2, P1, P2, normal);
+  }
+
+  /// @brief Whether the traversal process can stop early
+  bool canStop(CoalScalar c) const {
+    if ((c >= this->result->min_distance - abs_err) &&
+        (c * (1 + rel_err) >= this->result->min_distance))
+      return true;
+    return false;
+  }
+
+  Vec3s* vertices1;
+  Vec3s* vertices2;
+
+  Triangle* tri_indices1;
+  Triangle* tri_indices2;
+
+  /// @brief relative and absolute error, default value is 0.01 for both terms
+  CoalScalar rel_err;
+  CoalScalar abs_err;
+
+  details::RelativeTransformation<!bool(RTIsIdentity)> RT;
+
+ private:
+  void preprocessOrientedNode() {
+    const int init_tri_id1 = 0, init_tri_id2 = 0;
+    const Triangle& init_tri1 = tri_indices1[init_tri_id1];
+    const Triangle& init_tri2 = tri_indices2[init_tri_id2];
+
+    Vec3s init_tri1_points[3];
+    Vec3s init_tri2_points[3];
+
+    init_tri1_points[0] = vertices1[init_tri1[0]];
+    init_tri1_points[1] = vertices1[init_tri1[1]];
+    init_tri1_points[2] = vertices1[init_tri1[2]];
+
+    init_tri2_points[0] = vertices2[init_tri2[0]];
+    init_tri2_points[1] = vertices2[init_tri2[1]];
+    init_tri2_points[2] = vertices2[init_tri2[2]];
+
+    Vec3s p1, p2, normal;
+    CoalScalar distance = sqrt(TriangleDistance::sqrTriDistance(
+        init_tri1_points[0], init_tri1_points[1], init_tri1_points[2],
+        init_tri2_points[0], init_tri2_points[1], init_tri2_points[2], RT._R(),
+        RT._T(), p1, p2));
+
+    result->update(distance, model1, model2, init_tri_id1, init_tri_id2, p1, p2,
+                   normal);
+  }
+  void postprocessOrientedNode() {
+    /// the points obtained by triDistance are not in world space: both are in
+    /// object1's local coordinate system, so we need to convert them into the
+    /// world space.
+    if (request.enable_nearest_points && (result->o1 == model1) &&
+        (result->o2 == model2)) {
+      result->nearest_points[0] = tf1.transform(result->nearest_points[0]);
+      result->nearest_points[1] = tf1.transform(result->nearest_points[1]);
+    }
+  }
+};
+
+/// @brief Traversal node for distance computation between two meshes if their
+/// underlying BVH node is oriented node (RSS, OBBRSS, kIOS)
+typedef MeshDistanceTraversalNode<RSS, 0> MeshDistanceTraversalNodeRSS;
+typedef MeshDistanceTraversalNode<kIOS, 0> MeshDistanceTraversalNodekIOS;
+typedef MeshDistanceTraversalNode<OBBRSS, 0> MeshDistanceTraversalNodeOBBRSS;
+
+/// @}
+
+/// @brief for OBB and RSS, there is local coordinate of BV, so normal need to
+/// be transformed
+namespace details {
+
+template <typename BV>
+inline const Matrix3s& getBVAxes(const BV& bv) {
+  return bv.axes;
+}
+
+template <>
+inline const Matrix3s& getBVAxes<OBBRSS>(const OBBRSS& bv) {
+  return bv.obb.axes;
+}
+
+}  // namespace details
+
+}  // namespace coal
+
+/// @endcond
+
+#endif

include/coal/internal/traversal_node_hfield_shape.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2021-2024, 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.
+ */
+
+/** \author Jia Pan */
+
+#ifndef COAL_TRAVERSAL_NODE_HFIELD_SHAPE_H
+#define COAL_TRAVERSAL_NODE_HFIELD_SHAPE_H
+
+/// @cond INTERNAL
+
+#include "coal/collision_data.h"
+#include "coal/shape/geometric_shapes.h"
+#include "coal/narrowphase/narrowphase.h"
+#include "coal/shape/geometric_shapes_utility.h"
+#include "coal/internal/shape_shape_func.h"
+#include "coal/internal/traversal_node_base.h"
+#include "coal/internal/traversal.h"
+#include "coal/internal/intersect.h"
+#include "coal/hfield.h"
+#include "coal/shape/convex.h"
+
+namespace coal {
+
+/// @addtogroup Traversal_For_Collision
+/// @{
+
+namespace details {
+template <typename BV>
+Convex<Quadrilateral> buildConvexQuadrilateral(const HFNode<BV>& node,
+                                               const HeightField<BV>& model) {
+  const MatrixXs& heights = model.getHeights();
+  const VecXs& x_grid = model.getXGrid();
+  const VecXs& y_grid = model.getYGrid();
+
+  const CoalScalar min_height = model.getMinHeight();
+
+  const CoalScalar x0 = x_grid[node.x_id], x1 = x_grid[node.x_id + 1],
+                   y0 = y_grid[node.y_id], y1 = y_grid[node.y_id + 1];
+  const Eigen::Block<const MatrixXs, 2, 2> cell =
+      heights.block<2, 2>(node.y_id, node.x_id);
+
+  assert(cell.maxCoeff() > min_height &&
+         "max_height is lower than min_height");  // Check whether the geometry
+                                                  // is degenerated
+
+  std::shared_ptr<std::vector<Vec3s>> pts(new std::vector<Vec3s>({
+      Vec3s(x0, y0, min_height),
+      Vec3s(x0, y1, min_height),
+      Vec3s(x1, y1, min_height),
+      Vec3s(x1, y0, min_height),
+      Vec3s(x0, y0, cell(0, 0)),
+      Vec3s(x0, y1, cell(1, 0)),
+      Vec3s(x1, y1, cell(1, 1)),
+      Vec3s(x1, y0, cell(0, 1)),
+  }));
+
+  std::shared_ptr<std::vector<Quadrilateral>> polygons(
+      new std::vector<Quadrilateral>(6));
+  (*polygons)[0].set(0, 3, 2, 1);  // x+ side
+  (*polygons)[1].set(0, 1, 5, 4);  // y- side
+  (*polygons)[2].set(1, 2, 6, 5);  // x- side
+  (*polygons)[3].set(2, 3, 7, 6);  // y+ side
+  (*polygons)[4].set(3, 0, 4, 7);  // z- side
+  (*polygons)[5].set(4, 5, 6, 7);  // z+ side
+
+  return Convex<Quadrilateral>(pts,  // points
+                               8,    // num points
+                               polygons,
+                               6  // number of polygons
+  );
+}
+
+enum class FaceOrientationConvexPart1 {
+  BOTTOM = 0,
+  TOP = 1,
+  WEST = 2,
+  SOUTH_EAST = 4,
+  NORTH = 8,
+};
+
+enum class FaceOrientationConvexPart2 {
+  BOTTOM = 0,
+  TOP = 1,
+  SOUTH = 2,
+  NORTH_WEST = 4,
+  EAST = 8,
+};
+
+template <typename BV>
+void buildConvexTriangles(const HFNode<BV>& node, const HeightField<BV>& model,
+                          Convex<Triangle>& convex1, int& convex1_active_faces,
+                          Convex<Triangle>& convex2,
+                          int& convex2_active_faces) {
+  const MatrixXs& heights = model.getHeights();
+  const VecXs& x_grid = model.getXGrid();
+  const VecXs& y_grid = model.getYGrid();
+
+  const CoalScalar min_height = model.getMinHeight();
+
+  const CoalScalar x0 = x_grid[node.x_id], x1 = x_grid[node.x_id + 1],
+                   y0 = y_grid[node.y_id], y1 = y_grid[node.y_id + 1];
+  const CoalScalar max_height = node.max_height;
+  const Eigen::Block<const MatrixXs, 2, 2> cell =
+      heights.block<2, 2>(node.y_id, node.x_id);
+
+  const int contact_active_faces = node.contact_active_faces;
+  convex1_active_faces = 0;
+  convex2_active_faces = 0;
+
+  typedef HFNodeBase::FaceOrientation FaceOrientation;
+
+  if (contact_active_faces & FaceOrientation::TOP) {
+    convex1_active_faces |= int(details::FaceOrientationConvexPart1::TOP);
+    convex2_active_faces |= int(details::FaceOrientationConvexPart2::TOP);
+  }
+
+  if (contact_active_faces & FaceOrientation::BOTTOM) {
+    convex1_active_faces |= int(details::FaceOrientationConvexPart1::BOTTOM);
+    convex2_active_faces |= int(details::FaceOrientationConvexPart2::BOTTOM);
+  }
+
+  // Specific orientation for Convex1
+  if (contact_active_faces & FaceOrientation::WEST) {
+    convex1_active_faces |= int(details::FaceOrientationConvexPart1::WEST);
+  }
+
+  if (contact_active_faces & FaceOrientation::NORTH) {
+    convex1_active_faces |= int(details::FaceOrientationConvexPart1::NORTH);
+  }
+
+  // Specific orientation for Convex2
+  if (contact_active_faces & FaceOrientation::EAST) {
+    convex2_active_faces |= int(details::FaceOrientationConvexPart2::EAST);
+  }
+
+  if (contact_active_faces & FaceOrientation::SOUTH) {
+    convex2_active_faces |= int(details::FaceOrientationConvexPart2::SOUTH);
+  }
+
+  assert(max_height > min_height &&
+         "max_height is lower than min_height");  // Check whether the geometry
+                                                  // is degenerated
+  COAL_UNUSED_VARIABLE(max_height);
+
+  {
+    std::shared_ptr<std::vector<Vec3s>> pts(new std::vector<Vec3s>({
+        Vec3s(x0, y0, min_height),  // A
+        Vec3s(x0, y1, min_height),  // B
+        Vec3s(x1, y0, min_height),  // C
+        Vec3s(x0, y0, cell(0, 0)),  // D
+        Vec3s(x0, y1, cell(1, 0)),  // E
+        Vec3s(x1, y0, cell(0, 1)),  // F
+    }));
+
+    std::shared_ptr<std::vector<Triangle>> triangles(
+        new std::vector<Triangle>(8));
+    (*triangles)[0].set(0, 2, 1);  // bottom
+    (*triangles)[1].set(3, 4, 5);  // top
+    (*triangles)[2].set(0, 1, 3);  // West 1
+    (*triangles)[3].set(3, 1, 4);  // West 2
+    (*triangles)[4].set(1, 2, 5);  // South-East 1
+    (*triangles)[5].set(1, 5, 4);  // South-East 1
+    (*triangles)[6].set(0, 5, 2);  // North 1
+    (*triangles)[7].set(5, 0, 3);  // North 2
+
+    convex1.set(pts,  // points
+                6,    // num points
+                triangles,
+                8  // number of polygons
+    );
+  }
+
+  {
+    std::shared_ptr<std::vector<Vec3s>> pts(new std::vector<Vec3s>({
+        Vec3s(x0, y1, min_height),  // A
+        Vec3s(x1, y1, min_height),  // B
+        Vec3s(x1, y0, min_height),  // C
+        Vec3s(x0, y1, cell(1, 0)),  // D
+        Vec3s(x1, y1, cell(1, 1)),  // E
+        Vec3s(x1, y0, cell(0, 1)),  // F
+    }));
+
+    std::shared_ptr<std::vector<Triangle>> triangles(
+        new std::vector<Triangle>(8));
+    (*triangles)[0].set(2, 1, 0);  // bottom
+    (*triangles)[1].set(3, 4, 5);  // top
+    (*triangles)[2].set(0, 1, 3);  // South 1
+    (*triangles)[3].set(3, 1, 4);  // South 2
+    (*triangles)[4].set(0, 5, 2);  // North West 1
+    (*triangles)[5].set(0, 3, 5);  // North West 2
+    (*triangles)[6].set(1, 2, 5);  // East 1
+    (*triangles)[7].set(4, 1, 2);  // East 2
+
+    convex2.set(pts,  // points
+                6,    // num points
+                triangles,
+                8  // number of polygons
+    );
+  }
+}
+
+inline Vec3s projectTriangle(const Vec3s& pointA, const Vec3s& pointB,
+                             const Vec3s& pointC, const Vec3s& point) {
+  const Project::ProjectResult result =
+      Project::projectTriangle(pointA, pointB, pointC, point);
+  Vec3s res = result.parameterization[0] * pointA +
+              result.parameterization[1] * pointB +
+              result.parameterization[2] * pointC;
+
+  return res;
+}
+
+inline Vec3s projectTetrahedra(const Vec3s& pointA, const Vec3s& pointB,
+                               const Vec3s& pointC, const Vec3s& pointD,
+                               const Vec3s& point) {
+  const Project::ProjectResult result =
+      Project::projectTetrahedra(pointA, pointB, pointC, pointD, point);
+  Vec3s res = result.parameterization[0] * pointA +
+              result.parameterization[1] * pointB +
+              result.parameterization[2] * pointC +
+              result.parameterization[3] * pointD;
+
+  return res;
+}
+
+inline Vec3s computeTriangleNormal(const Triangle& triangle,
+                                   const std::vector<Vec3s>& points) {
+  const Vec3s pointA = points[triangle[0]];
+  const Vec3s pointB = points[triangle[1]];
+  const Vec3s pointC = points[triangle[2]];
+
+  const Vec3s normal = (pointB - pointA).cross(pointC - pointA).normalized();
+  assert(!normal.array().isNaN().any() && "normal is ill-defined");
+
+  return normal;
+}
+
+inline Vec3s projectPointOnTriangle(const Vec3s& contact_point,
+                                    const Triangle& triangle,
+                                    const std::vector<Vec3s>& points) {
+  const Vec3s pointA = points[triangle[0]];
+  const Vec3s pointB = points[triangle[1]];
+  const Vec3s pointC = points[triangle[2]];
+
+  const Vec3s contact_point_projected =
+      projectTriangle(pointA, pointB, pointC, contact_point);
+
+  return contact_point_projected;
+}
+
+inline CoalScalar distanceContactPointToTriangle(
+    const Vec3s& contact_point, const Triangle& triangle,
+    const std::vector<Vec3s>& points) {
+  const Vec3s contact_point_projected =
+      projectPointOnTriangle(contact_point, triangle, points);
+  return (contact_point_projected - contact_point).norm();
+}
+
+inline CoalScalar distanceContactPointToFace(const size_t face_id,
+                                             const Vec3s& contact_point,
+                                             const Convex<Triangle>& convex,
+                                             size_t& closest_face_id) {
+  assert((face_id >= 0 && face_id < 8) && "face_id should be in [0;7]");
+
+  const std::vector<Vec3s>& points = *(convex.points);
+  if (face_id <= 1) {
+    const Triangle& triangle = (*(convex.polygons))[face_id];
+    closest_face_id = face_id;
+    return distanceContactPointToTriangle(contact_point, triangle, points);
+  } else {
+    const Triangle& triangle1 = (*(convex.polygons))[face_id];
+    const CoalScalar distance_to_triangle1 =
+        distanceContactPointToTriangle(contact_point, triangle1, points);
+
+    const Triangle& triangle2 = (*(convex.polygons))[face_id + 1];
+    const CoalScalar distance_to_triangle2 =
+        distanceContactPointToTriangle(contact_point, triangle2, points);
+
+    if (distance_to_triangle1 > distance_to_triangle2) {
+      closest_face_id = face_id + 1;
+      return distance_to_triangle2;
+    } else {
+      closest_face_id = face_id;
+      return distance_to_triangle1;
+    }
+  }
+}
+
+template <typename Polygone, typename Shape>
+bool binCorrection(const Convex<Polygone>& convex,
+                   const int convex_active_faces, const Shape& shape,
+                   const Transform3s& shape_pose, CoalScalar& distance,
+                   Vec3s& contact_1, Vec3s& contact_2, Vec3s& normal,
+                   Vec3s& face_normal, const bool is_collision) {
+  const CoalScalar prec = 1e-12;
+  const std::vector<Vec3s>& points = *(convex.points);
+
+  bool hfield_witness_is_on_bin_side = true;
+
+  //  int closest_face_id_bottom_face = -1;
+  //  int closest_face_id_top_face = -1;
+
+  std::vector<size_t> active_faces;
+  active_faces.reserve(5);
+  active_faces.push_back(0);
+  active_faces.push_back(1);
+
+  if (convex_active_faces & 2) active_faces.push_back(2);
+  if (convex_active_faces & 4) active_faces.push_back(4);
+  if (convex_active_faces & 8) active_faces.push_back(6);
+
+  Triangle face_triangle;
+  CoalScalar shortest_distance_to_face =
+      (std::numeric_limits<CoalScalar>::max)();
+  face_normal = normal;
+  for (const size_t active_face : active_faces) {
+    size_t closest_face_id;
+    const CoalScalar distance_to_face = distanceContactPointToFace(
+        active_face, contact_1, convex, closest_face_id);
+
+    const bool contact_point_is_on_face = distance_to_face <= prec;
+    if (contact_point_is_on_face) {
+      hfield_witness_is_on_bin_side = false;
+      face_triangle = (*(convex.polygons))[closest_face_id];
+      shortest_distance_to_face = distance_to_face;
+      break;
+    } else if (distance_to_face < shortest_distance_to_face) {
+      face_triangle = (*(convex.polygons))[closest_face_id];
+      shortest_distance_to_face = distance_to_face;
+    }
+  }
+
+  // We correct only if there is a collision with the bin
+  if (is_collision) {
+    if (!face_triangle.isValid())
+      COAL_THROW_PRETTY("face_triangle is not initialized", std::logic_error);
+
+    const Vec3s face_pointA = points[face_triangle[0]];
+    face_normal = computeTriangleNormal(face_triangle, points);
+
+    int hint = 0;
+    // Since we compute the support manually, we need to take into account the
+    // sphere swept radius of the shape.
+    // TODO: take into account the swept-sphere radius of the bin.
+    const Vec3s _support = getSupport<details::SupportOptions::WithSweptSphere>(
+        &shape, -shape_pose.rotation().transpose() * face_normal, hint);
+    const Vec3s support =
+        shape_pose.rotation() * _support + shape_pose.translation();
+
+    // Project support into the inclined bin having triangle
+    const CoalScalar offset_plane = face_normal.dot(face_pointA);
+    const Plane projection_plane(face_normal, offset_plane);
+    const CoalScalar distance_support_projection_plane =
+        projection_plane.signedDistance(support);
+
+    const Vec3s projected_support =
+        support - distance_support_projection_plane * face_normal;
+
+    // We need now to project the projected in the triangle shape
+    contact_1 =
+        projectPointOnTriangle(projected_support, face_triangle, points);
+    contact_2 = contact_1 + distance_support_projection_plane * face_normal;
+    normal = face_normal;
+    distance = -std::fabs(distance_support_projection_plane);
+  }
+
+  return hfield_witness_is_on_bin_side;
+}
+
+template <typename Polygone, typename Shape, int Options>
+bool shapeDistance(const GJKSolver* nsolver, const CollisionRequest& request,
+                   const Convex<Polygone>& convex1,
+                   const int convex1_active_faces,
+                   const Convex<Polygone>& convex2,
+                   const int convex2_active_faces, const Transform3s& tf1,
+                   const Shape& shape, const Transform3s& tf2,
+                   CoalScalar& distance, Vec3s& c1, Vec3s& c2, Vec3s& normal,
+                   Vec3s& normal_top, bool& hfield_witness_is_on_bin_side) {
+  enum { RTIsIdentity = Options & RelativeTransformationIsIdentity };
+
+  const Transform3s Id;
+  // The solver `nsolver` has already been set up by the collision request
+  // `request`. If GJK early stopping is enabled through `request`, it will be
+  // used.
+  // The only thing we need to make sure is that in case of collision, the
+  // penetration information is computed (as we do bins comparison).
+  const bool compute_penetration = true;
+  Vec3s contact1_1, contact1_2, contact2_1, contact2_2;
+  Vec3s normal1, normal1_top, normal2, normal2_top;
+  CoalScalar distance1, distance2;
+
+  if (RTIsIdentity) {
+    distance1 = internal::ShapeShapeDistance<Convex<Polygone>, Shape>(
+        &convex1, Id, &shape, tf2, nsolver, compute_penetration, contact1_1,
+        contact1_2, normal1);
+  } else {
+    distance1 = internal::ShapeShapeDistance<Convex<Polygone>, Shape>(
+        &convex1, tf1, &shape, tf2, nsolver, compute_penetration, contact1_1,
+        contact1_2, normal1);
+  }
+  bool collision1 = (distance1 - request.security_margin <=
+                     request.collision_distance_threshold);
+
+  bool hfield_witness_is_on_bin_side1 =
+      binCorrection(convex1, convex1_active_faces, shape, tf2, distance1,
+                    contact1_1, contact1_2, normal1, normal1_top, collision1);
+
+  if (RTIsIdentity) {
+    distance2 = internal::ShapeShapeDistance<Convex<Polygone>, Shape>(
+        &convex2, Id, &shape, tf2, nsolver, compute_penetration, contact2_1,
+        contact2_2, normal2);
+  } else {
+    distance2 = internal::ShapeShapeDistance<Convex<Polygone>, Shape>(
+        &convex2, tf1, &shape, tf2, nsolver, compute_penetration, contact2_1,
+        contact2_2, normal2);
+  }
+  bool collision2 = (distance2 - request.security_margin <=
+                     request.collision_distance_threshold);
+
+  bool hfield_witness_is_on_bin_side2 =
+      binCorrection(convex2, convex2_active_faces, shape, tf2, distance2,
+                    contact2_1, contact2_2, normal2, normal2_top, collision2);
+
+  if (collision1 && collision2) {
+    if (distance1 > distance2)  // switch values
+    {
+      distance = distance2;
+      c1 = contact2_1;
+      c2 = contact2_2;
+      normal = normal2;
+      normal_top = normal2_top;
+      hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side2;
+    } else {
+      distance = distance1;
+      c1 = contact1_1;
+      c2 = contact1_2;
+      normal = normal1;
+      normal_top = normal1_top;
+      hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side1;
+    }
+    return true;
+  } else if (collision1) {
+    distance = distance1;
+    c1 = contact1_1;
+    c2 = contact1_2;
+    normal = normal1;
+    normal_top = normal1_top;
+    hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side1;
+    return true;
+  } else if (collision2) {
+    distance = distance2;
+    c1 = contact2_1;
+    c2 = contact2_2;
+    normal = normal2;
+    normal_top = normal2_top;
+    hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side2;
+    return true;
+  }
+
+  if (distance1 > distance2)  // switch values
+  {
+    distance = distance2;
+    c1 = contact2_1;
+    c2 = contact2_2;
+    normal = normal2;
+    normal_top = normal2_top;
+    hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side2;
+  } else {
+    distance = distance1;
+    c1 = contact1_1;
+    c2 = contact1_2;
+    normal = normal1;
+    normal_top = normal1_top;
+    hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side1;
+  }
+  return false;
+}
+
+}  // namespace details
+
+/// @brief Traversal node for collision between height field and shape
+template <typename BV, typename S,
+          int _Options = RelativeTransformationIsIdentity>
+class HeightFieldShapeCollisionTraversalNode
+    : public CollisionTraversalNodeBase {
+ public:
+  typedef CollisionTraversalNodeBase Base;
+  typedef Eigen::Array<CoalScalar, 1, 2> Array2d;
+
+  enum {
+    Options = _Options,
+    RTIsIdentity = _Options & RelativeTransformationIsIdentity
+  };
+
+  HeightFieldShapeCollisionTraversalNode(const CollisionRequest& request)
+      : CollisionTraversalNodeBase(request) {
+    model1 = NULL;
+    model2 = NULL;
+
+    num_bv_tests = 0;
+    num_leaf_tests = 0;
+    query_time_seconds = 0.0;
+
+    nsolver = NULL;
+    count = 0;
+  }
+
+  /// @brief Whether the BV node in the first BVH tree is leaf
+  bool isFirstNodeLeaf(unsigned int b) const {
+    return model1->getBV(b).isLeaf();
+  }
+
+  /// @brief Obtain the left child of BV node in the first BVH
+  int getFirstLeftChild(unsigned int b) const {
+    return static_cast<int>(model1->getBV(b).leftChild());
+  }
+
+  /// @brief Obtain the right child of BV node in the first BVH
+  int getFirstRightChild(unsigned int b) const {
+    return static_cast<int>(model1->getBV(b).rightChild());
+  }
+
+  /// test between BV b1 and shape
+  /// @param b1 BV to test,
+  /// @retval sqrDistLowerBound square of a lower bound of the minimal
+  ///         distance between bounding volumes.
+  /// @brief BV culling test in one BVTT node
+  bool BVDisjoints(unsigned int b1, unsigned int /*b2*/,
+                   CoalScalar& sqrDistLowerBound) const {
+    if (this->enable_statistics) this->num_bv_tests++;
+
+    bool disjoint;
+    if (RTIsIdentity) {
+      assert(false && "must never happened");
+      disjoint = !this->model1->getBV(b1).bv.overlap(
+          this->model2_bv, this->request, sqrDistLowerBound);
+    } else {
+      disjoint = !overlap(this->tf1.getRotation(), this->tf1.getTranslation(),
+                          this->model1->getBV(b1).bv, this->model2_bv,
+                          this->request, sqrDistLowerBound);
+    }
+
+    if (disjoint)
+      internal::updateDistanceLowerBoundFromBV(this->request, *this->result,
+                                               sqrDistLowerBound);
+
+    assert(!disjoint || sqrDistLowerBound > 0);
+    return disjoint;
+  }
+
+  /// @brief Intersection testing between leaves (one Convex and one shape)
+  void leafCollides(unsigned int b1, unsigned int /*b2*/,
+                    CoalScalar& sqrDistLowerBound) const {
+    count++;
+    if (this->enable_statistics) this->num_leaf_tests++;
+    const HFNode<BV>& node = this->model1->getBV(b1);
+
+    // Split quadrilateral primitives into two convex shapes corresponding to
+    // polyhedron with triangular bases. This is essential to keep the convexity
+
+    //    typedef Convex<Quadrilateral> ConvexQuadrilateral;
+    //    const ConvexQuadrilateral convex =
+    //    details::buildConvexQuadrilateral(node,*this->model1);
+
+    typedef Convex<Triangle> ConvexTriangle;
+    ConvexTriangle convex1, convex2;
+    int convex1_active_faces, convex2_active_faces;
+    // TODO: inherit from hfield's inflation here
+    details::buildConvexTriangles(node, *this->model1, convex1,
+                                  convex1_active_faces, convex2,
+                                  convex2_active_faces);
+
+    // Compute aabb_local for BoundingVolumeGuess case in the GJK solver
+    if (nsolver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
+      convex1.computeLocalAABB();
+      convex2.computeLocalAABB();
+    }
+
+    CoalScalar distance;
+    //    Vec3s contact_point, normal;
+    Vec3s c1, c2, normal, normal_face;
+    bool hfield_witness_is_on_bin_side;
+
+    bool collision = details::shapeDistance<Triangle, S, Options>(
+        nsolver, this->request, convex1, convex1_active_faces, convex2,
+        convex2_active_faces, this->tf1, *(this->model2), this->tf2, distance,
+        c1, c2, normal, normal_face, hfield_witness_is_on_bin_side);
+
+    CoalScalar distToCollision = distance - this->request.security_margin;
+    if (distToCollision <= this->request.collision_distance_threshold) {
+      sqrDistLowerBound = 0;
+      if (this->result->numContacts() < this->request.num_max_contacts) {
+        if (normal_face.isApprox(normal) &&
+            (collision || !hfield_witness_is_on_bin_side)) {
+          this->result->addContact(Contact(this->model1, this->model2, (int)b1,
+                                           (int)Contact::NONE, c1, c2, normal,
+                                           distance));
+          assert(this->result->isCollision());
+        }
+      }
+    } else
+      sqrDistLowerBound = distToCollision * distToCollision;
+
+    //    const Vec3s c1 = contact_point - distance * 0.5 * normal;
+    //    const Vec3s c2 = contact_point + distance * 0.5 * normal;
+    internal::updateDistanceLowerBoundFromLeaf(this->request, *this->result,
+                                               distToCollision, c1, c2, normal);
+
+    assert(this->result->isCollision() || sqrDistLowerBound > 0);
+  }
+
+  const GJKSolver* nsolver;
+
+  const HeightField<BV>* model1;
+  const S* model2;
+  BV model2_bv;
+
+  mutable int num_bv_tests;
+  mutable int num_leaf_tests;
+  mutable CoalScalar query_time_seconds;
+  mutable int count;
+};
+
+/// @}
+
+/// @addtogroup Traversal_For_Distance
+/// @{
+
+/// @brief Traversal node for distance between height field and shape
+template <typename BV, typename S,
+          int _Options = RelativeTransformationIsIdentity>
+class HeightFieldShapeDistanceTraversalNode : public DistanceTraversalNodeBase {
+ public:
+  typedef DistanceTraversalNodeBase Base;
+
+  enum {
+    Options = _Options,
+    RTIsIdentity = _Options & RelativeTransformationIsIdentity
+  };
+
+  HeightFieldShapeDistanceTraversalNode() : DistanceTraversalNodeBase() {
+    model1 = NULL;
+    model2 = NULL;
+
+    num_leaf_tests = 0;
+    query_time_seconds = 0.0;
+
+    rel_err = 0;
+    abs_err = 0;
+    nsolver = NULL;
+  }
+
+  /// @brief Whether the BV node in the first BVH tree is leaf
+  bool isFirstNodeLeaf(unsigned int b) const {
+    return model1->getBV(b).isLeaf();
+  }
+
+  /// @brief Obtain the left child of BV node in the first BVH
+  int getFirstLeftChild(unsigned int b) const {
+    return model1->getBV(b).leftChild();
+  }
+
+  /// @brief Obtain the right child of BV node in the first BVH
+  int getFirstRightChild(unsigned int b) const {
+    return model1->getBV(b).rightChild();
+  }
+
+  /// @brief BV culling test in one BVTT node
+  CoalScalar BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const {
+    return model1->getBV(b1).bv.distance(
+        model2_bv);  // TODO(jcarpent): tf1 is not taken into account here.
+  }
+
+  /// @brief Distance testing between leaves (one bin of the height field and
+  /// one shape)
+  /// TODO(louis): deal with Hfield-Shape distance just like in Hfield-Shape
+  /// collision (bin correction etc).
+  void leafComputeDistance(unsigned int b1, unsigned int /*b2*/) const {
+    if (this->enable_statistics) this->num_leaf_tests++;
+
+    const BVNode<BV>& node = this->model1->getBV(b1);
+
+    typedef Convex<Quadrilateral> ConvexQuadrilateral;
+    const ConvexQuadrilateral convex =
+        details::buildConvexQuadrilateral(node, *this->model1);
+
+    Vec3s p1, p2, normal;
+    const CoalScalar distance =
+        internal::ShapeShapeDistance<ConvexQuadrilateral, S>(
+            &convex, this->tf1, this->model2, this->tf2, this->nsolver,
+            this->request.enable_signed_distance, p1, p2, normal);
+
+    this->result->update(distance, this->model1, this->model2, b1,
+                         DistanceResult::NONE, p1, p2, normal);
+  }
+
+  /// @brief Whether the traversal process can stop early
+  bool canStop(CoalScalar c) const {
+    if ((c >= this->result->min_distance - abs_err) &&
+        (c * (1 + rel_err) >= this->result->min_distance))
+      return true;
+    return false;
+  }
+
+  CoalScalar rel_err;
+  CoalScalar abs_err;
+
+  const GJKSolver* nsolver;
+
+  const HeightField<BV>* model1;
+  const S* model2;
+  BV model2_bv;
+
+  mutable int num_bv_tests;
+  mutable int num_leaf_tests;
+  mutable CoalScalar query_time_seconds;
+};
+
+/// @}
+
+}  // namespace coal
+
+/// @endcond
+
+#endif

include/coal/internal/traversal_node_octree.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, Open Source Robotics Foundation
+ *  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 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_TRAVERSAL_NODE_OCTREE_H
+#define COAL_TRAVERSAL_NODE_OCTREE_H
+
+/// @cond INTERNAL
+
+#include "coal/collision_data.h"
+#include "coal/internal/traversal_node_base.h"
+#include "coal/internal/traversal_node_hfield_shape.h"
+#include "coal/narrowphase/narrowphase.h"
+#include "coal/hfield.h"
+#include "coal/octree.h"
+#include "coal/BVH/BVH_model.h"
+#include "coal/shape/geometric_shapes_utility.h"
+#include "coal/internal/shape_shape_func.h"
+
+namespace coal {
+
+/// @brief Algorithms for collision related with octree
+class COAL_DLLAPI OcTreeSolver {
+ private:
+  const GJKSolver* solver;
+
+  mutable const CollisionRequest* crequest;
+  mutable const DistanceRequest* drequest;
+
+  mutable CollisionResult* cresult;
+  mutable DistanceResult* dresult;
+
+ public:
+  OcTreeSolver(const GJKSolver* solver_)
+      : solver(solver_),
+        crequest(NULL),
+        drequest(NULL),
+        cresult(NULL),
+        dresult(NULL) {}
+
+  /// @brief collision between two octrees
+  void OcTreeIntersect(const OcTree* tree1, const OcTree* tree2,
+                       const Transform3s& tf1, const Transform3s& tf2,
+                       const CollisionRequest& request_,
+                       CollisionResult& result_) const {
+    crequest = &request_;
+    cresult = &result_;
+
+    OcTreeIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), tree2,
+                           tree2->getRoot(), tree2->getRootBV(), tf1, tf2);
+  }
+
+  /// @brief distance between two octrees
+  void OcTreeDistance(const OcTree* tree1, const OcTree* tree2,
+                      const Transform3s& tf1, const Transform3s& tf2,
+                      const DistanceRequest& request_,
+                      DistanceResult& result_) const {
+    drequest = &request_;
+    dresult = &result_;
+
+    OcTreeDistanceRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), tree2,
+                          tree2->getRoot(), tree2->getRootBV(), tf1, tf2);
+  }
+
+  /// @brief collision between octree and mesh
+  template <typename BV>
+  void OcTreeMeshIntersect(const OcTree* tree1, const BVHModel<BV>* tree2,
+                           const Transform3s& tf1, const Transform3s& tf2,
+                           const CollisionRequest& request_,
+                           CollisionResult& result_) const {
+    crequest = &request_;
+    cresult = &result_;
+
+    OcTreeMeshIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(),
+                               tree2, 0, tf1, tf2);
+  }
+
+  /// @brief distance between octree and mesh
+  template <typename BV>
+  void OcTreeMeshDistance(const OcTree* tree1, const BVHModel<BV>* tree2,
+                          const Transform3s& tf1, const Transform3s& tf2,
+                          const DistanceRequest& request_,
+                          DistanceResult& result_) const {
+    drequest = &request_;
+    dresult = &result_;
+
+    OcTreeMeshDistanceRecurse(tree1, tree1->getRoot(), tree1->getRootBV(),
+                              tree2, 0, tf1, tf2);
+  }
+
+  /// @brief collision between mesh and octree
+  template <typename BV>
+  void MeshOcTreeIntersect(const BVHModel<BV>* tree1, const OcTree* tree2,
+                           const Transform3s& tf1, const Transform3s& tf2,
+                           const CollisionRequest& request_,
+                           CollisionResult& result_) const
+
+  {
+    crequest = &request_;
+    cresult = &result_;
+
+    OcTreeMeshIntersectRecurse(tree2, tree2->getRoot(), tree2->getRootBV(),
+                               tree1, 0, tf2, tf1);
+  }
+
+  /// @brief distance between mesh and octree
+  template <typename BV>
+  void MeshOcTreeDistance(const BVHModel<BV>* tree1, const OcTree* tree2,
+                          const Transform3s& tf1, const Transform3s& tf2,
+                          const DistanceRequest& request_,
+                          DistanceResult& result_) const {
+    drequest = &request_;
+    dresult = &result_;
+
+    OcTreeMeshDistanceRecurse(tree1, 0, tree2, tree2->getRoot(),
+                              tree2->getRootBV(), tf1, tf2);
+  }
+
+  template <typename BV>
+  void OcTreeHeightFieldIntersect(
+      const OcTree* tree1, const HeightField<BV>* tree2, const Transform3s& tf1,
+      const Transform3s& tf2, const CollisionRequest& request_,
+      CollisionResult& result_, CoalScalar& sqrDistLowerBound) const {
+    crequest = &request_;
+    cresult = &result_;
+
+    OcTreeHeightFieldIntersectRecurse(tree1, tree1->getRoot(),
+                                      tree1->getRootBV(), tree2, 0, tf1, tf2,
+                                      sqrDistLowerBound);
+  }
+
+  template <typename BV>
+  void HeightFieldOcTreeIntersect(const HeightField<BV>* tree1,
+                                  const OcTree* tree2, const Transform3s& tf1,
+                                  const Transform3s& tf2,
+                                  const CollisionRequest& request_,
+                                  CollisionResult& result_,
+                                  CoalScalar& sqrDistLowerBound) const {
+    crequest = &request_;
+    cresult = &result_;
+
+    HeightFieldOcTreeIntersectRecurse(tree1, 0, tree2, tree2->getRoot(),
+                                      tree2->getRootBV(), tf1, tf2,
+                                      sqrDistLowerBound);
+  }
+
+  /// @brief collision between octree and shape
+  template <typename S>
+  void OcTreeShapeIntersect(const OcTree* tree, const S& s,
+                            const Transform3s& tf1, const Transform3s& tf2,
+                            const CollisionRequest& request_,
+                            CollisionResult& result_) const {
+    crequest = &request_;
+    cresult = &result_;
+
+    AABB bv2;
+    computeBV<AABB>(s, Transform3s(), bv2);
+    OBB obb2;
+    convertBV(bv2, tf2, obb2);
+    OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(), s,
+                                obb2, tf1, tf2);
+  }
+
+  /// @brief collision between shape and octree
+  template <typename S>
+  void ShapeOcTreeIntersect(const S& s, const OcTree* tree,
+                            const Transform3s& tf1, const Transform3s& tf2,
+                            const CollisionRequest& request_,
+                            CollisionResult& result_) const {
+    crequest = &request_;
+    cresult = &result_;
+
+    AABB bv1;
+    computeBV<AABB>(s, Transform3s(), bv1);
+    OBB obb1;
+    convertBV(bv1, tf1, obb1);
+    OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(), s,
+                                obb1, tf2, tf1);
+  }
+
+  /// @brief distance between octree and shape
+  template <typename S>
+  void OcTreeShapeDistance(const OcTree* tree, const S& s,
+                           const Transform3s& tf1, const Transform3s& tf2,
+                           const DistanceRequest& request_,
+                           DistanceResult& result_) const {
+    drequest = &request_;
+    dresult = &result_;
+
+    AABB aabb2;
+    computeBV<AABB>(s, tf2, aabb2);
+    OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(), s,
+                               aabb2, tf1, tf2);
+  }
+
+  /// @brief distance between shape and octree
+  template <typename S>
+  void ShapeOcTreeDistance(const S& s, const OcTree* tree,
+                           const Transform3s& tf1, const Transform3s& tf2,
+                           const DistanceRequest& request_,
+                           DistanceResult& result_) const {
+    drequest = &request_;
+    dresult = &result_;
+
+    AABB aabb1;
+    computeBV<AABB>(s, tf1, aabb1);
+    OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(), s,
+                               aabb1, tf2, tf1);
+  }
+
+ private:
+  template <typename S>
+  bool OcTreeShapeDistanceRecurse(const OcTree* tree1,
+                                  const OcTree::OcTreeNode* root1,
+                                  const AABB& bv1, const S& s,
+                                  const AABB& aabb2, const Transform3s& tf1,
+                                  const Transform3s& tf2) const {
+    if (!tree1->nodeHasChildren(root1)) {
+      if (tree1->isNodeOccupied(root1)) {
+        Box box;
+        Transform3s box_tf;
+        constructBox(bv1, tf1, box, box_tf);
+
+        if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
+          box.computeLocalAABB();
+        }
+
+        Vec3s p1, p2, normal;
+        const CoalScalar distance = internal::ShapeShapeDistance<Box, S>(
+            &box, box_tf, &s, tf2, this->solver,
+            this->drequest->enable_signed_distance, p1, p2, normal);
+
+        this->dresult->update(distance, tree1, &s,
+                              (int)(root1 - tree1->getRoot()),
+                              DistanceResult::NONE, p1, p2, normal);
+
+        return drequest->isSatisfied(*dresult);
+      } else
+        return false;
+    }
+
+    if (!tree1->isNodeOccupied(root1)) return false;
+
+    for (unsigned int i = 0; i < 8; ++i) {
+      if (tree1->nodeChildExists(root1, i)) {
+        const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
+        AABB child_bv;
+        computeChildBV(bv1, i, child_bv);
+
+        AABB aabb1;
+        convertBV(child_bv, tf1, aabb1);
+        CoalScalar d = aabb1.distance(aabb2);
+        if (d < dresult->min_distance) {
+          if (OcTreeShapeDistanceRecurse(tree1, child, child_bv, s, aabb2, tf1,
+                                         tf2))
+            return true;
+        }
+      }
+    }
+
+    return false;
+  }
+
+  template <typename S>
+  bool OcTreeShapeIntersectRecurse(const OcTree* tree1,
+                                   const OcTree::OcTreeNode* root1,
+                                   const AABB& bv1, const S& s, const OBB& obb2,
+                                   const Transform3s& tf1,
+                                   const Transform3s& tf2) const {
+    // Empty OcTree is considered free.
+    if (!root1) return false;
+
+    /// stop when 1) bounding boxes of two objects not overlap; OR
+    ///           2) at least of one the nodes is free; OR
+    ///           2) (two uncertain nodes or one node occupied and one node
+    ///           uncertain) AND cost not required
+    if (tree1->isNodeFree(root1))
+      return false;
+    else if ((tree1->isNodeUncertain(root1) || s.isUncertain()))
+      return false;
+    else {
+      OBB obb1;
+      convertBV(bv1, tf1, obb1);
+      CoalScalar sqrDistLowerBound;
+      if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound)) {
+        internal::updateDistanceLowerBoundFromBV(*crequest, *cresult,
+                                                 sqrDistLowerBound);
+        return false;
+      }
+    }
+
+    if (!tree1->nodeHasChildren(root1)) {
+      assert(tree1->isNodeOccupied(root1));  // it isn't free nor uncertain.
+
+      Box box;
+      Transform3s box_tf;
+      constructBox(bv1, tf1, box, box_tf);
+      if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
+        box.computeLocalAABB();
+      }
+
+      bool contactNotAdded =
+          (cresult->numContacts() >= crequest->num_max_contacts);
+      std::size_t ncontact = ShapeShapeCollide<Box, S>(
+          &box, box_tf, &s, tf2, solver, *crequest, *cresult);
+      assert(ncontact == 0 || ncontact == 1);
+      if (!contactNotAdded && ncontact == 1) {
+        // Update contact information.
+        const Contact& c = cresult->getContact(cresult->numContacts() - 1);
+        cresult->setContact(
+            cresult->numContacts() - 1,
+            Contact(tree1, c.o2, static_cast<int>(root1 - tree1->getRoot()),
+                    c.b2, c.pos, c.normal, c.penetration_depth));
+      }
+
+      // no need to call `internal::updateDistanceLowerBoundFromLeaf` here
+      // as it is already done internally in `ShapeShapeCollide` above.
+      return crequest->isSatisfied(*cresult);
+    }
+
+    for (unsigned int i = 0; i < 8; ++i) {
+      if (tree1->nodeChildExists(root1, i)) {
+        const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
+        AABB child_bv;
+        computeChildBV(bv1, i, child_bv);
+
+        if (OcTreeShapeIntersectRecurse(tree1, child, child_bv, s, obb2, tf1,
+                                        tf2))
+          return true;
+      }
+    }
+
+    return false;
+  }
+
+  template <typename BV>
+  bool OcTreeMeshDistanceRecurse(const OcTree* tree1,
+                                 const OcTree::OcTreeNode* root1,
+                                 const AABB& bv1, const BVHModel<BV>* tree2,
+                                 unsigned int root2, const Transform3s& tf1,
+                                 const Transform3s& tf2) const {
+    if (!tree1->nodeHasChildren(root1) && tree2->getBV(root2).isLeaf()) {
+      if (tree1->isNodeOccupied(root1)) {
+        Box box;
+        Transform3s box_tf;
+        constructBox(bv1, tf1, box, box_tf);
+
+        if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
+          box.computeLocalAABB();
+        }
+
+        size_t primitive_id =
+            static_cast<size_t>(tree2->getBV(root2).primitiveId());
+        const Triangle& tri_id = (*(tree2->tri_indices))[primitive_id];
+        const TriangleP tri((*(tree2->vertices))[tri_id[0]],
+                            (*(tree2->vertices))[tri_id[1]],
+                            (*(tree2->vertices))[tri_id[2]]);
+
+        Vec3s p1, p2, normal;
+        const CoalScalar distance =
+            internal::ShapeShapeDistance<Box, TriangleP>(
+                &box, box_tf, &tri, tf2, this->solver,
+                this->drequest->enable_signed_distance, p1, p2, normal);
+
+        this->dresult->update(distance, tree1, tree2,
+                              (int)(root1 - tree1->getRoot()),
+                              static_cast<int>(primitive_id), p1, p2, normal);
+
+        return this->drequest->isSatisfied(*dresult);
+      } else
+        return false;
+    }
+
+    if (!tree1->isNodeOccupied(root1)) return false;
+
+    if (tree2->getBV(root2).isLeaf() ||
+        (tree1->nodeHasChildren(root1) &&
+         (bv1.size() > tree2->getBV(root2).bv.size()))) {
+      for (unsigned int i = 0; i < 8; ++i) {
+        if (tree1->nodeChildExists(root1, i)) {
+          const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
+          AABB child_bv;
+          computeChildBV(bv1, i, child_bv);
+
+          CoalScalar d;
+          AABB aabb1, aabb2;
+          convertBV(child_bv, tf1, aabb1);
+          convertBV(tree2->getBV(root2).bv, tf2, aabb2);
+          d = aabb1.distance(aabb2);
+
+          if (d < dresult->min_distance) {
+            if (OcTreeMeshDistanceRecurse(tree1, child, child_bv, tree2, root2,
+                                          tf1, tf2))
+              return true;
+          }
+        }
+      }
+    } else {
+      CoalScalar d;
+      AABB aabb1, aabb2;
+      convertBV(bv1, tf1, aabb1);
+      unsigned int child = (unsigned int)tree2->getBV(root2).leftChild();
+      convertBV(tree2->getBV(child).bv, tf2, aabb2);
+      d = aabb1.distance(aabb2);
+
+      if (d < dresult->min_distance) {
+        if (OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1,
+                                      tf2))
+          return true;
+      }
+
+      child = (unsigned int)tree2->getBV(root2).rightChild();
+      convertBV(tree2->getBV(child).bv, tf2, aabb2);
+      d = aabb1.distance(aabb2);
+
+      if (d < dresult->min_distance) {
+        if (OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1,
+                                      tf2))
+          return true;
+      }
+    }
+
+    return false;
+  }
+
+  /// \return True if the request is satisfied.
+  template <typename BV>
+  bool OcTreeMeshIntersectRecurse(const OcTree* tree1,
+                                  const OcTree::OcTreeNode* root1,
+                                  const AABB& bv1, const BVHModel<BV>* tree2,
+                                  unsigned int root2, const Transform3s& tf1,
+                                  const Transform3s& tf2) const {
+    // FIXME(jmirabel) I do not understand why the BVHModel was traversed. The
+    // code in this if(!root1) did not output anything so the empty OcTree is
+    // considered free. Should an empty OcTree be considered free ?
+
+    // Empty OcTree is considered free.
+    if (!root1) return false;
+    BVNode<BV> const& bvn2 = tree2->getBV(root2);
+
+    /// stop when 1) bounding boxes of two objects not overlap; OR
+    ///           2) at least one of the nodes is free; OR
+    ///           2) (two uncertain nodes OR one node occupied and one node
+    ///           uncertain) AND cost not required
+    if (tree1->isNodeFree(root1))
+      return false;
+    else if ((tree1->isNodeUncertain(root1) || tree2->isUncertain()))
+      return false;
+    else {
+      OBB obb1, obb2;
+      convertBV(bv1, tf1, obb1);
+      convertBV(bvn2.bv, tf2, obb2);
+      CoalScalar sqrDistLowerBound;
+      if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound)) {
+        internal::updateDistanceLowerBoundFromBV(*crequest, *cresult,
+                                                 sqrDistLowerBound);
+        return false;
+      }
+    }
+
+    // Check if leaf collides.
+    if (!tree1->nodeHasChildren(root1) && bvn2.isLeaf()) {
+      assert(tree1->isNodeOccupied(root1));  // it isn't free nor uncertain.
+      Box box;
+      Transform3s box_tf;
+      constructBox(bv1, tf1, box, box_tf);
+      if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
+        box.computeLocalAABB();
+      }
+
+      size_t primitive_id = static_cast<size_t>(bvn2.primitiveId());
+      const Triangle& tri_id = (*(tree2->tri_indices))[primitive_id];
+      const TriangleP tri((*(tree2->vertices))[tri_id[0]],
+                          (*(tree2->vertices))[tri_id[1]],
+                          (*(tree2->vertices))[tri_id[2]]);
+
+      // When reaching this point, `this->solver` has already been set up
+      // by the CollisionRequest `this->crequest`.
+      // The only thing we need to (and can) pass to `ShapeShapeDistance` is
+      // whether or not penetration information is should be computed in case of
+      // collision.
+      const bool compute_penetration = this->crequest->enable_contact ||
+                                       (this->crequest->security_margin < 0);
+      Vec3s c1, c2, normal;
+      const CoalScalar distance = internal::ShapeShapeDistance<Box, TriangleP>(
+          &box, box_tf, &tri, tf2, this->solver, compute_penetration, c1, c2,
+          normal);
+      const CoalScalar distToCollision =
+          distance - this->crequest->security_margin;
+
+      internal::updateDistanceLowerBoundFromLeaf(
+          *(this->crequest), *(this->cresult), distToCollision, c1, c2, normal);
+
+      if (cresult->numContacts() < crequest->num_max_contacts) {
+        if (distToCollision <= crequest->collision_distance_threshold) {
+          cresult->addContact(Contact(
+              tree1, tree2, (int)(root1 - tree1->getRoot()),
+              static_cast<int>(primitive_id), c1, c2, normal, distance));
+        }
+      }
+      return crequest->isSatisfied(*cresult);
+    }
+
+    // Determine which tree to traverse first.
+    if (bvn2.isLeaf() ||
+        (tree1->nodeHasChildren(root1) && (bv1.size() > bvn2.bv.size()))) {
+      for (unsigned int i = 0; i < 8; ++i) {
+        if (tree1->nodeChildExists(root1, i)) {
+          const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
+          AABB child_bv;
+          computeChildBV(bv1, i, child_bv);
+
+          if (OcTreeMeshIntersectRecurse(tree1, child, child_bv, tree2, root2,
+                                         tf1, tf2))
+            return true;
+        }
+      }
+    } else {
+      if (OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2,
+                                     (unsigned int)bvn2.leftChild(), tf1, tf2))
+        return true;
+
+      if (OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2,
+                                     (unsigned int)bvn2.rightChild(), tf1, tf2))
+        return true;
+    }
+
+    return false;
+  }
+
+  /// \return True if the request is satisfied.
+  template <typename BV>
+  bool OcTreeHeightFieldIntersectRecurse(
+      const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1,
+      const HeightField<BV>* tree2, unsigned int root2, const Transform3s& tf1,
+      const Transform3s& tf2, CoalScalar& sqrDistLowerBound) const {
+    // FIXME(jmirabel) I do not understand why the BVHModel was traversed. The
+    // code in this if(!root1) did not output anything so the empty OcTree is
+    // considered free. Should an empty OcTree be considered free ?
+
+    // Empty OcTree is considered free.
+    if (!root1) return false;
+    HFNode<BV> const& bvn2 = tree2->getBV(root2);
+
+    /// stop when 1) bounding boxes of two objects not overlap; OR
+    ///           2) at least one of the nodes is free; OR
+    ///           2) (two uncertain nodes OR one node occupied and one node
+    ///           uncertain) AND cost not required
+    if (tree1->isNodeFree(root1))
+      return false;
+    else if ((tree1->isNodeUncertain(root1) || tree2->isUncertain()))
+      return false;
+    else {
+      OBB obb1, obb2;
+      convertBV(bv1, tf1, obb1);
+      convertBV(bvn2.bv, tf2, obb2);
+      CoalScalar sqrDistLowerBound_;
+      if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound_)) {
+        if (sqrDistLowerBound_ < sqrDistLowerBound)
+          sqrDistLowerBound = sqrDistLowerBound_;
+        internal::updateDistanceLowerBoundFromBV(*crequest, *cresult,
+                                                 sqrDistLowerBound);
+        return false;
+      }
+    }
+
+    // Check if leaf collides.
+    if (!tree1->nodeHasChildren(root1) && bvn2.isLeaf()) {
+      assert(tree1->isNodeOccupied(root1));  // it isn't free nor uncertain.
+      Box box;
+      Transform3s box_tf;
+      constructBox(bv1, tf1, box, box_tf);
+      if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
+        box.computeLocalAABB();
+      }
+
+      typedef Convex<Triangle> ConvexTriangle;
+      ConvexTriangle convex1, convex2;
+      int convex1_active_faces, convex2_active_faces;
+      details::buildConvexTriangles(bvn2, *tree2, convex1, convex1_active_faces,
+                                    convex2, convex2_active_faces);
+      if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
+        convex1.computeLocalAABB();
+        convex2.computeLocalAABB();
+      }
+
+      Vec3s c1, c2, normal, normal_top;
+      CoalScalar distance;
+      bool hfield_witness_is_on_bin_side;
+
+      bool collision = details::shapeDistance<Triangle, Box, 0>(
+          solver, *crequest, convex1, convex1_active_faces, convex2,
+          convex2_active_faces, tf2, box, box_tf, distance, c2, c1, normal,
+          normal_top, hfield_witness_is_on_bin_side);
+
+      CoalScalar distToCollision =
+          distance - crequest->security_margin * (normal_top.dot(normal));
+
+      if (distToCollision <= crequest->collision_distance_threshold) {
+        sqrDistLowerBound = 0;
+        if (crequest->num_max_contacts > cresult->numContacts()) {
+          if (normal_top.isApprox(normal) &&
+              (collision || !hfield_witness_is_on_bin_side)) {
+            cresult->addContact(
+                Contact(tree1, tree2, (int)(root1 - tree1->getRoot()),
+                        (int)Contact::NONE, c1, c2, -normal, distance));
+          }
+        }
+      } else
+        sqrDistLowerBound = distToCollision * distToCollision;
+
+      //    const Vec3s c1 = contact_point - distance * 0.5 * normal;
+      //    const Vec3s c2 = contact_point + distance * 0.5 * normal;
+      internal::updateDistanceLowerBoundFromLeaf(
+          *crequest, *cresult, distToCollision, c1, c2, -normal);
+
+      assert(cresult->isCollision() || sqrDistLowerBound > 0);
+      return crequest->isSatisfied(*cresult);
+    }
+
+    // Determine which tree to traverse first.
+    if (bvn2.isLeaf() ||
+        (tree1->nodeHasChildren(root1) && (bv1.size() > bvn2.bv.size()))) {
+      for (unsigned int i = 0; i < 8; ++i) {
+        if (tree1->nodeChildExists(root1, i)) {
+          const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
+          AABB child_bv;
+          computeChildBV(bv1, i, child_bv);
+
+          if (OcTreeHeightFieldIntersectRecurse(tree1, child, child_bv, tree2,
+                                                root2, tf1, tf2,
+                                                sqrDistLowerBound))
+            return true;
+        }
+      }
+    } else {
+      if (OcTreeHeightFieldIntersectRecurse(tree1, root1, bv1, tree2,
+                                            (unsigned int)bvn2.leftChild(), tf1,
+                                            tf2, sqrDistLowerBound))
+        return true;
+
+      if (OcTreeHeightFieldIntersectRecurse(tree1, root1, bv1, tree2,
+                                            (unsigned int)bvn2.rightChild(),
+                                            tf1, tf2, sqrDistLowerBound))
+        return true;
+    }
+
+    return false;
+  }
+
+  /// \return True if the request is satisfied.
+  template <typename BV>
+  bool HeightFieldOcTreeIntersectRecurse(
+      const HeightField<BV>* tree1, unsigned int root1, const OcTree* tree2,
+      const OcTree::OcTreeNode* root2, const AABB& bv2, const Transform3s& tf1,
+      const Transform3s& tf2, CoalScalar& sqrDistLowerBound) const {
+    // FIXME(jmirabel) I do not understand why the BVHModel was traversed. The
+    // code in this if(!root1) did not output anything so the empty OcTree is
+    // considered free. Should an empty OcTree be considered free ?
+
+    // Empty OcTree is considered free.
+    if (!root2) return false;
+    HFNode<BV> const& bvn1 = tree1->getBV(root1);
+
+    /// stop when 1) bounding boxes of two objects not overlap; OR
+    ///           2) at least one of the nodes is free; OR
+    ///           2) (two uncertain nodes OR one node occupied and one node
+    ///           uncertain) AND cost not required
+    if (tree2->isNodeFree(root2))
+      return false;
+    else if ((tree2->isNodeUncertain(root2) || tree1->isUncertain()))
+      return false;
+    else {
+      OBB obb1, obb2;
+      convertBV(bvn1.bv, tf1, obb1);
+      convertBV(bv2, tf2, obb2);
+      CoalScalar sqrDistLowerBound_;
+      if (!obb2.overlap(obb1, *crequest, sqrDistLowerBound_)) {
+        if (sqrDistLowerBound_ < sqrDistLowerBound)
+          sqrDistLowerBound = sqrDistLowerBound_;
+        internal::updateDistanceLowerBoundFromBV(*crequest, *cresult,
+                                                 sqrDistLowerBound);
+        return false;
+      }
+    }
+
+    // Check if leaf collides.
+    if (!tree2->nodeHasChildren(root2) && bvn1.isLeaf()) {
+      assert(tree2->isNodeOccupied(root2));  // it isn't free nor uncertain.
+      Box box;
+      Transform3s box_tf;
+      constructBox(bv2, tf2, box, box_tf);
+      if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
+        box.computeLocalAABB();
+      }
+
+      typedef Convex<Triangle> ConvexTriangle;
+      ConvexTriangle convex1, convex2;
+      int convex1_active_faces, convex2_active_faces;
+      details::buildConvexTriangles(bvn1, *tree1, convex1, convex1_active_faces,
+                                    convex2, convex2_active_faces);
+      if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
+        convex1.computeLocalAABB();
+        convex2.computeLocalAABB();
+      }
+
+      Vec3s c1, c2, normal, normal_top;
+      CoalScalar distance;
+      bool hfield_witness_is_on_bin_side;
+
+      bool collision = details::shapeDistance<Triangle, Box, 0>(
+          solver, *crequest, convex1, convex1_active_faces, convex2,
+          convex2_active_faces, tf1, box, box_tf, distance, c1, c2, normal,
+          normal_top, hfield_witness_is_on_bin_side);
+
+      CoalScalar distToCollision =
+          distance - crequest->security_margin * (normal_top.dot(normal));
+
+      if (distToCollision <= crequest->collision_distance_threshold) {
+        sqrDistLowerBound = 0;
+        if (crequest->num_max_contacts > cresult->numContacts()) {
+          if (normal_top.isApprox(normal) &&
+              (collision || !hfield_witness_is_on_bin_side)) {
+            cresult->addContact(Contact(tree1, tree2, (int)Contact::NONE,
+                                        (int)(root2 - tree2->getRoot()), c1, c2,
+                                        normal, distance));
+          }
+        }
+      } else
+        sqrDistLowerBound = distToCollision * distToCollision;
+
+      //    const Vec3s c1 = contact_point - distance * 0.5 * normal;
+      //    const Vec3s c2 = contact_point + distance * 0.5 * normal;
+      internal::updateDistanceLowerBoundFromLeaf(
+          *crequest, *cresult, distToCollision, c1, c2, normal);
+
+      assert(cresult->isCollision() || sqrDistLowerBound > 0);
+      return crequest->isSatisfied(*cresult);
+    }
+
+    // Determine which tree to traverse first.
+    if (bvn1.isLeaf() ||
+        (tree2->nodeHasChildren(root2) && (bv2.size() > bvn1.bv.size()))) {
+      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(bv2, i, child_bv);
+
+          if (HeightFieldOcTreeIntersectRecurse(tree1, root1, tree2, child,
+                                                child_bv, tf1, tf2,
+                                                sqrDistLowerBound))
+            return true;
+        }
+      }
+    } else {
+      if (HeightFieldOcTreeIntersectRecurse(
+              tree1, (unsigned int)bvn1.leftChild(), tree2, root2, bv2, tf1,
+              tf2, sqrDistLowerBound))
+        return true;
+
+      if (HeightFieldOcTreeIntersectRecurse(
+              tree1, (unsigned int)bvn1.rightChild(), tree2, root2, bv2, tf1,
+              tf2, sqrDistLowerBound))
+        return true;
+    }
+
+    return false;
+  }
+
+  bool OcTreeDistanceRecurse(const OcTree* tree1,
+                             const OcTree::OcTreeNode* root1, const AABB& bv1,
+                             const OcTree* tree2,
+                             const OcTree::OcTreeNode* root2, const AABB& bv2,
+                             const Transform3s& tf1,
+                             const Transform3s& tf2) const {
+    if (!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2)) {
+      if (tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2)) {
+        Box box1, box2;
+        Transform3s box1_tf, box2_tf;
+        constructBox(bv1, tf1, box1, box1_tf);
+        constructBox(bv2, tf2, box2, box2_tf);
+
+        if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
+          box1.computeLocalAABB();
+          box2.computeLocalAABB();
+        }
+
+        Vec3s p1, p2, normal;
+        const CoalScalar distance = internal::ShapeShapeDistance<Box, Box>(
+            &box1, box1_tf, &box2, box2_tf, this->solver,
+            this->drequest->enable_signed_distance, p1, p2, normal);
+
+        this->dresult->update(distance, tree1, tree2,
+                              (int)(root1 - tree1->getRoot()),
+                              (int)(root2 - tree2->getRoot()), p1, p2, normal);
+
+        return drequest->isSatisfied(*dresult);
+      } else
+        return false;
+    }
+
+    if (!tree1->isNodeOccupied(root1) || !tree2->isNodeOccupied(root2))
+      return false;
+
+    if (!tree2->nodeHasChildren(root2) ||
+        (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size()))) {
+      for (unsigned int i = 0; i < 8; ++i) {
+        if (tree1->nodeChildExists(root1, i)) {
+          const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
+          AABB child_bv;
+          computeChildBV(bv1, i, child_bv);
+
+          CoalScalar d;
+          AABB aabb1, aabb2;
+          convertBV(bv1, tf1, aabb1);
+          convertBV(bv2, tf2, aabb2);
+          d = aabb1.distance(aabb2);
+
+          if (d < dresult->min_distance) {
+            if (OcTreeDistanceRecurse(tree1, child, child_bv, tree2, root2, bv2,
+                                      tf1, tf2))
+              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(bv2, i, child_bv);
+
+          CoalScalar d;
+          AABB aabb1, aabb2;
+          convertBV(bv1, tf1, aabb1);
+          convertBV(bv2, tf2, aabb2);
+          d = aabb1.distance(aabb2);
+
+          if (d < dresult->min_distance) {
+            if (OcTreeDistanceRecurse(tree1, root1, bv1, tree2, child, child_bv,
+                                      tf1, tf2))
+              return true;
+          }
+        }
+      }
+    }
+
+    return false;
+  }
+
+  bool OcTreeIntersectRecurse(const OcTree* tree1,
+                              const OcTree::OcTreeNode* root1, const AABB& bv1,
+                              const OcTree* tree2,
+                              const OcTree::OcTreeNode* root2, const AABB& bv2,
+                              const Transform3s& tf1,
+                              const Transform3s& tf2) const {
+    // Empty OcTree is considered free.
+    if (!root1) return false;
+    if (!root2) return false;
+
+    /// stop when 1) bounding boxes of two objects not overlap; OR
+    ///           2) at least one of the nodes is free; OR
+    ///           2) (two uncertain nodes OR one node occupied and one node
+    ///           uncertain) AND cost not required
+    if (tree1->isNodeFree(root1) || tree2->isNodeFree(root2))
+      return false;
+    else if ((tree1->isNodeUncertain(root1) || tree2->isNodeUncertain(root2)))
+      return false;
+
+    bool bothAreLeaves =
+        (!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2));
+    if (!bothAreLeaves || !crequest->enable_contact) {
+      OBB obb1, obb2;
+      convertBV(bv1, tf1, obb1);
+      convertBV(bv2, tf2, obb2);
+      CoalScalar sqrDistLowerBound;
+      if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound)) {
+        if (cresult->distance_lower_bound > 0 &&
+            sqrDistLowerBound <
+                cresult->distance_lower_bound * cresult->distance_lower_bound)
+          cresult->distance_lower_bound =
+              sqrt(sqrDistLowerBound) - crequest->security_margin;
+        return false;
+      }
+      if (crequest->enable_contact) {  // Overlap
+        if (cresult->numContacts() < crequest->num_max_contacts)
+          cresult->addContact(
+              Contact(tree1, tree2, static_cast<int>(root1 - tree1->getRoot()),
+                      static_cast<int>(root2 - tree2->getRoot())));
+        return crequest->isSatisfied(*cresult);
+      }
+    }
+
+    // Both node are leaves
+    if (bothAreLeaves) {
+      assert(tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2));
+
+      Box box1, box2;
+      Transform3s box1_tf, box2_tf;
+      constructBox(bv1, tf1, box1, box1_tf);
+      constructBox(bv2, tf2, box2, box2_tf);
+
+      if (this->solver->gjk_initial_guess ==
+          GJKInitialGuess::BoundingVolumeGuess) {
+        box1.computeLocalAABB();
+        box2.computeLocalAABB();
+      }
+
+      // When reaching this point, `this->solver` has already been set up
+      // by the CollisionRequest `this->request`.
+      // The only thing we need to (and can) pass to `ShapeShapeDistance` is
+      // whether or not penetration information is should be computed in case of
+      // collision.
+      const bool compute_penetration = (this->crequest->enable_contact ||
+                                        (this->crequest->security_margin < 0));
+      Vec3s c1, c2, normal;
+      CoalScalar distance = internal::ShapeShapeDistance<Box, Box>(
+          &box1, box1_tf, &box2, box2_tf, this->solver, compute_penetration, c1,
+          c2, normal);
+
+      const CoalScalar distToCollision =
+          distance - this->crequest->security_margin;
+
+      internal::updateDistanceLowerBoundFromLeaf(
+          *(this->crequest), *(this->cresult), distToCollision, c1, c2, normal);
+
+      if (this->cresult->numContacts() < this->crequest->num_max_contacts) {
+        if (distToCollision <= this->crequest->collision_distance_threshold)
+          this->cresult->addContact(
+              Contact(tree1, tree2, static_cast<int>(root1 - tree1->getRoot()),
+                      static_cast<int>(root2 - tree2->getRoot()), c1, c2,
+                      normal, distance));
+      }
+
+      return crequest->isSatisfied(*cresult);
+    }
+
+    // Determine which tree to traverse first.
+    if (!tree2->nodeHasChildren(root2) ||
+        (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size()))) {
+      for (unsigned int i = 0; i < 8; ++i) {
+        if (tree1->nodeChildExists(root1, i)) {
+          const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
+          AABB child_bv;
+          computeChildBV(bv1, i, child_bv);
+
+          if (OcTreeIntersectRecurse(tree1, child, child_bv, tree2, root2, bv2,
+                                     tf1, tf2))
+            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(bv2, i, child_bv);
+
+          if (OcTreeIntersectRecurse(tree1, root1, bv1, tree2, child, child_bv,
+                                     tf1, tf2))
+            return true;
+        }
+      }
+    }
+
+    return false;
+  }
+};
+
+/// @addtogroup Traversal_For_Collision
+/// @{
+
+/// @brief Traversal node for octree collision
+class COAL_DLLAPI OcTreeCollisionTraversalNode
+    : public CollisionTraversalNodeBase {
+ public:
+  OcTreeCollisionTraversalNode(const CollisionRequest& request)
+      : CollisionTraversalNodeBase(request) {
+    model1 = NULL;
+    model2 = NULL;
+
+    otsolver = NULL;
+  }
+
+  bool BVDisjoints(unsigned, unsigned, CoalScalar&) const { return false; }
+
+  void leafCollides(unsigned, unsigned, CoalScalar& sqrDistLowerBound) const {
+    otsolver->OcTreeIntersect(model1, model2, tf1, tf2, request, *result);
+    sqrDistLowerBound = std::max((CoalScalar)0, result->distance_lower_bound);
+    sqrDistLowerBound *= sqrDistLowerBound;
+  }
+
+  const OcTree* model1;
+  const OcTree* model2;
+
+  Transform3s tf1, tf2;
+
+  const OcTreeSolver* otsolver;
+};
+
+/// @brief Traversal node for shape-octree collision
+template <typename S>
+class COAL_DLLAPI ShapeOcTreeCollisionTraversalNode
+    : public CollisionTraversalNodeBase {
+ public:
+  ShapeOcTreeCollisionTraversalNode(const CollisionRequest& request)
+      : CollisionTraversalNodeBase(request) {
+    model1 = NULL;
+    model2 = NULL;
+
+    otsolver = NULL;
+  }
+
+  bool BVDisjoints(unsigned int, unsigned int, CoalScalar&) const {
+    return false;
+  }
+
+  void leafCollides(unsigned int, unsigned int,
+                    CoalScalar& sqrDistLowerBound) const {
+    otsolver->OcTreeShapeIntersect(model2, *model1, tf2, tf1, request, *result);
+    sqrDistLowerBound = std::max((CoalScalar)0, result->distance_lower_bound);
+    sqrDistLowerBound *= sqrDistLowerBound;
+  }
+
+  const S* model1;
+  const OcTree* model2;
+
+  Transform3s tf1, tf2;
+
+  const OcTreeSolver* otsolver;
+};
+
+/// @brief Traversal node for octree-shape collision
+
+template <typename S>
+class COAL_DLLAPI OcTreeShapeCollisionTraversalNode
+    : public CollisionTraversalNodeBase {
+ public:
+  OcTreeShapeCollisionTraversalNode(const CollisionRequest& request)
+      : CollisionTraversalNodeBase(request) {
+    model1 = NULL;
+    model2 = NULL;
+
+    otsolver = NULL;
+  }
+
+  bool BVDisjoints(unsigned int, unsigned int, coal::CoalScalar&) const {
+    return false;
+  }
+
+  void leafCollides(unsigned int, unsigned int,
+                    CoalScalar& sqrDistLowerBound) const {
+    otsolver->OcTreeShapeIntersect(model1, *model2, tf1, tf2, request, *result);
+    sqrDistLowerBound = std::max((CoalScalar)0, result->distance_lower_bound);
+    sqrDistLowerBound *= sqrDistLowerBound;
+  }
+
+  const OcTree* model1;
+  const S* model2;
+
+  Transform3s tf1, tf2;
+
+  const OcTreeSolver* otsolver;
+};
+
+/// @brief Traversal node for mesh-octree collision
+template <typename BV>
+class COAL_DLLAPI MeshOcTreeCollisionTraversalNode
+    : public CollisionTraversalNodeBase {
+ public:
+  MeshOcTreeCollisionTraversalNode(const CollisionRequest& request)
+      : CollisionTraversalNodeBase(request) {
+    model1 = NULL;
+    model2 = NULL;
+
+    otsolver = NULL;
+  }
+
+  bool BVDisjoints(unsigned int, unsigned int, CoalScalar&) const {
+    return false;
+  }
+
+  void leafCollides(unsigned int, unsigned int,
+                    CoalScalar& sqrDistLowerBound) const {
+    otsolver->OcTreeMeshIntersect(model2, model1, tf2, tf1, request, *result);
+    sqrDistLowerBound = std::max((CoalScalar)0, result->distance_lower_bound);
+    sqrDistLowerBound *= sqrDistLowerBound;
+  }
+
+  const BVHModel<BV>* model1;
+  const OcTree* model2;
+
+  Transform3s tf1, tf2;
+
+  const OcTreeSolver* otsolver;
+};
+
+/// @brief Traversal node for octree-mesh collision
+template <typename BV>
+class COAL_DLLAPI OcTreeMeshCollisionTraversalNode
+    : public CollisionTraversalNodeBase {
+ public:
+  OcTreeMeshCollisionTraversalNode(const CollisionRequest& request)
+      : CollisionTraversalNodeBase(request) {
+    model1 = NULL;
+    model2 = NULL;
+
+    otsolver = NULL;
+  }
+
+  bool BVDisjoints(unsigned int, unsigned int, CoalScalar&) const {
+    return false;
+  }
+
+  void leafCollides(unsigned int, unsigned int,
+                    CoalScalar& sqrDistLowerBound) const {
+    otsolver->OcTreeMeshIntersect(model1, model2, tf1, tf2, request, *result);
+    sqrDistLowerBound = std::max((CoalScalar)0, result->distance_lower_bound);
+    sqrDistLowerBound *= sqrDistLowerBound;
+  }
+
+  const OcTree* model1;
+  const BVHModel<BV>* model2;
+
+  Transform3s tf1, tf2;
+
+  const OcTreeSolver* otsolver;
+};
+
+/// @brief Traversal node for octree-height-field collision
+template <typename BV>
+class COAL_DLLAPI OcTreeHeightFieldCollisionTraversalNode
+    : public CollisionTraversalNodeBase {
+ public:
+  OcTreeHeightFieldCollisionTraversalNode(const CollisionRequest& request)
+      : CollisionTraversalNodeBase(request) {
+    model1 = NULL;
+    model2 = NULL;
+
+    otsolver = NULL;
+  }
+
+  bool BVDisjoints(unsigned int, unsigned int, CoalScalar&) const {
+    return false;
+  }
+
+  void leafCollides(unsigned int, unsigned int,
+                    CoalScalar& sqrDistLowerBound) const {
+    otsolver->OcTreeHeightFieldIntersect(model1, model2, tf1, tf2, request,
+                                         *result, sqrDistLowerBound);
+  }
+
+  const OcTree* model1;
+  const HeightField<BV>* model2;
+
+  Transform3s tf1, tf2;
+
+  const OcTreeSolver* otsolver;
+};
+
+/// @brief Traversal node for octree-height-field collision
+template <typename BV>
+class COAL_DLLAPI HeightFieldOcTreeCollisionTraversalNode
+    : public CollisionTraversalNodeBase {
+ public:
+  HeightFieldOcTreeCollisionTraversalNode(const CollisionRequest& request)
+      : CollisionTraversalNodeBase(request) {
+    model1 = NULL;
+    model2 = NULL;
+
+    otsolver = NULL;
+  }
+
+  bool BVDisjoints(unsigned int, unsigned int, CoalScalar&) const {
+    return false;
+  }
+
+  void leafCollides(unsigned int, unsigned int,
+                    CoalScalar& sqrDistLowerBound) const {
+    otsolver->HeightFieldOcTreeIntersect(model1, model2, tf1, tf2, request,
+                                         *result, sqrDistLowerBound);
+  }
+
+  const HeightField<BV>* model1;
+  const OcTree* model2;
+
+  Transform3s tf1, tf2;
+
+  const OcTreeSolver* otsolver;
+};
+
+/// @}
+
+/// @addtogroup Traversal_For_Distance
+/// @{
+
+/// @brief Traversal node for octree distance
+class COAL_DLLAPI OcTreeDistanceTraversalNode
+    : public DistanceTraversalNodeBase {
+ public:
+  OcTreeDistanceTraversalNode() {
+    model1 = NULL;
+    model2 = NULL;
+
+    otsolver = NULL;
+  }
+
+  CoalScalar BVDistanceLowerBound(unsigned, unsigned) const { return -1; }
+
+  bool BVDistanceLowerBound(unsigned, unsigned, CoalScalar&) const {
+    return false;
+  }
+
+  void leafComputeDistance(unsigned, unsigned int) const {
+    otsolver->OcTreeDistance(model1, model2, tf1, tf2, request, *result);
+  }
+
+  const OcTree* model1;
+  const OcTree* model2;
+
+  const OcTreeSolver* otsolver;
+};
+
+/// @brief Traversal node for shape-octree distance
+template <typename Shape>
+class COAL_DLLAPI ShapeOcTreeDistanceTraversalNode
+    : public DistanceTraversalNodeBase {
+ public:
+  ShapeOcTreeDistanceTraversalNode() {
+    model1 = NULL;
+    model2 = NULL;
+
+    otsolver = NULL;
+  }
+
+  CoalScalar BVDistanceLowerBound(unsigned int, unsigned int) const {
+    return -1;
+  }
+
+  void leafComputeDistance(unsigned int, unsigned int) const {
+    otsolver->OcTreeShapeDistance(model2, *model1, tf2, tf1, request, *result);
+  }
+
+  const Shape* model1;
+  const OcTree* model2;
+
+  const OcTreeSolver* otsolver;
+};
+
+/// @brief Traversal node for octree-shape distance
+template <typename Shape>
+class COAL_DLLAPI OcTreeShapeDistanceTraversalNode
+    : public DistanceTraversalNodeBase {
+ public:
+  OcTreeShapeDistanceTraversalNode() {
+    model1 = NULL;
+    model2 = NULL;
+
+    otsolver = NULL;
+  }
+
+  CoalScalar BVDistanceLowerBound(unsigned int, unsigned int) const {
+    return -1;
+  }
+
+  void leafComputeDistance(unsigned int, unsigned int) const {
+    otsolver->OcTreeShapeDistance(model1, *model2, tf1, tf2, request, *result);
+  }
+
+  const OcTree* model1;
+  const Shape* model2;
+
+  const OcTreeSolver* otsolver;
+};
+
+/// @brief Traversal node for mesh-octree distance
+template <typename BV>
+class COAL_DLLAPI MeshOcTreeDistanceTraversalNode
+    : public DistanceTraversalNodeBase {
+ public:
+  MeshOcTreeDistanceTraversalNode() {
+    model1 = NULL;
+    model2 = NULL;
+
+    otsolver = NULL;
+  }
+
+  CoalScalar BVDistanceLowerBound(unsigned int, unsigned int) const {
+    return -1;
+  }
+
+  void leafComputeDistance(unsigned int, unsigned int) const {
+    otsolver->OcTreeMeshDistance(model2, model1, tf2, tf1, request, *result);
+  }
+
+  const BVHModel<BV>* model1;
+  const OcTree* model2;
+
+  const OcTreeSolver* otsolver;
+};
+
+/// @brief Traversal node for octree-mesh distance
+template <typename BV>
+class COAL_DLLAPI OcTreeMeshDistanceTraversalNode
+    : public DistanceTraversalNodeBase {
+ public:
+  OcTreeMeshDistanceTraversalNode() {
+    model1 = NULL;
+    model2 = NULL;
+
+    otsolver = NULL;
+  }
+
+  CoalScalar BVDistanceLowerBound(unsigned int, unsigned int) const {
+    return -1;
+  }
+
+  void leafComputeDistance(unsigned int, unsigned int) const {
+    otsolver->OcTreeMeshDistance(model1, model2, tf1, tf2, request, *result);
+  }
+
+  const OcTree* model1;
+  const BVHModel<BV>* model2;
+
+  const OcTreeSolver* otsolver;
+};
+
+/// @}
+
+}  // namespace coal
+
+/// @endcond
+
+#endif

include/coal/internal/traversal_node_setup.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_TRAVERSAL_NODE_SETUP_H
+#define COAL_TRAVERSAL_NODE_SETUP_H
+
+/// @cond INTERNAL
+
+#include "coal/internal/tools.h"
+#include "coal/internal/traversal_node_shapes.h"
+
+#include "coal/internal/traversal_node_bvhs.h"
+#include "coal/internal/traversal_node_bvh_shape.h"
+
+// #include <hpp/fcl/internal/traversal_node_hfields.h>
+#include "coal/internal/traversal_node_hfield_shape.h"
+
+#ifdef COAL_HAS_OCTOMAP
+#include "coal/internal/traversal_node_octree.h"
+#endif
+
+#include "coal/BVH/BVH_utility.h"
+
+namespace coal {
+
+#ifdef COAL_HAS_OCTOMAP
+/// @brief Initialize traversal node for collision between two octrees, given
+/// current object transform
+inline bool initialize(OcTreeCollisionTraversalNode& node, const OcTree& model1,
+                       const Transform3s& tf1, const OcTree& model2,
+                       const Transform3s& tf2, const OcTreeSolver* otsolver,
+                       CollisionResult& result) {
+  node.result = &result;
+
+  node.model1 = &model1;
+  node.model2 = &model2;
+
+  node.otsolver = otsolver;
+
+  node.tf1 = tf1;
+  node.tf2 = tf2;
+
+  return true;
+}
+
+/// @brief Initialize traversal node for distance between two octrees, given
+/// current object transform
+inline bool initialize(OcTreeDistanceTraversalNode& node, const OcTree& model1,
+                       const Transform3s& tf1, const OcTree& model2,
+                       const Transform3s& tf2, const OcTreeSolver* otsolver,
+                       const DistanceRequest& request, DistanceResult& result)
+
+{
+  node.request = request;
+  node.result = &result;
+
+  node.model1 = &model1;
+  node.model2 = &model2;
+
+  node.otsolver = otsolver;
+
+  node.tf1 = tf1;
+  node.tf2 = tf2;
+
+  return true;
+}
+
+/// @brief Initialize traversal node for collision between one shape and one
+/// octree, given current object transform
+template <typename S>
+bool initialize(ShapeOcTreeCollisionTraversalNode<S>& node, const S& model1,
+                const Transform3s& tf1, const OcTree& model2,
+                const Transform3s& tf2, const OcTreeSolver* otsolver,
+                CollisionResult& result) {
+  node.result = &result;
+
+  node.model1 = &model1;
+  node.model2 = &model2;
+
+  node.otsolver = otsolver;
+
+  node.tf1 = tf1;
+  node.tf2 = tf2;
+
+  return true;
+}
+
+/// @brief Initialize traversal node for collision between one octree and one
+/// shape, given current object transform
+template <typename S>
+bool initialize(OcTreeShapeCollisionTraversalNode<S>& node,
+                const OcTree& model1, const Transform3s& tf1, const S& model2,
+                const Transform3s& tf2, const OcTreeSolver* otsolver,
+                CollisionResult& result) {
+  node.result = &result;
+
+  node.model1 = &model1;
+  node.model2 = &model2;
+
+  node.otsolver = otsolver;
+
+  node.tf1 = tf1;
+  node.tf2 = tf2;
+
+  return true;
+}
+
+/// @brief Initialize traversal node for distance between one shape and one
+/// octree, given current object transform
+template <typename S>
+bool initialize(ShapeOcTreeDistanceTraversalNode<S>& node, const S& model1,
+                const Transform3s& tf1, const OcTree& model2,
+                const Transform3s& tf2, const OcTreeSolver* otsolver,
+                const DistanceRequest& request, DistanceResult& result) {
+  node.request = request;
+  node.result = &result;
+
+  node.model1 = &model1;
+  node.model2 = &model2;
+
+  node.otsolver = otsolver;
+
+  node.tf1 = tf1;
+  node.tf2 = tf2;
+
+  return true;
+}
+
+/// @brief Initialize traversal node for distance between one octree and one
+/// shape, given current object transform
+template <typename S>
+bool initialize(OcTreeShapeDistanceTraversalNode<S>& node, const OcTree& model1,
+                const Transform3s& tf1, const S& model2, const Transform3s& tf2,
+                const OcTreeSolver* otsolver, const DistanceRequest& request,
+                DistanceResult& result) {
+  node.request = request;
+  node.result = &result;
+
+  node.model1 = &model1;
+  node.model2 = &model2;
+
+  node.otsolver = otsolver;
+
+  node.tf1 = tf1;
+  node.tf2 = tf2;
+
+  return true;
+}
+
+/// @brief Initialize traversal node for collision between one mesh and one
+/// octree, given current object transform
+template <typename BV>
+bool initialize(MeshOcTreeCollisionTraversalNode<BV>& node,
+                const BVHModel<BV>& model1, const Transform3s& tf1,
+                const OcTree& model2, const Transform3s& tf2,
+                const OcTreeSolver* otsolver, CollisionResult& result) {
+  node.result = &result;
+
+  node.model1 = &model1;
+  node.model2 = &model2;
+
+  node.otsolver = otsolver;
+
+  node.tf1 = tf1;
+  node.tf2 = tf2;
+
+  return true;
+}
+
+/// @brief Initialize traversal node for collision between one octree and one
+/// mesh, given current object transform
+template <typename BV>
+bool initialize(OcTreeMeshCollisionTraversalNode<BV>& node,
+                const OcTree& model1, const Transform3s& tf1,
+                const BVHModel<BV>& model2, const Transform3s& tf2,
+                const OcTreeSolver* otsolver, CollisionResult& result) {
+  node.result = &result;
+
+  node.model1 = &model1;
+  node.model2 = &model2;
+
+  node.otsolver = otsolver;
+
+  node.tf1 = tf1;
+  node.tf2 = tf2;
+
+  return true;
+}
+
+/// @brief Initialize traversal node for collision between one octree and one
+/// height field, given current object transform
+template <typename BV>
+bool initialize(OcTreeHeightFieldCollisionTraversalNode<BV>& node,
+                const OcTree& model1, const Transform3s& tf1,
+                const HeightField<BV>& model2, const Transform3s& tf2,
+                const OcTreeSolver* otsolver, CollisionResult& result) {
+  node.result = &result;
+
+  node.model1 = &model1;
+  node.model2 = &model2;
+
+  node.otsolver = otsolver;
+
+  node.tf1 = tf1;
+  node.tf2 = tf2;
+
+  return true;
+}
+
+/// @brief Initialize traversal node for collision between one height field and
+/// one octree, given current object transform
+template <typename BV>
+bool initialize(HeightFieldOcTreeCollisionTraversalNode<BV>& node,
+                const HeightField<BV>& model1, const Transform3s& tf1,
+                const OcTree& model2, const Transform3s& tf2,
+                const OcTreeSolver* otsolver, CollisionResult& result) {
+  node.result = &result;
+
+  node.model1 = &model1;
+  node.model2 = &model2;
+
+  node.otsolver = otsolver;
+
+  node.tf1 = tf1;
+  node.tf2 = tf2;
+
+  return true;
+}
+
+/// @brief Initialize traversal node for distance between one mesh and one
+/// octree, given current object transform
+template <typename BV>
+bool initialize(MeshOcTreeDistanceTraversalNode<BV>& node,
+                const BVHModel<BV>& model1, const Transform3s& tf1,
+                const OcTree& model2, const Transform3s& tf2,
+                const OcTreeSolver* otsolver, const DistanceRequest& request,
+                DistanceResult& result) {
+  node.request = request;
+  node.result = &result;
+
+  node.model1 = &model1;
+  node.model2 = &model2;
+
+  node.otsolver = otsolver;
+
+  node.tf1 = tf1;
+  node.tf2 = tf2;
+
+  return true;
+}
+
+/// @brief Initialize traversal node for collision between one octree and one
+/// mesh, given current object transform
+template <typename BV>
+bool initialize(OcTreeMeshDistanceTraversalNode<BV>& node, const OcTree& model1,
+                const Transform3s& tf1, const BVHModel<BV>& model2,
+                const Transform3s& tf2, const OcTreeSolver* otsolver,
+                const DistanceRequest& request, DistanceResult& result) {
+  node.request = request;
+  node.result = &result;
+
+  node.model1 = &model1;
+  node.model2 = &model2;
+
+  node.otsolver = otsolver;
+
+  node.tf1 = tf1;
+  node.tf2 = tf2;
+
+  return true;
+}
+
+#endif
+
+/// @brief Initialize traversal node for collision between two geometric shapes,
+/// given current object transform
+template <typename S1, typename S2>
+bool initialize(ShapeCollisionTraversalNode<S1, S2>& node, const S1& shape1,
+                const Transform3s& tf1, const S2& shape2,
+                const Transform3s& tf2, const GJKSolver* nsolver,
+                CollisionResult& result) {
+  node.model1 = &shape1;
+  node.tf1 = tf1;
+  node.model2 = &shape2;
+  node.tf2 = tf2;
+  node.nsolver = nsolver;
+
+  node.result = &result;
+
+  return true;
+}
+
+/// @brief Initialize traversal node for collision between one mesh and one
+/// shape, given current object transform
+template <typename BV, typename S>
+bool initialize(MeshShapeCollisionTraversalNode<BV, S>& node,
+                BVHModel<BV>& model1, Transform3s& tf1, const S& model2,
+                const Transform3s& tf2, const GJKSolver* nsolver,
+                CollisionResult& result, bool use_refit = false,
+                bool refit_bottomup = false) {
+  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
+    COAL_THROW_PRETTY(
+        "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
+        std::invalid_argument)
+
+  if (!tf1.isIdentity() &&
+      model1.vertices.get())  // TODO(jcarpent): vectorized version
+  {
+    std::vector<Vec3s> vertices_transformed(model1.num_vertices);
+    const std::vector<Vec3s>& model1_vertices_ = *(model1.vertices);
+    for (unsigned int i = 0; i < model1.num_vertices; ++i) {
+      const Vec3s& p = model1_vertices_[i];
+      Vec3s new_v = tf1.transform(p);
+      vertices_transformed[i] = new_v;
+    }
+
+    model1.beginReplaceModel();
+    model1.replaceSubModel(vertices_transformed);
+    model1.endReplaceModel(use_refit, refit_bottomup);
+
+    tf1.setIdentity();
+  }
+
+  node.model1 = &model1;
+  node.tf1 = tf1;
+  node.model2 = &model2;
+  node.tf2 = tf2;
+  node.nsolver = nsolver;
+
+  computeBV(model2, tf2, node.model2_bv);
+
+  node.vertices = model1.vertices.get() ? model1.vertices->data() : NULL;
+  node.tri_indices =
+      model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
+
+  node.result = &result;
+
+  return true;
+}
+
+/// @brief Initialize traversal node for collision between one mesh and one
+/// shape
+template <typename BV, typename S>
+bool initialize(MeshShapeCollisionTraversalNode<BV, S, 0>& node,
+                const BVHModel<BV>& model1, const Transform3s& tf1,
+                const S& model2, const Transform3s& tf2,
+                const GJKSolver* nsolver, CollisionResult& result) {
+  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
+    COAL_THROW_PRETTY(
+        "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
+        std::invalid_argument)
+
+  node.model1 = &model1;
+  node.tf1 = tf1;
+  node.model2 = &model2;
+  node.tf2 = tf2;
+  node.nsolver = nsolver;
+
+  computeBV(model2, tf2, node.model2_bv);
+
+  node.vertices = model1.vertices.get() ? model1.vertices->data() : NULL;
+  node.tri_indices =
+      model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
+
+  node.result = &result;
+
+  return true;
+}
+
+/// @brief Initialize traversal node for collision between one mesh and one
+/// shape, given current object transform
+template <typename BV, typename S>
+bool initialize(HeightFieldShapeCollisionTraversalNode<BV, S>& node,
+                HeightField<BV>& model1, Transform3s& tf1, const S& model2,
+                const Transform3s& tf2, const GJKSolver* nsolver,
+                CollisionResult& result, bool use_refit = false,
+                bool refit_bottomup = false);
+
+/// @brief Initialize traversal node for collision between one mesh and one
+/// shape
+template <typename BV, typename S>
+bool initialize(HeightFieldShapeCollisionTraversalNode<BV, S, 0>& node,
+                const HeightField<BV>& model1, const Transform3s& tf1,
+                const S& model2, const Transform3s& tf2,
+                const GJKSolver* nsolver, CollisionResult& result) {
+  node.model1 = &model1;
+  node.tf1 = tf1;
+  node.model2 = &model2;
+  node.tf2 = tf2;
+  node.nsolver = nsolver;
+
+  computeBV(model2, tf2, node.model2_bv);
+
+  node.result = &result;
+
+  return true;
+}
+
+/// @cond IGNORE
+namespace details {
+template <typename S, typename BV, template <typename> class OrientedNode>
+static inline bool setupShapeMeshCollisionOrientedNode(
+    OrientedNode<S>& node, const S& model1, const Transform3s& tf1,
+    const BVHModel<BV>& model2, const Transform3s& tf2,
+    const GJKSolver* nsolver, CollisionResult& result) {
+  if (model2.getModelType() != BVH_MODEL_TRIANGLES)
+    COAL_THROW_PRETTY(
+        "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
+        std::invalid_argument)
+
+  node.model1 = &model1;
+  node.tf1 = tf1;
+  node.model2 = &model2;
+  node.tf2 = tf2;
+  node.nsolver = nsolver;
+
+  computeBV(model1, tf1, node.model1_bv);
+
+  node.vertices = model2.vertices.get() ? model2.vertices->data() : NULL;
+  node.tri_indices =
+      model2.tri_indices.get() ? model2.tri_indices->data() : NULL;
+
+  node.result = &result;
+
+  return true;
+}
+}  // namespace details
+/// @endcond
+
+/// @brief Initialize traversal node for collision between two meshes, given the
+/// current transforms
+template <typename BV>
+bool initialize(
+    MeshCollisionTraversalNode<BV, RelativeTransformationIsIdentity>& node,
+    BVHModel<BV>& model1, Transform3s& tf1, BVHModel<BV>& model2,
+    Transform3s& tf2, CollisionResult& result, bool use_refit = false,
+    bool refit_bottomup = false) {
+  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
+    COAL_THROW_PRETTY(
+        "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
+        std::invalid_argument)
+  if (model2.getModelType() != BVH_MODEL_TRIANGLES)
+    COAL_THROW_PRETTY(
+        "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
+        std::invalid_argument)
+
+  if (!tf1.isIdentity() && model1.vertices.get()) {
+    std::vector<Vec3s> vertices_transformed1(model1.num_vertices);
+    const std::vector<Vec3s>& model1_vertices_ = *(model1.vertices);
+    for (unsigned int i = 0; i < model1.num_vertices; ++i) {
+      const Vec3s& p = model1_vertices_[i];
+      Vec3s new_v = tf1.transform(p);
+      vertices_transformed1[i] = new_v;
+    }
+
+    model1.beginReplaceModel();
+    model1.replaceSubModel(vertices_transformed1);
+    model1.endReplaceModel(use_refit, refit_bottomup);
+
+    tf1.setIdentity();
+  }
+
+  if (!tf2.isIdentity() && model2.vertices.get()) {
+    std::vector<Vec3s> vertices_transformed2(model2.num_vertices);
+    const std::vector<Vec3s>& model2_vertices_ = *(model2.vertices);
+    for (unsigned int i = 0; i < model2.num_vertices; ++i) {
+      const Vec3s& p = model2_vertices_[i];
+      Vec3s new_v = tf2.transform(p);
+      vertices_transformed2[i] = new_v;
+    }
+
+    model2.beginReplaceModel();
+    model2.replaceSubModel(vertices_transformed2);
+    model2.endReplaceModel(use_refit, refit_bottomup);
+
+    tf2.setIdentity();
+  }
+
+  node.model1 = &model1;
+  node.tf1 = tf1;
+  node.model2 = &model2;
+  node.tf2 = tf2;
+
+  node.vertices1 = model1.vertices.get() ? model1.vertices->data() : NULL;
+  node.vertices2 = model2.vertices.get() ? model2.vertices->data() : NULL;
+
+  node.tri_indices1 =
+      model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
+  node.tri_indices2 =
+      model2.tri_indices.get() ? model2.tri_indices->data() : NULL;
+
+  node.result = &result;
+
+  return true;
+}
+
+template <typename BV>
+bool initialize(MeshCollisionTraversalNode<BV, 0>& node,
+                const BVHModel<BV>& model1, const Transform3s& tf1,
+                const BVHModel<BV>& model2, const Transform3s& tf2,
+                CollisionResult& result) {
+  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
+    COAL_THROW_PRETTY(
+        "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
+        std::invalid_argument)
+  if (model2.getModelType() != BVH_MODEL_TRIANGLES)
+    COAL_THROW_PRETTY(
+        "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
+        std::invalid_argument)
+
+  node.vertices1 = model1.vertices ? model1.vertices->data() : NULL;
+  node.vertices2 = model2.vertices ? model2.vertices->data() : NULL;
+
+  node.tri_indices1 =
+      model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
+  node.tri_indices2 =
+      model2.tri_indices.get() ? model2.tri_indices->data() : NULL;
+
+  node.model1 = &model1;
+  node.tf1 = tf1;
+  node.model2 = &model2;
+  node.tf2 = tf2;
+
+  node.result = &result;
+
+  node.RT.R.noalias() = tf1.getRotation().transpose() * tf2.getRotation();
+  node.RT.T.noalias() = tf1.getRotation().transpose() *
+                        (tf2.getTranslation() - tf1.getTranslation());
+
+  return true;
+}
+
+/// @brief Initialize traversal node for distance between two geometric shapes
+template <typename S1, typename S2>
+bool initialize(ShapeDistanceTraversalNode<S1, S2>& node, const S1& shape1,
+                const Transform3s& tf1, const S2& shape2,
+                const Transform3s& tf2, const GJKSolver* nsolver,
+                const DistanceRequest& request, DistanceResult& result) {
+  node.request = request;
+  node.result = &result;
+
+  node.model1 = &shape1;
+  node.tf1 = tf1;
+  node.model2 = &shape2;
+  node.tf2 = tf2;
+  node.nsolver = nsolver;
+
+  return true;
+}
+
+/// @brief Initialize traversal node for distance computation between two
+/// meshes, given the current transforms
+template <typename BV>
+bool initialize(
+    MeshDistanceTraversalNode<BV, RelativeTransformationIsIdentity>& node,
+    BVHModel<BV>& model1, Transform3s& tf1, BVHModel<BV>& model2,
+    Transform3s& tf2, const DistanceRequest& request, DistanceResult& result,
+    bool use_refit = false, bool refit_bottomup = false) {
+  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
+    COAL_THROW_PRETTY(
+        "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
+        std::invalid_argument)
+  if (model2.getModelType() != BVH_MODEL_TRIANGLES)
+    COAL_THROW_PRETTY(
+        "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
+        std::invalid_argument)
+
+  if (!tf1.isIdentity() && model1.vertices.get()) {
+    std::vector<Vec3s> vertices_transformed1(model1.num_vertices);
+    const std::vector<Vec3s>& model1_vertices_ = *(model1.vertices);
+    for (unsigned int i = 0; i < model1.num_vertices; ++i) {
+      const Vec3s& p = model1_vertices_[i];
+      Vec3s new_v = tf1.transform(p);
+      vertices_transformed1[i] = new_v;
+    }
+
+    model1.beginReplaceModel();
+    model1.replaceSubModel(vertices_transformed1);
+    model1.endReplaceModel(use_refit, refit_bottomup);
+
+    tf1.setIdentity();
+  }
+
+  if (!tf2.isIdentity() && model2.vertices.get()) {
+    std::vector<Vec3s> vertices_transformed2(model2.num_vertices);
+    const std::vector<Vec3s>& model2_vertices_ = *(model2.vertices);
+    for (unsigned int i = 0; i < model2.num_vertices; ++i) {
+      const Vec3s& p = model2_vertices_[i];
+      Vec3s new_v = tf2.transform(p);
+      vertices_transformed2[i] = new_v;
+    }
+
+    model2.beginReplaceModel();
+    model2.replaceSubModel(vertices_transformed2);
+    model2.endReplaceModel(use_refit, refit_bottomup);
+
+    tf2.setIdentity();
+  }
+
+  node.request = request;
+  node.result = &result;
+
+  node.model1 = &model1;
+  node.tf1 = tf1;
+  node.model2 = &model2;
+  node.tf2 = tf2;
+
+  node.vertices1 = model1.vertices.get() ? model1.vertices->data() : NULL;
+  node.vertices2 = model2.vertices.get() ? model2.vertices->data() : NULL;
+
+  node.tri_indices1 =
+      model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
+  node.tri_indices2 =
+      model2.tri_indices.get() ? model2.tri_indices->data() : NULL;
+
+  return true;
+}
+
+/// @brief Initialize traversal node for distance computation between two meshes
+template <typename BV>
+bool initialize(MeshDistanceTraversalNode<BV, 0>& node,
+                const BVHModel<BV>& model1, const Transform3s& tf1,
+                const BVHModel<BV>& model2, const Transform3s& tf2,
+                const DistanceRequest& request, DistanceResult& result) {
+  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
+    COAL_THROW_PRETTY(
+        "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
+        std::invalid_argument)
+  if (model2.getModelType() != BVH_MODEL_TRIANGLES)
+    COAL_THROW_PRETTY(
+        "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
+        std::invalid_argument)
+
+  node.request = request;
+  node.result = &result;
+
+  node.model1 = &model1;
+  node.tf1 = tf1;
+  node.model2 = &model2;
+  node.tf2 = tf2;
+
+  node.vertices1 = model1.vertices.get() ? model1.vertices->data() : NULL;
+  node.vertices2 = model2.vertices.get() ? model2.vertices->data() : NULL;
+
+  node.tri_indices1 =
+      model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
+  node.tri_indices2 =
+      model2.tri_indices.get() ? model2.tri_indices->data() : NULL;
+
+  COAL_COMPILER_DIAGNOSTIC_PUSH
+  COAL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
+
+  relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(),
+                    tf2.getTranslation(), node.RT.R, node.RT.T);
+
+  COAL_COMPILER_DIAGNOSTIC_POP
+
+  return true;
+}
+
+/// @brief Initialize traversal node for distance computation between one mesh
+/// and one shape, given the current transforms
+template <typename BV, typename S>
+bool initialize(MeshShapeDistanceTraversalNode<BV, S>& node,
+                BVHModel<BV>& model1, Transform3s& tf1, const S& model2,
+                const Transform3s& tf2, const GJKSolver* nsolver,
+                const DistanceRequest& request, DistanceResult& result,
+                bool use_refit = false, bool refit_bottomup = false) {
+  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
+    COAL_THROW_PRETTY(
+        "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
+        std::invalid_argument)
+
+  if (!tf1.isIdentity() && model1.vertices.get()) {
+    const std::vector<Vec3s>& model1_vertices_ = *(model1.vertices);
+    std::vector<Vec3s> vertices_transformed1(model1.num_vertices);
+    for (unsigned int i = 0; i < model1.num_vertices; ++i) {
+      const Vec3s& p = model1_vertices_[i];
+      Vec3s new_v = tf1.transform(p);
+      vertices_transformed1[i] = new_v;
+    }
+
+    model1.beginReplaceModel();
+    model1.replaceSubModel(vertices_transformed1);
+    model1.endReplaceModel(use_refit, refit_bottomup);
+
+    tf1.setIdentity();
+  }
+
+  node.request = request;
+  node.result = &result;
+
+  node.model1 = &model1;
+  node.tf1 = tf1;
+  node.model2 = &model2;
+  node.tf2 = tf2;
+  node.nsolver = nsolver;
+
+  node.vertices = model1.vertices.get() ? model1.vertices->data() : NULL;
+  node.tri_indices =
+      model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
+
+  computeBV(model2, tf2, node.model2_bv);
+
+  return true;
+}
+
+/// @cond IGNORE
+namespace details {
+
+template <typename BV, typename S, template <typename> class OrientedNode>
+static inline bool setupMeshShapeDistanceOrientedNode(
+    OrientedNode<S>& node, const BVHModel<BV>& model1, const Transform3s& tf1,
+    const S& model2, const Transform3s& tf2, const GJKSolver* nsolver,
+    const DistanceRequest& request, DistanceResult& result) {
+  if (model1.getModelType() != BVH_MODEL_TRIANGLES)
+    COAL_THROW_PRETTY(
+        "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
+        std::invalid_argument)
+
+  node.request = request;
+  node.result = &result;
+
+  node.model1 = &model1;
+  node.tf1 = tf1;
+  node.model2 = &model2;
+  node.tf2 = tf2;
+  node.nsolver = nsolver;
+
+  computeBV(model2, tf2, node.model2_bv);
+
+  node.vertices = model1.vertices.get() ? model1.vertices->data() : NULL;
+  node.tri_indices =
+      model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
+
+  return true;
+}
+}  // namespace details
+/// @endcond
+
+/// @brief Initialize traversal node for distance computation between one mesh
+/// and one shape, specialized for RSS type
+template <typename S>
+bool initialize(MeshShapeDistanceTraversalNodeRSS<S>& node,
+                const BVHModel<RSS>& model1, const Transform3s& tf1,
+                const S& model2, const Transform3s& tf2,
+                const GJKSolver* nsolver, const DistanceRequest& request,
+                DistanceResult& result) {
+  return details::setupMeshShapeDistanceOrientedNode(
+      node, model1, tf1, model2, tf2, nsolver, request, result);
+}
+
+/// @brief Initialize traversal node for distance computation between one mesh
+/// and one shape, specialized for kIOS type
+template <typename S>
+bool initialize(MeshShapeDistanceTraversalNodekIOS<S>& node,
+                const BVHModel<kIOS>& model1, const Transform3s& tf1,
+                const S& model2, const Transform3s& tf2,
+                const GJKSolver* nsolver, const DistanceRequest& request,
+                DistanceResult& result) {
+  return details::setupMeshShapeDistanceOrientedNode(
+      node, model1, tf1, model2, tf2, nsolver, request, result);
+}
+
+/// @brief Initialize traversal node for distance computation between one mesh
+/// and one shape, specialized for OBBRSS type
+template <typename S>
+bool initialize(MeshShapeDistanceTraversalNodeOBBRSS<S>& node,
+                const BVHModel<OBBRSS>& model1, const Transform3s& tf1,
+                const S& model2, const Transform3s& tf2,
+                const GJKSolver* nsolver, const DistanceRequest& request,
+                DistanceResult& result) {
+  return details::setupMeshShapeDistanceOrientedNode(
+      node, model1, tf1, model2, tf2, nsolver, request, result);
+}
+
+}  // namespace coal
+
+/// @endcond
+
+#endif

include/fcl/ccd/interval_matrix.h → include/coal/internal/traversal_node_shapes.h

@@ -32,77 +32,93 @@
  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  *  POSSIBILITY OF SUCH DAMAGE.
  */
-// This code is based on code developed by Stephane Redon at UNC and Inria for the CATCH library: http://graphics.ewha.ac.kr/CATCH/
+
 /** \author Jia Pan */
 
+#ifndef COAL_TRAVERSAL_NODE_SHAPES_H
+#define COAL_TRAVERSAL_NODE_SHAPES_H
 
-#ifndef FCL_CCD_INTERVAL_MATRIX_H
-#define FCL_CCD_INTERVAL_MATRIX_H
+/// @cond INTERNAL
 
-#include "fcl/ccd/interval.h"
-#include "fcl/ccd/interval_vector.h"
-#include "fcl/math/matrix_3f.h"
+#include "coal/collision_data.h"
+#include "coal/BV/BV.h"
+#include "coal/shape/geometric_shapes_utility.h"
+#include "coal/internal/traversal_node_base.h"
+#include "coal/internal/shape_shape_func.h"
 
-namespace fcl
-{
+namespace coal {
 
-struct IMatrix3
-{
-  IVector3 v_[3];
+/// @addtogroup Traversal_For_Collision
+/// @{
 
-  IMatrix3();
-  IMatrix3(FCL_REAL v);
-  IMatrix3(const Matrix3f& m);
-  IMatrix3(FCL_REAL m[3][3][2]);
-  IMatrix3(FCL_REAL m[3][3]);
-  IMatrix3(Interval m[3][3]);
-  IMatrix3(const IVector3& v1, const IVector3& v2, const IVector3& v3);
+/// @brief Traversal node for collision between two shapes
+template <typename S1, typename S2>
+class COAL_DLLAPI ShapeCollisionTraversalNode
+    : public CollisionTraversalNodeBase {
+ public:
+  ShapeCollisionTraversalNode(const CollisionRequest& request)
+      : CollisionTraversalNodeBase(request) {
+    model1 = NULL;
+    model2 = NULL;
 
-  void setIdentity();
+    nsolver = NULL;
+  }
 
-  IVector3 getColumn(size_t i) const;
-  const IVector3& getRow(size_t i) const;
+  /// @brief BV culling test in one BVTT node
+  bool BVDisjoints(int, int, CoalScalar&) const {
+    COAL_THROW_PRETTY("Not implemented", std::runtime_error);
+  }
 
-  Vec3f getColumnLow(size_t i) const;
-  Vec3f getRowLow(size_t i) const;
+  /// @brief Intersection testing between leaves (two shapes)
+  void leafCollides(int, int, CoalScalar&) const {
+    ShapeShapeCollide<S1, S2>(this->model1, this->tf1, this->model2, this->tf2,
+                              this->nsolver, this->request, *(this->result));
+  }
 
-  Vec3f getColumnHigh(size_t i) const;
-  Vec3f getRowHigh(size_t i) const;
+  const S1* model1;
+  const S2* model2;
 
-  Matrix3f getLow() const;
-  Matrix3f getHigh() const;
+  const GJKSolver* nsolver;
+};
 
-  inline const Interval& operator () (size_t i, size_t j) const
-  {
-    return v_[i][j];
-  }
+/// @}
 
-  inline Interval& operator () (size_t i, size_t j)
-  {
-    return v_[i][j];
-  }
+/// @addtogroup Traversal_For_Distance
+/// @{
 
-  IMatrix3 operator + (const IMatrix3& m) const;
-  IMatrix3& operator += (const IMatrix3& m);
+/// @brief Traversal node for distance between two shapes
+template <typename S1, typename S2>
+class COAL_DLLAPI ShapeDistanceTraversalNode
+    : public DistanceTraversalNodeBase {
+ public:
+  ShapeDistanceTraversalNode() : DistanceTraversalNodeBase() {
+    model1 = NULL;
+    model2 = NULL;
 
-  IMatrix3 operator - (const IMatrix3& m) const;
-  IMatrix3& operator -= (const IMatrix3& m);
+    nsolver = NULL;
+  }
 
-  IVector3 operator * (const Vec3f& v) const;
-  IVector3 operator * (const IVector3& v) const;
-  IMatrix3 operator * (const IMatrix3& m) const;
-  IMatrix3 operator * (const Matrix3f& m) const;
+  /// @brief BV culling test in one BVTT node
+  CoalScalar BVDistanceLowerBound(unsigned int, unsigned int) const {
+    return -1;  // should not be used
+  }
 
-  IMatrix3& operator *= (const IMatrix3& m);
-  IMatrix3& operator *= (const Matrix3f& m);
+  /// @brief Distance testing between leaves (two shapes)
+  void leafComputeDistance(unsigned int, unsigned int) const {
+    ShapeShapeDistance<S1, S2>(this->model1, this->tf1, this->model2, this->tf2,
+                               this->nsolver, this->request, *(this->result));
+  }
 
-  IMatrix3& rotationConstrain();
+  const S1* model1;
+  const S2* model2;
 
-  void print() const;
+  const GJKSolver* nsolver;
 };
 
-IMatrix3 rotationConstrain(const IMatrix3& m);
+/// @}
+
+}  // namespace coal
 
-}
+/// @endcond
 
 #endif

include/fcl/traversal/traversal_recurse.h → include/coal/internal/traversal_recurse.h

@@ -35,46 +35,47 @@
 
 /** \author Jia Pan */
 
+#ifndef COAL_TRAVERSAL_RECURSE_H
+#define COAL_TRAVERSAL_RECURSE_H
 
-#ifndef FCL_TRAVERSAL_RECURSE_H
-#define FCL_TRAVERSAL_RECURSE_H
+/// @cond INTERNAL
 
-#include "fcl/traversal/traversal_node_base.h"
-#include "fcl/traversal/traversal_node_bvhs.h"
-#include "fcl/BVH/BVH_front.h"
+#include "coal/BVH/BVH_front.h"
+#include "coal/internal/traversal_node_base.h"
+#include "coal/internal/traversal_node_bvhs.h"
 #include <queue>
 
-namespace fcl
-{
+namespace coal {
 
 /// Recurse function for collision
-/// \param node collision node,
-/// \param b1, b2 ids of bounding volume nodes for object 1 and object 2
-/// \retval sqrDistLowerBound squared lower bound on distance between objects.
-void collisionRecurse(CollisionTraversalNodeBase* node, int b1, int b2,
-		      BVHFrontList* front_list, FCL_REAL& sqrDistLowerBound);
+/// @param node collision node,
+/// @param b1, b2 ids of bounding volume nodes for object 1 and object 2
+/// @retval sqrDistLowerBound squared lower bound on distance between objects.
+void collisionRecurse(CollisionTraversalNodeBase* node, unsigned int b1,
+                      unsigned int b2, BVHFrontList* front_list,
+                      CoalScalar& sqrDistLowerBound);
 
-/// @brief Recurse function for collision, specialized for OBB type
-void collisionRecurse(MeshCollisionTraversalNodeOBB* node, int b1, int b2, const Matrix3f& R, const Vec3f& T, BVHFrontList* front_list);
-
-/// @brief Recurse function for collision, specialized for RSS type
-void collisionRecurse(MeshCollisionTraversalNodeRSS* node, int b1, int b2, const Matrix3f& R, const Vec3f& T, BVHFrontList* front_list);
-
-/// @brief Recurse function for self collision. Make sure node is set correctly so that the first and second tree are the same
-void selfCollisionRecurse(CollisionTraversalNodeBase* node, int b, BVHFrontList* front_list);
+void collisionNonRecurse(CollisionTraversalNodeBase* node,
+                         BVHFrontList* front_list,
+                         CoalScalar& sqrDistLowerBound);
 
 /// @brief Recurse function for distance
-void distanceRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list);
+void distanceRecurse(DistanceTraversalNodeBase* node, unsigned int b1,
+                     unsigned int b2, BVHFrontList* front_list);
 
 /// @brief Recurse function for distance, using queue acceleration
-void distanceQueueRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list, int qsize);
+void distanceQueueRecurse(DistanceTraversalNodeBase* node, unsigned int b1,
+                          unsigned int b2, BVHFrontList* front_list,
+                          unsigned int qsize);
 
 /// @brief Recurse function for front list propagation
-void propagateBVHFrontListCollisionRecurse
-  (CollisionTraversalNodeBase* node, BVHFrontList* front_list,
-   FCL_REAL& sqrDistLowerBound);
+void propagateBVHFrontListCollisionRecurse(CollisionTraversalNodeBase* node,
+                                           const CollisionRequest& request,
+                                           CollisionResult& result,
+                                           BVHFrontList* front_list);
 
+}  // namespace coal
 
-}
+/// @endcond
 
 #endif