include/coal/collision_func_matrix.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_COLLISION_FUNC_MATRIX_H
+#define COAL_COLLISION_FUNC_MATRIX_H
+
+#include "coal/collision_object.h"
+#include "coal/collision_data.h"
+#include "coal/narrowphase/narrowphase.h"
+
+namespace coal {
+
+/// @brief collision matrix stores the functions for collision between different
+/// types of objects and provides a uniform call interface
+
+struct COAL_DLLAPI CollisionFunctionMatrix {
+  /// @brief the uniform call interface for collision: for collision, we need
+  /// know
+  /// 1. two objects o1 and o2 and their configuration in world coordinate tf1
+  /// and tf2;
+  /// 2. the solver for narrow phase collision, this is for the collision
+  /// between geometric shapes;
+  /// 3. the request setting for collision (e.g., whether need to return normal
+  /// information, whether need to compute cost);
+  /// 4. the structure to return collision result
+  typedef std::size_t (*CollisionFunc)(const CollisionGeometry* o1,
+                                       const Transform3s& tf1,
+                                       const CollisionGeometry* o2,
+                                       const Transform3s& tf2,
+                                       const GJKSolver* nsolver,
+                                       const CollisionRequest& request,
+                                       CollisionResult& result);
+
+  /// @brief each item in the collision matrix is a function to handle collision
+  /// between objects of type1 and type2
+  CollisionFunc collision_matrix[NODE_COUNT][NODE_COUNT];
+
+  CollisionFunctionMatrix();
+};
+
+}  // namespace coal
+
+#endif

include/coal/collision_object.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_COLLISION_OBJECT_BASE_H
+#define COAL_COLLISION_OBJECT_BASE_H
+
+#include <limits>
+#include <typeinfo>
+
+#include "coal/deprecated.hh"
+#include "coal/fwd.hh"
+#include "coal/BV/AABB.h"
+#include "coal/math/transform.h"
+
+namespace coal {
+
+/// @brief object type: BVH (mesh, points), basic geometry, octree
+enum OBJECT_TYPE {
+  OT_UNKNOWN,
+  OT_BVH,
+  OT_GEOM,
+  OT_OCTREE,
+  OT_HFIELD,
+  OT_COUNT
+};
+
+/// @brief traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS,
+/// KDOP16, KDOP18, kDOP24), basic shape (box, sphere, ellipsoid, capsule, cone,
+/// cylinder, convex, plane, triangle), and octree
+enum NODE_TYPE {
+  BV_UNKNOWN,
+  BV_AABB,
+  BV_OBB,
+  BV_RSS,
+  BV_kIOS,
+  BV_OBBRSS,
+  BV_KDOP16,
+  BV_KDOP18,
+  BV_KDOP24,
+  GEOM_BOX,
+  GEOM_SPHERE,
+  GEOM_CAPSULE,
+  GEOM_CONE,
+  GEOM_CYLINDER,
+  GEOM_CONVEX,
+  GEOM_PLANE,
+  GEOM_HALFSPACE,
+  GEOM_TRIANGLE,
+  GEOM_OCTREE,
+  GEOM_ELLIPSOID,
+  HF_AABB,
+  HF_OBBRSS,
+  NODE_COUNT
+};
+
+/// @addtogroup Construction_Of_BVH
+/// @{
+
+/// @brief The geometry for the object for collision or distance computation
+class COAL_DLLAPI CollisionGeometry {
+ public:
+  CollisionGeometry()
+      : aabb_center(Vec3s::Constant((std::numeric_limits<CoalScalar>::max)())),
+        aabb_radius(-1),
+        cost_density(1),
+        threshold_occupied(1),
+        threshold_free(0) {}
+
+  /// \brief Copy constructor
+  CollisionGeometry(const CollisionGeometry& other) = default;
+
+  virtual ~CollisionGeometry() {}
+
+  /// @brief Clone *this into a new CollisionGeometry
+  virtual CollisionGeometry* clone() const = 0;
+
+  /// \brief Equality operator
+  bool operator==(const CollisionGeometry& other) const {
+    return cost_density == other.cost_density &&
+           threshold_occupied == other.threshold_occupied &&
+           threshold_free == other.threshold_free &&
+           aabb_center == other.aabb_center &&
+           aabb_radius == other.aabb_radius && aabb_local == other.aabb_local &&
+           isEqual(other);
+  }
+
+  /// \brief Difference operator
+  bool operator!=(const CollisionGeometry& other) const {
+    return isNotEqual(other);
+  }
+
+  /// @brief get the type of the object
+  virtual OBJECT_TYPE getObjectType() const { return OT_UNKNOWN; }
+
+  /// @brief get the node type
+  virtual NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
+
+  /// @brief compute the AABB for object in local coordinate
+  virtual void computeLocalAABB() = 0;
+
+  /// @brief get user data in geometry
+  void* getUserData() const { return user_data; }
+
+  /// @brief set user data in geometry
+  void setUserData(void* data) { user_data = data; }
+
+  /// @brief whether the object is completely occupied
+  inline bool isOccupied() const { return cost_density >= threshold_occupied; }
+
+  /// @brief whether the object is completely free
+  inline bool isFree() const { return cost_density <= threshold_free; }
+
+  /// @brief whether the object has some uncertainty
+  bool isUncertain() const;
+
+  /// @brief AABB center in local coordinate
+  Vec3s aabb_center;
+
+  /// @brief AABB radius
+  CoalScalar aabb_radius;
+
+  /// @brief AABB in local coordinate, used for tight AABB when only translation
+  /// transform
+  AABB aabb_local;
+
+  /// @brief pointer to user defined data specific to this object
+  void* user_data;
+
+  /// @brief collision cost for unit volume
+  CoalScalar cost_density;
+
+  /// @brief threshold for occupied ( >= is occupied)
+  CoalScalar threshold_occupied;
+
+  /// @brief threshold for free (<= is free)
+  CoalScalar threshold_free;
+
+  /// @brief compute center of mass
+  virtual Vec3s computeCOM() const { return Vec3s::Zero(); }
+
+  /// @brief compute the inertia matrix, related to the origin
+  virtual Matrix3s computeMomentofInertia() const {
+    return Matrix3s::Constant(NAN);
+  }
+
+  /// @brief compute the volume
+  virtual CoalScalar computeVolume() const { return 0; }
+
+  /// @brief compute the inertia matrix, related to the com
+  virtual Matrix3s computeMomentofInertiaRelatedToCOM() const {
+    Matrix3s C = computeMomentofInertia();
+    Vec3s com = computeCOM();
+    CoalScalar V = computeVolume();
+
+    return (Matrix3s() << C(0, 0) - V * (com[1] * com[1] + com[2] * com[2]),
+            C(0, 1) + V * com[0] * com[1], C(0, 2) + V * com[0] * com[2],
+            C(1, 0) + V * com[1] * com[0],
+            C(1, 1) - V * (com[0] * com[0] + com[2] * com[2]),
+            C(1, 2) + V * com[1] * com[2], C(2, 0) + V * com[2] * com[0],
+            C(2, 1) + V * com[2] * com[1],
+            C(2, 2) - V * (com[0] * com[0] + com[1] * com[1]))
+        .finished();
+  }
+
+ private:
+  /// @brief equal operator with another object of derived type.
+  virtual bool isEqual(const CollisionGeometry& other) const = 0;
+
+  /// @brief not equal operator with another object of derived type.
+  virtual bool isNotEqual(const CollisionGeometry& other) const {
+    return !(*this == other);
+  }
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+/// @brief the object for collision or distance computation, contains the
+/// geometry and the transform information
+class COAL_DLLAPI CollisionObject {
+ public:
+  CollisionObject(const shared_ptr<CollisionGeometry>& cgeom_,
+                  bool compute_local_aabb = true)
+      : cgeom(cgeom_), user_data(nullptr) {
+    init(compute_local_aabb);
+  }
+
+  CollisionObject(const shared_ptr<CollisionGeometry>& cgeom_,
+                  const Transform3s& tf, bool compute_local_aabb = true)
+      : cgeom(cgeom_), t(tf), user_data(nullptr) {
+    init(compute_local_aabb);
+  }
+
+  CollisionObject(const shared_ptr<CollisionGeometry>& cgeom_,
+                  const Matrix3s& R, const Vec3s& T,
+                  bool compute_local_aabb = true)
+      : cgeom(cgeom_), t(R, T), user_data(nullptr) {
+    init(compute_local_aabb);
+  }
+
+  bool operator==(const CollisionObject& other) const {
+    return cgeom == other.cgeom && t == other.t && user_data == other.user_data;
+  }
+
+  bool operator!=(const CollisionObject& other) const {
+    return !(*this == other);
+  }
+
+  ~CollisionObject() {}
+
+  /// @brief get the type of the object
+  OBJECT_TYPE getObjectType() const { return cgeom->getObjectType(); }
+
+  /// @brief get the node type
+  NODE_TYPE getNodeType() const { return cgeom->getNodeType(); }
+
+  /// @brief get the AABB in world space
+  const AABB& getAABB() const { return aabb; }
+
+  /// @brief get the AABB in world space
+  AABB& getAABB() { return aabb; }
+
+  /// @brief compute the AABB in world space
+  void computeAABB() {
+    if (t.getRotation().isIdentity()) {
+      aabb = translate(cgeom->aabb_local, t.getTranslation());
+    } else {
+      aabb.min_ = aabb.max_ = t.getTranslation();
+
+      Vec3s min_world, max_world;
+      for (int k = 0; k < 3; ++k) {
+        min_world.array() = t.getRotation().row(k).array() *
+                            cgeom->aabb_local.min_.transpose().array();
+        max_world.array() = t.getRotation().row(k).array() *
+                            cgeom->aabb_local.max_.transpose().array();
+
+        aabb.min_[k] += (min_world.array().min)(max_world.array()).sum();
+        aabb.max_[k] += (min_world.array().max)(max_world.array()).sum();
+      }
+    }
+  }
+
+  /// @brief get user data in object
+  void* getUserData() const { return user_data; }
+
+  /// @brief set user data in object
+  void setUserData(void* data) { user_data = data; }
+
+  /// @brief get translation of the object
+  inline const Vec3s& getTranslation() const { return t.getTranslation(); }
+
+  /// @brief get matrix rotation of the object
+  inline const Matrix3s& getRotation() const { return t.getRotation(); }
+
+  /// @brief get object's transform
+  inline const Transform3s& getTransform() const { return t; }
+
+  /// @brief set object's rotation matrix
+  void setRotation(const Matrix3s& R) { t.setRotation(R); }
+
+  /// @brief set object's translation
+  void setTranslation(const Vec3s& T) { t.setTranslation(T); }
+
+  /// @brief set object's transform
+  void setTransform(const Matrix3s& R, const Vec3s& T) { t.setTransform(R, T); }
+
+  /// @brief set object's transform
+  void setTransform(const Transform3s& tf) { t = tf; }
+
+  /// @brief whether the object is in local coordinate
+  bool isIdentityTransform() const { return t.isIdentity(); }
+
+  /// @brief set the object in local coordinate
+  void setIdentityTransform() { t.setIdentity(); }
+
+  /// @brief get shared pointer to collision geometry of the object instance
+  const shared_ptr<const CollisionGeometry> collisionGeometry() const {
+    return cgeom;
+  }
+
+  /// @brief get shared pointer to collision geometry of the object instance
+  const shared_ptr<CollisionGeometry>& collisionGeometry() { return cgeom; }
+
+  /// @brief get raw pointer to collision geometry of the object instance
+  const CollisionGeometry* collisionGeometryPtr() const { return cgeom.get(); }
+
+  /// @brief get raw pointer to collision geometry of the object instance
+  CollisionGeometry* collisionGeometryPtr() { return cgeom.get(); }
+
+  /// @brief Associate a new CollisionGeometry
+  ///
+  /// @param[in] collision_geometry The new CollisionGeometry
+  /// @param[in] compute_local_aabb Whether the local aabb of the input new has
+  /// to be computed.
+  ///
+  void setCollisionGeometry(
+      const shared_ptr<CollisionGeometry>& collision_geometry,
+      bool compute_local_aabb = true) {
+    if (collision_geometry.get() != cgeom.get()) {
+      cgeom = collision_geometry;
+      init(compute_local_aabb);
+    }
+  }
+
+ protected:
+  void init(bool compute_local_aabb = true) {
+    if (cgeom) {
+      if (compute_local_aabb) cgeom->computeLocalAABB();
+      computeAABB();
+    }
+  }
+
+  shared_ptr<CollisionGeometry> cgeom;
+
+  Transform3s t;
+
+  /// @brief AABB in global coordinate
+  mutable AABB aabb;
+
+  /// @brief pointer to user defined data specific to this object
+  void* user_data;
+};
+
+}  // namespace coal
+
+#endif

include/coal/collision_utility.h

+// Copyright (c) 2017 CNRS
+// Copyright (c) 2022 INRIA
+// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
+//
+// This file is part of Coal.
+// Coal is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Coal is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// General Lesser Public License for more details.  You should have
+// received a copy of the GNU Lesser General Public License along with
+// Coal. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef COAL_COLLISION_UTILITY_H
+#define COAL_COLLISION_UTILITY_H
+
+#include "coal/collision_object.h"
+
+namespace coal {
+
+COAL_DLLAPI CollisionGeometry* extract(const CollisionGeometry* model,
+                                       const Transform3s& pose,
+                                       const AABB& aabb);
+
+/**
+ * \brief Returns the name associated to a NODE_TYPE
+ */
+inline const char* get_node_type_name(NODE_TYPE node_type) {
+  static const char* node_type_name_all[] = {
+      "BV_UNKNOWN",     "BV_AABB",       "BV_OBB",      "BV_RSS",
+      "BV_kIOS",        "BV_OBBRSS",     "BV_KDOP16",   "BV_KDOP18",
+      "BV_KDOP24",      "GEOM_BOX",      "GEOM_SPHERE", "GEOM_CAPSULE",
+      "GEOM_CONE",      "GEOM_CYLINDER", "GEOM_CONVEX", "GEOM_PLANE",
+      "GEOM_HALFSPACE", "GEOM_TRIANGLE", "GEOM_OCTREE", "GEOM_ELLIPSOID",
+      "HF_AABB",        "HF_OBBRSS",     "NODE_COUNT"};
+
+  return node_type_name_all[node_type];
+}
+
+/**
+ * \brief Returns the name associated to a OBJECT_TYPE
+ */
+inline const char* get_object_type_name(OBJECT_TYPE object_type) {
+  static const char* object_type_name_all[] = {
+      "OT_UNKNOWN", "OT_BVH", "OT_GEOM", "OT_OCTREE", "OT_HFIELD", "OT_COUNT"};
+
+  return object_type_name_all[object_type];
+}
+
+}  // namespace coal
+
+#endif  // COAL_COLLISION_UTILITY_H

include/coal/contact_patch.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 INRIA nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Louis Montaut */
+
+#ifndef COAL_CONTACT_PATCH_H
+#define COAL_CONTACT_PATCH_H
+
+#include "coal/data_types.h"
+#include "coal/collision_data.h"
+#include "coal/contact_patch/contact_patch_solver.h"
+#include "coal/contact_patch_func_matrix.h"
+
+namespace coal {
+
+/// @brief Main contact patch computation interface.
+/// @note Please see @ref ContactPatchRequest and @ref ContactPatchResult for
+/// more info on the content of the input/output of this function. Also, please
+/// read @ref ContactPatch if you want to fully understand what is meant by
+/// "contact patch".
+COAL_DLLAPI void computeContactPatch(const CollisionGeometry* o1,
+                                     const Transform3s& tf1,
+                                     const CollisionGeometry* o2,
+                                     const Transform3s& tf2,
+                                     const CollisionResult& collision_result,
+                                     const ContactPatchRequest& request,
+                                     ContactPatchResult& result);
+
+/// @copydoc computeContactPatch(const CollisionGeometry*, const Transform3s&,
+// const CollisionGeometry*, const Transform3s&, const CollisionResult&, const
+// ContactPatchRequest&, ContactPatchResult&);
+COAL_DLLAPI void computeContactPatch(const CollisionObject* o1,
+                                     const CollisionObject* o2,
+                                     const CollisionResult& collision_result,
+                                     const ContactPatchRequest& request,
+                                     ContactPatchResult& result);
+
+/// @brief This class reduces the cost of identifying the geometry pair.
+/// This is usefull for repeated shape-shape queries.
+/// @note This needs to be called after `collide` or after `ComputeCollision`.
+///
+/// \code
+///   ComputeContactPatch calc_patch (o1, o2);
+///   calc_patch(tf1, tf2, collision_result, patch_request, patch_result);
+/// \endcode
+class COAL_DLLAPI ComputeContactPatch {
+ public:
+  /// @brief Default constructor from two Collision Geometries.
+  ComputeContactPatch(const CollisionGeometry* o1, const CollisionGeometry* o2);
+
+  void operator()(const Transform3s& tf1, const Transform3s& tf2,
+                  const CollisionResult& collision_result,
+                  const ContactPatchRequest& request,
+                  ContactPatchResult& result) const;
+
+  bool operator==(const ComputeContactPatch& other) const {
+    return this->o1 == other.o1 && this->o2 == other.o2 &&
+           this->csolver == other.csolver;
+  }
+
+  bool operator!=(const ComputeContactPatch& other) const {
+    return !(*this == other);
+  }
+
+  virtual ~ComputeContactPatch() = default;
+
+ protected:
+  // These pointers are made mutable to let the derived classes to update
+  // their values when updating the collision geometry (e.g. creating a new
+  // one). This feature should be used carefully to avoid any mis usage (e.g,
+  // changing the type of the collision geometry should be avoided).
+  mutable const CollisionGeometry* o1;
+  mutable const CollisionGeometry* o2;
+
+  mutable ContactPatchSolver csolver;
+
+  ContactPatchFunctionMatrix::ContactPatchFunc func;
+  bool swap_geoms;
+
+  virtual void run(const Transform3s& tf1, const Transform3s& tf2,
+                   const CollisionResult& collision_result,
+                   const ContactPatchRequest& request,
+                   ContactPatchResult& result) const;
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+}  // namespace coal
+
+#endif

include/coal/contact_patch/contact_patch_solver.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 INRIA nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Louis Montaut */
+
+#ifndef COAL_CONTACT_PATCH_SOLVER_H
+#define COAL_CONTACT_PATCH_SOLVER_H
+
+#include "coal/collision_data.h"
+#include "coal/logging.h"
+#include "coal/narrowphase/gjk.h"
+
+namespace coal {
+
+/// @brief Solver to compute contact patches, i.e. the intersection between two
+/// contact surfaces projected onto the shapes' separating plane.
+/// Otherwise said, a contact patch is simply the intersection between two
+/// support sets: the support set of shape S1 in direction `n` and the support
+/// set of shape S2 in direction `-n`, where `n` is the contact normal
+/// (satisfying the optimality conditions of GJK/EPA).
+/// @note A contact patch is **not** the support set of the Minkowski Difference
+/// in the direction of the normal.
+/// A contact patch is actually the support set of the Minkowski difference in
+/// the direction of the normal, i.e. the instersection of the shapes support
+/// sets as mentioned above.
+///
+/// TODO(louis): algo improvement:
+/// - The clipping algo is currently n1 * n2; it can be done in n1 + n2.
+struct COAL_DLLAPI ContactPatchSolver {
+  // Note: `ContactPatch` is an alias for `SupportSet`.
+  // The two can be used interchangeably.
+  using ShapeSupportData = details::ShapeSupportData;
+  using SupportSetDirection = SupportSet::PatchDirection;
+
+  /// @brief Support set function for shape si.
+  /// @param[in] shape the shape.
+  /// @param[in/out] support_set a support set of the shape. A support set is
+  /// attached to a frame. All the points of the set computed by this function
+  /// will be expressed in the local frame of the support set. The support set
+  /// is computed in the direction of the positive z-axis if its direction is
+  /// DEFAULT, negative z-axis if its direction is INVERTED.
+  /// @param[in/out] hint for the support computation of ConvexBase shapes. Gets
+  /// updated after calling the function onto ConvexBase shapes.
+  /// @param[in/out] support_data for the support computation of ConvexBase
+  /// shapes. Gets updated with visited vertices after calling the function onto
+  /// ConvexBase shapes.
+  /// @param[in] num_sampled_supports for shapes like cone or cylinders which
+  /// have smooth non-strictly convex sides (their bases are circles), we need
+  /// to know how many supports we sample from these sides. For any other shape,
+  /// this parameter is not used.
+  /// @param[in] tol the "thickness" of the support plane. Any point v which
+  /// satisfies `max_{x in shape}(x.dot(dir)) - v.dot(dir) <= tol` is tol
+  /// distant from the support plane and is added to the support set.
+  typedef void (*SupportSetFunction)(const ShapeBase* shape,
+                                     SupportSet& support_set, int& hint,
+                                     ShapeSupportData& support_data,
+                                     size_t num_sampled_supports,
+                                     CoalScalar tol);
+
+  /// @brief Number of vectors to pre-allocate in the `m_clipping_sets` vectors.
+  static constexpr size_t default_num_preallocated_supports = 16;
+
+  /// @brief Number of points sampled for Cone and Cylinder when the normal is
+  /// orthogonal to the shapes' basis.
+  /// See @ref ContactPatchRequest::m_num_samples_curved_shapes for more
+  /// details.
+  size_t num_samples_curved_shapes;
+
+  /// @brief Tolerance below which points are added to the shapes support sets.
+  /// See @ref ContactPatchRequest::m_patch_tolerance for more details.
+  CoalScalar patch_tolerance;
+
+  /// @brief Support set function for shape s1.
+  mutable SupportSetFunction supportFuncShape1;
+
+  /// @brief Support set function for shape s2.
+  mutable SupportSetFunction supportFuncShape2;
+
+  /// @brief Temporary data to compute the support sets on each shape.
+  mutable std::array<ShapeSupportData, 2> supports_data;
+
+  /// @brief Guess for the support sets computation.
+  mutable support_func_guess_t support_guess;
+
+  /// @brief Holder for support set of shape 1, used for internal computation.
+  /// After `computePatch` has been called, this support set is no longer valid.
+  mutable SupportSet support_set_shape1;
+
+  /// @brief Holder for support set of shape 2, used for internal computation.
+  /// After `computePatch` has been called, this support set is no longer valid.
+  mutable SupportSet support_set_shape2;
+
+  /// @brief Temporary support set used for the Sutherland-Hodgman algorithm.
+  mutable SupportSet support_set_buffer;
+
+  /// @brief Tracks which point of the Sutherland-Hodgman result have been added
+  /// to the contact patch. Only used if the post-processing step occurs, i.e.
+  /// if the result of Sutherland-Hodgman has a size bigger than
+  /// `max_patch_size`.
+  mutable std::vector<bool> added_to_patch;
+
+  /// @brief Default constructor.
+  explicit ContactPatchSolver() {
+    const size_t num_contact_patch = 1;
+    const size_t preallocated_patch_size =
+        ContactPatch::default_preallocated_size;
+    const CoalScalar patch_tolerance = 1e-3;
+    const ContactPatchRequest request(num_contact_patch,
+                                      preallocated_patch_size, patch_tolerance);
+    this->set(request);
+  }
+
+  /// @brief Construct the solver with a `ContactPatchRequest`.
+  explicit ContactPatchSolver(const ContactPatchRequest& request) {
+    this->set(request);
+  }
+
+  /// @brief Set up the solver using a `ContactPatchRequest`.
+  void set(const ContactPatchRequest& request);
+
+  /// @brief Sets the support guess used during support set computation of
+  /// shapes s1 and s2.
+  void setSupportGuess(const support_func_guess_t guess) const {
+    this->support_guess = guess;
+  }
+
+  /// @brief Main API of the solver: compute a contact patch from a contact
+  /// between shapes s1 and s2.
+  /// The contact patch is the (triple) intersection between the separating
+  /// plane passing (by `contact.pos` and supported by `contact.normal`) and the
+  /// shapes s1 and s2.
+  template <typename ShapeType1, typename ShapeType2>
+  void computePatch(const ShapeType1& s1, const Transform3s& tf1,
+                    const ShapeType2& s2, const Transform3s& tf2,
+                    const Contact& contact, ContactPatch& contact_patch) const;
+
+  /// @brief Reset the internal quantities of the solver.
+  template <typename ShapeType1, typename ShapeType2>
+  void reset(const ShapeType1& shape1, const Transform3s& tf1,
+             const ShapeType2& shape2, const Transform3s& tf2,
+             const ContactPatch& contact_patch) const;
+
+  /// @brief Retrieve result, adds a post-processing step if result has bigger
+  /// size than `this->max_patch_size`.
+  void getResult(const Contact& contact, const ContactPatch::Polygon* result,
+                 ContactPatch& contact_patch) const;
+
+  /// @return the intersecting point between line defined by ray (a, b) and
+  /// the segment [c, d].
+  /// @note we make the following hypothesis:
+  /// 1) c != d (should be when creating initial polytopes)
+  /// 2) (c, d) is not parallel to ray -> if so, we return d.
+  static Vec2s computeLineSegmentIntersection(const Vec2s& a, const Vec2s& b,
+                                              const Vec2s& c, const Vec2s& d);
+
+  /// @brief Construct support set function for shape.
+  static SupportSetFunction makeSupportSetFunction(
+      const ShapeBase* shape, ShapeSupportData& support_data);
+
+  bool operator==(const ContactPatchSolver& other) const {
+    return this->num_samples_curved_shapes == other.num_samples_curved_shapes &&
+           this->patch_tolerance == other.patch_tolerance &&
+           this->support_guess == other.support_guess &&
+           this->support_set_shape1 == other.support_set_shape1 &&
+           this->support_set_shape2 == other.support_set_shape2 &&
+           this->support_set_buffer == other.support_set_buffer &&
+           this->added_to_patch == other.added_to_patch &&
+           this->supportFuncShape1 == other.supportFuncShape1 &&
+           this->supportFuncShape2 == other.supportFuncShape2;
+  }
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+}  // namespace coal
+
+#include "coal/contact_patch/contact_patch_solver.hxx"
+
+#endif  // COAL_CONTACT_PATCH_SOLVER_H

include/coal/contact_patch/contact_patch_solver.hxx

+/*
+ * 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 INRIA nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Louis Montaut */
+
+#ifndef COAL_CONTACT_PATCH_SOLVER_HXX
+#define COAL_CONTACT_PATCH_SOLVER_HXX
+
+#include "coal/data_types.h"
+#include "coal/shape/geometric_shapes_traits.h"
+
+namespace coal {
+
+// ============================================================================
+inline void ContactPatchSolver::set(const ContactPatchRequest& request) {
+  // Note: it's important for the number of pre-allocated Vec3s in
+  // `m_clipping_sets` to be larger than `request.max_size_patch`
+  // because we don't know in advance how many supports will be discarded to
+  // form the convex-hulls of the shapes supports which will serve as the
+  // input of the Sutherland-Hodgman algorithm.
+  size_t num_preallocated_supports = default_num_preallocated_supports;
+  if (num_preallocated_supports < 2 * request.getNumSamplesCurvedShapes()) {
+    num_preallocated_supports = 2 * request.getNumSamplesCurvedShapes();
+  }
+
+  // Used for support set computation of shape1 and for the first iterate of the
+  // Sutherland-Hodgman algo.
+  this->support_set_shape1.points().reserve(num_preallocated_supports);
+  this->support_set_shape1.direction = SupportSetDirection::DEFAULT;
+
+  // Used for computing the next iterate of the Sutherland-Hodgman algo.
+  this->support_set_buffer.points().reserve(num_preallocated_supports);
+
+  // Used for support set computation of shape2 and acts as the "clipper" set in
+  // the Sutherland-Hodgman algo.
+  this->support_set_shape2.points().reserve(num_preallocated_supports);
+  this->support_set_shape2.direction = SupportSetDirection::INVERTED;
+
+  this->num_samples_curved_shapes = request.getNumSamplesCurvedShapes();
+  this->patch_tolerance = request.getPatchTolerance();
+}
+
+// ============================================================================
+template <typename ShapeType1, typename ShapeType2>
+void ContactPatchSolver::computePatch(const ShapeType1& s1,
+                                      const Transform3s& tf1,
+                                      const ShapeType2& s2,
+                                      const Transform3s& tf2,
+                                      const Contact& contact,
+                                      ContactPatch& contact_patch) const {
+  // Note: `ContactPatch` is an alias for `SupportSet`.
+  // Step 1
+  constructContactPatchFrameFromContact(contact, contact_patch);
+  contact_patch.points().clear();
+  if ((bool)(shape_traits<ShapeType1>::IsStrictlyConvex) ||
+      (bool)(shape_traits<ShapeType2>::IsStrictlyConvex)) {
+    // If a shape is strictly convex, the support set in any direction is
+    // reduced to a single point. Thus, the contact point `contact.pos` is the
+    // only point belonging to the contact patch, and it has already been
+    // computed.
+    // TODO(louis): even for strictly convex shapes, we can sample the support
+    // function around the normal and return a pseudo support set. This would
+    // allow spheres and ellipsoids to have a contact surface, which does make
+    // sense in certain physics simulation cases.
+    // Do the same for strictly convex regions of non-strictly convex shapes
+    // like the ends of capsules.
+    contact_patch.addPoint(contact.pos);
+    return;
+  }
+
+  // Step 2 - Compute support set of each shape, in the direction of
+  // the contact's normal.
+  // The first shape's support set is called "current"; it will be the first
+  // iterate of the Sutherland-Hodgman algorithm. The second shape's support set
+  // is called "clipper"; it will be used to clip "current". The support set
+  // computation step computes a convex polygon; its vertices are ordered
+  // counter-clockwise. This is important as the Sutherland-Hodgman algorithm
+  // expects points to be ranked counter-clockwise.
+  this->reset(s1, tf1, s2, tf2, contact_patch);
+  assert(this->num_samples_curved_shapes > 3);
+
+  this->supportFuncShape1(&s1, this->support_set_shape1, this->support_guess[0],
+                          this->supports_data[0],
+                          this->num_samples_curved_shapes,
+                          this->patch_tolerance);
+
+  this->supportFuncShape2(&s2, this->support_set_shape2, this->support_guess[1],
+                          this->supports_data[1],
+                          this->num_samples_curved_shapes,
+                          this->patch_tolerance);
+
+  // We can immediatly return if one of the support set has only
+  // one point.
+  if (this->support_set_shape1.size() <= 1 ||
+      this->support_set_shape2.size() <= 1) {
+    contact_patch.addPoint(contact.pos);
+    return;
+  }
+
+  // `eps` is be used to check strict positivity of determinants.
+  const CoalScalar eps = Eigen::NumTraits<CoalScalar>::dummy_precision();
+  using Polygon = SupportSet::Polygon;
+
+  if ((this->support_set_shape1.size() == 2) &&
+      (this->support_set_shape2.size() == 2)) {
+    // Segment-Segment case
+    // We compute the determinant; if it is non-zero, the intersection
+    // has already been computed: it's `Contact::pos`.
+    const Polygon& pts1 = this->support_set_shape1.points();
+    const Vec2s& a = pts1[0];
+    const Vec2s& b = pts1[1];
+
+    const Polygon& pts2 = this->support_set_shape2.points();
+    const Vec2s& c = pts2[0];
+    const Vec2s& d = pts2[1];
+
+    const CoalScalar det =
+        (b(0) - a(0)) * (d(1) - c(1)) >= (b(1) - a(1)) * (d(0) - c(0));
+    if ((std::abs(det) > eps) || ((c - d).squaredNorm() < eps) ||
+        ((b - a).squaredNorm() < eps)) {
+      contact_patch.addPoint(contact.pos);
+      return;
+    }
+
+    const Vec2s cd = (d - c);
+    const CoalScalar l = cd.squaredNorm();
+    Polygon& patch = contact_patch.points();
+
+    // Project a onto [c, d]
+    CoalScalar t1 = (a - c).dot(cd);
+    t1 = (t1 >= l) ? 1.0 : ((t1 <= 0) ? 0.0 : (t1 / l));
+    const Vec2s p1 = c + t1 * cd;
+    patch.emplace_back(p1);
+
+    // Project b onto [c, d]
+    CoalScalar t2 = (b - c).dot(cd);
+    t2 = (t2 >= l) ? 1.0 : ((t2 <= 0) ? 0.0 : (t2 / l));
+    const Vec2s p2 = c + t2 * cd;
+    if ((p1 - p2).squaredNorm() >= eps) {
+      patch.emplace_back(p2);
+    }
+    return;
+  }
+
+  //
+  // Step 3 - Main loop of the algorithm: use the "clipper" polygon to clip the
+  // "current" polygon. The resulting intersection is the contact patch of the
+  // contact between s1 and s2. "clipper" and "current" are the support sets of
+  // shape1 and shape2 (they can be swapped, i.e. clipper can be assigned to
+  // shape1 and current to shape2, depending on which case we are). Currently,
+  // to clip one polygon with the other, we use the Sutherland-Hodgman
+  // algorithm:
+  // https://en.wikipedia.org/wiki/Sutherland%E2%80%93Hodgman_algorithm
+  // In the general case, Sutherland-Hodgman clips one polygon of size >=3 using
+  // another polygon of size >=3. However, it can be easily extended to handle
+  // the segment-polygon case.
+  //
+  // The maximum size of the output of the Sutherland-Hodgman algorithm is n1 +
+  // n2 where n1 and n2 are the sizes of the first and second polygon.
+  const size_t max_result_size =
+      this->support_set_shape1.size() + this->support_set_shape2.size();
+  if (this->added_to_patch.size() < max_result_size) {
+    this->added_to_patch.assign(max_result_size, false);
+  }
+
+  const Polygon* clipper_ptr = nullptr;
+  Polygon* current_ptr = nullptr;
+  Polygon* previous_ptr = &(this->support_set_buffer.points());
+
+  // Let the clipper set be the one with the most vertices, to make sure it is
+  // at least a triangle.
+  if (this->support_set_shape1.size() < this->support_set_shape2.size()) {
+    current_ptr = &(this->support_set_shape1.points());
+    clipper_ptr = &(this->support_set_shape2.points());
+  } else {
+    current_ptr = &(this->support_set_shape2.points());
+    clipper_ptr = &(this->support_set_shape1.points());
+  }
+
+  const Polygon& clipper = *(clipper_ptr);
+  const size_t clipper_size = clipper.size();
+  for (size_t i = 0; i < clipper_size; ++i) {
+    // Swap `current` and `previous`.
+    // `previous` tracks the last iteration of the algorithm; `current` is
+    // filled by clipping `current` using `clipper`.
+    Polygon* tmp_ptr = previous_ptr;
+    previous_ptr = current_ptr;
+    current_ptr = tmp_ptr;
+
+    const Polygon& previous = *(previous_ptr);
+    Polygon& current = *(current_ptr);
+    current.clear();
+
+    const Vec2s& a = clipper[i];
+    const Vec2s& b = clipper[(i + 1) % clipper_size];
+    const Vec2s ab = b - a;
+
+    if (previous.size() == 2) {
+      //
+      // Segment-Polygon case
+      //
+      const Vec2s& p1 = previous[0];
+      const Vec2s& p2 = previous[1];
+
+      const Vec2s ap1 = p1 - a;
+      const Vec2s ap2 = p2 - a;
+
+      const CoalScalar det1 = ab(0) * ap1(1) - ab(1) * ap1(0);
+      const CoalScalar det2 = ab(0) * ap2(1) - ab(1) * ap2(0);
+
+      if (det1 < 0 && det2 < 0) {
+        // Both p1 and p2 are outside the clipping polygon, i.e. there is no
+        // intersection. The algorithm can stop.
+        break;
+      }
+
+      if (det1 >= 0 && det2 >= 0) {
+        // Both p1 and p2 are inside the clipping polygon, there is nothing to
+        // do; move to the next iteration.
+        current = previous;
+        continue;
+      }
+
+      // Compute the intersection between the line (a, b) and the segment
+      // [p1, p2].
+      if (det1 >= 0) {
+        if (det1 > eps) {
+          const Vec2s p = computeLineSegmentIntersection(a, b, p1, p2);
+          current.emplace_back(p1);
+          current.emplace_back(p);
+          continue;
+        } else {
+          // p1 is the only point of current which is also a point of the
+          // clipper. We can exit.
+          current.emplace_back(p1);
+          break;
+        }
+      } else {
+        if (det2 > eps) {
+          const Vec2s p = computeLineSegmentIntersection(a, b, p1, p2);
+          current.emplace_back(p2);
+          current.emplace_back(p);
+          continue;
+        } else {
+          // p2 is the only point of current which is also a point of the
+          // clipper. We can exit.
+          current.emplace_back(p2);
+          break;
+        }
+      }
+    } else {
+      //
+      // Polygon-Polygon case.
+      //
+      std::fill(this->added_to_patch.begin(),  //
+                this->added_to_patch.end(),    //
+                false);
+
+      const size_t previous_size = previous.size();
+      for (size_t j = 0; j < previous_size; ++j) {
+        const Vec2s& p1 = previous[j];
+        const Vec2s& p2 = previous[(j + 1) % previous_size];
+
+        const Vec2s ap1 = p1 - a;
+        const Vec2s ap2 = p2 - a;
+
+        const CoalScalar det1 = ab(0) * ap1(1) - ab(1) * ap1(0);
+        const CoalScalar det2 = ab(0) * ap2(1) - ab(1) * ap2(0);
+
+        if (det1 < 0 && det2 < 0) {
+          // No intersection. Continue to next segment of previous.
+          continue;
+        }
+
+        if (det1 >= 0 && det2 >= 0) {
+          // Both p1 and p2 are inside the clipping polygon, add p1 to current
+          // (only if it has not already been added).
+          if (!this->added_to_patch[j]) {
+            current.emplace_back(p1);
+            this->added_to_patch[j] = true;
+          }
+          // Continue to next segment of previous.
+          continue;
+        }
+
+        if (det1 >= 0) {
+          if (det1 > eps) {
+            if (!this->added_to_patch[j]) {
+              current.emplace_back(p1);
+              this->added_to_patch[j] = true;
+            }
+            const Vec2s p = computeLineSegmentIntersection(a, b, p1, p2);
+            current.emplace_back(p);
+          } else {
+            // a, b and p1 are colinear; we add only p1.
+            if (!this->added_to_patch[j]) {
+              current.emplace_back(p1);
+              this->added_to_patch[j] = true;
+            }
+          }
+        } else {
+          if (det2 > eps) {
+            const Vec2s p = computeLineSegmentIntersection(a, b, p1, p2);
+            current.emplace_back(p);
+          } else {
+            if (!this->added_to_patch[(j + 1) % previous.size()]) {
+              current.emplace_back(p2);
+              this->added_to_patch[(j + 1) % previous.size()] = true;
+            }
+          }
+        }
+      }
+    }
+    //
+    // End of iteration i of Sutherland-Hodgman.
+    if (current.size() <= 1) {
+      // No intersection or one point found, the algo can early stop.
+      break;
+    }
+  }
+
+  // Transfer the result of the Sutherland-Hodgman algorithm to the contact
+  // patch.
+  this->getResult(contact, current_ptr, contact_patch);
+}
+
+// ============================================================================
+inline void ContactPatchSolver::getResult(
+    const Contact& contact, const ContactPatch::Polygon* result_ptr,
+    ContactPatch& contact_patch) const {
+  if (result_ptr->size() <= 1) {
+    contact_patch.addPoint(contact.pos);
+    return;
+  }
+
+  const ContactPatch::Polygon& result = *(result_ptr);
+  ContactPatch::Polygon& patch = contact_patch.points();
+  patch = result;
+}
+
+// ============================================================================
+template <typename ShapeType1, typename ShapeType2>
+inline void ContactPatchSolver::reset(const ShapeType1& shape1,
+                                      const Transform3s& tf1,
+                                      const ShapeType2& shape2,
+                                      const Transform3s& tf2,
+                                      const ContactPatch& contact_patch) const {
+  // Reset internal quantities
+  this->support_set_shape1.clear();
+  this->support_set_shape2.clear();
+  this->support_set_buffer.clear();
+
+  // Get the support function of each shape
+  const Transform3s& tfc = contact_patch.tf;
+
+  this->support_set_shape1.direction = SupportSetDirection::DEFAULT;
+  // Set the reference frame of the support set of the first shape to be the
+  // local frame of shape 1.
+  Transform3s& tf1c = this->support_set_shape1.tf;
+  tf1c.rotation().noalias() = tf1.rotation().transpose() * tfc.rotation();
+  tf1c.translation().noalias() =
+      tf1.rotation().transpose() * (tfc.translation() - tf1.translation());
+  this->supportFuncShape1 =
+      this->makeSupportSetFunction(&shape1, this->supports_data[0]);
+
+  this->support_set_shape2.direction = SupportSetDirection::INVERTED;
+  // Set the reference frame of the support set of the second shape to be the
+  // local frame of shape 2.
+  Transform3s& tf2c = this->support_set_shape2.tf;
+  tf2c.rotation().noalias() = tf2.rotation().transpose() * tfc.rotation();
+  tf2c.translation().noalias() =
+      tf2.rotation().transpose() * (tfc.translation() - tf2.translation());
+  this->supportFuncShape2 =
+      this->makeSupportSetFunction(&shape2, this->supports_data[1]);
+}
+
+// ==========================================================================
+inline Vec2s ContactPatchSolver::computeLineSegmentIntersection(
+    const Vec2s& a, const Vec2s& b, const Vec2s& c, const Vec2s& d) {
+  const Vec2s ab = b - a;
+  const Vec2s n(-ab(1), ab(0));
+  const CoalScalar denominator = n.dot(c - d);
+  if (std::abs(denominator) < std::numeric_limits<double>::epsilon()) {
+    return d;
+  }
+  const CoalScalar nominator = n.dot(a - d);
+  CoalScalar alpha = nominator / denominator;
+  alpha = std::min<double>(1.0, std::max<CoalScalar>(0.0, alpha));
+  return alpha * c + (1 - alpha) * d;
+}
+
+}  // namespace coal
+
+#endif  // COAL_CONTACT_PATCH_SOLVER_HXX

include/coal/contact_patch_func_matrix.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 INRIA nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Louis Montaut */
+
+#ifndef COAL_CONTACT_PATCH_FUNC_MATRIX_H
+#define COAL_CONTACT_PATCH_FUNC_MATRIX_H
+
+#include "coal/collision_data.h"
+#include "coal/contact_patch/contact_patch_solver.h"
+#include "coal/narrowphase/narrowphase.h"
+
+namespace coal {
+
+/// @brief The contact patch matrix stores the functions for contact patches
+/// computation between different types of objects and provides a uniform call
+/// interface
+struct COAL_DLLAPI ContactPatchFunctionMatrix {
+  /// @brief the uniform call interface for computing contact patches: we need
+  /// know
+  /// 1. two objects o1 and o2 and their configuration in world coordinate tf1
+  ///    and tf2;
+  /// 2. the collision result that generated contact patches candidates
+  ///    (`coal::Contact`), from which contact patches will be expanded;
+  /// 3. the solver for computation of contact patches;
+  /// 4. the request setting for contact patches (e.g. maximum amount of
+  ///    patches, patch tolerance etc.)
+  /// 5. the structure to return contact patches
+  ///    (`coal::ContactPatchResult`).
+  ///
+  /// Note: we pass a GJKSolver, because it allows to reuse internal computation
+  /// that was made during the narrow phase. It also allows to experiment with
+  /// different ways to compute contact patches. We could, for example, perturb
+  /// tf1 and tf2 and make multiple calls to the GJKSolver (although this is not
+  /// the approach done by default).
+  typedef void (*ContactPatchFunc)(const CollisionGeometry* o1,
+                                   const Transform3s& tf1,
+                                   const CollisionGeometry* o2,
+                                   const Transform3s& tf2,
+                                   const CollisionResult& collision_result,
+                                   const ContactPatchSolver* csolver,
+                                   const ContactPatchRequest& request,
+                                   ContactPatchResult& result);
+
+  /// @brief Each item in the contact patch matrix is a function to handle
+  /// contact patch computation between objects of type1 and type2.
+  ContactPatchFunc contact_patch_matrix[NODE_COUNT][NODE_COUNT];
+
+  ContactPatchFunctionMatrix();
+};
+
+}  // namespace coal
+
+#endif

include/coal/data_types.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, Open Source Robotics Foundation
+ *  Copyright (c) 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_DATA_TYPES_H
+#define COAL_DATA_TYPES_H
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include "coal/config.hh"
+
+#ifdef COAL_HAS_OCTOMAP
+#define OCTOMAP_VERSION_AT_LEAST(x, y, z) \
+  (OCTOMAP_MAJOR_VERSION > x ||           \
+   (OCTOMAP_MAJOR_VERSION >= x &&         \
+    (OCTOMAP_MINOR_VERSION > y ||         \
+     (OCTOMAP_MINOR_VERSION >= y && OCTOMAP_PATCH_VERSION >= z))))
+
+#define OCTOMAP_VERSION_AT_MOST(x, y, z) \
+  (OCTOMAP_MAJOR_VERSION < x ||          \
+   (OCTOMAP_MAJOR_VERSION <= x &&        \
+    (OCTOMAP_MINOR_VERSION < y ||        \
+     (OCTOMAP_MINOR_VERSION <= y && OCTOMAP_PATCH_VERSION <= z))))
+#endif  // COAL_HAS_OCTOMAP
+
+namespace coal {
+#ifdef COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL
+// We keep the FCL_REAL typedef and the Vec[..]f typedefs for backward
+// compatibility.
+typedef double FCL_REAL;
+typedef Eigen::Matrix<FCL_REAL, 3, 1> Vec3f;
+typedef Eigen::Matrix<FCL_REAL, 2, 1> Vec2f;
+typedef Eigen::Matrix<FCL_REAL, 6, 1> Vec6f;
+typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, 1> VecXf;
+typedef Eigen::Matrix<FCL_REAL, 3, 3> Matrix3f;
+typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, 3, Eigen::RowMajor> Matrixx3f;
+typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, 2, Eigen::RowMajor> Matrixx2f;
+typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, Eigen::Dynamic> MatrixXf;
+#endif
+
+typedef double CoalScalar;
+typedef Eigen::Matrix<CoalScalar, 3, 1> Vec3s;
+typedef Eigen::Matrix<CoalScalar, 2, 1> Vec2s;
+typedef Eigen::Matrix<CoalScalar, 6, 1> Vec6s;
+typedef Eigen::Matrix<CoalScalar, Eigen::Dynamic, 1> VecXs;
+typedef Eigen::Matrix<CoalScalar, 3, 3> Matrix3s;
+typedef Eigen::Matrix<CoalScalar, Eigen::Dynamic, 3, Eigen::RowMajor> MatrixX3s;
+typedef Eigen::Matrix<CoalScalar, Eigen::Dynamic, 2, Eigen::RowMajor> MatrixX2s;
+typedef Eigen::Matrix<Eigen::DenseIndex, Eigen::Dynamic, 3, Eigen::RowMajor>
+    Matrixx3i;
+typedef Eigen::Matrix<CoalScalar, Eigen::Dynamic, Eigen::Dynamic> MatrixXs;
+typedef Eigen::Vector2i support_func_guess_t;
+
+/// @brief Initial guess to use for the GJK algorithm
+/// DefaultGuess: Vec3s(1, 0, 0)
+/// CachedGuess: previous vector found by GJK or guess cached by the user
+/// BoundingVolumeGuess: guess using the centers of the shapes' AABB
+/// WARNING: to use BoundingVolumeGuess, computeLocalAABB must have been called
+/// on the two shapes.
+enum GJKInitialGuess { DefaultGuess, CachedGuess, BoundingVolumeGuess };
+
+/// @brief Variant to use for the GJK algorithm
+enum GJKVariant { DefaultGJK, PolyakAcceleration, NesterovAcceleration };
+
+/// @brief Which convergence criterion is used to stop the algorithm (when the
+/// shapes are not in collision). (default) VDB: Van den Bergen (A Fast and
+/// Robust GJK Implementation, 1999) DG: duality-gap, as used in the Frank-Wolfe
+/// and the vanilla 1988 GJK algorithms Hybrid: a mix between VDB and DG.
+enum GJKConvergenceCriterion { Default, DualityGap, Hybrid };
+
+/// @brief Wether the convergence criterion is scaled on the norm of the
+/// solution or not
+enum GJKConvergenceCriterionType { Relative, Absolute };
+
+/// @brief Triangle with 3 indices for points
+class COAL_DLLAPI Triangle {
+ public:
+  typedef std::size_t index_type;
+  typedef int size_type;
+
+  /// @brief Default constructor
+  Triangle() {}
+
+  /// @brief Create a triangle with given vertex indices
+  Triangle(index_type p1, index_type p2, index_type p3) { set(p1, p2, p3); }
+
+  /// @brief Set the vertex indices of the triangle
+  inline void set(index_type p1, index_type p2, index_type p3) {
+    vids[0] = p1;
+    vids[1] = p2;
+    vids[2] = p3;
+  }
+
+  /// @brief Access the triangle index
+  inline index_type operator[](index_type i) const { return vids[i]; }
+
+  inline index_type& operator[](index_type i) { return vids[i]; }
+
+  static inline size_type size() { return 3; }
+
+  bool operator==(const Triangle& other) const {
+    return vids[0] == other.vids[0] && vids[1] == other.vids[1] &&
+           vids[2] == other.vids[2];
+  }
+
+  bool operator!=(const Triangle& other) const { return !(*this == other); }
+
+  bool isValid() const {
+    return vids[0] != (std::numeric_limits<index_type>::max)() &&
+           vids[1] != (std::numeric_limits<index_type>::max)() &&
+           vids[2] != (std::numeric_limits<index_type>::max)();
+  }
+
+ private:
+  /// @brief indices for each vertex of triangle
+  index_type vids[3] = {(std::numeric_limits<index_type>::max)(),
+                        (std::numeric_limits<index_type>::max)(),
+                        (std::numeric_limits<index_type>::max)()};
+};
+
+/// @brief Quadrilateral with 4 indices for points
+struct COAL_DLLAPI Quadrilateral {
+  typedef std::size_t index_type;
+  typedef int size_type;
+
+  Quadrilateral() {}
+
+  Quadrilateral(index_type p0, index_type p1, index_type p2, index_type p3) {
+    set(p0, p1, p2, p3);
+  }
+
+  /// @brief Set the vertex indices of the quadrilateral
+  inline void set(index_type p0, index_type p1, index_type p2, index_type p3) {
+    vids[0] = p0;
+    vids[1] = p1;
+    vids[2] = p2;
+    vids[3] = p3;
+  }
+
+  /// @access the quadrilateral index
+  inline index_type operator[](index_type i) const { return vids[i]; }
+
+  inline index_type& operator[](index_type i) { return vids[i]; }
+
+  static inline size_type size() { return 4; }
+
+  bool operator==(const Quadrilateral& other) const {
+    return vids[0] == other.vids[0] && vids[1] == other.vids[1] &&
+           vids[2] == other.vids[2] && vids[3] == other.vids[3];
+  }
+
+  bool operator!=(const Quadrilateral& other) const {
+    return !(*this == other);
+  }
+
+ private:
+  index_type vids[4];
+};
+
+}  // namespace coal
+
+#endif

include/coal/distance.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_DISTANCE_H
+#define COAL_DISTANCE_H
+
+#include "coal/collision_object.h"
+#include "coal/collision_data.h"
+#include "coal/distance_func_matrix.h"
+#include "coal/timings.h"
+
+namespace coal {
+
+/// @brief Main distance interface: given two collision objects, and the
+/// requirements for contacts, including whether return the nearest points, this
+/// function performs the distance between them. Return value is the minimum
+/// distance generated between the two objects.
+COAL_DLLAPI CoalScalar distance(const CollisionObject* o1,
+                                const CollisionObject* o2,
+                                const DistanceRequest& request,
+                                DistanceResult& result);
+
+/// @copydoc distance(const CollisionObject*, const CollisionObject*, const
+/// DistanceRequest&, DistanceResult&)
+COAL_DLLAPI CoalScalar distance(const CollisionGeometry* o1,
+                                const Transform3s& tf1,
+                                const CollisionGeometry* o2,
+                                const Transform3s& tf2,
+                                const DistanceRequest& request,
+                                DistanceResult& result);
+
+/// This class reduces the cost of identifying the geometry pair.
+/// This is mostly useful for repeated shape-shape queries.
+///
+/// \code
+///   ComputeDistance calc_distance (o1, o2);
+///   CoalScalar distance = calc_distance(tf1, tf2, request, result);
+/// \endcode
+class COAL_DLLAPI ComputeDistance {
+ public:
+  ComputeDistance(const CollisionGeometry* o1, const CollisionGeometry* o2);
+
+  CoalScalar operator()(const Transform3s& tf1, const Transform3s& tf2,
+                        const DistanceRequest& request,
+                        DistanceResult& result) const;
+
+  bool operator==(const ComputeDistance& other) const {
+    return o1 == other.o1 && o2 == other.o2 && swap_geoms == other.swap_geoms &&
+           solver == other.solver && func == other.func;
+  }
+
+  bool operator!=(const ComputeDistance& other) const {
+    return !(*this == other);
+  }
+
+  virtual ~ComputeDistance() {};
+
+ protected:
+  // These pointers are made mutable to let the derived classes to update
+  // their values when updating the collision geometry (e.g. creating a new
+  // one). This feature should be used carefully to avoid any mis usage (e.g,
+  // changing the type of the collision geometry should be avoided).
+  mutable const CollisionGeometry* o1;
+  mutable const CollisionGeometry* o2;
+
+  mutable GJKSolver solver;
+
+  DistanceFunctionMatrix::DistanceFunc func;
+  bool swap_geoms;
+
+  virtual CoalScalar run(const Transform3s& tf1, const Transform3s& tf2,
+                         const DistanceRequest& request,
+                         DistanceResult& result) const;
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+}  // namespace coal
+
+#endif

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

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

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

include/coal/fwd.hh

+//
+// Software License Agreement (BSD License)
+//
+//  Copyright (c) 2014, CNRS-LAAS
+//  Copyright (c) 2022-2024, Inria
+//  Author: Florent Lamiraux
+//
+//  All rights reserved.
+//
+//  Redistribution and use in source and binary forms, with or without
+//  modification, are permitted provided that the following conditions
+//  are met:
+//
+//   * Redistributions of source code must retain the above copyright
+//     notice, this list of conditions and the following disclaimer.
+//   * Redistributions in binary form must reproduce the above
+//     copyright notice, this list of conditions and the following
+//     disclaimer in the documentation and/or other materials provided
+//     with the distribution.
+//   * Neither the name of CNRS-LAAS nor the names of its
+//     contributors may be used to endorse or promote products derived
+//     from this software without specific prior written permission.
+//
+//  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+//  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+//  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+//  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+//  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+//  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+//  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+//  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+//  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+//  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+//  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+//  POSSIBILITY OF SUCH DAMAGE.
+//
+
+#ifndef COAL_FWD_HH
+#define COAL_FWD_HH
+
+#include <cassert>
+#include <memory>
+#include <sstream>
+#include <stdexcept>
+
+#include "coal/config.hh"
+#include "coal/deprecated.hh"
+#include "coal/warning.hh"
+
+#if _WIN32
+#define COAL_PRETTY_FUNCTION __FUNCSIG__
+#else
+#define COAL_PRETTY_FUNCTION __PRETTY_FUNCTION__
+#endif
+
+#define COAL_UNUSED_VARIABLE(var) (void)(var)
+
+#ifdef NDEBUG
+#define COAL_ONLY_USED_FOR_DEBUG(var) COAL_UNUSED_VARIABLE(var)
+#else
+#define COAL_ONLY_USED_FOR_DEBUG(var)
+#endif
+
+#define COAL_THROW_PRETTY(message, exception)              \
+  {                                                        \
+    std::stringstream ss;                                  \
+    ss << "From file: " << __FILE__ << "\n";               \
+    ss << "in function: " << COAL_PRETTY_FUNCTION << "\n"; \
+    ss << "at line: " << __LINE__ << "\n";                 \
+    ss << "message: " << message << "\n";                  \
+    throw exception(ss.str());                             \
+  }
+
+#ifdef COAL_TURN_ASSERT_INTO_EXCEPTION
+#define COAL_ASSERT(check, message, exception) \
+  do {                                         \
+    if (!(check)) {                            \
+      COAL_THROW_PRETTY(message, exception);   \
+    }                                          \
+  } while (0)
+#else
+#define COAL_ASSERT(check, message, exception) \
+  {                                            \
+    COAL_UNUSED_VARIABLE(exception(message));  \
+    assert((check) && message);                \
+  }
+#endif
+
+#if (__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1600))
+#define COAL_WITH_CXX11_SUPPORT
+#endif
+
+#if defined(__GNUC__) || defined(__clang__)
+#define COAL_COMPILER_DIAGNOSTIC_PUSH _Pragma("GCC diagnostic push")
+#define COAL_COMPILER_DIAGNOSTIC_POP _Pragma("GCC diagnostic pop")
+#define COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS \
+  _Pragma("GCC diagnostic ignored \"-Wdeprecated-declarations\"")
+
+// GCC version 4.6 and higher supports -Wmaybe-uninitialized
+#if (defined(__GNUC__) && \
+     ((__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 6)))
+#define COAL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED \
+  _Pragma("GCC diagnostic ignored \"-Wmaybe-uninitialized\"")
+// Use __has_warning with clang. Clang introduced it in 2024 (3.5+)
+#elif (defined(__clang__) && defined(__has_warning) && \
+       __has_warning("-Wmaybe-uninitialized"))
+#define COAL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED \
+  _Pragma("clang diagnostic ignored \"-Wmaybe-uninitialized\"")
+#else
+#define COAL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
+#endif
+#elif defined(WIN32)
+#define COAL_COMPILER_DIAGNOSTIC_PUSH _Pragma("warning(push)")
+#define COAL_COMPILER_DIAGNOSTIC_POP _Pragma("warning(pop)")
+#define COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS \
+  _Pragma("warning(disable : 4996)")
+#define COAL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED \
+  _Pragma("warning(disable : 4700)")
+#else
+#define COAL_COMPILER_DIAGNOSTIC_PUSH
+#define COAL_COMPILER_DIAGNOSTIC_POP
+#define COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+#define COAL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
+#endif  // __GNUC__
+
+namespace coal {
+using std::dynamic_pointer_cast;
+using std::make_shared;
+using std::shared_ptr;
+
+class CollisionObject;
+typedef shared_ptr<CollisionObject> CollisionObjectPtr_t;
+typedef shared_ptr<const CollisionObject> CollisionObjectConstPtr_t;
+class CollisionGeometry;
+typedef shared_ptr<CollisionGeometry> CollisionGeometryPtr_t;
+typedef shared_ptr<const CollisionGeometry> CollisionGeometryConstPtr_t;
+class Transform3s;
+
+class AABB;
+
+class BVHModelBase;
+typedef shared_ptr<BVHModelBase> BVHModelPtr_t;
+
+class OcTree;
+typedef shared_ptr<OcTree> OcTreePtr_t;
+typedef shared_ptr<const OcTree> OcTreeConstPtr_t;
+}  // namespace coal
+
+#ifdef COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL
+namespace hpp {
+namespace fcl {
+using namespace coal;
+using Transform3f = Transform3s;  // For backward compatibility
+}  // namespace fcl
+}  // namespace hpp
+#endif
+
+#endif  // COAL_FWD_HH

include/coal/hfield.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2021-2024, INRIA
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Justin Carpentier */
+
+#ifndef COAL_HEIGHT_FIELD_H
+#define COAL_HEIGHT_FIELD_H
+
+#include "coal/fwd.hh"
+#include "coal/data_types.h"
+#include "coal/collision_object.h"
+#include "coal/BV/BV_node.h"
+#include "coal/BVH/BVH_internal.h"
+
+#include <vector>
+
+namespace coal {
+
+/// @addtogroup Construction_Of_HeightField
+/// @{
+
+struct COAL_DLLAPI HFNodeBase {
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  enum class FaceOrientation {
+    TOP = 1,
+    BOTTOM = 1,
+    NORTH = 2,
+    EAST = 4,
+    SOUTH = 8,
+    WEST = 16
+  };
+
+  /// @brief An index for first child node or primitive
+  /// If the value is positive, it is the index of the first child bv node
+  /// If the value is negative, it is -(primitive index + 1)
+  /// Zero is not used.
+  size_t first_child;
+
+  Eigen::DenseIndex x_id, x_size;
+  Eigen::DenseIndex y_id, y_size;
+
+  CoalScalar max_height;
+  int contact_active_faces;
+
+  /// @brief Default constructor
+  HFNodeBase()
+      : first_child(0),
+        x_id(-1),
+        x_size(0),
+        y_id(-1),
+        y_size(0),
+        max_height(std::numeric_limits<CoalScalar>::lowest()),
+        contact_active_faces(0) {}
+
+  /// @brief Comparison operator
+  bool operator==(const HFNodeBase& other) const {
+    return first_child == other.first_child && x_id == other.x_id &&
+           x_size == other.x_size && y_id == other.y_id &&
+           y_size == other.y_size && max_height == other.max_height &&
+           contact_active_faces == other.contact_active_faces;
+  }
+
+  /// @brief Difference operator
+  bool operator!=(const HFNodeBase& other) const { return !(*this == other); }
+
+  /// @brief Whether current node is a leaf node (i.e. contains a primitive
+  /// index)
+  inline bool isLeaf() const { return x_size == 1 && y_size == 1; }
+
+  /// @brief Return the index of the first child. The index is referred to the
+  /// bounding volume array (i.e. bvs) in BVHModel
+  inline size_t leftChild() const { return first_child; }
+
+  /// @brief Return the index of the second child. The index is referred to the
+  /// bounding volume array (i.e. bvs) in BVHModel
+  inline size_t rightChild() const { return first_child + 1; }
+
+  inline Eigen::Vector2i leftChildIndexes() const {
+    return Eigen::Vector2i(x_id, y_id);
+  }
+  inline Eigen::Vector2i rightChildIndexes() const {
+    return Eigen::Vector2i(x_id + x_size / 2, y_id + y_size / 2);
+  }
+};
+
+inline HFNodeBase::FaceOrientation operator&(HFNodeBase::FaceOrientation a,
+                                             HFNodeBase::FaceOrientation b) {
+  return HFNodeBase::FaceOrientation(int(a) & int(b));
+}
+
+inline int operator&(int a, HFNodeBase::FaceOrientation b) {
+  return a & int(b);
+}
+
+template <typename BV>
+struct COAL_DLLAPI HFNode : public HFNodeBase {
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  typedef HFNodeBase Base;
+
+  /// @brief bounding volume storing the geometry
+  BV bv;
+
+  /// @brief Equality operator
+  bool operator==(const HFNode& other) const {
+    return Base::operator==(other) && bv == other.bv;
+  }
+
+  /// @brief Difference operator
+  bool operator!=(const HFNode& other) const { return !(*this == other); }
+
+  /// @brief Check whether two BVNode collide
+  bool overlap(const HFNode& other) const { return bv.overlap(other.bv); }
+  /// @brief Check whether two BVNode collide
+  bool overlap(const HFNode& other, const CollisionRequest& request,
+               CoalScalar& sqrDistLowerBound) const {
+    return bv.overlap(other.bv, request, sqrDistLowerBound);
+  }
+
+  /// @brief Compute the distance between two BVNode. P1 and P2, if not NULL and
+  /// the underlying BV supports distance, return the nearest points.
+  CoalScalar distance(const HFNode& other, Vec3s* P1 = NULL,
+                      Vec3s* P2 = NULL) const {
+    return bv.distance(other.bv, P1, P2);
+  }
+
+  /// @brief Access to the center of the BV
+  Vec3s getCenter() const { return bv.center(); }
+
+  /// @brief Access to the orientation of the BV
+  coal::Matrix3s::IdentityReturnType getOrientation() const {
+    return Matrix3s::Identity();
+  }
+
+  virtual ~HFNode() {}
+};
+
+namespace details {
+
+template <typename BV>
+struct UpdateBoundingVolume {
+  static void run(const Vec3s& pointA, const Vec3s& pointB, BV& bv) {
+    AABB bv_aabb(pointA, pointB);
+    //      AABB bv_aabb;
+    //      bv_aabb.update(pointA,pointB);
+    convertBV(bv_aabb, bv);
+  }
+};
+
+template <>
+struct UpdateBoundingVolume<AABB> {
+  static void run(const Vec3s& pointA, const Vec3s& pointB, AABB& bv) {
+    AABB bv_aabb(pointA, pointB);
+    convertBV(bv_aabb, bv);
+    //      bv.update(pointA,pointB);
+  }
+};
+
+}  // namespace details
+
+/// @brief Data structure depicting a height field given by the base grid
+/// dimensions and the elevation along the grid. \tparam BV one of the bounding
+/// volume class in \ref Bounding_Volume.
+///
+/// An height field is defined by its base dimensions along the X and Y axes and
+/// a set ofpoints defined by their altitude, regularly dispatched on the grid.
+/// The height field is centered at the origin and the corners of the geometry
+/// correspond to the following coordinates [± x_dim/2; ± y_dim/2].
+template <typename BV>
+class COAL_DLLAPI HeightField : public CollisionGeometry {
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  typedef CollisionGeometry Base;
+
+  typedef HFNode<BV> Node;
+  typedef std::vector<Node, Eigen::aligned_allocator<Node>> BVS;
+
+  /// @brief Constructing an empty HeightField
+  HeightField()
+      : CollisionGeometry(),
+        min_height((std::numeric_limits<CoalScalar>::min)()),
+        max_height((std::numeric_limits<CoalScalar>::max)()) {}
+
+  /// @brief Constructing an HeightField from its base dimensions and the set of
+  /// heights points.
+  ///        The granularity of the height field along X and Y direction is
+  ///        extraded from the Z grid.
+  ///
+  /// \param[in] x_dim Dimension along the X axis
+  /// \param[in] y_dim Dimension along the Y axis
+  /// \param[in] heights Matrix containing the altitude of each point compositng
+  /// the height field
+  /// \param[in] min_height Minimal height of the height field
+  ///
+  HeightField(const CoalScalar x_dim, const CoalScalar y_dim,
+              const MatrixXs& heights,
+              const CoalScalar min_height = (CoalScalar)0)
+      : CollisionGeometry() {
+    init(x_dim, y_dim, heights, min_height);
+  }
+
+  /// @brief Copy contructor from another HeightField
+  ///
+  /// \param[in] other to copy.
+  ///
+  HeightField(const HeightField& other)
+      : CollisionGeometry(other),
+        x_dim(other.x_dim),
+        y_dim(other.y_dim),
+        heights(other.heights),
+        min_height(other.min_height),
+        max_height(other.max_height),
+        x_grid(other.x_grid),
+        y_grid(other.y_grid),
+        bvs(other.bvs),
+        num_bvs(other.num_bvs) {}
+
+  /// @brief Returns a const reference of the grid along the X direction.
+  const VecXs& getXGrid() const { return x_grid; }
+  /// @brief Returns a const reference of the grid along the Y direction.
+  const VecXs& getYGrid() const { return y_grid; }
+
+  /// @brief Returns a const reference of the heights
+  const MatrixXs& getHeights() const { return heights; }
+
+  /// @brief Returns the dimension of the Height Field along the X direction.
+  CoalScalar getXDim() const { return x_dim; }
+  /// @brief Returns the dimension of the Height Field along the Y direction.
+  CoalScalar getYDim() const { return y_dim; }
+
+  /// @brief Returns the minimal height value of the Height Field.
+  CoalScalar getMinHeight() const { return min_height; }
+  /// @brief Returns the maximal height value of the Height Field.
+  CoalScalar getMaxHeight() const { return max_height; }
+
+  virtual HeightField<BV>* clone() const { return new HeightField(*this); }
+
+  const BVS& getNodes() const { return bvs; }
+
+  /// @brief deconstruction, delete mesh data related.
+  virtual ~HeightField() {}
+
+  /// @brief Compute the AABB for the HeightField, used for broad-phase
+  /// collision
+  void computeLocalAABB() {
+    const Vec3s A(x_grid[0], y_grid[0], min_height);
+    const Vec3s B(x_grid[x_grid.size() - 1], y_grid[y_grid.size() - 1],
+                  max_height);
+    const AABB aabb_(A, B);
+
+    aabb_radius = (A - B).norm() / 2.;
+    aabb_local = aabb_;
+    aabb_center = aabb_.center();
+  }
+
+  /// @brief Update Height Field height
+  void updateHeights(const MatrixXs& new_heights) {
+    if (new_heights.rows() != heights.rows() ||
+        new_heights.cols() != heights.cols())
+      COAL_THROW_PRETTY(
+          "The matrix containing the new heights values does not have the same "
+          "matrix size as the original one.\n"
+          "\tinput values - rows: "
+              << new_heights.rows() << " - cols: " << new_heights.cols() << "\n"
+              << "\texpected values - rows: " << heights.rows()
+              << " - cols: " << heights.cols() << "\n",
+          std::invalid_argument);
+
+    heights = new_heights.cwiseMax(min_height);
+    this->max_height = recursiveUpdateHeight(0);
+    assert(this->max_height == heights.maxCoeff());
+  }
+
+ protected:
+  void init(const CoalScalar x_dim, const CoalScalar y_dim,
+            const MatrixXs& heights, const CoalScalar min_height) {
+    this->x_dim = x_dim;
+    this->y_dim = y_dim;
+    this->heights = heights.cwiseMax(min_height);
+    this->min_height = min_height;
+    this->max_height = heights.maxCoeff();
+
+    const Eigen::DenseIndex NX = heights.cols(), NY = heights.rows();
+    assert(NX >= 2 && "The number of columns is too small.");
+    assert(NY >= 2 && "The number of rows is too small.");
+
+    x_grid = VecXs::LinSpaced(NX, -0.5 * x_dim, 0.5 * x_dim);
+    y_grid = VecXs::LinSpaced(NY, 0.5 * y_dim, -0.5 * y_dim);
+
+    // Allocate BVS
+    const size_t num_tot_bvs =
+        (size_t)(NX * NY) - 1 + (size_t)((NX - 1) * (NY - 1));
+    bvs.resize(num_tot_bvs);
+    num_bvs = 0;
+
+    // Build tree
+    buildTree();
+  }
+
+  /// @brief Get the object type: it is a HFIELD
+  OBJECT_TYPE getObjectType() const { return OT_HFIELD; }
+
+  Vec3s computeCOM() const { return Vec3s::Zero(); }
+
+  CoalScalar computeVolume() const { return 0; }
+
+  Matrix3s computeMomentofInertia() const { return Matrix3s::Zero(); }
+
+ protected:
+  /// @brief Dimensions in meters along X and Y directions
+  CoalScalar x_dim, y_dim;
+
+  /// @brief Elevation values in meters of the Height Field
+  MatrixXs heights;
+
+  /// @brief Minimal height of the Height Field: all values bellow min_height
+  /// will be discarded.
+  CoalScalar min_height, max_height;
+
+  /// @brief Grids along the X and Y directions. Useful for plotting or other
+  /// related things.
+  VecXs x_grid, y_grid;
+
+  /// @brief Bounding volume hierarchy
+  BVS bvs;
+  unsigned int num_bvs;
+
+  /// @brief Build the bounding volume hierarchy
+  int buildTree() {
+    num_bvs = 1;
+    const CoalScalar max_recursive_height =
+        recursiveBuildTree(0, 0, heights.cols() - 1, 0, heights.rows() - 1);
+    assert(max_recursive_height == max_height &&
+           "the maximal height is not correct");
+    COAL_UNUSED_VARIABLE(max_recursive_height);
+
+    bvs.resize(num_bvs);
+    return BVH_OK;
+  }
+
+  CoalScalar recursiveUpdateHeight(const size_t bv_id) {
+    HFNode<BV>& bv_node = bvs[bv_id];
+
+    CoalScalar max_height;
+    if (bv_node.isLeaf()) {
+      max_height = heights.block<2, 2>(bv_node.y_id, bv_node.x_id).maxCoeff();
+    } else {
+      CoalScalar max_left_height = recursiveUpdateHeight(bv_node.leftChild()),
+                 max_right_height = recursiveUpdateHeight(bv_node.rightChild());
+
+      max_height = (std::max)(max_left_height, max_right_height);
+    }
+
+    bv_node.max_height = max_height;
+
+    const Vec3s pointA(x_grid[bv_node.x_id], y_grid[bv_node.y_id], min_height);
+    const Vec3s pointB(x_grid[bv_node.x_id + bv_node.x_size],
+                       y_grid[bv_node.y_id + bv_node.y_size], max_height);
+
+    details::UpdateBoundingVolume<BV>::run(pointA, pointB, bv_node.bv);
+
+    return max_height;
+  }
+
+  CoalScalar recursiveBuildTree(const size_t bv_id,
+                                const Eigen::DenseIndex x_id,
+                                const Eigen::DenseIndex x_size,
+                                const Eigen::DenseIndex y_id,
+                                const Eigen::DenseIndex y_size) {
+    assert(x_id < heights.cols() && "x_id is out of bounds");
+    assert(y_id < heights.rows() && "y_id is out of bounds");
+    assert(x_size >= 0 && y_size >= 0 &&
+           "x_size or y_size are not of correct value");
+    assert(bv_id < bvs.size() && "bv_id exceeds the vector dimension");
+
+    HFNode<BV>& bv_node = bvs[bv_id];
+    CoalScalar max_height;
+    if (x_size == 1 &&
+        y_size == 1)  // don't build any BV for the current child node
+    {
+      max_height = heights.block<2, 2>(y_id, x_id).maxCoeff();
+    } else {
+      bv_node.first_child = num_bvs;
+      num_bvs += 2;
+
+      CoalScalar max_left_height = min_height, max_right_height = min_height;
+      if (x_size >= y_size)  // splitting along the X axis
+      {
+        Eigen::DenseIndex x_size_half = x_size / 2;
+        if (x_size == 1) x_size_half = 1;
+        max_left_height = recursiveBuildTree(bv_node.leftChild(), x_id,
+                                             x_size_half, y_id, y_size);
+
+        max_right_height =
+            recursiveBuildTree(bv_node.rightChild(), x_id + x_size_half,
+                               x_size - x_size_half, y_id, y_size);
+      } else  // splitting along the Y axis
+      {
+        Eigen::DenseIndex y_size_half = y_size / 2;
+        if (y_size == 1) y_size_half = 1;
+        max_left_height = recursiveBuildTree(bv_node.leftChild(), x_id, x_size,
+                                             y_id, y_size_half);
+
+        max_right_height =
+            recursiveBuildTree(bv_node.rightChild(), x_id, x_size,
+                               y_id + y_size_half, y_size - y_size_half);
+      }
+
+      max_height = (std::max)(max_left_height, max_right_height);
+    }
+
+    bv_node.max_height = max_height;
+    //    max_height = std::max(max_height,min_height);
+
+    const Vec3s pointA(x_grid[x_id], y_grid[y_id], min_height);
+    assert(x_id + x_size < x_grid.size());
+    assert(y_id + y_size < y_grid.size());
+    const Vec3s pointB(x_grid[x_id + x_size], y_grid[y_id + y_size],
+                       max_height);
+
+    details::UpdateBoundingVolume<BV>::run(pointA, pointB, bv_node.bv);
+    bv_node.x_id = x_id;
+    bv_node.y_id = y_id;
+    bv_node.x_size = x_size;
+    bv_node.y_size = y_size;
+
+    if (bv_node.isLeaf()) {
+      int& contact_active_faces = bv_node.contact_active_faces;
+      contact_active_faces |= int(HFNodeBase::FaceOrientation::TOP);
+      contact_active_faces |= int(HFNodeBase::FaceOrientation::BOTTOM);
+
+      if (bv_node.x_id == 0)  // first col
+        contact_active_faces |= int(HFNodeBase::FaceOrientation::WEST);
+
+      if (bv_node.y_id == 0)  // first row (TOP)
+        contact_active_faces |= int(HFNodeBase::FaceOrientation::NORTH);
+
+      if (bv_node.x_id + 1 == heights.cols() - 1)  // last col
+        contact_active_faces |= int(HFNodeBase::FaceOrientation::EAST);
+
+      if (bv_node.y_id + 1 == heights.rows() - 1)  // last row (BOTTOM)
+        contact_active_faces |= int(HFNodeBase::FaceOrientation::SOUTH);
+    }
+
+    return max_height;
+  }
+
+ public:
+  /// @brief Access the bv giving the its index
+  const HFNode<BV>& getBV(unsigned int i) const {
+    if (i >= num_bvs)
+      COAL_THROW_PRETTY("Index out of bounds", std::invalid_argument);
+    return bvs[i];
+  }
+
+  /// @brief Access the bv giving the its index
+  HFNode<BV>& getBV(unsigned int i) {
+    if (i >= num_bvs)
+      COAL_THROW_PRETTY("Index out of bounds", std::invalid_argument);
+    return bvs[i];
+  }
+
+  /// @brief Get the BV type: default is unknown
+  NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
+
+ private:
+  virtual bool isEqual(const CollisionGeometry& _other) const {
+    const HeightField* other_ptr = dynamic_cast<const HeightField*>(&_other);
+    if (other_ptr == nullptr) return false;
+    const HeightField& other = *other_ptr;
+
+    return x_dim == other.x_dim && y_dim == other.y_dim &&
+           heights == other.heights && min_height == other.min_height &&
+           max_height == other.max_height && x_grid == other.x_grid &&
+           y_grid == other.y_grid && bvs == other.bvs &&
+           num_bvs == other.num_bvs;
+  }
+};
+
+/// @brief Specialization of getNodeType() for HeightField with different BV
+/// types
+template <>
+NODE_TYPE HeightField<AABB>::getNodeType() const;
+
+template <>
+NODE_TYPE HeightField<OBB>::getNodeType() const;
+
+template <>
+NODE_TYPE HeightField<RSS>::getNodeType() const;
+
+template <>
+NODE_TYPE HeightField<kIOS>::getNodeType() const;
+
+template <>
+NODE_TYPE HeightField<OBBRSS>::getNodeType() const;
+
+template <>
+NODE_TYPE HeightField<KDOP<16>>::getNodeType() const;
+
+template <>
+NODE_TYPE HeightField<KDOP<18>>::getNodeType() const;
+
+template <>
+NODE_TYPE HeightField<KDOP<24>>::getNodeType() const;
+
+/// @}
+
+}  // namespace coal
+
+#endif

include/coal/internal/BV_fitter.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_BV_FITTER_H
+#define COAL_BV_FITTER_H
+
+#include "coal/BVH/BVH_internal.h"
+#include "coal/BV/kIOS.h"
+#include "coal/BV/OBBRSS.h"
+#include "coal/BV/AABB.h"
+#include <iostream>
+
+namespace coal {
+
+/// @brief Compute a bounding volume that fits a set of n points.
+template <typename BV>
+void fit(Vec3s* ps, unsigned int n, BV& bv) {
+  for (unsigned int i = 0; i < n; ++i)  // TODO(jcarpent): vectorize
+  {
+    bv += ps[i];
+  }
+}
+
+template <>
+void fit<OBB>(Vec3s* ps, unsigned int n, OBB& bv);
+
+template <>
+void fit<RSS>(Vec3s* ps, unsigned int n, RSS& bv);
+
+template <>
+void fit<kIOS>(Vec3s* ps, unsigned int n, kIOS& bv);
+
+template <>
+void fit<OBBRSS>(Vec3s* ps, unsigned int n, OBBRSS& bv);
+
+template <>
+void fit<AABB>(Vec3s* ps, unsigned int n, AABB& bv);
+
+/// @brief The class for the default algorithm fitting a bounding volume to a
+/// set of points
+template <typename BV>
+class COAL_DLLAPI BVFitterTpl {
+ public:
+  /// @brief default deconstructor
+  virtual ~BVFitterTpl() {}
+
+  /// @brief Prepare the geometry primitive data for fitting
+  void set(Vec3s* vertices_, Triangle* tri_indices_, BVHModelType type_) {
+    vertices = vertices_;
+    prev_vertices = NULL;
+    tri_indices = tri_indices_;
+    type = type_;
+  }
+
+  /// @brief Prepare the geometry primitive data for fitting, for deformable
+  /// mesh
+  void set(Vec3s* vertices_, Vec3s* prev_vertices_, Triangle* tri_indices_,
+           BVHModelType type_) {
+    vertices = vertices_;
+    prev_vertices = prev_vertices_;
+    tri_indices = tri_indices_;
+    type = type_;
+  }
+
+  /// @brief Compute the fitting BV
+  virtual BV fit(unsigned int* primitive_indices,
+                 unsigned int num_primitives) = 0;
+
+  /// @brief Clear the geometry primitive data
+  void clear() {
+    vertices = NULL;
+    prev_vertices = NULL;
+    tri_indices = NULL;
+    type = BVH_MODEL_UNKNOWN;
+  }
+
+ protected:
+  Vec3s* vertices;
+  Vec3s* prev_vertices;
+  Triangle* tri_indices;
+  BVHModelType type;
+};
+
+/// @brief The class for the default algorithm fitting a bounding volume to a
+/// set of points
+template <typename BV>
+class COAL_DLLAPI BVFitter : public BVFitterTpl<BV> {
+  typedef BVFitterTpl<BV> Base;
+
+ public:
+  /// @brief Compute a bounding volume that fits a set of primitives (points or
+  /// triangles). The primitive data was set by set function and
+  /// primitive_indices is the primitive index relative to the data
+  BV fit(unsigned int* primitive_indices, unsigned int num_primitives) {
+    BV bv;
+
+    if (type == BVH_MODEL_TRIANGLES)  /// The primitive is triangle
+    {
+      for (unsigned int i = 0; i < num_primitives; ++i) {
+        Triangle t = tri_indices[primitive_indices[i]];
+        bv += vertices[t[0]];
+        bv += vertices[t[1]];
+        bv += vertices[t[2]];
+
+        if (prev_vertices)  /// can fitting both current and previous frame
+        {
+          bv += prev_vertices[t[0]];
+          bv += prev_vertices[t[1]];
+          bv += prev_vertices[t[2]];
+        }
+      }
+    } else if (type == BVH_MODEL_POINTCLOUD)  /// The primitive is point
+    {
+      for (unsigned int i = 0; i < num_primitives; ++i) {
+        bv += vertices[primitive_indices[i]];
+
+        if (prev_vertices)  /// can fitting both current and previous frame
+        {
+          bv += prev_vertices[primitive_indices[i]];
+        }
+      }
+    }
+
+    return bv;
+  }
+
+ protected:
+  using Base::prev_vertices;
+  using Base::tri_indices;
+  using Base::type;
+  using Base::vertices;
+};
+
+/// @brief Specification of BVFitter for OBB bounding volume
+template <>
+class COAL_DLLAPI BVFitter<OBB> : public BVFitterTpl<OBB> {
+ public:
+  /// @brief Compute a bounding volume that fits a set of primitives (points or
+  /// triangles). The primitive data was set by set function and
+  /// primitive_indices is the primitive index relative to the data.
+  OBB fit(unsigned int* primitive_indices, unsigned int num_primitives);
+};
+
+/// @brief Specification of BVFitter for RSS bounding volume
+template <>
+class COAL_DLLAPI BVFitter<RSS> : public BVFitterTpl<RSS> {
+ public:
+  /// @brief Compute a bounding volume that fits a set of primitives (points or
+  /// triangles). The primitive data was set by set function and
+  /// primitive_indices is the primitive index relative to the data.
+  RSS fit(unsigned int* primitive_indices, unsigned int num_primitives);
+};
+
+/// @brief Specification of BVFitter for kIOS bounding volume
+template <>
+class COAL_DLLAPI BVFitter<kIOS> : public BVFitterTpl<kIOS> {
+ public:
+  /// @brief Compute a bounding volume that fits a set of primitives (points or
+  /// triangles). The primitive data was set by set function and
+  /// primitive_indices is the primitive index relative to the data.
+  kIOS fit(unsigned int* primitive_indices, unsigned int num_primitives);
+};
+
+/// @brief Specification of BVFitter for OBBRSS bounding volume
+template <>
+class COAL_DLLAPI BVFitter<OBBRSS> : public BVFitterTpl<OBBRSS> {
+ public:
+  /// @brief Compute a bounding volume that fits a set of primitives (points or
+  /// triangles). The primitive data was set by set function and
+  /// primitive_indices is the primitive index relative to the data.
+  OBBRSS fit(unsigned int* primitive_indices, unsigned int num_primitives);
+};
+
+/// @brief Specification of BVFitter for AABB bounding volume
+template <>
+class COAL_DLLAPI BVFitter<AABB> : public BVFitterTpl<AABB> {
+ public:
+  /// @brief Compute a bounding volume that fits a set of primitives (points or
+  /// triangles). The primitive data was set by set function and
+  /// primitive_indices is the primitive index relative to the data.
+  AABB fit(unsigned int* primitive_indices, unsigned int num_primitives);
+};
+
+}  // namespace coal
+
+#endif

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

include/coal/internal/intersect.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Jia Pan */
+
+#ifndef COAL_INTERSECT_H
+#define COAL_INTERSECT_H
+
+/// @cond INTERNAL
+
+#include "coal/math/transform.h"
+
+namespace coal {
+
+/// @brief CCD intersect kernel among primitives
+class COAL_DLLAPI Intersect {
+ public:
+  static bool buildTrianglePlane(const Vec3s& v1, const Vec3s& v2,
+                                 const Vec3s& v3, Vec3s* n, CoalScalar* t);
+};  // class Intersect
+
+/// @brief Project functions
+class COAL_DLLAPI Project {
+ public:
+  struct COAL_DLLAPI ProjectResult {
+    /// @brief Parameterization of the projected point (based on the simplex to
+    /// be projected, use 2 or 3 or 4 of the array)
+    CoalScalar parameterization[4];
+
+    /// @brief square distance from the query point to the projected simplex
+    CoalScalar sqr_distance;
+
+    /// @brief the code of the projection type
+    unsigned int encode;
+
+    ProjectResult() : sqr_distance(-1), encode(0) {}
+  };
+
+  /// @brief Project point p onto line a-b
+  static ProjectResult projectLine(const Vec3s& a, const Vec3s& b,
+                                   const Vec3s& p);
+
+  /// @brief Project point p onto triangle a-b-c
+  static ProjectResult projectTriangle(const Vec3s& a, const Vec3s& b,
+                                       const Vec3s& c, const Vec3s& p);
+
+  /// @brief Project point p onto tetrahedra a-b-c-d
+  static ProjectResult projectTetrahedra(const Vec3s& a, const Vec3s& b,
+                                         const Vec3s& c, const Vec3s& d,
+                                         const Vec3s& p);
+
+  /// @brief Project origin (0) onto line a-b
+  static ProjectResult projectLineOrigin(const Vec3s& a, const Vec3s& b);
+
+  /// @brief Project origin (0) onto triangle a-b-c
+  static ProjectResult projectTriangleOrigin(const Vec3s& a, const Vec3s& b,
+                                             const Vec3s& c);
+
+  /// @brief Project origin (0) onto tetrahedran a-b-c-d
+  static ProjectResult projectTetrahedraOrigin(const Vec3s& a, const Vec3s& b,
+                                               const Vec3s& c, const Vec3s& d);
+};
+
+/// @brief Triangle distance functions
+class COAL_DLLAPI TriangleDistance {
+ public:
+  /// @brief Returns closest points between an segment pair.
+  /// The first segment is P + t * A
+  /// The second segment is Q + t * B
+  /// X, Y are the closest points on the two segments
+  /// VEC is the vector between X and Y
+  static void segPoints(const Vec3s& P, const Vec3s& A, const Vec3s& Q,
+                        const Vec3s& B, Vec3s& VEC, Vec3s& X, Vec3s& Y);
+
+  /// Compute squared distance between triangles
+  /// @param S and T are two triangles
+  /// @retval P, Q closest points if triangles do not intersect.
+  /// @return squared distance if triangles do not intersect, 0 otherwise.
+  /// If the triangles are disjoint, P and Q give the closet points of
+  /// S and T respectively. However,
+  /// if the triangles overlap, P and Q are basically a random pair of points
+  /// from the triangles, not coincident points on the intersection of the
+  /// triangles, as might be expected.
+  static CoalScalar sqrTriDistance(const Vec3s S[3], const Vec3s T[3], Vec3s& P,
+                                   Vec3s& Q);
+
+  static CoalScalar sqrTriDistance(const Vec3s& S1, const Vec3s& S2,
+                                   const Vec3s& S3, const Vec3s& T1,
+                                   const Vec3s& T2, const Vec3s& T3, Vec3s& P,
+                                   Vec3s& Q);
+
+  /// Compute squared distance between triangles
+  /// @param S and T are two triangles
+  /// @param R, Tl, rotation and translation applied to T,
+  /// @retval P, Q closest points if triangles do not intersect.
+  /// @return squared distance if triangles do not intersect, 0 otherwise.
+  /// If the triangles are disjoint, P and Q give the closet points of
+  /// S and T respectively. However,
+  /// if the triangles overlap, P and Q are basically a random pair of points
+  /// from the triangles, not coincident points on the intersection of the
+  /// triangles, as might be expected.
+  static CoalScalar sqrTriDistance(const Vec3s S[3], const Vec3s T[3],
+                                   const Matrix3s& R, const Vec3s& Tl, Vec3s& P,
+                                   Vec3s& Q);
+
+  /// Compute squared distance between triangles
+  /// @param S and T are two triangles
+  /// @param tf, rotation and translation applied to T,
+  /// @retval P, Q closest points if triangles do not intersect.
+  /// @return squared distance if triangles do not intersect, 0 otherwise.
+  /// If the triangles are disjoint, P and Q give the closet points of
+  /// S and T respectively. However,
+  /// if the triangles overlap, P and Q are basically a random pair of points
+  /// from the triangles, not coincident points on the intersection of the
+  /// triangles, as might be expected.
+  static CoalScalar sqrTriDistance(const Vec3s S[3], const Vec3s T[3],
+                                   const Transform3s& tf, Vec3s& P, Vec3s& Q);
+
+  /// Compute squared distance between triangles
+  /// @param S1, S2, S3 and T1, T2, T3 are triangle vertices
+  /// @param R, Tl, rotation and translation applied to T1, T2, T3,
+  /// @retval P, Q closest points if triangles do not intersect.
+  /// @return squared distance if triangles do not intersect, 0 otherwise.
+  /// If the triangles are disjoint, P and Q give the closet points of
+  /// S and T respectively. However,
+  /// if the triangles overlap, P and Q are basically a random pair of points
+  /// from the triangles, not coincident points on the intersection of the
+  /// triangles, as might be expected.
+  static CoalScalar sqrTriDistance(const Vec3s& S1, const Vec3s& S2,
+                                   const Vec3s& S3, const Vec3s& T1,
+                                   const Vec3s& T2, const Vec3s& T3,
+                                   const Matrix3s& R, const Vec3s& Tl, Vec3s& P,
+                                   Vec3s& Q);
+
+  /// Compute squared distance between triangles
+  /// @param S1, S2, S3 and T1, T2, T3 are triangle vertices
+  /// @param tf, rotation and translation applied to T1, T2, T3,
+  /// @retval P, Q closest points if triangles do not intersect.
+  /// @return squared distance if triangles do not intersect, 0 otherwise.
+  /// If the triangles are disjoint, P and Q give the closet points of
+  /// S and T respectively. However,
+  /// if the triangles overlap, P and Q are basically a random pair of points
+  /// from the triangles, not coincident points on the intersection of the
+  /// triangles, as might be expected.
+  static CoalScalar sqrTriDistance(const Vec3s& S1, const Vec3s& S2,
+                                   const Vec3s& S3, const Vec3s& T1,
+                                   const Vec3s& T2, const Vec3s& T3,
+                                   const Transform3s& tf, Vec3s& P, Vec3s& Q);
+};
+
+}  // namespace coal
+
+/// @endcond
+
+#endif

include/coal/internal/shape_shape_contact_patch_func.h

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

include/coal/internal/shape_shape_func.h

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

include/coal/internal/tools.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Joseph Mirabel */
+
+#ifndef COAL_INTERNAL_TOOLS_H
+#define COAL_INTERNAL_TOOLS_H
+
+#include "coal/fwd.hh"
+
+#include <cmath>
+#include <iostream>
+#include <limits>
+
+#include "coal/data_types.h"
+
+namespace coal {
+
+template <typename Derived>
+static inline typename Derived::Scalar triple(
+    const Eigen::MatrixBase<Derived>& x, const Eigen::MatrixBase<Derived>& y,
+    const Eigen::MatrixBase<Derived>& z) {
+  return x.derived().dot(y.derived().cross(z.derived()));
+}
+
+template <typename Derived1, typename Derived2, typename Derived3>
+void generateCoordinateSystem(const Eigen::MatrixBase<Derived1>& _w,
+                              const Eigen::MatrixBase<Derived2>& _u,
+                              const Eigen::MatrixBase<Derived3>& _v) {
+  typedef typename Derived1::Scalar T;
+
+  Eigen::MatrixBase<Derived1>& w = const_cast<Eigen::MatrixBase<Derived1>&>(_w);
+  Eigen::MatrixBase<Derived2>& u = const_cast<Eigen::MatrixBase<Derived2>&>(_u);
+  Eigen::MatrixBase<Derived3>& v = const_cast<Eigen::MatrixBase<Derived3>&>(_v);
+
+  T inv_length;
+  if (std::abs(w[0]) >= std::abs(w[1])) {
+    inv_length = (T)1.0 / sqrt(w[0] * w[0] + w[2] * w[2]);
+    u[0] = -w[2] * inv_length;
+    u[1] = (T)0;
+    u[2] = w[0] * inv_length;
+    v[0] = w[1] * u[2];
+    v[1] = w[2] * u[0] - w[0] * u[2];
+    v[2] = -w[1] * u[0];
+  } else {
+    inv_length = (T)1.0 / sqrt(w[1] * w[1] + w[2] * w[2]);
+    u[0] = (T)0;
+    u[1] = w[2] * inv_length;
+    u[2] = -w[1] * inv_length;
+    v[0] = w[1] * u[2] - w[2] * u[1];
+    v[1] = -w[0] * u[2];
+    v[2] = w[0] * u[1];
+  }
+}
+
+/* ----- Start Matrices ------ */
+template <typename Derived, typename OtherDerived>
+void relativeTransform(const Eigen::MatrixBase<Derived>& R1,
+                       const Eigen::MatrixBase<OtherDerived>& t1,
+                       const Eigen::MatrixBase<Derived>& R2,
+                       const Eigen::MatrixBase<OtherDerived>& t2,
+                       const Eigen::MatrixBase<Derived>& R,
+                       const Eigen::MatrixBase<OtherDerived>& t) {
+  const_cast<Eigen::MatrixBase<Derived>&>(R) = R1.transpose() * R2;
+  const_cast<Eigen::MatrixBase<OtherDerived>&>(t) = R1.transpose() * (t2 - t1);
+}
+
+/// @brief compute the eigen vector and eigen vector of a matrix. dout is the
+/// eigen values, vout is the eigen vectors
+template <typename Derived, typename Vector>
+void eigen(const Eigen::MatrixBase<Derived>& m,
+           typename Derived::Scalar dout[3], Vector* vout) {
+  typedef typename Derived::Scalar S;
+  Derived R(m.derived());
+  int n = 3;
+  int j, iq, ip, i;
+  S tresh, theta, tau, t, sm, s, h, g, c;
+
+  S b[3];
+  S z[3];
+  S v[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
+  S d[3];
+
+  for (ip = 0; ip < n; ++ip) {
+    b[ip] = d[ip] = R(ip, ip);
+    z[ip] = 0;
+  }
+
+  for (i = 0; i < 50; ++i) {
+    sm = 0;
+    for (ip = 0; ip < n; ++ip)
+      for (iq = ip + 1; iq < n; ++iq) sm += std::abs(R(ip, iq));
+    if (sm == 0.0) {
+      vout[0] << v[0][0], v[0][1], v[0][2];
+      vout[1] << v[1][0], v[1][1], v[1][2];
+      vout[2] << v[2][0], v[2][1], v[2][2];
+      dout[0] = d[0];
+      dout[1] = d[1];
+      dout[2] = d[2];
+      return;
+    }
+
+    if (i < 3)
+      tresh = 0.2 * sm / (n * n);
+    else
+      tresh = 0.0;
+
+    for (ip = 0; ip < n; ++ip) {
+      for (iq = ip + 1; iq < n; ++iq) {
+        g = 100.0 * std::abs(R(ip, iq));
+        if (i > 3 && std::abs(d[ip]) + g == std::abs(d[ip]) &&
+            std::abs(d[iq]) + g == std::abs(d[iq]))
+          R(ip, iq) = 0.0;
+        else if (std::abs(R(ip, iq)) > tresh) {
+          h = d[iq] - d[ip];
+          if (std::abs(h) + g == std::abs(h))
+            t = (R(ip, iq)) / h;
+          else {
+            theta = 0.5 * h / (R(ip, iq));
+            t = 1.0 / (std::abs(theta) + std::sqrt(1.0 + theta * theta));
+            if (theta < 0.0) t = -t;
+          }
+          c = 1.0 / std::sqrt(1 + t * t);
+          s = t * c;
+          tau = s / (1.0 + c);
+          h = t * R(ip, iq);
+          z[ip] -= h;
+          z[iq] += h;
+          d[ip] -= h;
+          d[iq] += h;
+          R(ip, iq) = 0.0;
+          for (j = 0; j < ip; ++j) {
+            g = R(j, ip);
+            h = R(j, iq);
+            R(j, ip) = g - s * (h + g * tau);
+            R(j, iq) = h + s * (g - h * tau);
+          }
+          for (j = ip + 1; j < iq; ++j) {
+            g = R(ip, j);
+            h = R(j, iq);
+            R(ip, j) = g - s * (h + g * tau);
+            R(j, iq) = h + s * (g - h * tau);
+          }
+          for (j = iq + 1; j < n; ++j) {
+            g = R(ip, j);
+            h = R(iq, j);
+            R(ip, j) = g - s * (h + g * tau);
+            R(iq, j) = h + s * (g - h * tau);
+          }
+          for (j = 0; j < n; ++j) {
+            g = v[j][ip];
+            h = v[j][iq];
+            v[j][ip] = g - s * (h + g * tau);
+            v[j][iq] = h + s * (g - h * tau);
+          }
+        }
+      }
+    }
+    for (ip = 0; ip < n; ++ip) {
+      b[ip] += z[ip];
+      d[ip] = b[ip];
+      z[ip] = 0.0;
+    }
+  }
+
+  std::cerr << "eigen: too many iterations in Jacobi transform." << std::endl;
+
+  return;
+}
+
+template <typename Derived, typename OtherDerived>
+bool isEqual(const Eigen::MatrixBase<Derived>& lhs,
+             const Eigen::MatrixBase<OtherDerived>& rhs,
+             const CoalScalar tol = std::numeric_limits<CoalScalar>::epsilon() *
+                                    100) {
+  return ((lhs - rhs).array().abs() < tol).all();
+}
+
+}  // namespace coal
+
+#endif

src/distance_func_matrix.h → include/coal/internal/traversal.h

 /*
  * Software License Agreement (BSD License)
  *
- *  Copyright (c) 2014, CNRS-LAAS
+ *  Copyright (c) 2019, LAAS CNRS
  *  All rights reserved.
  *
  *  Redistribution and use in source and binary forms, with or without
@@ -14,7 +14,7 @@
  *     copyright notice, this list of conditions and the following
  *     disclaimer in the documentation and/or other materials provided
  *     with the distribution.
- *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *   * 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.
  *
@@ -32,37 +32,42 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  */
 
-/** \author Florent Lamiraux */
+/** \author Joseph Mirabel */
 
-#ifndef HPP_FCL_DISTANCE_FUNC_MATRIX_H
-#define HPP_FCL_DISTANCE_FUNC_MATRIX_H
+#ifndef COAL_TRAVERSAL_DETAILS_TRAVERSAL_H
+#define COAL_TRAVERSAL_DETAILS_TRAVERSAL_H
 
 /// @cond INTERNAL
 
-#include <hpp/fcl/collision_data.h>
-#include <hpp/fcl/narrowphase/narrowphase.h>
+namespace coal {
 
-namespace hpp
-{
-namespace fcl
-{
-  template<typename T_SH1, typename T_SH2>
-    FCL_REAL ShapeShapeDistance
-    (const CollisionGeometry* o1, const Transform3f& tf1,
-     const CollisionGeometry* o2, const Transform3f& tf2,
-     const GJKSolver* nsolver, const DistanceRequest& request,
-     DistanceResult& result);
+enum { RelativeTransformationIsIdentity = 1 };
 
-  template<typename T_SH1, typename T_SH2>
-    std::size_t ShapeShapeCollide
-    (const CollisionGeometry* o1, const Transform3f& tf1,
-     const CollisionGeometry* o2, const Transform3f& tf2, 
-     const GJKSolver* nsolver, const CollisionRequest& request,
-     CollisionResult& result);
-}
+namespace details {
+template <bool enabled>
+struct COAL_DLLAPI RelativeTransformation {
+  RelativeTransformation() : R(Matrix3s::Identity()) {}
 
-} // namespace hpp
+  const Matrix3s& _R() const { return R; }
+  const Vec3s& _T() const { return T; }
+
+  Matrix3s R;
+  Vec3s T;
+};
+
+template <>
+struct COAL_DLLAPI RelativeTransformation<false> {
+  static const Matrix3s& _R() {
+    COAL_THROW_PRETTY("should never reach this point", std::logic_error);
+  }
+  static const Vec3s& _T() {
+    COAL_THROW_PRETTY("should never reach this point", std::logic_error);
+  }
+};
+}  // namespace details
+
+}  // namespace coal
 
 /// @endcond
 
-#endif
\ No newline at end of file
+#endif