include/coal/internal/traversal_node_base.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_BASE_H
+#define COAL_TRAVERSAL_NODE_BASE_H
+
+/// @cond INTERNAL
+
+#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:
+  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(unsigned int /*b*/) const { return true; }
+
+  /// @brief Whether b is a leaf node in the second BVH tree
+  virtual bool isSecondNodeLeaf(unsigned int /*b*/) const { return true; }
+
+  /// @brief Traverse the subtree of the node in the first tree first
+  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(unsigned int b) const { return (int)b; }
+
+  /// @brief Get the right child of the node b in the first tree
+  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(unsigned int b) const { return (int)b; }
+
+  /// @brief Get the right child of the node b in the second tree
+  virtual int getSecondRightChild(unsigned int b) const { return (int)b; }
+
+  /// @brief Whether store some statistics information during traversal
+  void enableStatistics(bool enable) { enable_statistics = enable; }
+
+  /// @brief configuation of first object
+  Transform3s tf1;
+
+  /// @brief configuration of second object
+  Transform3s tf2;
+
+  /// @brief Whether stores statistics
+  bool enable_statistics;
+};
+
+/// @defgroup Traversal_For_Collision
+/// regroup class about traversal for distance.
+/// @{
+
+/// @brief Node structure encoding the information required for collision
+/// traversal.
+class CollisionTraversalNodeBase : public TraversalNodeBase {
+ public:
+  CollisionTraversalNodeBase(const CollisionRequest& request_)
+      : request(request_), result(NULL) {}
+
+  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
+  ///         distance between bounding volumes.
+  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 leafCollides(unsigned int /*b1*/, unsigned int /*b2*/,
+                            CoalScalar& /*sqrDistLowerBound*/) const = 0;
+
+  /// @brief Check whether the traversal can stop
+  bool canStop() const { return this->request.isSatisfied(*(this->result)); }
+
+  /// @brief request setting for collision
+  const CollisionRequest& request;
+
+  /// @brief collision result kept during the traversal iteration
+  CollisionResult* result;
+};
+
+/// @}
+
+/// @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) {}
+
+  virtual ~DistanceTraversalNodeBase() {}
+
+  /// @brief BV test between b1 and b2
+  /// @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 leafComputeDistance(unsigned int b1, unsigned int b2) const = 0;
+
+  /// @brief Check whether the traversal can stop
+  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;
+};
+
+///@}
+
+}  // 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/coal/internal/traversal_node_shapes.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_SHAPES_H
+#define COAL_TRAVERSAL_NODE_SHAPES_H
+
+/// @cond INTERNAL
+
+#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 coal {
+
+/// @addtogroup Traversal_For_Collision
+/// @{
+
+/// @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;
+
+    nsolver = NULL;
+  }
+
+  /// @brief BV culling test in one BVTT node
+  bool BVDisjoints(int, int, CoalScalar&) const {
+    COAL_THROW_PRETTY("Not implemented", std::runtime_error);
+  }
+
+  /// @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));
+  }
+
+  const S1* model1;
+  const S2* model2;
+
+  const GJKSolver* nsolver;
+};
+
+/// @}
+
+/// @addtogroup Traversal_For_Distance
+/// @{
+
+/// @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;
+
+    nsolver = NULL;
+  }
+
+  /// @brief BV culling test in one BVTT node
+  CoalScalar BVDistanceLowerBound(unsigned int, unsigned int) const {
+    return -1;  // should not be used
+  }
+
+  /// @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));
+  }
+
+  const S1* model1;
+  const S2* model2;
+
+  const GJKSolver* nsolver;
+};
+
+/// @}
+
+}  // namespace coal
+
+/// @endcond
+
+#endif

src/distance_capsule_plane.cpp → include/coal/internal/traversal_recurse.h

@@ -3,7 +3,6 @@
  *
  *  Copyright (c) 2011-2014, Willow Garage, Inc.
  *  Copyright (c) 2014-2015, Open Source Robotics Foundation
- *  Copyright (c) 2018-2019, Center National de la Recherche Scientifique
  *  All rights reserved.
  *
  *  Redistribution and use in source and binary forms, with or without
@@ -34,52 +33,49 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  */
 
-/** \author Florent Lamiraux */
+/** \author Jia Pan */
 
-#include <cmath>
-#include <limits>
-#include <hpp/fcl/math/transform.h>
-#include <hpp/fcl/shape/geometric_shapes.h>
-#include <../src/distance_func_matrix.h>
-#include "narrowphase/details.h"
+#ifndef COAL_TRAVERSAL_RECURSE_H
+#define COAL_TRAVERSAL_RECURSE_H
 
-namespace hpp
-{
-namespace fcl {
-    class GJKSolver;
+/// @cond INTERNAL
 
-  template <>
-  FCL_REAL ShapeShapeDistance <Capsule, Plane>
-  (const CollisionGeometry* o1, const Transform3f& tf1,
-   const CollisionGeometry* o2, const Transform3f& tf2,
-   const GJKSolver*, const DistanceRequest&,
-   DistanceResult& result)
-  {
-    const Capsule& s1 = static_cast <const Capsule&> (*o1);
-    const Plane& s2 = static_cast <const Plane&> (*o2);
-    details::capsulePlaneIntersect
-      (s1, tf1, s2, tf2, result.min_distance, result.nearest_points [0],
-       result.nearest_points [1], result.normal);
-    result.o1 = o1; result.o2 = o2; result.b1 = -1; result.b2 = -1;
-    return result.min_distance;
-  }
+#include "coal/BVH/BVH_front.h"
+#include "coal/internal/traversal_node_base.h"
+#include "coal/internal/traversal_node_bvhs.h"
+#include <queue>
 
-  template <>
-  FCL_REAL ShapeShapeDistance <Plane, Capsule>
-  (const CollisionGeometry* o1, const Transform3f& tf1,
-   const CollisionGeometry* o2, const Transform3f& tf2,
-   const GJKSolver*, const DistanceRequest&,
-   DistanceResult& result)
-  {
-    const Plane& s1 = static_cast <const Plane&> (*o1);
-    const Capsule& s2 = static_cast <const Capsule&> (*o2);
-    details::capsulePlaneIntersect
-      (s2, tf2, s1, tf1, result.min_distance, result.nearest_points [1],
-       result.nearest_points [0], result.normal);
-    result.o1 = o1; result.o2 = o2; result.b1 = -1; result.b2 = -1;
-    result.normal = -result.normal;
-    return result.min_distance;
-  }
-} // namespace fcl
+namespace coal {
 
-} // namespace hpp
+/// 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, unsigned int b1,
+                      unsigned int b2, BVHFrontList* front_list,
+                      CoalScalar& sqrDistLowerBound);
+
+void collisionNonRecurse(CollisionTraversalNodeBase* node,
+                         BVHFrontList* front_list,
+                         CoalScalar& sqrDistLowerBound);
+
+/// @brief Recurse function for distance
+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, unsigned int b1,
+                          unsigned int b2, BVHFrontList* front_list,
+                          unsigned int qsize);
+
+/// @brief Recurse function for front list propagation
+void propagateBVHFrontListCollisionRecurse(CollisionTraversalNodeBase* node,
+                                           const CollisionRequest& request,
+                                           CollisionResult& result,
+                                           BVHFrontList* front_list);
+
+}  // namespace coal
+
+/// @endcond
+
+#endif

include/coal/logging.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 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.
+ */
+
+/// This file defines basic logging macros for Coal, based on Boost.Log.
+/// To enable logging, define the preprocessor macro `COAL_ENABLE_LOGGING`.
+
+#ifndef COAL_LOGGING_H
+#define COAL_LOGGING_H
+
+#ifdef COAL_ENABLE_LOGGING
+#include <boost/log/trivial.hpp>
+#define COAL_LOG_INFO(message) BOOST_LOG_TRIVIAL(info) << message
+#define COAL_LOG_DEBUG(message) BOOST_LOG_TRIVIAL(debug) << message
+#define COAL_LOG_WARNING(message) BOOST_LOG_TRIVIAL(warning) << message
+#define COAL_LOG_ERROR(message) BOOST_LOG_TRIVIAL(error) << message
+#else
+#define COAL_LOG_INFO(message)
+#define COAL_LOG_DEBUG(message)
+#define COAL_LOG_WARNING(message)
+#define COAL_LOG_ERROR(message)
+#endif
+
+#endif  // COAL_LOGGING_H

src/traversal/traversal_node_base.cpp → include/coal/math/matrix_3f.h

@@ -35,78 +35,12 @@
 
 /** \author Jia Pan */
 
+#ifndef COAL_MATRIX_3F_H
+#define COAL_MATRIX_3F_H
 
-#include <hpp/fcl/internal/traversal_node_base.h>
-#include <limits>
+#warning "This file is deprecated. Include <coal/data_types.h> instead."
 
-namespace hpp
-{
-namespace fcl
-{
+// List of equivalent includes.
+#include "coal/data_types.h"
 
-TraversalNodeBase::~TraversalNodeBase()
-{
-}
-
-  bool TraversalNodeBase::isFirstNodeLeaf(int /*b*/) const
-{
-  return true;
-}
-
-  bool TraversalNodeBase::isSecondNodeLeaf(int /*b*/) const
-{
-  return true;
-}
-
-  bool TraversalNodeBase::firstOverSecond(int /*b1*/, int /*b2*/) const
-{
-  return true;
-}
-
-int TraversalNodeBase::getFirstLeftChild(int b) const
-{
-  return b;
-}
-
-int TraversalNodeBase::getFirstRightChild(int b) const
-{
-  return b;
-}
-
-int TraversalNodeBase::getSecondLeftChild(int b) const
-{
-  return b;
-}
-
-int TraversalNodeBase::getSecondRightChild(int b) const
-{
-  return b;
-}
-
-CollisionTraversalNodeBase::~CollisionTraversalNodeBase()
-{
-}
-
-bool CollisionTraversalNodeBase::canStop() const
-{
-  return false;
-}
-
-
-DistanceTraversalNodeBase::~DistanceTraversalNodeBase()
-{
-}
-
-  FCL_REAL DistanceTraversalNodeBase::BVDistanceLowerBound(int /*b1*/, int /*b2*/) const
-{
-  return std::numeric_limits<FCL_REAL>::max();
-}
-
-  bool DistanceTraversalNodeBase::canStop(FCL_REAL /*c*/) const
-{
-  return false;
-}
-
-}
-
-} // namespace hpp
+#endif

include/coal/math/transform.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_TRANSFORM_H
+#define COAL_TRANSFORM_H
+
+#include "coal/fwd.hh"
+#include "coal/data_types.h"
+
+namespace coal {
+
+COAL_DEPRECATED typedef Eigen::Quaternion<CoalScalar> Quaternion3f;
+typedef Eigen::Quaternion<CoalScalar> Quatf;
+
+static inline std::ostream& operator<<(std::ostream& o, const Quatf& q) {
+  o << "(" << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << ")";
+  return o;
+}
+
+/// @brief Simple transform class used locally by InterpMotion
+class COAL_DLLAPI Transform3s {
+  /// @brief Matrix cache
+  Matrix3s R;
+
+  /// @brief Translation vector
+  Vec3s T;
+
+ public:
+  /// @brief Default transform is no movement
+  Transform3s() {
+    setIdentity();  // set matrix_set true
+  }
+
+  static Transform3s Identity() { return Transform3s(); }
+
+  /// @brief Construct transform from rotation and translation
+  template <typename Matrixx3Like, typename Vector3Like>
+  Transform3s(const Eigen::MatrixBase<Matrixx3Like>& R_,
+              const Eigen::MatrixBase<Vector3Like>& T_)
+      : R(R_), T(T_) {}
+
+  /// @brief Construct transform from rotation and translation
+  template <typename Vector3Like>
+  Transform3s(const Quatf& q_, const Eigen::MatrixBase<Vector3Like>& T_)
+      : R(q_.toRotationMatrix()), T(T_) {}
+
+  /// @brief Construct transform from rotation
+  Transform3s(const Matrix3s& R_) : R(R_), T(Vec3s::Zero()) {}
+
+  /// @brief Construct transform from rotation
+  Transform3s(const Quatf& q_) : R(q_), T(Vec3s::Zero()) {}
+
+  /// @brief Construct transform from translation
+  Transform3s(const Vec3s& T_) : R(Matrix3s::Identity()), T(T_) {}
+
+  /// @brief Construct transform from other transform
+  Transform3s(const Transform3s& tf) : R(tf.R), T(tf.T) {}
+
+  /// @brief operator =
+  Transform3s& operator=(const Transform3s& tf) {
+    R = tf.R;
+    T = tf.T;
+    return *this;
+  }
+
+  /// @brief get translation
+  inline const Vec3s& getTranslation() const { return T; }
+
+  /// @brief get translation
+  inline const Vec3s& translation() const { return T; }
+
+  /// @brief get translation
+  inline Vec3s& translation() { return T; }
+
+  /// @brief get rotation
+  inline const Matrix3s& getRotation() const { return R; }
+
+  /// @brief get rotation
+  inline const Matrix3s& rotation() const { return R; }
+
+  /// @brief get rotation
+  inline Matrix3s& rotation() { return R; }
+
+  /// @brief get quaternion
+  inline Quatf getQuatRotation() const { return Quatf(R); }
+
+  /// @brief set transform from rotation and translation
+  template <typename Matrix3Like, typename Vector3Like>
+  inline void setTransform(const Eigen::MatrixBase<Matrix3Like>& R_,
+                           const Eigen::MatrixBase<Vector3Like>& T_) {
+    R.noalias() = R_;
+    T.noalias() = T_;
+  }
+
+  /// @brief set transform from rotation and translation
+  inline void setTransform(const Quatf& q_, const Vec3s& T_) {
+    R = q_.toRotationMatrix();
+    T = T_;
+  }
+
+  /// @brief set transform from rotation
+  template <typename Derived>
+  inline void setRotation(const Eigen::MatrixBase<Derived>& R_) {
+    R.noalias() = R_;
+  }
+
+  /// @brief set transform from translation
+  template <typename Derived>
+  inline void setTranslation(const Eigen::MatrixBase<Derived>& T_) {
+    T.noalias() = T_;
+  }
+
+  /// @brief set transform from rotation
+  inline void setQuatRotation(const Quatf& q_) { R = q_.toRotationMatrix(); }
+
+  /// @brief transform a given vector by the transform
+  template <typename Derived>
+  inline Vec3s transform(const Eigen::MatrixBase<Derived>& v) const {
+    return R * v + T;
+  }
+
+  /// @brief transform a given vector by the inverse of the transform
+  template <typename Derived>
+  inline Vec3s inverseTransform(const Eigen::MatrixBase<Derived>& v) const {
+    return R.transpose() * (v - T);
+  }
+
+  /// @brief inverse transform
+  inline Transform3s& inverseInPlace() {
+    R.transposeInPlace();
+    T = -R * T;
+    return *this;
+  }
+
+  /// @brief inverse transform
+  inline Transform3s inverse() {
+    return Transform3s(R.transpose(), -R.transpose() * T);
+  }
+
+  /// @brief inverse the transform and multiply with another
+  inline Transform3s inverseTimes(const Transform3s& other) const {
+    return Transform3s(R.transpose() * other.R, R.transpose() * (other.T - T));
+  }
+
+  /// @brief multiply with another transform
+  inline const Transform3s& operator*=(const Transform3s& other) {
+    T += R * other.T;
+    R *= other.R;
+    return *this;
+  }
+
+  /// @brief multiply with another transform
+  inline Transform3s operator*(const Transform3s& other) const {
+    return Transform3s(R * other.R, R * other.T + T);
+  }
+
+  /// @brief check whether the transform is identity
+  inline bool isIdentity(
+      const CoalScalar& prec =
+          Eigen::NumTraits<CoalScalar>::dummy_precision()) const {
+    return R.isIdentity(prec) && T.isZero(prec);
+  }
+
+  /// @brief set the transform to be identity transform
+  inline void setIdentity() {
+    R.setIdentity();
+    T.setZero();
+  }
+
+  /// @brief return a random transform
+  static Transform3s Random() {
+    Transform3s tf = Transform3s();
+    tf.setRandom();
+    return tf;
+  }
+
+  /// @brief set the transform to a random transform
+  inline void setRandom();
+
+  bool operator==(const Transform3s& other) const {
+    return (R == other.getRotation()) && (T == other.getTranslation());
+  }
+
+  bool operator!=(const Transform3s& other) const { return !(*this == other); }
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+template <typename Derived>
+inline Quatf fromAxisAngle(const Eigen::MatrixBase<Derived>& axis,
+                           CoalScalar angle) {
+  return Quatf(Eigen::AngleAxis<CoalScalar>(angle, axis));
+}
+
+/// @brief Uniformly random quaternion sphere.
+/// Code taken from Pinocchio (https://github.com/stack-of-tasks/pinocchio).
+inline Quatf uniformRandomQuaternion() {
+  // Rotational part
+  const CoalScalar u1 = (CoalScalar)rand() / RAND_MAX;
+  const CoalScalar u2 = (CoalScalar)rand() / RAND_MAX;
+  const CoalScalar u3 = (CoalScalar)rand() / RAND_MAX;
+
+  const CoalScalar mult1 = std::sqrt(CoalScalar(1.0) - u1);
+  const CoalScalar mult2 = std::sqrt(u1);
+
+  static const CoalScalar PI_value = static_cast<CoalScalar>(EIGEN_PI);
+  CoalScalar s2 = std::sin(2 * PI_value * u2);
+  CoalScalar c2 = std::cos(2 * PI_value * u2);
+  CoalScalar s3 = std::sin(2 * PI_value * u3);
+  CoalScalar c3 = std::cos(2 * PI_value * u3);
+
+  Quatf q;
+  q.w() = mult1 * s2;
+  q.x() = mult1 * c2;
+  q.y() = mult2 * s3;
+  q.z() = mult2 * c3;
+  return q;
+}
+
+inline void Transform3s::setRandom() {
+  const Quatf q = uniformRandomQuaternion();
+  this->rotation() = q.matrix();
+  this->translation().setRandom();
+}
+
+/// @brief Construct othonormal basis from vector.
+/// The z-axis is the normalized input vector.
+inline Matrix3s constructOrthonormalBasisFromVector(const Vec3s& vec) {
+  Matrix3s basis = Matrix3s::Zero();
+  basis.col(2) = vec.normalized();
+  basis.col(1) = -vec.unitOrthogonal();
+  basis.col(0) = basis.col(1).cross(vec);
+  return basis;
+}
+
+}  // namespace coal
+
+#endif

include/coal/math/types.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_MATH_TYPES_H
+#define COAL_MATH_TYPES_H
+
+#warning "This file is deprecated. Include <coal/data_types.h> instead."
+
+// List of equivalent includes.
+#include "coal/data_types.h"
+
+#endif

include/coal/math/vec_3f.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_VEC_3F_H
+#define COAL_VEC_3F_H
+
+#warning "This file is deprecated. Include <coal/data_types.h> instead."
+
+// List of equivalent includes.
+#include "coal/data_types.h"
+
+#endif

src/traversal/traversal_node_setup.cpp → include/coal/mesh_loader/assimp.h

@@ -3,6 +3,8 @@
  *
  *  Copyright (c) 2011-2014, Willow Garage, Inc.
  *  Copyright (c) 2014-2015, Open Source Robotics Foundation
+ *  Copyright (c) 2016-2019, CNRS - LAAS
+ *  Copyright (c) 2019, INRIA
  *  All rights reserved.
  *
  *  Redistribution and use in source and binary forms, with or without
@@ -33,85 +35,93 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  */
 
-/** \author Jia Pan */
+#ifndef COAL_MESH_LOADER_ASSIMP_H
+#define COAL_MESH_LOADER_ASSIMP_H
 
+#include "coal/fwd.hh"
+#include "coal/config.hh"
+#include "coal/BV/OBBRSS.h"
+#include "coal/BVH/BVH_model.h"
 
-#include <hpp/fcl/internal/traversal_node_setup.h>
-#include <hpp/fcl/internal/tools.h>
-namespace hpp
-{
-namespace fcl
-{
+struct aiScene;
+namespace Assimp {
+class Importer;
+}
 
-namespace details
-{
-template<typename BV, typename OrientedNode>
-static inline bool setupMeshDistanceOrientedNode(OrientedNode& node,
-                                                 const BVHModel<BV>& model1, const Transform3f& tf1,
-                                                 const BVHModel<BV>& model2, const Transform3f& tf2,
-                                                 const DistanceRequest& request,
-                                                 DistanceResult& result)
-{
-  if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
-    return false;
+namespace coal {
 
-  node.request = request;
-  node.result = &result;
+namespace internal {
 
-  node.model1 = &model1;
-  node.tf1 = tf1;
-  node.model2 = &model2;
-  node.tf2 = tf2;
+struct COAL_DLLAPI TriangleAndVertices {
+  std::vector<coal::Vec3s> vertices_;
+  std::vector<coal::Triangle> triangles_;
+};
 
-  node.vertices1 = model1.vertices;
-  node.vertices2 = model2.vertices;
+struct COAL_DLLAPI Loader {
+  Loader();
+  ~Loader();
 
-  node.tri_indices1 = model1.tri_indices;
-  node.tri_indices2 = model2.tri_indices;
+  void load(const std::string& resource_path);
 
-  relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation(), node.R, node.T);
+  Assimp::Importer* importer;
+  aiScene const* scene;
+};
 
-  return true;
-}
+/**
+ * @brief      Recursive procedure for building a mesh
+ *
+ * @param[in]  scale           Scale to apply when reading the ressource
+ * @param[in]  scene           Pointer to the assimp scene
+ * @param[in]  vertices_offset Current number of vertices in the model
+ * @param      tv              Triangles and Vertices of the mesh submodels
+ */
+COAL_DLLAPI void buildMesh(const coal::Vec3s& scale, const aiScene* scene,
+                           unsigned vertices_offset, TriangleAndVertices& tv);
 
+/**
+ * @brief      Convert an assimp scene to a mesh
+ *
+ * @param[in]  scale  Scale to apply when reading the ressource
+ * @param[in]  scene  Pointer to the assimp scene
+ * @param[out] mesh  The mesh that must be built
+ */
+template <class BoundingVolume>
+inline void meshFromAssimpScene(
+    const coal::Vec3s& scale, const aiScene* scene,
+    const shared_ptr<BVHModel<BoundingVolume> >& mesh) {
+  TriangleAndVertices tv;
 
-}
+  int res = mesh->beginModel();
 
-bool initialize(MeshDistanceTraversalNodeRSS& node,
-                const BVHModel<RSS>& model1, const Transform3f& tf1,
-                const BVHModel<RSS>& model2, const Transform3f& tf2,
-                const DistanceRequest& request,
-                DistanceResult& result)
-{
-  return details::setupMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, request, result);
-}
+  if (res != coal::BVH_OK) {
+    COAL_THROW_PRETTY("fcl BVHReturnCode = " << res, std::runtime_error);
+  }
 
+  buildMesh(scale, scene, (unsigned)mesh->num_vertices, tv);
+  mesh->addSubModel(tv.vertices_, tv.triangles_);
 
-bool initialize(MeshDistanceTraversalNodekIOS& node,
-                const BVHModel<kIOS>& model1, const Transform3f& tf1,
-                const BVHModel<kIOS>& model2, const Transform3f& tf2,
-                const DistanceRequest& request,
-                DistanceResult& result)
-{
-  return details::setupMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, request, result);
+  mesh->endModel();
 }
 
-bool initialize(MeshDistanceTraversalNodeOBBRSS& node,
-                const BVHModel<OBBRSS>& model1, const Transform3f& tf1,
-                const BVHModel<OBBRSS>& model2, const Transform3f& tf2,
-                const DistanceRequest& request,
-                DistanceResult& result)
-{
-  return details::setupMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, request, result);
-}
-
-namespace details
-{
-
+}  // namespace internal
 
+/**
+ * @brief      Read a mesh file and convert it to a polyhedral mesh
+ *
+ * @param[in]  resource_path  Path to the ressource mesh file to be read
+ * @param[in]  scale          Scale to apply when reading the ressource
+ * @param[out] polyhedron     The resulted polyhedron
+ */
+template <class BoundingVolume>
+inline void loadPolyhedronFromResource(
+    const std::string& resource_path, const coal::Vec3s& scale,
+    const shared_ptr<BVHModel<BoundingVolume> >& polyhedron) {
+  internal::Loader scene;
+  scene.load(resource_path);
+
+  internal::meshFromAssimpScene(scale, scene.scene, polyhedron);
 }
 
+}  // namespace coal
 
-}
-
-} // namespace hpp
+#endif  // COAL_MESH_LOADER_ASSIMP_H

src/distance_capsule_halfspace.cpp → include/coal/mesh_loader/loader.h

@@ -3,7 +3,8 @@
  *
  *  Copyright (c) 2011-2014, Willow Garage, Inc.
  *  Copyright (c) 2014-2015, Open Source Robotics Foundation
- *  Copyright (c) 2018-2019, Center National de la Recherche Scientifique
+ *  Copyright (c) 2016, CNRS - LAAS
+ *  Copyright (c) 2020, INRIA
  *  All rights reserved.
  *
  *  Redistribution and use in source and binary forms, with or without
@@ -34,52 +35,68 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  */
 
-/** \author Florent Lamiraux */
-
-#include <cmath>
-#include <limits>
-#include <hpp/fcl/math/transform.h>
-#include <hpp/fcl/shape/geometric_shapes.h>
-#include <../src/distance_func_matrix.h>
-#include "narrowphase/details.h"
-
-namespace hpp
-{
-namespace fcl {
-    class GJKSolver;
-
-  template <>
-  FCL_REAL ShapeShapeDistance <Capsule, Halfspace>
-  (const CollisionGeometry* o1, const Transform3f& tf1,
-   const CollisionGeometry* o2, const Transform3f& tf2,
-   const GJKSolver*, const DistanceRequest&,
-   DistanceResult& result)
-  {
-    const Capsule& s1 = static_cast <const Capsule&> (*o1);
-    const Halfspace& s2 = static_cast <const Halfspace&> (*o2);
-    details::capsuleHalfspaceIntersect
-      (s1, tf1, s2, tf2, result.min_distance, result.nearest_points [0],
-       result.nearest_points [1], result.normal);
-    result.o1 = o1; result.o2 = o2; result.b1 = -1; result.b2 = -1;
-    return result.min_distance;
-  }
-
-  template <>
-  FCL_REAL ShapeShapeDistance <Halfspace, Capsule>
-  (const CollisionGeometry* o1, const Transform3f& tf1,
-   const CollisionGeometry* o2, const Transform3f& tf2,
-   const GJKSolver*, const DistanceRequest&,
-   DistanceResult& result)
-  {
-    const Halfspace& s1 = static_cast <const Halfspace&> (*o1);
-    const Capsule& s2 = static_cast <const Capsule&> (*o2);
-    details::capsuleHalfspaceIntersect
-      (s2, tf2, s1, tf1, result.min_distance, result.nearest_points [1],
-       result.nearest_points [0], result.normal);
-    result.o1 = o1; result.o2 = o2; result.b1 = -1; result.b2 = -1;
-    result.normal = -result.normal;
-    return result.min_distance;
-  }
-} // namespace fcl
-
-} // namespace hpp
+#ifndef COAL_MESH_LOADER_LOADER_H
+#define COAL_MESH_LOADER_LOADER_H
+
+#include "coal/fwd.hh"
+#include "coal/config.hh"
+#include "coal/data_types.h"
+#include "coal/collision_object.h"
+
+#include <map>
+#include <ctime>
+
+namespace coal {
+/// Base class for building polyhedron from files.
+/// This class builds a new object for each file.
+class COAL_DLLAPI MeshLoader {
+ public:
+  virtual ~MeshLoader() {}
+
+  virtual BVHModelPtr_t load(const std::string& filename,
+                             const Vec3s& scale = Vec3s::Ones());
+
+  /// Create an OcTree from a file in binary octomap format.
+  /// \todo add OctreePtr_t
+  virtual CollisionGeometryPtr_t loadOctree(const std::string& filename);
+
+  MeshLoader(const NODE_TYPE& bvType = BV_OBBRSS) : bvType_(bvType) {}
+
+ private:
+  const NODE_TYPE bvType_;
+};
+
+/// Class for building polyhedron from files with cache mechanism.
+/// This class builds a new object for each different file.
+/// If method CachedMeshLoader::load is called twice with the same arguments,
+/// the second call returns the result of the first call.
+class COAL_DLLAPI CachedMeshLoader : public MeshLoader {
+ public:
+  virtual ~CachedMeshLoader() {}
+
+  CachedMeshLoader(const NODE_TYPE& bvType = BV_OBBRSS) : MeshLoader(bvType) {}
+
+  virtual BVHModelPtr_t load(const std::string& filename, const Vec3s& scale);
+
+  struct COAL_DLLAPI Key {
+    std::string filename;
+    Vec3s scale;
+
+    Key(const std::string& f, const Vec3s& s) : filename(f), scale(s) {}
+
+    bool operator<(const CachedMeshLoader::Key& b) const;
+  };
+  struct COAL_DLLAPI Value {
+    BVHModelPtr_t model;
+    std::time_t mtime;
+  };
+  typedef std::map<Key, Value> Cache_t;
+
+  const Cache_t& cache() const { return cache_; }
+
+ private:
+  Cache_t cache_;
+};
+}  // namespace coal
+
+#endif  // COAL_MESH_LOADER_LOADER_H

include/coal/narrowphase/gjk.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, Open Source Robotics Foundation
+ *  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_GJK_H
+#define COAL_GJK_H
+
+#include <vector>
+
+#include "coal/narrowphase/minkowski_difference.h"
+
+namespace coal {
+
+namespace details {
+
+/// @brief class for GJK algorithm
+///
+/// @note The computations are performed in the frame of the first shape.
+struct COAL_DLLAPI GJK {
+  struct COAL_DLLAPI SimplexV {
+    /// @brief support vector for shape 0 and 1.
+    Vec3s w0, w1;
+    /// @brief support vector (i.e., the furthest point on the shape along the
+    /// support direction)
+    Vec3s w;
+  };
+
+  typedef unsigned char vertex_id_t;
+
+  /// @brief A simplex is a set of up to 4 vertices.
+  /// Its rank is the number of vertices it contains.
+  /// @note This data structure does **not** own the vertices it refers to.
+  /// To be efficient, the constructor of `GJK` creates storage for 4 vertices.
+  /// Since GJK does not need any more storage, it reuses these vertices
+  /// throughout the algorithm by using multiple instance of this `Simplex`
+  /// class.
+  struct COAL_DLLAPI Simplex {
+    /// @brief simplex vertex
+    SimplexV* vertex[4];
+    /// @brief size of simplex (number of vertices)
+    vertex_id_t rank;
+
+    Simplex() {}
+
+    void reset() {
+      rank = 0;
+      for (size_t i = 0; i < 4; ++i) vertex[i] = nullptr;
+    }
+  };
+
+  /// @brief Status of the GJK algorithm:
+  /// DidNotRun: GJK has not been run.
+  /// Failed: GJK did not converge (it exceeded the maximum number of
+  /// iterations).
+  /// NoCollisionEarlyStopped: GJK found a separating hyperplane and exited
+  ///     before converting. The shapes are not in collision.
+  /// NoCollision: GJK converged and the shapes are not in collision.
+  /// Collision: GJK converged and the shapes are in collision.
+  /// Failed: GJK did not converge.
+  enum Status {
+    DidNotRun,
+    Failed,
+    NoCollisionEarlyStopped,
+    NoCollision,
+    CollisionWithPenetrationInformation,
+    Collision
+  };
+
+ public:
+  CoalScalar distance_upper_bound;
+  Status status;
+  GJKVariant gjk_variant;
+  GJKConvergenceCriterion convergence_criterion;
+  GJKConvergenceCriterionType convergence_criterion_type;
+
+  MinkowskiDiff const* shape;
+  Vec3s ray;
+  support_func_guess_t support_hint;
+  /// @brief The distance between the two shapes, computed by GJK.
+  /// If the distance is below GJK's threshold, the shapes are in collision in
+  /// the eyes of GJK. If `distance_upper_bound` is set to a value lower than
+  /// infinity, GJK will early stop as soon as it finds `distance` to be greater
+  /// than `distance_upper_bound`.
+  CoalScalar distance;
+  Simplex* simplex;  // Pointer to the result of the last run of GJK.
+
+ private:
+  // max_iteration and tolerance are made private
+  // because they are meant to be set by the `reset` function.
+  size_t max_iterations;
+  CoalScalar tolerance;
+
+  SimplexV store_v[4];
+  SimplexV* free_v[4];
+  vertex_id_t nfree;
+  vertex_id_t current;
+  Simplex simplices[2];
+  size_t iterations;
+  size_t iterations_momentum_stop;
+
+ public:
+  /// \param max_iterations_ number of iteration before GJK returns failure.
+  /// \param tolerance_ precision of the algorithm.
+  ///
+  /// The tolerance argument is useful for continuous shapes and for polyhedron
+  /// with some vertices closer than this threshold.
+  ///
+  /// Suggested values are 100 iterations and a tolerance of 1e-6.
+  GJK(size_t max_iterations_, CoalScalar tolerance_)
+      : max_iterations(max_iterations_), tolerance(tolerance_) {
+    COAL_ASSERT(tolerance_ > 0, "Tolerance must be positive.",
+                std::invalid_argument);
+    initialize();
+  }
+
+  /// @brief resets the GJK algorithm, preparing it for a new run.
+  /// Other than the maximum number of iterations and the tolerance,
+  /// this function does **not** modify the parameters of the GJK algorithm.
+  void reset(size_t max_iterations_, CoalScalar tolerance_);
+
+  /// @brief GJK algorithm, given the initial value guess
+  Status evaluate(
+      const MinkowskiDiff& shape, const Vec3s& guess,
+      const support_func_guess_t& supportHint = support_func_guess_t::Zero());
+
+  /// @brief apply the support function along a direction, the result is return
+  /// in sv
+  inline void getSupport(const Vec3s& d, SimplexV& sv,
+                         support_func_guess_t& hint) const {
+    shape->support(d, sv.w0, sv.w1, hint);
+    sv.w = sv.w0 - sv.w1;
+  }
+
+  /// @brief whether the simplex enclose the origin
+  bool encloseOrigin();
+
+  /// @brief get the underlying simplex using in GJK, can be used for cache in
+  /// next iteration
+  inline Simplex* getSimplex() const { return simplex; }
+
+  /// Tells whether the closest points are available.
+  bool hasClosestPoints() const { return distance < distance_upper_bound; }
+
+  /// Get the witness points on each object, and the corresponding normal.
+  /// @param[in] shape is the Minkowski difference of the two shapes.
+  /// @param[out] w0 is the witness point on shape0.
+  /// @param[out] w1 is the witness point on shape1.
+  /// @param[out] normal is the normal of the separating plane found by
+  /// GJK. It points from shape0 to shape1.
+  void getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3s& w0,
+                                 Vec3s& w1, Vec3s& normal) const;
+
+  /// @brief get the guess from current simplex
+  Vec3s getGuessFromSimplex() const;
+
+  /// @brief Distance threshold for early break.
+  /// GJK stops when it proved the distance is more than this threshold.
+  /// @note The closest points will be erroneous in this case.
+  ///       If you want the closest points, set this to infinity (the default).
+  void setDistanceEarlyBreak(const CoalScalar& dup) {
+    distance_upper_bound = dup;
+  }
+
+  /// @brief Convergence check used to stop GJK when shapes are not in
+  /// collision.
+  bool checkConvergence(const Vec3s& w, const CoalScalar& rl, CoalScalar& alpha,
+                        const CoalScalar& omega) const;
+
+  /// @brief Get the max number of iterations of GJK.
+  size_t getNumMaxIterations() const { return max_iterations; }
+
+  /// @brief Get the tolerance of GJK.
+  CoalScalar getTolerance() const { return tolerance; }
+
+  /// @brief Get the number of iterations of the last run of GJK.
+  size_t getNumIterations() const { return iterations; }
+
+  /// @brief Get GJK number of iterations before momentum stops.
+  /// Only usefull if the Nesterov or Polyak acceleration activated.
+  size_t getNumIterationsMomentumStopped() const {
+    return iterations_momentum_stop;
+  }
+
+ private:
+  /// @brief Initializes the GJK algorithm.
+  /// This function should only be called by the constructor.
+  /// Otherwise use \ref reset.
+  void initialize();
+
+  /// @brief discard one vertex from the simplex
+  inline void removeVertex(Simplex& simplex);
+
+  /// @brief append one vertex to the simplex
+  inline void appendVertex(Simplex& simplex, const Vec3s& v,
+                           support_func_guess_t& hint);
+
+  /// @brief Project origin (0) onto line a-b
+  /// For a detailed explanation of how to efficiently project onto a simplex,
+  /// check out Ericson's book, page 403:
+  /// https://realtimecollisiondetection.net/ To sum up, a simplex has a voronoi
+  /// region for each feature it has (vertex, edge, face). We find the voronoi
+  /// region in which the origin lies and stop as soon as we find it; we then
+  /// project onto it and return the result. We start by voronoi regions
+  /// generated by vertices then move on to edges then faces. Checking voronoi
+  /// regions is done using simple dot products. Moreover, edges voronoi checks
+  /// reuse computations of vertices voronoi checks. The same goes for faces
+  /// which reuse checks from edges.
+  /// Finally, in addition to the voronoi procedure, checks relying on the order
+  /// of construction
+  /// of the simplex are added. To know more about these, visit
+  /// https://caseymuratori.com/blog_0003.
+  bool projectLineOrigin(const Simplex& current, Simplex& next);
+
+  /// @brief Project origin (0) onto triangle a-b-c
+  /// See \ref projectLineOrigin for an explanation on simplex projections.
+  bool projectTriangleOrigin(const Simplex& current, Simplex& next);
+
+  /// @brief Project origin (0) onto tetrahedron a-b-c-d
+  /// See \ref projectLineOrigin for an explanation on simplex projections.
+  bool projectTetrahedraOrigin(const Simplex& current, Simplex& next);
+};
+
+/// @brief class for EPA algorithm
+struct COAL_DLLAPI EPA {
+  typedef GJK::SimplexV SimplexVertex;
+  struct COAL_DLLAPI SimplexFace {
+    Vec3s n;
+    CoalScalar d;
+    bool ignore;          // If the origin does not project inside the face, we
+                          // ignore this face.
+    size_t vertex_id[3];  // Index of vertex in sv_store.
+    SimplexFace* adjacent_faces[3];  // A face has three adjacent faces.
+    SimplexFace* prev_face;          // The previous face in the list.
+    SimplexFace* next_face;          // The next face in the list.
+    size_t adjacent_edge[3];         // Each face has 3 edges: `0`, `1` and `2`.
+                              // The i-th adjacent face is bound (to this face)
+                              // along its `adjacent_edge[i]`-th edge
+                              // (with 0 <= i <= 2).
+    size_t pass;
+
+    SimplexFace() : n(Vec3s::Zero()), ignore(false) {};
+  };
+
+  /// @brief The simplex list of EPA is a linked list of faces.
+  /// Note: EPA's linked list does **not** own any memory.
+  /// The memory it refers to is contiguous and owned by a std::vector.
+  struct COAL_DLLAPI SimplexFaceList {
+    SimplexFace* root;
+    size_t count;
+    SimplexFaceList() : root(nullptr), count(0) {}
+
+    void reset() {
+      root = nullptr;
+      count = 0;
+    }
+
+    void append(SimplexFace* face) {
+      face->prev_face = nullptr;
+      face->next_face = root;
+      if (root != nullptr) root->prev_face = face;
+      root = face;
+      ++count;
+    }
+
+    void remove(SimplexFace* face) {
+      if (face->next_face != nullptr)
+        face->next_face->prev_face = face->prev_face;
+      if (face->prev_face != nullptr)
+        face->prev_face->next_face = face->next_face;
+      if (face == root) root = face->next_face;
+      --count;
+    }
+  };
+
+  /// @brief We bind the face `fa` along its edge `ea` to the face `fb` along
+  /// its edge `fb`.
+  static inline void bind(SimplexFace* fa, size_t ea, SimplexFace* fb,
+                          size_t eb) {
+    assert(ea == 0 || ea == 1 || ea == 2);
+    assert(eb == 0 || eb == 1 || eb == 2);
+    fa->adjacent_edge[ea] = eb;
+    fa->adjacent_faces[ea] = fb;
+    fb->adjacent_edge[eb] = ea;
+    fb->adjacent_faces[eb] = fa;
+  }
+
+  struct COAL_DLLAPI SimplexHorizon {
+    SimplexFace* current_face;  // current face in the horizon
+    SimplexFace* first_face;    // first face in the horizon
+    size_t num_faces;           // number of faces in the horizon
+    SimplexHorizon()
+        : current_face(nullptr), first_face(nullptr), num_faces(0) {}
+  };
+
+  enum Status {
+    DidNotRun = -1,
+    Failed = 0,
+    Valid = 1,
+    AccuracyReached = 1 << 1 | Valid,
+    Degenerated = 1 << 1 | Failed,
+    NonConvex = 2 << 1 | Failed,
+    InvalidHull = 3 << 1 | Failed,
+    OutOfFaces = 4 << 1 | Failed,
+    OutOfVertices = 5 << 1 | Failed,
+    FallBack = 6 << 1 | Failed
+  };
+
+ public:
+  Status status;
+  GJK::Simplex result;
+  Vec3s normal;
+  support_func_guess_t support_hint;
+  CoalScalar depth;
+  SimplexFace* closest_face;
+
+ private:
+  // max_iteration and tolerance are made private
+  // because they are meant to be set by the `reset` function.
+  size_t max_iterations;
+  CoalScalar tolerance;
+
+  std::vector<SimplexVertex> sv_store;
+  std::vector<SimplexFace> fc_store;
+  SimplexFaceList hull, stock;
+  size_t num_vertices;  // number of vertices in polytpoe constructed by EPA
+  size_t iterations;
+
+ public:
+  EPA(size_t max_iterations_, CoalScalar tolerance_)
+      : max_iterations(max_iterations_), tolerance(tolerance_) {
+    initialize();
+  }
+
+  /// @brief Copy constructor of EPA.
+  /// Mostly needed for the copy constructor of `GJKSolver`.
+  EPA(const EPA& other)
+      : max_iterations(other.max_iterations),
+        tolerance(other.tolerance),
+        sv_store(other.sv_store),
+        fc_store(other.fc_store) {
+    initialize();
+  }
+
+  /// @brief Get the max number of iterations of EPA.
+  size_t getNumMaxIterations() const { return max_iterations; }
+
+  /// @brief Get the max number of vertices of EPA.
+  size_t getNumMaxVertices() const { return sv_store.size(); }
+
+  /// @brief Get the max number of faces of EPA.
+  size_t getNumMaxFaces() const { return fc_store.size(); }
+
+  /// @brief Get the tolerance of EPA.
+  CoalScalar getTolerance() const { return tolerance; }
+
+  /// @brief Get the number of iterations of the last run of EPA.
+  size_t getNumIterations() const { return iterations; }
+
+  /// @brief Get the number of vertices in the polytope of the last run of EPA.
+  size_t getNumVertices() const { return num_vertices; }
+
+  /// @brief Get the number of faces in the polytope of the last run of EPA.
+  size_t getNumFaces() const { return hull.count; }
+
+  /// @brief resets the EPA algorithm, preparing it for a new run.
+  /// It potentially reallocates memory for the vertices and faces
+  /// if the passed parameters are bigger than the previous ones.
+  /// This function does **not** modify the parameters of the EPA algorithm,
+  /// i.e. the maximum number of iterations and the tolerance.
+  /// @note calling this function destroys the previous state of EPA.
+  /// In the future, we may want to copy it instead, i.e. when EPA will
+  /// be (properly) warm-startable.
+  void reset(size_t max_iterations, CoalScalar tolerance);
+
+  /// \return a Status which can be demangled using (status & Valid) or
+  ///         (status & Failed). The other values provide a more detailled
+  ///         status
+  Status evaluate(GJK& gjk, const Vec3s& guess);
+
+  /// Get the witness points on each object, and the corresponding normal.
+  /// @param[in] shape is the Minkowski difference of the two shapes.
+  /// @param[out] w0 is the witness point on shape0.
+  /// @param[out] w1 is the witness point on shape1.
+  /// @param[in] normal is the normal found by EPA. It points from shape0 to
+  /// shape1. The normal is used to correct the witness points on the shapes if
+  /// the shapes have a non-zero swept-sphere radius.
+  void getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3s& w0,
+                                 Vec3s& w1, Vec3s& normal) const;
+
+ private:
+  /// @brief Allocates memory for the EPA algorithm.
+  /// This function should only be called by the constructor.
+  /// Otherwise use \ref reset.
+  void initialize();
+
+  bool getEdgeDist(SimplexFace* face, const SimplexVertex& a,
+                   const SimplexVertex& b, CoalScalar& dist);
+
+  /// @brief Add a new face to the polytope.
+  /// This function sets the `ignore` flag to `true` if the origin does not
+  /// project inside the face.
+  SimplexFace* newFace(size_t id_a, size_t id_b, size_t id_vertex,
+                       bool force = false);
+
+  /// @brief Find the best polytope face to split
+  SimplexFace* findClosestFace();
+
+  /// @brief the goal is to add a face connecting vertex w and face edge f[e]
+  bool expand(size_t pass, const SimplexVertex& w, SimplexFace* f, size_t e,
+              SimplexHorizon& horizon);
+
+  // @brief Use this function to debug expand if needed.
+  // void PrintExpandLooping(const SimplexFace* f, const SimplexVertex& w);
+};
+
+}  // namespace details
+
+}  // namespace coal
+
+#endif

include/coal/narrowphase/minkowski_difference.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, Open Source Robotics Foundation
+ *  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.
+ */
+
+/** \authors Jia Pan, Florent Lamiraux, Josef Mirabel, Louis Montaut */
+
+#ifndef COAL_MINKOWSKI_DIFFERENCE_H
+#define COAL_MINKOWSKI_DIFFERENCE_H
+
+#include "coal/shape/geometric_shapes.h"
+#include "coal/math/transform.h"
+#include "coal/narrowphase/support_functions.h"
+
+namespace coal {
+
+namespace details {
+
+/// @brief Minkowski difference class of two shapes
+///
+/// @note The Minkowski difference is expressed in the frame of the first shape.
+struct COAL_DLLAPI MinkowskiDiff {
+  typedef Eigen::Array<CoalScalar, 1, 2> Array2d;
+
+  /// @brief points to two shapes
+  const ShapeBase* shapes[2];
+
+  /// @brief Store temporary data for the computation of the support point for
+  /// each shape.
+  ShapeSupportData data[2];
+
+  /// @brief rotation from shape1 to shape0
+  /// such that @f$ p_in_0 = oR1 * p_in_1 + ot1 @f$.
+  Matrix3s oR1;
+
+  /// @brief translation from shape1 to shape0
+  /// such that @f$ p_in_0 = oR1 * p_in_1 + ot1 @f$.
+  Vec3s ot1;
+
+  /// @brief The radii of the sphere swepted around each shape of the Minkowski
+  /// difference. The 2 values correspond to the swept-sphere radius of shape 0
+  /// and shape 1.
+  Array2d swept_sphere_radius;
+
+  /// @brief Wether or not to use the normalize heuristic in the GJK Nesterov
+  /// acceleration. This setting is only applied if the Nesterov acceleration in
+  /// the GJK class is active.
+  bool normalize_support_direction;
+
+  typedef void (*GetSupportFunction)(const MinkowskiDiff& minkowskiDiff,
+                                     const Vec3s& dir, Vec3s& support0,
+                                     Vec3s& support1,
+                                     support_func_guess_t& hint,
+                                     ShapeSupportData data[2]);
+  GetSupportFunction getSupportFunc;
+
+  MinkowskiDiff() : normalize_support_direction(false), getSupportFunc(NULL) {}
+
+  /// @brief Set the two shapes, assuming the relative transformation between
+  /// them is identity.
+  /// Consequently, all support points computed with the MinkowskiDiff
+  /// will be expressed in the world frame.
+  /// @param shape0 the first shape.
+  /// @param shape1 the second shape.
+  /// @tparam SupportOptions is a value of the SupportOptions enum. If set to
+  /// `WithSweptSphere`, the support computation will take into account the
+  /// swept sphere radius of the shapes. If set to `NoSweptSphere`, where
+  /// this information is simply stored in the Minkowski's difference
+  /// `swept_sphere_radius` array. This array is then used to correct the
+  /// solution found when GJK or EPA have converged.
+  ///
+  /// @note In practice, there is no need to take into account the swept-sphere
+  /// radius in the iterations of GJK/EPA. It is in fact detrimental to the
+  /// convergence of both algos. This is because it makes corners and edges of
+  /// shapes look strictly convex to the algorithms, which leads to poor
+  /// convergence. This swept sphere template parameter is only here for
+  /// debugging purposes and for specific uses cases where the swept sphere
+  /// radius is needed in the support function. The rule is simple. When
+  /// interacting with GJK/EPA, the `SupportOptions` template
+  /// parameter should **always** be set to `NoSweptSphere` (except for
+  /// debugging or testing purposes). When working directly with the shapes
+  /// involved in the collision, and not relying on GJK/EPA, the
+  /// `SupportOptions` template parameter should be set to `WithSweptSphere`.
+  /// This is for example the case for specialized collision/distance functions.
+  template <int _SupportOptions = SupportOptions::NoSweptSphere>
+  void set(const ShapeBase* shape0, const ShapeBase* shape1);
+
+  /// @brief Set the two shapes, with a relative transformation from shape0 to
+  /// shape1. Consequently, all support points computed with the MinkowskiDiff
+  /// will be expressed in the frame of shape0.
+  /// @param shape0 the first shape.
+  /// @param shape1 the second shape.
+  /// @param tf0 the transformation of the first shape.
+  /// @param tf1 the transformation of the second shape.
+  /// @tparam `SupportOptions` see `set(const ShapeBase*, const
+  /// ShapeBase*)` for more details.
+  template <int _SupportOptions = SupportOptions::NoSweptSphere>
+  void set(const ShapeBase* shape0, const ShapeBase* shape1,
+           const Transform3s& tf0, const Transform3s& tf1);
+
+  /// @brief support function for shape0.
+  /// The output vector is expressed in the local frame of shape0.
+  /// @return a point which belongs to the set {argmax_{v in shape0}
+  /// v.dot(dir)}, i.e a support of shape0 in direction dir.
+  /// @param dir support direction.
+  /// @param hint used to initialize the search when shape is a ConvexBase
+  /// object.
+  /// @tparam `SupportOptions` see `set(const ShapeBase*, const
+  /// ShapeBase*)` for more details.
+  template <int _SupportOptions = SupportOptions::NoSweptSphere>
+  inline Vec3s support0(const Vec3s& dir, int& hint) const {
+    return getSupport<_SupportOptions>(shapes[0], dir, hint);
+  }
+
+  /// @brief support function for shape1.
+  /// The output vector is expressed in the local frame of shape0.
+  /// This is mandatory because in the end we are interested in support points
+  /// of the Minkowski difference; hence the supports of shape0 and shape1 must
+  /// live in the same frame.
+  /// @return a point which belongs to the set {tf * argmax_{v in shape1}
+  /// v.dot(R^T * dir)}, i.e. the support of shape1 in direction dir (tf is the
+  /// tranform from shape1 to shape0).
+  /// @param dir support direction.
+  /// @param hint used to initialize the search when shape is a ConvexBase.
+  /// @tparam `SupportOptions` see `set(const ShapeBase*, const
+  /// ShapeBase*)` for more details.
+  template <int _SupportOptions = SupportOptions::NoSweptSphere>
+  inline Vec3s support1(const Vec3s& dir, int& hint) const {
+    // clang-format off
+    return oR1 * getSupport<_SupportOptions>(shapes[1], oR1.transpose() * dir, hint) + ot1;
+    // clang-format on
+  }
+
+  /// @brief Support function for the pair of shapes. This method assumes `set`
+  /// has already been called.
+  /// @param[in] dir the support direction.
+  /// @param[out] supp0 support of shape0 in direction dir, expressed in the
+  /// frame of shape0.
+  /// @param[out] supp1 support of shape1 in direction -dir, expressed in the
+  /// frame of shape0.
+  /// @param[in/out] hint used to initialize the search when shape is a
+  /// ConvexBase object.
+  inline void support(const Vec3s& dir, Vec3s& supp0, Vec3s& supp1,
+                      support_func_guess_t& hint) const {
+    assert(getSupportFunc != NULL);
+    getSupportFunc(*this, dir, supp0, supp1, hint,
+                   const_cast<ShapeSupportData*>(data));
+  }
+};
+
+}  // namespace details
+
+}  // namespace coal
+
+#endif  // COAL_MINKOWSKI_DIFFERENCE_H

include/coal/narrowphase/narrowphase.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, Open Source Robotics Foundation
+ *  Copyright (c) 2018-2019, Centre National de la Recherche Scientifique
+ *  All rights reserved.
+ *  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, Florent Lamiraux */
+
+#ifndef COAL_NARROWPHASE_H
+#define COAL_NARROWPHASE_H
+
+#include <limits>
+
+#include "coal/narrowphase/gjk.h"
+#include "coal/collision_data.h"
+#include "coal/narrowphase/narrowphase_defaults.h"
+#include "coal/logging.h"
+
+namespace coal {
+
+/// @brief collision and distance solver based on the GJK and EPA algorithms.
+/// Originally, GJK and EPA were implemented in fcl which itself took
+/// inspiration from the code of the GJK in bullet. Since then, both GJK and EPA
+/// have been largely modified to be faster and more robust to numerical
+/// accuracy and edge cases.
+struct COAL_DLLAPI GJKSolver {
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  /// @brief GJK algorithm
+  mutable details::GJK gjk;
+
+  /// @brief maximum number of iterations of GJK
+  size_t gjk_max_iterations;
+
+  /// @brief tolerance of GJK
+  CoalScalar gjk_tolerance;
+
+  /// @brief which warm start to use for GJK
+  GJKInitialGuess gjk_initial_guess;
+
+  /// @brief Whether smart guess can be provided
+  /// @Deprecated Use gjk_initial_guess instead
+  COAL_DEPRECATED_MESSAGE(Use gjk_initial_guess instead)
+  bool enable_cached_guess;
+
+  /// @brief smart guess
+  mutable Vec3s cached_guess;
+
+  /// @brief smart guess for the support function
+  mutable support_func_guess_t support_func_cached_guess;
+
+  /// @brief If GJK can guarantee that the distance between the shapes is
+  /// greater than this value, it will early stop.
+  CoalScalar distance_upper_bound;
+
+  /// @brief Variant of the GJK algorithm (Default, Nesterov or Polyak).
+  GJKVariant gjk_variant;
+
+  /// @brief Convergence criterion for GJK
+  GJKConvergenceCriterion gjk_convergence_criterion;
+
+  /// @brief Absolute or relative convergence criterion for GJK
+  GJKConvergenceCriterionType gjk_convergence_criterion_type;
+
+  /// @brief EPA algorithm
+  mutable details::EPA epa;
+
+  /// @brief maximum number of iterations of EPA
+  size_t epa_max_iterations;
+
+  /// @brief tolerance of EPA
+  CoalScalar epa_tolerance;
+
+  /// @brief Minkowski difference used by GJK and EPA algorithms
+  mutable details::MinkowskiDiff minkowski_difference;
+
+ private:
+  // Used internally for assertion checks.
+  static constexpr CoalScalar m_dummy_precision = 1e-6;
+
+ public:
+  COAL_COMPILER_DIAGNOSTIC_PUSH
+  COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+  /// @brief Default constructor for GJK algorithm
+  /// By default, we don't want EPA to allocate memory because
+  /// certain functions of the `GJKSolver` class have specializations
+  /// which don't use EPA (and/or GJK).
+  /// So we give EPA's constructor a max number of iterations of zero.
+  /// Only the functions that need EPA will reset the algorithm and allocate
+  /// memory if needed.
+  GJKSolver()
+      : gjk(GJK_DEFAULT_MAX_ITERATIONS, GJK_DEFAULT_TOLERANCE),
+        gjk_max_iterations(GJK_DEFAULT_MAX_ITERATIONS),
+        gjk_tolerance(GJK_DEFAULT_TOLERANCE),
+        gjk_initial_guess(GJKInitialGuess::DefaultGuess),
+        enable_cached_guess(false),  // Use gjk_initial_guess instead
+        cached_guess(Vec3s(1, 0, 0)),
+        support_func_cached_guess(support_func_guess_t::Zero()),
+        distance_upper_bound((std::numeric_limits<CoalScalar>::max)()),
+        gjk_variant(GJKVariant::DefaultGJK),
+        gjk_convergence_criterion(GJKConvergenceCriterion::Default),
+        gjk_convergence_criterion_type(GJKConvergenceCriterionType::Absolute),
+        epa(0, EPA_DEFAULT_TOLERANCE),
+        epa_max_iterations(EPA_DEFAULT_MAX_ITERATIONS),
+        epa_tolerance(EPA_DEFAULT_TOLERANCE) {}
+
+  /// @brief Constructor from a DistanceRequest
+  ///
+  /// \param[in] request DistanceRequest input
+  ///
+  /// See the default constructor; by default, we don't want
+  /// EPA to allocate memory so we call EPA's constructor with 0 max
+  /// number of iterations.
+  /// However, the `set` method stores the actual values of the request.
+  /// EPA will thus allocate memory only if needed.
+  explicit GJKSolver(const DistanceRequest& request)
+      : gjk(request.gjk_max_iterations, request.gjk_tolerance),
+        epa(0, request.epa_tolerance) {
+    this->cached_guess = Vec3s(1, 0, 0);
+    this->support_func_cached_guess = support_func_guess_t::Zero();
+
+    set(request);
+  }
+
+  /// @brief setter from a DistanceRequest
+  ///
+  /// \param[in] request DistanceRequest input
+  ///
+  void set(const DistanceRequest& request) {
+    // ---------------------
+    // GJK settings
+    this->gjk_initial_guess = request.gjk_initial_guess;
+    this->enable_cached_guess = request.enable_cached_gjk_guess;
+    if (this->gjk_initial_guess == GJKInitialGuess::CachedGuess ||
+        this->enable_cached_guess) {
+      this->cached_guess = request.cached_gjk_guess;
+      this->support_func_cached_guess = request.cached_support_func_guess;
+    }
+    this->gjk_max_iterations = request.gjk_max_iterations;
+    this->gjk_tolerance = request.gjk_tolerance;
+    // For distance computation, we don't want GJK to early stop
+    this->distance_upper_bound = (std::numeric_limits<CoalScalar>::max)();
+    this->gjk_variant = request.gjk_variant;
+    this->gjk_convergence_criterion = request.gjk_convergence_criterion;
+    this->gjk_convergence_criterion_type =
+        request.gjk_convergence_criterion_type;
+
+    // ---------------------
+    // EPA settings
+    this->epa_max_iterations = request.epa_max_iterations;
+    this->epa_tolerance = request.epa_tolerance;
+
+    // ---------------------
+    // Reset GJK and EPA status
+    this->epa.status = details::EPA::Status::DidNotRun;
+    this->gjk.status = details::GJK::Status::DidNotRun;
+  }
+
+  /// @brief Constructor from a CollisionRequest
+  ///
+  /// \param[in] request CollisionRequest input
+  ///
+  /// See the default constructor; by default, we don't want
+  /// EPA to allocate memory so we call EPA's constructor with 0 max
+  /// number of iterations.
+  /// However, the `set` method stores the actual values of the request.
+  /// EPA will thus allocate memory only if needed.
+  explicit GJKSolver(const CollisionRequest& request)
+      : gjk(request.gjk_max_iterations, request.gjk_tolerance),
+        epa(0, request.epa_tolerance) {
+    this->cached_guess = Vec3s(1, 0, 0);
+    this->support_func_cached_guess = support_func_guess_t::Zero();
+
+    set(request);
+  }
+
+  /// @brief setter from a CollisionRequest
+  ///
+  /// \param[in] request CollisionRequest input
+  ///
+  void set(const CollisionRequest& request) {
+    // ---------------------
+    // GJK settings
+    this->gjk_initial_guess = request.gjk_initial_guess;
+    this->enable_cached_guess = request.enable_cached_gjk_guess;
+    if (this->gjk_initial_guess == GJKInitialGuess::CachedGuess ||
+        this->enable_cached_guess) {
+      this->cached_guess = request.cached_gjk_guess;
+      this->support_func_cached_guess = request.cached_support_func_guess;
+    }
+    this->gjk_tolerance = request.gjk_tolerance;
+    this->gjk_max_iterations = request.gjk_max_iterations;
+    // The distance upper bound should be at least greater to the requested
+    // security margin. Otherwise, we will likely miss some collisions.
+    this->distance_upper_bound = (std::max)(
+        0., (std::max)(request.distance_upper_bound, request.security_margin));
+    this->gjk_variant = request.gjk_variant;
+    this->gjk_convergence_criterion = request.gjk_convergence_criterion;
+    this->gjk_convergence_criterion_type =
+        request.gjk_convergence_criterion_type;
+
+    // ---------------------
+    // EPA settings
+    this->epa_max_iterations = request.epa_max_iterations;
+    this->epa_tolerance = request.epa_tolerance;
+
+    // ---------------------
+    // Reset GJK and EPA status
+    this->gjk.status = details::GJK::Status::DidNotRun;
+    this->epa.status = details::EPA::Status::DidNotRun;
+  }
+
+  /// @brief Copy constructor
+  GJKSolver(const GJKSolver& other) = default;
+
+  COAL_COMPILER_DIAGNOSTIC_PUSH
+  COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+  bool operator==(const GJKSolver& other) const {
+    return this->enable_cached_guess ==
+               other.enable_cached_guess &&  // use gjk_initial_guess instead
+           this->cached_guess == other.cached_guess &&
+           this->support_func_cached_guess == other.support_func_cached_guess &&
+           this->gjk_max_iterations == other.gjk_max_iterations &&
+           this->gjk_tolerance == other.gjk_tolerance &&
+           this->distance_upper_bound == other.distance_upper_bound &&
+           this->gjk_variant == other.gjk_variant &&
+           this->gjk_convergence_criterion == other.gjk_convergence_criterion &&
+           this->gjk_convergence_criterion_type ==
+               other.gjk_convergence_criterion_type &&
+           this->gjk_initial_guess == other.gjk_initial_guess &&
+           this->epa_max_iterations == other.epa_max_iterations &&
+           this->epa_tolerance == other.epa_tolerance;
+  }
+  COAL_COMPILER_DIAGNOSTIC_POP
+
+  bool operator!=(const GJKSolver& other) const { return !(*this == other); }
+
+  /// @brief Helper to return the precision of the solver on the distance
+  /// estimate, depending on whether or not `compute_penetration` is true.
+  CoalScalar getDistancePrecision(const bool compute_penetration) const {
+    return compute_penetration
+               ? (std::max)(this->gjk_tolerance, this->epa_tolerance)
+               : this->gjk_tolerance;
+  }
+
+  /// @brief Uses GJK and EPA to compute the distance between two shapes.
+  /// @param `s1` the first shape.
+  /// @param `tf1` the transformation of the first shape.
+  /// @param `s2` the second shape.
+  /// @param `tf2` the transformation of the second shape.
+  /// @param `compute_penetration` if true and GJK finds the shape in collision,
+  /// the EPA algorithm is also ran to compute penetration information.
+  /// @param[out] `p1` the witness point on the first shape.
+  /// @param[out] `p2` the witness point on the second shape.
+  /// @param[out] `normal` the normal of the collision, pointing from the first
+  /// to the second shape.
+  /// @return the estimate of the distance between the two shapes.
+  ///
+  /// @note: if `this->distance_upper_bound` is set to a positive value, GJK
+  /// will early stop if it finds the distance to be above this value. The
+  /// distance returned by `this->shapeDistance` will be a lower bound on the
+  /// distance between the two shapes.
+  ///
+  /// @note: the variables `this->gjk.status` and `this->epa.status` can be used
+  /// to examine the status of GJK and EPA.
+  ///
+  /// @note: GJK and EPA give an estimate of the distance between the two
+  /// shapes. This estimate is precise up to the tolerance of the algorithms:
+  ///   - If `compute_penetration` is false, the distance is precise up to
+  ///     `gjk_tolerance`.
+  ///   - If `compute_penetration` is true, the distance is precise up to
+  ///     `std::max(gjk_tolerance, epa_tolerance)`
+  /// It's up to the user to decide whether the shapes are in collision or not,
+  /// based on that estimate.
+  template <typename S1, typename S2>
+  CoalScalar shapeDistance(const S1& s1, const Transform3s& tf1, const S2& s2,
+                           const Transform3s& tf2,
+                           const bool compute_penetration, Vec3s& p1, Vec3s& p2,
+                           Vec3s& normal) const {
+    constexpr bool relative_transformation_already_computed = false;
+    CoalScalar distance;
+    this->runGJKAndEPA(s1, tf1, s2, tf2, compute_penetration, distance, p1, p2,
+                       normal, relative_transformation_already_computed);
+    return distance;
+  }
+
+  /// @brief Partial specialization of `shapeDistance` for the case where the
+  /// second shape is a triangle. It is more efficient to pre-compute the
+  /// relative transformation between the two shapes before calling GJK/EPA.
+  template <typename S1>
+  CoalScalar shapeDistance(const S1& s1, const Transform3s& tf1,
+                           const TriangleP& s2, const Transform3s& tf2,
+                           const bool compute_penetration, Vec3s& p1, Vec3s& p2,
+                           Vec3s& normal) const {
+    const Transform3s tf_1M2(tf1.inverseTimes(tf2));
+    TriangleP tri(tf_1M2.transform(s2.a), tf_1M2.transform(s2.b),
+                  tf_1M2.transform(s2.c));
+
+    constexpr bool relative_transformation_already_computed = true;
+    CoalScalar distance;
+    this->runGJKAndEPA(s1, tf1, tri, tf_1M2, compute_penetration, distance, p1,
+                       p2, normal, relative_transformation_already_computed);
+    return distance;
+  }
+
+  /// @brief See other partial template specialization of shapeDistance above.
+  template <typename S2>
+  CoalScalar shapeDistance(const TriangleP& s1, const Transform3s& tf1,
+                           const S2& s2, const Transform3s& tf2,
+                           const bool compute_penetration, Vec3s& p1, Vec3s& p2,
+                           Vec3s& normal) const {
+    CoalScalar distance = this->shapeDistance<S2>(
+        s2, tf2, s1, tf1, compute_penetration, p2, p1, normal);
+    normal = -normal;
+    return distance;
+  }
+
+ protected:
+  /// @brief initialize GJK.
+  /// This method assumes `minkowski_difference` has been set.
+  template <typename S1, typename S2>
+  void getGJKInitialGuess(const S1& s1, const S2& s2, Vec3s& guess,
+                          support_func_guess_t& support_hint,
+                          const Vec3s& default_guess = Vec3s(1, 0, 0)) const {
+    // There is no reason not to warm-start the support function, so we always
+    // do it.
+    support_hint = this->support_func_cached_guess;
+    // The following switch takes care of the GJK warm-start.
+    switch (gjk_initial_guess) {
+      case GJKInitialGuess::DefaultGuess:
+        guess = default_guess;
+        break;
+      case GJKInitialGuess::CachedGuess:
+        guess = this->cached_guess;
+        break;
+      case GJKInitialGuess::BoundingVolumeGuess:
+        if (s1.aabb_local.volume() < 0 || s2.aabb_local.volume() < 0) {
+          COAL_THROW_PRETTY(
+              "computeLocalAABB must have been called on the shapes before "
+              "using "
+              "GJKInitialGuess::BoundingVolumeGuess.",
+              std::logic_error);
+        }
+        guess.noalias() =
+            s1.aabb_local.center() -
+            (this->minkowski_difference.oR1 * s2.aabb_local.center() +
+             this->minkowski_difference.ot1);
+        break;
+      default:
+        COAL_THROW_PRETTY("Wrong initial guess for GJK.", std::logic_error);
+    }
+    // TODO: use gjk_initial_guess instead
+    COAL_COMPILER_DIAGNOSTIC_PUSH
+    COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+    if (this->enable_cached_guess) {
+      guess = this->cached_guess;
+    }
+    COAL_COMPILER_DIAGNOSTIC_POP
+  }
+
+  /// @brief Runs the GJK algorithm.
+  /// @param `s1` the first shape.
+  /// @param `tf1` the transformation of the first shape.
+  /// @param `s2` the second shape.
+  /// @param `tf2` the transformation of the second shape.
+  /// @param `compute_penetration` if true and if the shapes are in found in
+  /// collision, the EPA algorithm is also ran to compute penetration
+  /// information.
+  /// @param[out] `distance` the distance between the two shapes.
+  /// @param[out] `p1` the witness point on the first shape.
+  /// @param[out] `p2` the witness point on the second shape.
+  /// @param[out] `normal` the normal of the collision, pointing from the first
+  /// to the second shape.
+  /// @param `relative_transformation_already_computed` whether the relative
+  /// transformation between the two shapes has already been computed.
+  /// @tparam SupportOptions, see `MinkowskiDiff::set`. Whether the support
+  /// computations should take into account the shapes' swept-sphere radii
+  /// during the iterations of GJK and EPA. Please leave this default value to
+  /// `false` unless you know what you are doing. This template parameter is
+  /// only used for debugging/testing purposes. In short, there is no need to
+  /// take into account the swept sphere radius when computing supports in the
+  /// iterations of GJK and EPA. GJK and EPA will correct the solution once they
+  /// have converged.
+  /// @return the estimate of the distance between the two shapes.
+  ///
+  /// @note: The variables `this->gjk.status` and `this->epa.status` can be used
+  /// to examine the status of GJK and EPA.
+  template <typename S1, typename S2,
+            int _SupportOptions = details::SupportOptions::NoSweptSphere>
+  void runGJKAndEPA(
+      const S1& s1, const Transform3s& tf1, const S2& s2,
+      const Transform3s& tf2, const bool compute_penetration,
+      CoalScalar& distance, Vec3s& p1, Vec3s& p2, Vec3s& normal,
+      const bool relative_transformation_already_computed = false) const {
+    // Reset internal state of GJK algorithm
+    if (relative_transformation_already_computed)
+      this->minkowski_difference.set<_SupportOptions>(&s1, &s2);
+    else
+      this->minkowski_difference.set<_SupportOptions>(&s1, &s2, tf1, tf2);
+    this->gjk.reset(this->gjk_max_iterations, this->gjk_tolerance);
+    this->gjk.setDistanceEarlyBreak(this->distance_upper_bound);
+    this->gjk.gjk_variant = this->gjk_variant;
+    this->gjk.convergence_criterion = this->gjk_convergence_criterion;
+    this->gjk.convergence_criterion_type = this->gjk_convergence_criterion_type;
+    this->epa.status = details::EPA::Status::DidNotRun;
+
+    // Get initial guess for GJK: default, cached or bounding volume guess
+    Vec3s guess;
+    support_func_guess_t support_hint;
+    getGJKInitialGuess(*(this->minkowski_difference.shapes[0]),
+                       *(this->minkowski_difference.shapes[1]), guess,
+                       support_hint);
+
+    this->gjk.evaluate(this->minkowski_difference, guess, support_hint);
+
+    switch (this->gjk.status) {
+      case details::GJK::DidNotRun:
+        COAL_ASSERT(false, "GJK did not run. It should have!",
+                    std::logic_error);
+        this->cached_guess = Vec3s(1, 0, 0);
+        this->support_func_cached_guess.setZero();
+        distance = -(std::numeric_limits<CoalScalar>::max)();
+        p1 = p2 = normal =
+            Vec3s::Constant(std::numeric_limits<CoalScalar>::quiet_NaN());
+        break;
+      case details::GJK::Failed:
+        //
+        // GJK ran out of iterations.
+        COAL_LOG_WARNING("GJK ran out of iterations.");
+        GJKExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
+        break;
+      case details::GJK::NoCollisionEarlyStopped:
+        //
+        // Case where GJK early stopped because the distance was found to be
+        // above the `distance_upper_bound`.
+        // The two witness points have no meaning.
+        GJKEarlyStopExtractWitnessPointsAndNormal(tf1, distance, p1, p2,
+                                                  normal);
+        COAL_ASSERT(distance >= this->gjk.distance_upper_bound -
+                                    this->m_dummy_precision,
+                    "The distance should be bigger than GJK's "
+                    "`distance_upper_bound`.",
+                    std::logic_error);
+        break;
+      case details::GJK::NoCollision:
+        //
+        // Case where GJK converged and proved that the shapes are not in
+        // collision, i.e their distance is above GJK's tolerance (default
+        // 1e-6).
+        GJKExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
+        COAL_ASSERT(std::abs((p1 - p2).norm() - distance) <=
+                        this->gjk.getTolerance() + this->m_dummy_precision,
+                    "The distance found by GJK should coincide with the "
+                    "distance between the closest points.",
+                    std::logic_error);
+        break;
+      //
+      // Next are the cases where GJK found the shapes to be in collision, i.e.
+      // their distance is below GJK's tolerance (default 1e-6).
+      case details::GJK::CollisionWithPenetrationInformation:
+        GJKExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
+        COAL_ASSERT(
+            distance <= this->gjk.getTolerance() + this->m_dummy_precision,
+            "The distance found by GJK should be negative or at "
+            "least below GJK's tolerance.",
+            std::logic_error);
+        break;
+      case details::GJK::Collision:
+        if (!compute_penetration) {
+          // Skip EPA and set the witness points and the normal to nans.
+          GJKCollisionExtractWitnessPointsAndNormal(tf1, distance, p1, p2,
+                                                    normal);
+        } else {
+          //
+          // GJK was not enough to recover the penetration information.
+          // We need to run the EPA algorithm to find the witness points,
+          // penetration depth and the normal.
+
+          // Reset EPA algorithm. Potentially allocate memory if
+          // `epa_max_face_num` or `epa_max_vertex_num` are bigger than EPA's
+          // current storage.
+          this->epa.reset(this->epa_max_iterations, this->epa_tolerance);
+
+          // TODO: understand why EPA's performance is so bad on cylinders and
+          // cones.
+          this->epa.evaluate(this->gjk, -guess);
+
+          switch (epa.status) {
+            //
+            // In the following switch cases, until the "Valid" case,
+            // EPA either ran out of iterations, of faces or of vertices.
+            // The depth, witness points and the normal are still valid,
+            // simply not at the precision of EPA's tolerance.
+            // The flag `COAL_ENABLE_LOGGING` enables feebdack on these
+            // cases.
+            //
+            // TODO: Remove OutOfFaces and OutOfVertices statuses and simply
+            // compute the upper bound on max faces and max vertices as a
+            // function of the number of iterations.
+            case details::EPA::OutOfFaces:
+              COAL_LOG_WARNING("EPA ran out of faces.");
+              EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
+              break;
+            case details::EPA::OutOfVertices:
+              COAL_LOG_WARNING("EPA ran out of vertices.");
+              EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
+              break;
+            case details::EPA::Failed:
+              COAL_LOG_WARNING("EPA ran out of iterations.");
+              EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
+              break;
+            case details::EPA::Valid:
+            case details::EPA::AccuracyReached:
+              COAL_ASSERT(
+                  -epa.depth <= epa.getTolerance() + this->m_dummy_precision,
+                  "EPA's penetration distance should be negative (or "
+                  "at least below EPA's tolerance).",
+                  std::logic_error);
+              EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
+              break;
+            case details::EPA::Degenerated:
+              COAL_LOG_WARNING(
+                  "EPA warning: created a polytope with a degenerated face.");
+              EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
+              break;
+            case details::EPA::NonConvex:
+              COAL_LOG_WARNING(
+                  "EPA warning: EPA got called onto non-convex shapes.");
+              EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
+              break;
+            case details::EPA::InvalidHull:
+              COAL_LOG_WARNING("EPA warning: created an invalid polytope.");
+              EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
+              break;
+            case details::EPA::DidNotRun:
+              COAL_ASSERT(false, "EPA did not run. It should have!",
+                          std::logic_error);
+              COAL_LOG_ERROR("EPA error: did not run. It should have.");
+              EPAFailedExtractWitnessPointsAndNormal(tf1, distance, p1, p2,
+                                                     normal);
+              break;
+            case details::EPA::FallBack:
+              COAL_ASSERT(
+                  false,
+                  "EPA went into fallback mode. It should never do that.",
+                  std::logic_error);
+              COAL_LOG_ERROR("EPA error: FallBack.");
+              EPAFailedExtractWitnessPointsAndNormal(tf1, distance, p1, p2,
+                                                     normal);
+              break;
+          }
+        }
+        break;  // End of case details::GJK::Collision
+    }
+  }
+
+  void GJKEarlyStopExtractWitnessPointsAndNormal(const Transform3s& tf1,
+                                                 CoalScalar& distance,
+                                                 Vec3s& p1, Vec3s& p2,
+                                                 Vec3s& normal) const {
+    COAL_UNUSED_VARIABLE(tf1);
+    // Cache gjk result for potential future call to this GJKSolver.
+    this->cached_guess = this->gjk.ray;
+    this->support_func_cached_guess = this->gjk.support_hint;
+
+    distance = this->gjk.distance;
+    p1 = p2 = normal =
+        Vec3s::Constant(std::numeric_limits<CoalScalar>::quiet_NaN());
+    // If we absolutely want to return some witness points, we could use
+    // the following code (or simply merge the early stopped case with the
+    // valid case below):
+    // gjk.getWitnessPointsAndNormal(minkowski_difference, p1, p2, normal);
+    // p1 = tf1.transform(p1);
+    // p2 = tf1.transform(p2);
+    // normal = tf1.getRotation() * normal;
+  }
+
+  void GJKExtractWitnessPointsAndNormal(const Transform3s& tf1,
+                                        CoalScalar& distance, Vec3s& p1,
+                                        Vec3s& p2, Vec3s& normal) const {
+    // Apart from early stopping, there are two cases where GJK says there is no
+    // collision:
+    // 1. GJK proved the distance is above its tolerance (default 1e-6).
+    // 2. GJK ran out of iterations.
+    // In any case, `gjk.ray`'s norm is bigger than GJK's tolerance and thus
+    // it can safely be normalized.
+    COAL_ASSERT(this->gjk.ray.norm() >
+                    this->gjk.getTolerance() - this->m_dummy_precision,
+                "The norm of GJK's ray should be bigger than GJK's tolerance.",
+                std::logic_error);
+    // Cache gjk result for potential future call to this GJKSolver.
+    this->cached_guess = this->gjk.ray;
+    this->support_func_cached_guess = this->gjk.support_hint;
+
+    distance = this->gjk.distance;
+    // TODO: On degenerated case, the closest points may be non-unique.
+    // (i.e. an object face normal is colinear to `gjk.ray`)
+    gjk.getWitnessPointsAndNormal(this->minkowski_difference, p1, p2, normal);
+    Vec3s p = tf1.transform(0.5 * (p1 + p2));
+    normal = tf1.getRotation() * normal;
+    p1.noalias() = p - 0.5 * distance * normal;
+    p2.noalias() = p + 0.5 * distance * normal;
+  }
+
+  void GJKCollisionExtractWitnessPointsAndNormal(const Transform3s& tf1,
+                                                 CoalScalar& distance,
+                                                 Vec3s& p1, Vec3s& p2,
+                                                 Vec3s& normal) const {
+    COAL_UNUSED_VARIABLE(tf1);
+    COAL_ASSERT(this->gjk.distance <=
+                    this->gjk.getTolerance() + this->m_dummy_precision,
+                "The distance should be lower than GJK's tolerance.",
+                std::logic_error);
+    // Because GJK has returned the `Collision` status and EPA has not run,
+    // we purposefully do not cache the result of GJK, because ray is zero.
+    // However, we can cache the support function hint.
+    // this->cached_guess = this->gjk.ray;
+    this->support_func_cached_guess = this->gjk.support_hint;
+
+    distance = this->gjk.distance;
+    p1 = p2 = normal =
+        Vec3s::Constant(std::numeric_limits<CoalScalar>::quiet_NaN());
+  }
+
+  void EPAExtractWitnessPointsAndNormal(const Transform3s& tf1,
+                                        CoalScalar& distance, Vec3s& p1,
+                                        Vec3s& p2, Vec3s& normal) const {
+    // Cache EPA result for potential future call to this GJKSolver.
+    // This caching allows to warm-start the next GJK call.
+    this->cached_guess = -(this->epa.depth * this->epa.normal);
+    this->support_func_cached_guess = this->epa.support_hint;
+    distance = (std::min)(0., -this->epa.depth);
+    this->epa.getWitnessPointsAndNormal(this->minkowski_difference, p1, p2,
+                                        normal);
+    // The following is very important to understand why EPA can sometimes
+    // return a normal that is not colinear to the vector $p_1 - p_2$ when
+    // working with tolerances like $\epsilon = 10^{-3}$.
+    // It can be resumed with a simple idea:
+    //     EPA is an algorithm meant to find the penetration depth and the
+    //     normal. It is not meant to find the closest points.
+    // Again, the issue here is **not** the normal, it's $p_1$ and $p_2$.
+    //
+    // More details:
+    // We'll denote $S_1$ and $S_2$ the two shapes, $n$ the normal and $p_1$ and
+    // $p_2$ the witness points. In theory, when EPA converges to $\epsilon =
+    // 0$, the normal and witness points verify the following property (P):
+    //   - $p_1 \in \partial \sigma_{S_1}(n)$,
+    //   - $p_2 \in \partial \sigma_{S_2}(-n),
+    // where $\sigma_{S_1}$ and $\sigma_{S_2}$ are the support functions of
+    // $S_1$ and $S_2$. The $\partial \sigma(n)$ simply denotes the support set
+    // of the support function in the direction $n$. (Note: I am leaving out the
+    // details of frame choice for the support function, to avoid making the
+    // mathematical notation too heavy.)
+    // --> In practice, EPA converges to $\epsilon > 0$.
+    // On polytopes and the likes, this does not change much and the property
+    // given above is still valid.
+    // --> However, this is very different on curved surfaces, such as
+    // ellipsoids, cylinders, cones, capsules etc. For these shapes, converging
+    // at $\epsilon = 10^{-6}$ or to $\epsilon = 10^{-3}$ does not change the
+    // normal much, but the property (P) given above is no longer valid, which
+    // means that the points $p_1$ and $p_2$ do not necessarily belong to the
+    // support sets in the direction of $n$ and thus $n$ and $p_1 - p_2$ are not
+    // colinear.
+    //
+    // Do not panic! This is fine.
+    // Although the property above is not verified, it's almost verified,
+    // meaning that $p_1$ and $p_2$ belong to support sets in directions that
+    // are very close to $n$.
+    //
+    // Solution to compute better $p_1$ and $p_2$:
+    // We compute the middle points of the current $p_1$ and $p_2$ and we use
+    // the normal and the distance given by EPA to compute the new $p_1$ and
+    // $p_2$.
+    Vec3s p = tf1.transform(0.5 * (p1 + p2));
+    normal = tf1.getRotation() * normal;
+    p1.noalias() = p - 0.5 * distance * normal;
+    p2.noalias() = p + 0.5 * distance * normal;
+  }
+
+  void EPAFailedExtractWitnessPointsAndNormal(const Transform3s& tf1,
+                                              CoalScalar& distance, Vec3s& p1,
+                                              Vec3s& p2, Vec3s& normal) const {
+    this->cached_guess = Vec3s(1, 0, 0);
+    this->support_func_cached_guess.setZero();
+
+    COAL_UNUSED_VARIABLE(tf1);
+    distance = -(std::numeric_limits<CoalScalar>::max)();
+    p1 = p2 = normal =
+        Vec3s::Constant(std::numeric_limits<CoalScalar>::quiet_NaN());
+  }
+};
+
+}  // namespace coal
+
+#endif

include/coal/narrowphase/narrowphase_defaults.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 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.
+ */
+
+/// This file defines different macros used to characterize the default behavior
+/// of the narrowphase algorithms GJK and EPA.
+
+#ifndef COAL_NARROWPHASE_DEFAULTS_H
+#define COAL_NARROWPHASE_DEFAULTS_H
+
+#include "coal/data_types.h"
+
+namespace coal {
+
+/// GJK
+constexpr size_t GJK_DEFAULT_MAX_ITERATIONS = 128;
+constexpr CoalScalar GJK_DEFAULT_TOLERANCE = 1e-6;
+/// Note: if the considered shapes are on the order of the meter, and the
+/// convergence criterion of GJK is the default VDB criterion,
+/// setting a tolerance of 1e-6 on the GJK algorithm makes it precise up to
+/// the micro-meter.
+/// The same is true for EPA.
+constexpr CoalScalar GJK_MINIMUM_TOLERANCE = 1e-6;
+
+/// EPA
+/// EPA build a polytope which maximum size is:
+///   - `#iterations + 4` vertices
+///   - `2 x #iterations + 4` faces
+constexpr size_t EPA_DEFAULT_MAX_ITERATIONS = 64;
+constexpr CoalScalar EPA_DEFAULT_TOLERANCE = 1e-6;
+constexpr CoalScalar EPA_MINIMUM_TOLERANCE = 1e-6;
+
+}  // namespace coal
+
+#endif  // COAL_NARROWPHASE_DEFAULTS_H