include/coal/collision.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, Open Source Robotics Foundation
+ *  Copyright (c) 2021, INRIA
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Jia Pan */
+
+#ifndef COAL_COLLISION_H
+#define COAL_COLLISION_H
+
+#include "coal/data_types.h"
+#include "coal/collision_object.h"
+#include "coal/collision_data.h"
+#include "coal/collision_func_matrix.h"
+#include "coal/timings.h"
+
+namespace coal {
+
+/// @brief Main collision interface: given two collision objects, and the
+/// requirements for contacts, including num of max contacts, whether perform
+/// exhaustive collision (i.e., returning returning all the contact points),
+/// whether return detailed contact information (i.e., normal, contact point,
+/// depth; otherwise only contact primitive id is returned), this function
+/// performs the collision between them.
+/// Return value is the number of contacts generated between the two objects.
+COAL_DLLAPI std::size_t collide(const CollisionObject* o1,
+                                const CollisionObject* o2,
+                                const CollisionRequest& request,
+                                CollisionResult& result);
+
+/// @copydoc collide(const CollisionObject*, const CollisionObject*, const
+/// CollisionRequest&, CollisionResult&)
+COAL_DLLAPI std::size_t collide(const CollisionGeometry* o1,
+                                const Transform3s& tf1,
+                                const CollisionGeometry* o2,
+                                const Transform3s& tf2,
+                                const CollisionRequest& request,
+                                CollisionResult& result);
+
+/// @brief This class reduces the cost of identifying the geometry pair.
+/// This is mostly useful for repeated shape-shape queries.
+///
+/// \code
+///   ComputeCollision calc_collision (o1, o2);
+///   std::size_t ncontacts = calc_collision(tf1, tf2, request, result);
+/// \endcode
+class COAL_DLLAPI ComputeCollision {
+ public:
+  /// @brief Default constructor from two Collision Geometries.
+  ComputeCollision(const CollisionGeometry* o1, const CollisionGeometry* o2);
+
+  std::size_t operator()(const Transform3s& tf1, const Transform3s& tf2,
+                         const CollisionRequest& request,
+                         CollisionResult& result) const;
+
+  bool operator==(const ComputeCollision& other) const {
+    return o1 == other.o1 && o2 == other.o2 && solver == other.solver;
+  }
+
+  bool operator!=(const ComputeCollision& other) const {
+    return !(*this == other);
+  }
+
+  virtual ~ComputeCollision() {};
+
+ 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;
+
+  CollisionFunctionMatrix::CollisionFunc func;
+  bool swap_geoms;
+
+  virtual std::size_t run(const Transform3s& tf1, const Transform3s& tf2,
+                          const CollisionRequest& request,
+                          CollisionResult& result) const;
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+}  // namespace coal
+
+#endif

include/coal/collision_data.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, Open Source Robotics Foundation
+ *  Copyright (c) 2024, INRIA
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Jia Pan */
+
+#ifndef COAL_COLLISION_DATA_H
+#define COAL_COLLISION_DATA_H
+
+#include <vector>
+#include <array>
+#include <set>
+#include <limits>
+#include <numeric>
+
+#include "coal/collision_object.h"
+#include "coal/config.hh"
+#include "coal/data_types.h"
+#include "coal/timings.h"
+#include "coal/narrowphase/narrowphase_defaults.h"
+#include "coal/logging.h"
+
+namespace coal {
+
+/// @brief Contact information returned by collision
+struct COAL_DLLAPI Contact {
+  /// @brief collision object 1
+  const CollisionGeometry* o1;
+
+  /// @brief collision object 2
+  const CollisionGeometry* o2;
+
+  /// @brief contact primitive in object 1
+  /// if object 1 is mesh or point cloud, it is the triangle or point id
+  /// if object 1 is geometry shape, it is NONE (-1),
+  /// if object 1 is octree, it is the id of the cell
+  int b1;
+
+  /// @brief contact primitive in object 2
+  /// if object 2 is mesh or point cloud, it is the triangle or point id
+  /// if object 2 is geometry shape, it is NONE (-1),
+  /// if object 2 is octree, it is the id of the cell
+  int b2;
+
+  /// @brief contact normal, pointing from o1 to o2.
+  /// The normal defined as the normalized separation vector:
+  /// normal = (p2 - p1) / dist(o1, o2), where p1 = nearest_points[0]
+  /// belongs to o1 and p2 = nearest_points[1] belongs to o2 and dist(o1, o2) is
+  /// the **signed** distance between o1 and o2. The normal always points from
+  /// o1 to o2.
+  /// @note The separation vector is the smallest vector such that if o1 is
+  /// translated by it, o1 and o2 are in touching contact (they share at least
+  /// one contact point but have a zero intersection volume). If the shapes
+  /// overlap, dist(o1, o2) = -((p2-p1).norm()). Otherwise, dist(o1, o2) =
+  /// (p2-p1).norm().
+  Vec3s normal;
+
+  /// @brief nearest points associated to this contact.
+  /// @note Also referred as "witness points" in other collision libraries.
+  /// The points p1 = nearest_points[0] and p2 = nearest_points[1] verify the
+  /// property that dist(o1, o2) * (p1 - p2) is the separation vector between o1
+  /// and o2, with dist(o1, o2) being the **signed** distance separating o1 from
+  /// o2. See \ref DistanceResult::normal for the definition of the separation
+  /// vector. If o1 and o2 have multiple contacts, the nearest_points are
+  /// associated with the contact which has the greatest penetration depth.
+  /// TODO (louis): rename `nearest_points` to `witness_points`.
+  std::array<Vec3s, 2> nearest_points;
+
+  /// @brief contact position, in world space
+  Vec3s pos;
+
+  /// @brief penetration depth
+  CoalScalar penetration_depth;
+
+  /// @brief invalid contact primitive information
+  static const int NONE = -1;
+
+  /// @brief Default constructor
+  Contact() : o1(NULL), o2(NULL), b1(NONE), b2(NONE) {
+    penetration_depth = (std::numeric_limits<CoalScalar>::max)();
+    nearest_points[0] = nearest_points[1] = normal = pos =
+        Vec3s::Constant(std::numeric_limits<CoalScalar>::quiet_NaN());
+  }
+
+  Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_,
+          int b2_)
+      : o1(o1_), o2(o2_), b1(b1_), b2(b2_) {
+    penetration_depth = (std::numeric_limits<CoalScalar>::max)();
+    nearest_points[0] = nearest_points[1] = normal = pos =
+        Vec3s::Constant(std::numeric_limits<CoalScalar>::quiet_NaN());
+  }
+
+  Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_,
+          int b2_, const Vec3s& pos_, const Vec3s& normal_, CoalScalar depth_)
+      : o1(o1_),
+        o2(o2_),
+        b1(b1_),
+        b2(b2_),
+        normal(normal_),
+        nearest_points{pos_ - (depth_ * normal_ / 2),
+                       pos_ + (depth_ * normal_ / 2)},
+        pos(pos_),
+        penetration_depth(depth_) {}
+
+  Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_,
+          int b2_, const Vec3s& p1, const Vec3s& p2, const Vec3s& normal_,
+          CoalScalar depth_)
+      : o1(o1_),
+        o2(o2_),
+        b1(b1_),
+        b2(b2_),
+        normal(normal_),
+        nearest_points{p1, p2},
+        pos((p1 + p2) / 2),
+        penetration_depth(depth_) {}
+
+  bool operator<(const Contact& other) const {
+    if (b1 == other.b1) return b2 < other.b2;
+    return b1 < other.b1;
+  }
+
+  bool operator==(const Contact& other) const {
+    return o1 == other.o1 && o2 == other.o2 && b1 == other.b1 &&
+           b2 == other.b2 && normal == other.normal && pos == other.pos &&
+           nearest_points[0] == other.nearest_points[0] &&
+           nearest_points[1] == other.nearest_points[1] &&
+           penetration_depth == other.penetration_depth;
+  }
+
+  bool operator!=(const Contact& other) const { return !(*this == other); }
+
+  CoalScalar getDistanceToCollision(const CollisionRequest& request) const;
+};
+
+struct QueryResult;
+
+/// @brief base class for all query requests
+struct COAL_DLLAPI QueryRequest {
+  // @brief Initial guess to use for the GJK algorithm
+  GJKInitialGuess gjk_initial_guess;
+
+  /// @brief whether enable gjk initial guess
+  /// @Deprecated Use gjk_initial_guess instead
+  COAL_DEPRECATED_MESSAGE(Use gjk_initial_guess instead)
+  bool enable_cached_gjk_guess;
+
+  /// @brief the gjk initial guess set by user
+  mutable Vec3s cached_gjk_guess;
+
+  /// @brief the support function initial guess set by user
+  mutable support_func_guess_t cached_support_func_guess;
+
+  /// @brief maximum iteration for the GJK algorithm
+  size_t gjk_max_iterations;
+
+  /// @brief tolerance for the GJK algorithm.
+  /// Note: This tolerance determines the precision on the estimated distance
+  /// between two geometries which are not in collision.
+  /// It is recommended to not set this tolerance to less than 1e-6.
+  CoalScalar gjk_tolerance;
+
+  /// @brief whether to enable the Nesterov accleration of GJK
+  GJKVariant gjk_variant;
+
+  /// @brief convergence criterion used to stop GJK
+  GJKConvergenceCriterion gjk_convergence_criterion;
+
+  /// @brief convergence criterion used to stop GJK
+  GJKConvergenceCriterionType gjk_convergence_criterion_type;
+
+  /// @brief max number of iterations for EPA
+  size_t epa_max_iterations;
+
+  /// @brief tolerance for EPA.
+  /// Note: This tolerance determines the precision on the estimated distance
+  /// between two geometries which are in collision.
+  /// It is recommended to not set this tolerance to less than 1e-6.
+  /// Also, setting EPA's tolerance to less than GJK's is not recommended.
+  CoalScalar epa_tolerance;
+
+  /// @brief enable timings when performing collision/distance request
+  bool enable_timings;
+
+  /// @brief threshold below which a collision is considered.
+  CoalScalar collision_distance_threshold;
+
+  COAL_COMPILER_DIAGNOSTIC_PUSH
+  COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+  /// @brief Default constructor.
+  QueryRequest()
+      : gjk_initial_guess(GJKInitialGuess::DefaultGuess),
+        enable_cached_gjk_guess(false),
+        cached_gjk_guess(1, 0, 0),
+        cached_support_func_guess(support_func_guess_t::Zero()),
+        gjk_max_iterations(GJK_DEFAULT_MAX_ITERATIONS),
+        gjk_tolerance(GJK_DEFAULT_TOLERANCE),
+        gjk_variant(GJKVariant::DefaultGJK),
+        gjk_convergence_criterion(GJKConvergenceCriterion::Default),
+        gjk_convergence_criterion_type(GJKConvergenceCriterionType::Relative),
+        epa_max_iterations(EPA_DEFAULT_MAX_ITERATIONS),
+        epa_tolerance(EPA_DEFAULT_TOLERANCE),
+        enable_timings(false),
+        collision_distance_threshold(
+            Eigen::NumTraits<CoalScalar>::dummy_precision()) {}
+
+  /// @brief Copy  constructor.
+  QueryRequest(const QueryRequest& other) = default;
+
+  /// @brief Copy  assignment operator.
+  QueryRequest& operator=(const QueryRequest& other) = default;
+  COAL_COMPILER_DIAGNOSTIC_POP
+
+  /// @brief Updates the guess for the internal GJK algorithm in order to
+  /// warm-start it when reusing this collision request on the same collision
+  /// pair.
+  /// @note The option `gjk_initial_guess` must be set to
+  /// `GJKInitialGuess::CachedGuess` for this to work.
+  void updateGuess(const QueryResult& result) const;
+
+  /// @brief whether two QueryRequest are the same or not
+  inline bool operator==(const QueryRequest& other) const {
+    COAL_COMPILER_DIAGNOSTIC_PUSH
+    COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+    return gjk_initial_guess == other.gjk_initial_guess &&
+           enable_cached_gjk_guess == other.enable_cached_gjk_guess &&
+           gjk_variant == other.gjk_variant &&
+           gjk_convergence_criterion == other.gjk_convergence_criterion &&
+           gjk_convergence_criterion_type ==
+               other.gjk_convergence_criterion_type &&
+           gjk_tolerance == other.gjk_tolerance &&
+           gjk_max_iterations == other.gjk_max_iterations &&
+           cached_gjk_guess == other.cached_gjk_guess &&
+           cached_support_func_guess == other.cached_support_func_guess &&
+           epa_max_iterations == other.epa_max_iterations &&
+           epa_tolerance == other.epa_tolerance &&
+           enable_timings == other.enable_timings &&
+           collision_distance_threshold == other.collision_distance_threshold;
+    COAL_COMPILER_DIAGNOSTIC_POP
+  }
+};
+
+/// @brief base class for all query results
+struct COAL_DLLAPI QueryResult {
+  /// @brief stores the last GJK ray when relevant.
+  Vec3s cached_gjk_guess;
+
+  /// @brief stores the last support function vertex index, when relevant.
+  support_func_guess_t cached_support_func_guess;
+
+  /// @brief timings for the given request
+  CPUTimes timings;
+
+  QueryResult()
+      : cached_gjk_guess(Vec3s::Zero()),
+        cached_support_func_guess(support_func_guess_t::Constant(-1)) {}
+};
+
+inline void QueryRequest::updateGuess(const QueryResult& result) const {
+  COAL_COMPILER_DIAGNOSTIC_PUSH
+  COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+  if (gjk_initial_guess == GJKInitialGuess::CachedGuess ||
+      enable_cached_gjk_guess) {
+    cached_gjk_guess = result.cached_gjk_guess;
+    cached_support_func_guess = result.cached_support_func_guess;
+  }
+  COAL_COMPILER_DIAGNOSTIC_POP
+}
+
+struct CollisionResult;
+
+/// @brief flag declaration for specifying required params in CollisionResult
+enum CollisionRequestFlag {
+  CONTACT = 0x00001,
+  DISTANCE_LOWER_BOUND = 0x00002,
+  NO_REQUEST = 0x01000
+};
+
+/// @brief request to the collision algorithm
+struct COAL_DLLAPI CollisionRequest : QueryRequest {
+  /// @brief The maximum number of contacts that can be returned
+  size_t num_max_contacts;
+
+  /// @brief whether the contact information (normal, penetration depth and
+  /// contact position) will return.
+  bool enable_contact;
+
+  /// Whether a lower bound on distance is returned when objects are disjoint
+  COAL_DEPRECATED_MESSAGE(A lower bound on distance is always computed.)
+  bool enable_distance_lower_bound;
+
+  /// @brief Distance below which objects are considered in collision.
+  /// See \ref coal_collision_and_distance_lower_bound_computation
+  /// @note If set to -inf, the objects tested for collision are considered
+  ///       as collision free and no test is actually performed by functions
+  ///       coal::collide of class coal::ComputeCollision.
+  CoalScalar security_margin;
+
+  /// @brief Distance below which bounding volumes are broken down.
+  /// See \ref coal_collision_and_distance_lower_bound_computation
+  CoalScalar break_distance;
+
+  /// @brief Distance above which GJK solver makes an early stopping.
+  /// GJK stops searching for the closest points when it proves that the
+  /// distance between two geometries is above this threshold.
+  ///
+  /// @remarks Consequently, the closest points might be incorrect, but allows
+  /// to save computational resources.
+  CoalScalar distance_upper_bound;
+
+  /// @brief Constructor from a flag and a maximal number of contacts.
+  ///
+  /// @param[in] flag Collision request flag
+  /// @param[in] num_max_contacts  Maximal number of allowed contacts
+  ///
+  COAL_COMPILER_DIAGNOSTIC_PUSH
+  COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+  CollisionRequest(const CollisionRequestFlag flag, size_t num_max_contacts_)
+      : num_max_contacts(num_max_contacts_),
+        enable_contact(flag & CONTACT),
+        enable_distance_lower_bound(flag & DISTANCE_LOWER_BOUND),
+        security_margin(0),
+        break_distance(1e-3),
+        distance_upper_bound((std::numeric_limits<CoalScalar>::max)()) {}
+
+  /// @brief Default constructor.
+  CollisionRequest()
+      : num_max_contacts(1),
+        enable_contact(true),
+        enable_distance_lower_bound(false),
+        security_margin(0),
+        break_distance(1e-3),
+        distance_upper_bound((std::numeric_limits<CoalScalar>::max)()) {}
+  COAL_COMPILER_DIAGNOSTIC_POP
+
+  bool isSatisfied(const CollisionResult& result) const;
+
+  /// @brief whether two CollisionRequest are the same or not
+  inline bool operator==(const CollisionRequest& other) const {
+    COAL_COMPILER_DIAGNOSTIC_PUSH
+    COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+    return QueryRequest::operator==(other) &&
+           num_max_contacts == other.num_max_contacts &&
+           enable_contact == other.enable_contact &&
+           enable_distance_lower_bound == other.enable_distance_lower_bound &&
+           security_margin == other.security_margin &&
+           break_distance == other.break_distance &&
+           distance_upper_bound == other.distance_upper_bound;
+    COAL_COMPILER_DIAGNOSTIC_POP
+  }
+};
+
+inline CoalScalar Contact::getDistanceToCollision(
+    const CollisionRequest& request) const {
+  return penetration_depth - request.security_margin;
+}
+
+/// @brief collision result
+struct COAL_DLLAPI CollisionResult : QueryResult {
+ private:
+  /// @brief contact information
+  std::vector<Contact> contacts;
+
+ public:
+  /// Lower bound on distance between objects if they are disjoint.
+  /// See \ref coal_collision_and_distance_lower_bound_computation
+  /// @note Always computed. If \ref CollisionRequest::distance_upper_bound is
+  /// set to infinity, distance_lower_bound is the actual distance between the
+  /// shapes.
+  CoalScalar distance_lower_bound;
+
+  /// @brief normal associated to nearest_points.
+  /// Same as `CollisionResult::nearest_points` but for the normal.
+  Vec3s normal;
+
+  /// @brief nearest points.
+  /// A `CollisionResult` can have multiple contacts.
+  /// The nearest points in `CollisionResults` correspond to the witness points
+  /// associated with the smallest distance i.e the `distance_lower_bound`.
+  /// For bounding volumes and BVHs, these nearest points are available
+  /// only when distance_lower_bound is inferior to
+  /// CollisionRequest::break_distance.
+  std::array<Vec3s, 2> nearest_points;
+
+ public:
+  CollisionResult()
+      : distance_lower_bound((std::numeric_limits<CoalScalar>::max)()) {
+    nearest_points[0] = nearest_points[1] = normal =
+        Vec3s::Constant(std::numeric_limits<CoalScalar>::quiet_NaN());
+  }
+
+  /// @brief Update the lower bound only if the distance is inferior.
+  inline void updateDistanceLowerBound(
+      const CoalScalar& distance_lower_bound_) {
+    if (distance_lower_bound_ < distance_lower_bound)
+      distance_lower_bound = distance_lower_bound_;
+  }
+
+  /// @brief add one contact into result structure
+  inline void addContact(const Contact& c) { contacts.push_back(c); }
+
+  /// @brief whether two CollisionResult are the same or not
+  inline bool operator==(const CollisionResult& other) const {
+    return contacts == other.contacts &&
+           distance_lower_bound == other.distance_lower_bound &&
+           nearest_points[0] == other.nearest_points[0] &&
+           nearest_points[1] == other.nearest_points[1] &&
+           normal == other.normal;
+  }
+
+  /// @brief return binary collision result
+  bool isCollision() const { return contacts.size() > 0; }
+
+  /// @brief number of contacts found
+  size_t numContacts() const { return contacts.size(); }
+
+  /// @brief get the i-th contact calculated
+  const Contact& getContact(size_t i) const {
+    if (contacts.size() == 0)
+      COAL_THROW_PRETTY(
+          "The number of contacts is zero. No Contact can be returned.",
+          std::invalid_argument);
+
+    if (i < contacts.size())
+      return contacts[i];
+    else
+      return contacts.back();
+  }
+
+  /// @brief set the i-th contact calculated
+  void setContact(size_t i, const Contact& c) {
+    if (contacts.size() == 0)
+      COAL_THROW_PRETTY(
+          "The number of contacts is zero. No Contact can be returned.",
+          std::invalid_argument);
+
+    if (i < contacts.size())
+      contacts[i] = c;
+    else
+      contacts.back() = c;
+  }
+
+  /// @brief get all the contacts
+  void getContacts(std::vector<Contact>& contacts_) const {
+    contacts_.resize(contacts.size());
+    std::copy(contacts.begin(), contacts.end(), contacts_.begin());
+  }
+
+  const std::vector<Contact>& getContacts() const { return contacts; }
+
+  /// @brief clear the results obtained
+  void clear() {
+    distance_lower_bound = (std::numeric_limits<CoalScalar>::max)();
+    contacts.clear();
+    timings.clear();
+    nearest_points[0] = nearest_points[1] = normal =
+        Vec3s::Constant(std::numeric_limits<CoalScalar>::quiet_NaN());
+  }
+
+  /// @brief reposition Contact objects when fcl inverts them
+  /// during their construction.
+  void swapObjects();
+};
+
+/// @brief This structure allows to encode contact patches.
+/// A contact patch is defined by a set of points belonging to a subset of a
+/// plane passing by `p` and supported by `n`, where `n = Contact::normal` and
+/// `p = Contact::pos`. If we denote by P this plane and by S1 and S2 the first
+/// and second shape of a collision pair, a contact patch is represented as a
+/// polytope which vertices all belong to `P & S1 & S2`, where `&` denotes the
+/// set-intersection. Since a contact patch is a subset of a plane supported by
+/// `n`, it has a preferred direction. In Coal, the `Contact::normal` points
+/// from S1 to S2. In the same way, a contact patch points by default from S1
+/// to S2.
+///
+/// @note For now (April 2024), a `ContactPatch` is a polygon (2D polytope),
+/// so the points of the set, forming the convex-hull of the polytope, are
+/// stored in a counter-clockwise fashion.
+/// @note If needed, the internal algorithms of Coal can easily be extended
+/// to compute a contact volume instead of a contact patch.
+struct COAL_DLLAPI ContactPatch {
+ public:
+  using Polygon = std::vector<Vec2s>;
+
+  /// @brief Frame of the set, expressed in the world coordinates.
+  /// The z-axis of the frame's rotation is the contact patch normal.
+  Transform3s tf;
+
+  /// @brief Direction of ContactPatch.
+  /// When doing collision detection, the convention of Coal is that the
+  /// normal always points from the first to the second shape of the collision
+  /// pair i.e. from shape1 to shape2 when calling `collide(shape1, shape2)`.
+  /// The PatchDirection enum allows to identify if the patch points from
+  /// shape 1 to shape 2 (Default type) or from shape 2 to shape 1 (Inverted
+  /// type). The Inverted type should only be used for internal Coal
+  /// computations (it allows to properly define two separate contact patches in
+  /// the same frame).
+  enum PatchDirection { DEFAULT = 0, INVERTED = 1 };
+
+  /// @brief Direction of this contact patch.
+  PatchDirection direction;
+
+  /// @brief Penetration depth of the contact patch. This value corresponds to
+  /// the signed distance `d` between the shapes.
+  /// @note For each contact point `p` in the patch of normal `n`, `p1 = p -
+  /// 0.5*d*n` and `p2 = p + 0.5*d*n` define a pair of witness points. `p1`
+  /// belongs to the surface of the first shape and `p2` belongs to the surface
+  /// of the second shape. For any pair of witness points, we always have `p2 -
+  /// p1 = d * n`. The vector `d * n` is called a minimum separation vector:
+  /// if S1 is translated by it, S1 and S2 are not in collision anymore.
+  /// @note Although there may exist multiple minimum separation vectors between
+  /// two shapes, the term "minimum" comes from the fact that it's impossible to
+  /// find a different separation vector which has a smaller norm than `d * n`.
+  CoalScalar penetration_depth;
+
+  /// @brief Default maximum size of the polygon representing the contact patch.
+  /// Used to pre-allocate memory for the patch.
+  static constexpr size_t default_preallocated_size = 12;
+
+ protected:
+  /// @brief Container for the vertices of the set.
+  Polygon m_points;
+
+ public:
+  /// @brief Default constructor.
+  /// Note: the preallocated size does not determine the maximum number of
+  /// points in the patch, it only serves as preallocation if the maximum size
+  /// of the patch is known in advance. Coal will automatically expand/shrink
+  /// the contact patch if needed.
+  explicit ContactPatch(size_t preallocated_size = default_preallocated_size)
+      : tf(Transform3s::Identity()),
+        direction(PatchDirection::DEFAULT),
+        penetration_depth(0) {
+    this->m_points.reserve(preallocated_size);
+  }
+
+  /// @brief Normal of the contact patch, expressed in the WORLD frame.
+  Vec3s getNormal() const {
+    if (this->direction == PatchDirection::INVERTED) {
+      return -this->tf.rotation().col(2);
+    }
+    return this->tf.rotation().col(2);
+  }
+
+  /// @brief Returns the number of points in the contact patch.
+  size_t size() const { return this->m_points.size(); }
+
+  /// @brief Add a 3D point to the set, expressed in the world frame.
+  /// @note This function takes a 3D point and expresses it in the local frame
+  /// of the set. It then takes only the x and y components of the vector,
+  /// effectively doing a projection onto the plane to which the set belongs.
+  /// TODO(louis): if necessary, we can store the offset to the plane (x, y).
+  void addPoint(const Vec3s& point_3d) {
+    const Vec3s point = this->tf.inverseTransform(point_3d);
+    this->m_points.emplace_back(point.template head<2>());
+  }
+
+  /// @brief Get the i-th point of the set, expressed in the 3D world frame.
+  Vec3s getPoint(const size_t i) const {
+    Vec3s point(0, 0, 0);
+    point.head<2>() = this->point(i);
+    point = tf.transform(point);
+    return point;
+  }
+
+  /// @brief Get the i-th point of the contact patch, projected back onto the
+  /// first shape of the collision pair. This point is expressed in the 3D
+  /// world frame.
+  Vec3s getPointShape1(const size_t i) const {
+    Vec3s point = this->getPoint(i);
+    point -= (this->penetration_depth / 2) * this->getNormal();
+    return point;
+  }
+
+  /// @brief Get the i-th point of the contact patch, projected back onto the
+  /// first shape of the collision pair. This 3D point is expressed in the world
+  /// frame.
+  Vec3s getPointShape2(const size_t i) const {
+    Vec3s point = this->getPoint(i);
+    point += (this->penetration_depth / 2) * this->getNormal();
+    return point;
+  }
+
+  /// @brief Getter for the 2D points in the set.
+  Polygon& points() { return this->m_points; }
+
+  /// @brief Const getter for the 2D points in the set.
+  const Polygon& points() const { return this->m_points; }
+
+  /// @brief Getter for the i-th 2D point in the set.
+  Vec2s& point(const size_t i) {
+    COAL_ASSERT(this->m_points.size() > 0, "Patch is empty.", std::logic_error);
+    if (i < this->m_points.size()) {
+      return this->m_points[i];
+    }
+    return this->m_points.back();
+  }
+
+  /// @brief Const getter for the i-th 2D point in the set.
+  const Vec2s& point(const size_t i) const {
+    COAL_ASSERT(this->m_points.size() > 0, "Patch is empty.", std::logic_error);
+    if (i < this->m_points.size()) {
+      return this->m_points[i];
+    }
+    return this->m_points.back();
+  }
+
+  /// @brief Clear the set.
+  void clear() {
+    this->m_points.clear();
+    this->tf.setIdentity();
+    this->penetration_depth = 0;
+  }
+
+  /// @brief Whether two contact patches are the same or not.
+  /// @note This compares the two sets terms by terms.
+  /// However, two contact patches can be identical, but have a different
+  /// order for their points. Use `isEqual` in this case.
+  bool operator==(const ContactPatch& other) const {
+    return this->tf == other.tf && this->direction == other.direction &&
+           this->penetration_depth == other.penetration_depth &&
+           this->points() == other.points();
+  }
+
+  /// @brief Whether two contact patches are the same or not.
+  /// Checks for different order of the points.
+  bool isSame(const ContactPatch& other,
+              const CoalScalar tol =
+                  Eigen::NumTraits<CoalScalar>::dummy_precision()) const {
+    // The x and y axis of the set are arbitrary, but the z axis is
+    // always the normal. The position of the origin of the frame is also
+    // arbitrary. So we only check if the normals are the same.
+    if (!this->getNormal().isApprox(other.getNormal(), tol)) {
+      return false;
+    }
+
+    if (std::abs(this->penetration_depth - other.penetration_depth) > tol) {
+      return false;
+    }
+
+    if (this->direction != other.direction) {
+      return false;
+    }
+
+    if (this->size() != other.size()) {
+      return false;
+    }
+
+    // Check all points of the contact patch.
+    for (size_t i = 0; i < this->size(); ++i) {
+      bool found = false;
+      const Vec3s pi = this->getPoint(i);
+      for (size_t j = 0; j < other.size(); ++j) {
+        const Vec3s other_pj = other.getPoint(j);
+        if (pi.isApprox(other_pj, tol)) {
+          found = true;
+        }
+      }
+      if (!found) {
+        return false;
+      }
+    }
+
+    return true;
+  }
+};
+
+/// @brief Construct a frame from a `Contact`'s position and normal.
+/// Because both `Contact`'s position and normal are expressed in the world
+/// frame, this frame is also expressed w.r.t the world frame.
+/// The origin of the frame is `contact.pos` and the z-axis of the frame is
+/// `contact.normal`.
+inline void constructContactPatchFrameFromContact(const Contact& contact,
+                                                  ContactPatch& contact_patch) {
+  contact_patch.penetration_depth = contact.penetration_depth;
+  contact_patch.tf.translation() = contact.pos;
+  contact_patch.tf.rotation() =
+      constructOrthonormalBasisFromVector(contact.normal);
+  contact_patch.direction = ContactPatch::PatchDirection::DEFAULT;
+}
+
+/// @brief Structure used for internal computations. A support set and a
+/// contact patch can be represented by the same structure. In fact, a contact
+/// patch is the intersection of two support sets, one with
+/// `PatchDirection::DEFAULT` and one with `PatchDirection::INVERTED`.
+/// @note A support set with `DEFAULT` direction is the support set of a shape
+/// in the direction given by `+n`, where `n` is the z-axis of the frame's
+/// patch rotation. An `INVERTED` support set is the support set of a shape in
+/// the direction `-n`.
+using SupportSet = ContactPatch;
+
+/// @brief Request for a contact patch computation.
+struct COAL_DLLAPI ContactPatchRequest {
+  /// @brief Maximum number of contact patches that will be computed.
+  size_t max_num_patch;
+
+ protected:
+  /// @brief Maximum samples to compute the support sets of curved shapes,
+  /// i.e. when the normal is perpendicular to the base of a cylinder. For
+  /// now, only relevant for Cone and Cylinder. In the future this might be
+  /// extended to Sphere and Ellipsoid.
+  size_t m_num_samples_curved_shapes;
+
+  /// @brief Tolerance below which points are added to a contact patch.
+  /// In details, given two shapes S1 and S2, a contact patch is the triple
+  /// intersection between the separating plane (P) (passing by `Contact::pos`
+  /// and supported by `Contact::normal`), S1 and S2; i.e. a contact patch is
+  /// `P & S1 & S2` if we denote `&` the set intersection operator. If a point
+  /// p1 of S1 is at a distance below `patch_tolerance` from the separating
+  /// plane, it is taken into account in the computation of the contact patch.
+  /// Otherwise, it is not used for the computation.
+  /// @note Needs to be positive.
+  CoalScalar m_patch_tolerance;
+
+ public:
+  /// @brief Default constructor.
+  /// @param max_num_patch maximum number of contact patches per collision pair.
+  /// @param max_sub_patch_size maximum size of each sub contact patch. Each
+  /// contact patch contains an internal representation for an inscribed sub
+  /// contact patch. This allows physics simulation to always work with a
+  /// predetermined maximum size for each contact patch. A sub contact patch is
+  /// simply a subset of the vertices of a contact patch.
+  /// @param num_samples_curved_shapes for shapes like cones and cylinders,
+  /// which have smooth basis (circles in this case), we need to sample a
+  /// certain amount of point of this basis.
+  /// @param patch_tolerance the tolerance below which a point of a shape is
+  /// considered to belong to the support set of this shape in the direction of
+  /// the normal. Said otherwise, `patch_tolerance` determines the "thickness"
+  /// of the separating plane between shapes of a collision pair.
+  explicit ContactPatchRequest(size_t max_num_patch = 1,
+                               size_t num_samples_curved_shapes =
+                                   ContactPatch::default_preallocated_size,
+                               CoalScalar patch_tolerance = 1e-3)
+      : max_num_patch(max_num_patch) {
+    this->setNumSamplesCurvedShapes(num_samples_curved_shapes);
+    this->setPatchTolerance(patch_tolerance);
+  }
+
+  /// @brief Construct a contact patch request from a collision request.
+  explicit ContactPatchRequest(const CollisionRequest& collision_request,
+                               size_t num_samples_curved_shapes =
+                                   ContactPatch::default_preallocated_size,
+                               CoalScalar patch_tolerance = 1e-3)
+      : max_num_patch(collision_request.num_max_contacts) {
+    this->setNumSamplesCurvedShapes(num_samples_curved_shapes);
+    this->setPatchTolerance(patch_tolerance);
+  }
+
+  /// @copydoc m_num_samples_curved_shapes
+  void setNumSamplesCurvedShapes(const size_t num_samples_curved_shapes) {
+    if (num_samples_curved_shapes < 3) {
+      COAL_LOG_WARNING(
+          "`num_samples_curved_shapes` cannot be lower than 3. Setting it to "
+          "3 to prevent bugs.");
+      this->m_num_samples_curved_shapes = 3;
+    } else {
+      this->m_num_samples_curved_shapes = num_samples_curved_shapes;
+    }
+  }
+
+  /// @copydoc m_num_samples_curved_shapes
+  size_t getNumSamplesCurvedShapes() const {
+    return this->m_num_samples_curved_shapes;
+  }
+
+  /// @copydoc m_patch_tolerance
+  void setPatchTolerance(const CoalScalar patch_tolerance) {
+    if (patch_tolerance < 0) {
+      COAL_LOG_WARNING(
+          "`patch_tolerance` cannot be negative. Setting it to 0 to prevent "
+          "bugs.");
+      this->m_patch_tolerance = Eigen::NumTraits<CoalScalar>::dummy_precision();
+    } else {
+      this->m_patch_tolerance = patch_tolerance;
+    }
+  }
+
+  /// @copydoc m_patch_tolerance
+  CoalScalar getPatchTolerance() const { return this->m_patch_tolerance; }
+
+  /// @brief Whether two ContactPatchRequest are identical or not.
+  bool operator==(const ContactPatchRequest& other) const {
+    return this->max_num_patch == other.max_num_patch &&
+           this->getNumSamplesCurvedShapes() ==
+               other.getNumSamplesCurvedShapes() &&
+           this->getPatchTolerance() == other.getPatchTolerance();
+  }
+};
+
+/// @brief Result for a contact patch computation.
+struct COAL_DLLAPI ContactPatchResult {
+  using ContactPatchVector = std::vector<ContactPatch>;
+  using ContactPatchRef = std::reference_wrapper<ContactPatch>;
+  using ContactPatchRefVector = std::vector<ContactPatchRef>;
+
+ protected:
+  /// @brief Data container for the vector of contact patches.
+  /// @note Contrary to `CollisionResult` or `DistanceResult`, which have a
+  /// very small memory footprint, contact patches can contain relatively
+  /// large polytopes. In order to reuse a `ContactPatchResult` while avoiding
+  /// successive mallocs, we have a data container and a vector which points
+  /// to the currently active patches in this data container.
+  ContactPatchVector m_contact_patches_data;
+
+  /// @brief Contact patches in `m_contact_patches_data` can have two
+  /// statuses: used or unused. This index tracks the first unused patch in
+  /// the `m_contact_patches_data` vector.
+  size_t m_id_available_patch;
+
+  /// @brief Vector of contact patches of the result.
+  ContactPatchRefVector m_contact_patches;
+
+ public:
+  /// @brief Default constructor.
+  ContactPatchResult() : m_id_available_patch(0) {
+    const size_t max_num_patch = 1;
+    const ContactPatchRequest request(max_num_patch);
+    this->set(request);
+  }
+
+  /// @brief Constructor using a `ContactPatchRequest`.
+  explicit ContactPatchResult(const ContactPatchRequest& request)
+      : m_id_available_patch(0) {
+    this->set(request);
+  };
+
+  /// @brief Number of contact patches in the result.
+  size_t numContactPatches() const { return this->m_contact_patches.size(); }
+
+  /// @brief Returns a new unused contact patch from the internal data vector.
+  ContactPatchRef getUnusedContactPatch() {
+    if (this->m_id_available_patch >= this->m_contact_patches_data.size()) {
+      COAL_LOG_WARNING(
+          "Trying to get an unused contact patch but all contact patches are "
+          "used. Increasing size of contact patches vector, at the cost of a "
+          "copy. You should increase `max_num_patch` in the "
+          "`ContactPatchRequest`.");
+      this->m_contact_patches_data.emplace_back(
+          this->m_contact_patches_data.back());
+      this->m_contact_patches_data.back().clear();
+    }
+    ContactPatch& contact_patch =
+        this->m_contact_patches_data[this->m_id_available_patch];
+    contact_patch.clear();
+    this->m_contact_patches.emplace_back(contact_patch);
+    ++(this->m_id_available_patch);
+    return this->m_contact_patches.back();
+  }
+
+  /// @brief Const getter for the i-th contact patch of the result.
+  const ContactPatch& getContactPatch(const size_t i) const {
+    if (this->m_contact_patches.empty()) {
+      COAL_THROW_PRETTY(
+          "The number of contact patches is zero. No ContactPatch can be "
+          "returned.",
+          std::invalid_argument);
+    }
+    if (i < this->m_contact_patches.size()) {
+      return this->m_contact_patches[i];
+    }
+    return this->m_contact_patches.back();
+  }
+
+  /// @brief Getter for the i-th contact patch of the result.
+  ContactPatch& contactPatch(const size_t i) {
+    if (this->m_contact_patches.empty()) {
+      COAL_THROW_PRETTY(
+          "The number of contact patches is zero. No ContactPatch can be "
+          "returned.",
+          std::invalid_argument);
+    }
+    if (i < this->m_contact_patches.size()) {
+      return this->m_contact_patches[i];
+    }
+    return this->m_contact_patches.back();
+  }
+
+  /// @brief Clears the contact patch result.
+  void clear() {
+    this->m_contact_patches.clear();
+    this->m_id_available_patch = 0;
+    for (ContactPatch& patch : this->m_contact_patches_data) {
+      patch.clear();
+    }
+  }
+
+  /// @brief Set up a `ContactPatchResult` from a `ContactPatchRequest`
+  void set(const ContactPatchRequest& request) {
+    if (this->m_contact_patches_data.size() < request.max_num_patch) {
+      this->m_contact_patches_data.resize(request.max_num_patch);
+    }
+    for (ContactPatch& patch : this->m_contact_patches_data) {
+      patch.points().reserve(request.getNumSamplesCurvedShapes());
+    }
+    this->clear();
+  }
+
+  /// @brief Return true if this `ContactPatchResult` is aligned with the
+  /// `ContactPatchRequest` given as input.
+  bool check(const ContactPatchRequest& request) const {
+    assert(this->m_contact_patches_data.size() >= request.max_num_patch);
+    if (this->m_contact_patches_data.size() < request.max_num_patch) {
+      return false;
+    }
+
+    for (const ContactPatch& patch : this->m_contact_patches_data) {
+      if (patch.points().capacity() < request.getNumSamplesCurvedShapes()) {
+        assert(patch.points().capacity() >=
+               request.getNumSamplesCurvedShapes());
+        return false;
+      }
+    }
+    return true;
+  }
+
+  /// @brief Whether two ContactPatchResult are identical or not.
+  bool operator==(const ContactPatchResult& other) const {
+    if (this->numContactPatches() != other.numContactPatches()) {
+      return false;
+    }
+
+    for (size_t i = 0; i < this->numContactPatches(); ++i) {
+      const ContactPatch& patch = this->getContactPatch(i);
+      const ContactPatch& other_patch = other.getContactPatch(i);
+      if (!(patch == other_patch)) {
+        return false;
+      }
+    }
+
+    return true;
+  }
+
+  /// @brief Repositions the ContactPatch when they get inverted during their
+  /// construction.
+  void swapObjects() {
+    // Create new transform: it's the reflection of `tf` along the z-axis.
+    // This corresponds to doing a PI rotation along the y-axis, i.e. the x-axis
+    // becomes -x-axis, y-axis stays the same, z-axis becomes -z-axis.
+    for (size_t i = 0; i < this->numContactPatches(); ++i) {
+      ContactPatch& patch = this->contactPatch(i);
+      patch.tf.rotation().col(0) *= -1.0;
+      patch.tf.rotation().col(2) *= -1.0;
+
+      for (size_t j = 0; j < patch.size(); ++j) {
+        patch.point(i)(0) *= -1.0;  // only invert the x-axis
+      }
+    }
+  }
+};
+
+struct DistanceResult;
+
+/// @brief request to the distance computation
+struct COAL_DLLAPI DistanceRequest : QueryRequest {
+  /// @brief whether to return the nearest points.
+  /// Nearest points are always computed and are the points of the shapes that
+  /// achieve a distance of `DistanceResult::min_distance`.
+  COAL_DEPRECATED_MESSAGE(
+      Nearest points are always computed : they are the points of the shapes
+          that achieve a distance of
+      `DistanceResult::min_distance`
+              .\n Use `enable_signed_distance` if you want to compute a signed
+                  minimum distance(and thus its corresponding nearest points)
+              .)
+  bool enable_nearest_points;
+
+  /// @brief whether to compute the penetration depth when objects are in
+  /// collision.
+  /// Turning this off can save computation time if only the distance
+  /// when objects are disjoint is needed.
+  /// @note The minimum distance between the shapes is stored in
+  /// `DistanceResult::min_distance`.
+  /// If `enable_signed_distance` is off, `DistanceResult::min_distance`
+  /// is always positive.
+  /// If `enable_signed_distance` is on, `DistanceResult::min_distance`
+  /// can be positive or negative.
+  /// The nearest points are the points of the shapes that achieve
+  /// a distance of `DistanceResult::min_distance`.
+  bool enable_signed_distance;
+
+  /// @brief error threshold for approximate distance
+  CoalScalar rel_err;  // relative error, between 0 and 1
+  CoalScalar abs_err;  // absolute error
+
+  /// \param enable_nearest_points_ enables the nearest points computation.
+  /// \param enable_signed_distance_ allows to compute the penetration depth
+  /// \param rel_err_
+  /// \param abs_err_
+  COAL_COMPILER_DIAGNOSTIC_PUSH
+  COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+  DistanceRequest(bool enable_nearest_points_ = true,
+                  bool enable_signed_distance_ = true,
+                  CoalScalar rel_err_ = 0.0, CoalScalar abs_err_ = 0.0)
+      : enable_nearest_points(enable_nearest_points_),
+        enable_signed_distance(enable_signed_distance_),
+        rel_err(rel_err_),
+        abs_err(abs_err_) {}
+  COAL_COMPILER_DIAGNOSTIC_POP
+
+  bool isSatisfied(const DistanceResult& result) const;
+
+  COAL_COMPILER_DIAGNOSTIC_PUSH
+  COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+  DistanceRequest& operator=(const DistanceRequest& other) = default;
+  COAL_COMPILER_DIAGNOSTIC_POP
+
+  /// @brief whether two DistanceRequest are the same or not
+  inline bool operator==(const DistanceRequest& other) const {
+    COAL_COMPILER_DIAGNOSTIC_PUSH
+    COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+    return QueryRequest::operator==(other) &&
+           enable_nearest_points == other.enable_nearest_points &&
+           enable_signed_distance == other.enable_signed_distance &&
+           rel_err == other.rel_err && abs_err == other.abs_err;
+    COAL_COMPILER_DIAGNOSTIC_POP
+  }
+};
+
+/// @brief distance result
+struct COAL_DLLAPI DistanceResult : QueryResult {
+ public:
+  /// @brief minimum distance between two objects.
+  /// If two objects are in collision and
+  /// DistanceRequest::enable_signed_distance is activated, min_distance <= 0.
+  /// @note The nearest points are the points of the shapes that achieve a
+  /// distance of `DistanceResult::min_distance`.
+  CoalScalar min_distance;
+
+  /// @brief normal.
+  Vec3s normal;
+
+  /// @brief nearest points.
+  /// See CollisionResult::nearest_points.
+  std::array<Vec3s, 2> nearest_points;
+
+  /// @brief collision object 1
+  const CollisionGeometry* o1;
+
+  /// @brief collision object 2
+  const CollisionGeometry* o2;
+
+  /// @brief information about the nearest point in object 1
+  /// if object 1 is mesh or point cloud, it is the triangle or point id
+  /// if object 1 is geometry shape, it is NONE (-1),
+  /// if object 1 is octree, it is the id of the cell
+  int b1;
+
+  /// @brief information about the nearest point in object 2
+  /// if object 2 is mesh or point cloud, it is the triangle or point id
+  /// if object 2 is geometry shape, it is NONE (-1),
+  /// if object 2 is octree, it is the id of the cell
+  int b2;
+
+  /// @brief invalid contact primitive information
+  static const int NONE = -1;
+
+  DistanceResult(
+      CoalScalar min_distance_ = (std::numeric_limits<CoalScalar>::max)())
+      : min_distance(min_distance_), o1(NULL), o2(NULL), b1(NONE), b2(NONE) {
+    const Vec3s nan(
+        Vec3s::Constant(std::numeric_limits<CoalScalar>::quiet_NaN()));
+    nearest_points[0] = nearest_points[1] = normal = nan;
+  }
+
+  /// @brief add distance information into the result
+  void update(CoalScalar distance, const CollisionGeometry* o1_,
+              const CollisionGeometry* o2_, int b1_, int b2_) {
+    if (min_distance > distance) {
+      min_distance = distance;
+      o1 = o1_;
+      o2 = o2_;
+      b1 = b1_;
+      b2 = b2_;
+    }
+  }
+
+  /// @brief add distance information into the result
+  void update(CoalScalar distance, const CollisionGeometry* o1_,
+              const CollisionGeometry* o2_, int b1_, int b2_, const Vec3s& p1,
+              const Vec3s& p2, const Vec3s& normal_) {
+    if (min_distance > distance) {
+      min_distance = distance;
+      o1 = o1_;
+      o2 = o2_;
+      b1 = b1_;
+      b2 = b2_;
+      nearest_points[0] = p1;
+      nearest_points[1] = p2;
+      normal = normal_;
+    }
+  }
+
+  /// @brief add distance information into the result
+  void update(const DistanceResult& other_result) {
+    if (min_distance > other_result.min_distance) {
+      min_distance = other_result.min_distance;
+      o1 = other_result.o1;
+      o2 = other_result.o2;
+      b1 = other_result.b1;
+      b2 = other_result.b2;
+      nearest_points[0] = other_result.nearest_points[0];
+      nearest_points[1] = other_result.nearest_points[1];
+      normal = other_result.normal;
+    }
+  }
+
+  /// @brief clear the result
+  void clear() {
+    const Vec3s nan(
+        Vec3s::Constant(std::numeric_limits<CoalScalar>::quiet_NaN()));
+    min_distance = (std::numeric_limits<CoalScalar>::max)();
+    o1 = NULL;
+    o2 = NULL;
+    b1 = NONE;
+    b2 = NONE;
+    nearest_points[0] = nearest_points[1] = normal = nan;
+    timings.clear();
+  }
+
+  /// @brief whether two DistanceResult are the same or not
+  inline bool operator==(const DistanceResult& other) const {
+    bool is_same = min_distance == other.min_distance &&
+                   nearest_points[0] == other.nearest_points[0] &&
+                   nearest_points[1] == other.nearest_points[1] &&
+                   normal == other.normal && o1 == other.o1 && o2 == other.o2 &&
+                   b1 == other.b1 && b2 == other.b2;
+
+    // TODO: check also that two GeometryObject are indeed equal.
+    if ((o1 != NULL) ^ (other.o1 != NULL)) return false;
+    is_same &= (o1 == other.o1);
+    //    else if (o1 != NULL and other.o1 != NULL) is_same &= *o1 ==
+    //    *other.o1;
+
+    if ((o2 != NULL) ^ (other.o2 != NULL)) return false;
+    is_same &= (o2 == other.o2);
+    //    else if (o2 != NULL and other.o2 != NULL) is_same &= *o2 ==
+    //    *other.o2;
+
+    return is_same;
+  }
+};
+
+namespace internal {
+inline void updateDistanceLowerBoundFromBV(const CollisionRequest& /*req*/,
+                                           CollisionResult& res,
+                                           const CoalScalar sqrDistLowerBound) {
+  // BV cannot find negative distance.
+  if (res.distance_lower_bound <= 0) return;
+  CoalScalar new_dlb = std::sqrt(sqrDistLowerBound);  // - req.security_margin;
+  if (new_dlb < res.distance_lower_bound) res.distance_lower_bound = new_dlb;
+}
+
+inline void updateDistanceLowerBoundFromLeaf(const CollisionRequest&,
+                                             CollisionResult& res,
+                                             const CoalScalar& distance,
+                                             const Vec3s& p0, const Vec3s& p1,
+                                             const Vec3s& normal) {
+  if (distance < res.distance_lower_bound) {
+    res.distance_lower_bound = distance;
+    res.nearest_points[0] = p0;
+    res.nearest_points[1] = p1;
+    res.normal = normal;
+  }
+}
+}  // namespace internal
+
+inline CollisionRequestFlag operator~(CollisionRequestFlag a) {
+  return static_cast<CollisionRequestFlag>(~static_cast<int>(a));
+}
+
+inline CollisionRequestFlag operator|(CollisionRequestFlag a,
+                                      CollisionRequestFlag b) {
+  return static_cast<CollisionRequestFlag>(static_cast<int>(a) |
+                                           static_cast<int>(b));
+}
+
+inline CollisionRequestFlag operator&(CollisionRequestFlag a,
+                                      CollisionRequestFlag b) {
+  return static_cast<CollisionRequestFlag>(static_cast<int>(a) &
+                                           static_cast<int>(b));
+}
+
+inline CollisionRequestFlag operator^(CollisionRequestFlag a,
+                                      CollisionRequestFlag b) {
+  return static_cast<CollisionRequestFlag>(static_cast<int>(a) ^
+                                           static_cast<int>(b));
+}
+
+inline CollisionRequestFlag& operator|=(CollisionRequestFlag& a,
+                                        CollisionRequestFlag b) {
+  return (CollisionRequestFlag&)((int&)(a) |= static_cast<int>(b));
+}
+
+inline CollisionRequestFlag& operator&=(CollisionRequestFlag& a,
+                                        CollisionRequestFlag b) {
+  return (CollisionRequestFlag&)((int&)(a) &= static_cast<int>(b));
+}
+
+inline CollisionRequestFlag& operator^=(CollisionRequestFlag& a,
+                                        CollisionRequestFlag b) {
+  return (CollisionRequestFlag&)((int&)(a) ^= static_cast<int>(b));
+}
+
+}  // namespace coal
+
+#endif

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

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

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

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

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

src/intersect.h → 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 HPP_FCL_INTERSECT_H
-#define HPP_FCL_INTERSECT_H
-
-/// @cond INTERNAL
-
-#include <hpp/fcl/math/transform.h>
-#include <boost/math/special_functions/erf.hpp>
-
-namespace hpp
-{
-namespace fcl
-{
-
-/// @brief CCD intersect kernel among primitives
-class Intersect
-{
-public:
-  static bool buildTrianglePlane
-    (const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, Vec3f* n, FCL_REAL* t);
-}; // class Intersect
-
-/// @brief Project functions
-class Project
-{
-public:
-  struct ProjectResult
-  {
-    /// @brief Parameterization of the projected point (based on the simplex to be projected, use 2 or 3 or 4 of the array)
-    FCL_REAL parameterization[4];
-
-    /// @brief square distance from the query point to the projected simplex
-    FCL_REAL 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 Vec3f& a, const Vec3f& b, const Vec3f& p);
-
-  /// @brief Project point p onto triangle a-b-c
-  static ProjectResult projectTriangle(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& p);
-
-  /// @brief Project point p onto tetrahedra a-b-c-d
-  static ProjectResult projectTetrahedra(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& d, const Vec3f& p);
-
-  /// @brief Project origin (0) onto line a-b
-  static ProjectResult projectLineOrigin(const Vec3f& a, const Vec3f& b);
-
-  /// @brief Project origin (0) onto triangle a-b-c
-  static ProjectResult projectTriangleOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c);
-
-  /// @brief Project origin (0) onto tetrahedran a-b-c-d
-  static ProjectResult projectTetrahedraOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& d);
-};
-
-/// @brief Triangle distance functions
-class 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 Vec3f& P, const Vec3f& A, const Vec3f& Q, const Vec3f& B,
-                        Vec3f& VEC, Vec3f& X, Vec3f& 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 FCL_REAL sqrTriDistance (const Vec3f S[3], const Vec3f T[3],
-				  Vec3f& P, Vec3f& Q);
-
-  static FCL_REAL sqrTriDistance (const Vec3f& S1, const Vec3f& S2,
-				  const Vec3f& S3, const Vec3f& T1,
-				  const Vec3f& T2, const Vec3f& T3,
-				  Vec3f& P, Vec3f& 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 FCL_REAL sqrTriDistance (const Vec3f S[3], const Vec3f T[3],
-				  const Matrix3f& R, const Vec3f& Tl,
-				  Vec3f& P, Vec3f& 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 FCL_REAL sqrTriDistance (const Vec3f S[3], const Vec3f T[3],
-				  const Transform3f& tf,
-				  Vec3f& P, Vec3f& 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 FCL_REAL sqrTriDistance (const Vec3f& S1, const Vec3f& S2,
-				  const Vec3f& S3, const Vec3f& T1,
-				  const Vec3f& T2, const Vec3f& T3,
-				  const Matrix3f& R, const Vec3f& Tl,
-				  Vec3f& P, Vec3f& 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 FCL_REAL sqrTriDistance (const Vec3f& S1, const Vec3f& S2,
-				  const Vec3f& S3, const Vec3f& T1,
-				  const Vec3f& T2, const Vec3f& T3,
-				  const Transform3f& tf,
-				  Vec3f& P, Vec3f& Q);
-
-};
-
-}
-
-} // namespace hpp
-
-/// @endcond
-
-#endif
+/*
+ * 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