include/coal/broadphase/detail/node_base_array.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** @author Jia Pan  */
+
+#ifndef COAL_BROADPHASE_DETAIL_NODEBASEARRAY_H
+#define COAL_BROADPHASE_DETAIL_NODEBASEARRAY_H
+
+#include "coal/data_types.h"
+
+namespace coal {
+
+namespace detail {
+
+namespace implementation_array {
+
+template <typename BV>
+struct NodeBase {
+  BV bv;
+
+  union {
+    size_t parent;
+    size_t next;
+  };
+
+  union {
+    size_t children[2];
+    void* data;
+  };
+
+  uint32_t code;
+
+  bool isLeaf() const;
+  bool isInternal() const;
+};
+
+}  // namespace implementation_array
+}  // namespace detail
+}  // namespace coal
+
+#include "coal/broadphase/detail/node_base_array-inl.h"
+
+#endif

include/fcl/BVH/BVH_utility.h → include/coal/broadphase/detail/simple_hash_table-inl.h

@@ -2,7 +2,7 @@
  * Software License Agreement (BSD License)
  *
  *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, Open Source Robotics Foundation
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
  *  All rights reserved.
  *
  *  Redistribution and use in source and binary forms, with or without
@@ -33,66 +33,79 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  */
 
-/** \author Jia Pan */
+/** @author Jia Pan */
 
+#ifndef COAL_BROADPHASE_SIMPLEHASHTABLE_INL_H
+#define COAL_BROADPHASE_SIMPLEHASHTABLE_INL_H
 
-#ifndef FCL_BVH_UTILITY_H
-#define FCL_BVH_UTILITY_H
+#include "coal/broadphase/detail/simple_hash_table.h"
 
-#include "fcl/BVH/BVH_model.h"
+#include <iterator>
+namespace coal {
 
+namespace detail {
 
-namespace fcl
-{
-/// @brief Expand the BVH bounding boxes according to the variance matrix corresponding to the data stored within each BV node
-template<typename BV>
-void BVHExpand(BVHModel<BV>& model, const Variance3f* ucs, FCL_REAL r)
-{
-  for(int i = 0; i < model.num_bvs; ++i)
-  {
-    BVNode<BV>& bvnode = model.getBV(i);
-
-    BV bv;
-    for(int j = 0; j < bvnode.num_primitives; ++j)
-    {
-      int v_id = bvnode.first_primitive + j;
-      const Variance3f& uc = ucs[v_id];
-
-      Vec3f& v = model.vertices[bvnode.first_primitive + j];
-
-      for(int k = 0; k < 3; ++k)
-      {
-        bv += (v + uc.axis[k] * (r * uc.sigma[k]));
-        bv += (v - uc.axis[k] * (r * uc.sigma[k]));
-      }
-    }
-
-    bvnode.bv = bv;
-  }
+//==============================================================================
+template <typename Key, typename Data, typename HashFnc>
+SimpleHashTable<Key, Data, HashFnc>::SimpleHashTable(const HashFnc& h) : h_(h) {
+  // Do nothing
 }
 
-/// @brief Expand the BVH bounding boxes according to the corresponding variance information, for OBB
-void BVHExpand(BVHModel<OBB>& model, const Variance3f* ucs, FCL_REAL r);
-
-/// @brief Expand the BVH bounding boxes according to the corresponding variance information, for RSS
-void BVHExpand(BVHModel<RSS>& model, const Variance3f* ucs, FCL_REAL r);
-
-/// @brief Compute the covariance matrix for a set or subset of points. if ts = null, then indices refer to points directly; otherwise refer to triangles
-void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Matrix3f& M);
+//==============================================================================
+template <typename Key, typename Data, typename HashFnc>
+void SimpleHashTable<Key, Data, HashFnc>::init(size_t size) {
+  if (size == 0) {
+    COAL_THROW_PRETTY("SimpleHashTable must have non-zero size.",
+                      std::logic_error);
+  }
 
-/// @brief Compute the RSS bounding volume parameters: radius, rectangle size and the origin, given the BV axises.
-void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Vec3f axis[3], Vec3f& origin, FCL_REAL l[2], FCL_REAL& r);
+  table_.resize(size);
+  table_size_ = size;
+}
 
-/// @brief Compute the bounding volume extent and center for a set or subset of points, given the BV axises.
-void getExtentAndCenter(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Vec3f axis[3], Vec3f& center, Vec3f& extent);
+//==============================================================================
+template <typename Key, typename Data, typename HashFnc>
+void SimpleHashTable<Key, Data, HashFnc>::insert(Key key, Data value) {
+  std::vector<unsigned int> indices = h_(key);
+  size_t range = table_.size();
+  for (size_t i = 0; i < indices.size(); ++i)
+    table_[indices[i] % range].push_back(value);
+}
 
-/// @brief Compute the center and radius for a triangle's circumcircle
-void circumCircleComputation(const Vec3f& a, const Vec3f& b, const Vec3f& c, Vec3f& center, FCL_REAL& radius);
+//==============================================================================
+template <typename Key, typename Data, typename HashFnc>
+std::vector<Data> SimpleHashTable<Key, Data, HashFnc>::query(Key key) const {
+  size_t range = table_.size();
+  std::vector<unsigned int> indices = h_(key);
+  std::set<Data> result;
+  for (size_t i = 0; i < indices.size(); ++i) {
+    size_t index = indices[i] % range;
+    std::copy(table_[index].begin(), table_[index].end(),
+              std::inserter(result, result.end()));
+  }
 
-/// @brief Compute the maximum distance from a given center point to a point cloud
-FCL_REAL maximumDistance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, const Vec3f& query);
+  return std::vector<Data>(result.begin(), result.end());
+}
 
+//==============================================================================
+template <typename Key, typename Data, typename HashFnc>
+void SimpleHashTable<Key, Data, HashFnc>::remove(Key key, Data value) {
+  size_t range = table_.size();
+  std::vector<unsigned int> indices = h_(key);
+  for (size_t i = 0; i < indices.size(); ++i) {
+    size_t index = indices[i] % range;
+    table_[index].remove(value);
+  }
+}
 
+//==============================================================================
+template <typename Key, typename Data, typename HashFnc>
+void SimpleHashTable<Key, Data, HashFnc>::clear() {
+  table_.clear();
+  table_.resize(table_size_);
 }
 
+}  // namespace detail
+}  // namespace coal
+
 #endif

include/fcl/ccd/interpolation/interpolation_factory.h → include/coal/broadphase/detail/simple_hash_table.h

@@ -2,7 +2,7 @@
  * Software License Agreement (BSD License)
  *
  *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, Open Source Robotics Foundation
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
  *  All rights reserved.
  *
  *  Redistribution and use in source and binary forms, with or without
@@ -33,50 +33,55 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  */
 
-/** \author Dalibor Matura, Jia Pan */
+/** @author Jia Pan */
 
-#ifndef FCL_CCD_INTERPOLATION_INTERPOLATION_FACTORY_H
-#define FCL_CCD_INTERPOLATION_INTERPOLATION_FACTORY_H
+#ifndef COAL_BROADPHASE_SIMPLEHASHTABLE_H
+#define COAL_BROADPHASE_SIMPLEHASHTABLE_H
 
-#include "fcl/data_types.h"
-#include "fcl/ccd/interpolation/interpolation.h"
+#include <set>
+#include <vector>
+#include <list>
 
-#include <map>
+namespace coal {
 
-#include <boost/shared_ptr.hpp>
-#include <boost/function.hpp>
+namespace detail {
 
-namespace fcl 
-{
+/// @brief A simple hash table implemented as multiple buckets. HashFnc is any
+/// extended hash function: HashFnc(key) = {index1, index2, ..., }
+template <typename Key, typename Data, typename HashFnc>
+class SimpleHashTable {
+ protected:
+  typedef std::list<Data> Bin;
 
-class InterpolationFactory
-{	
-public:
-  typedef boost::function<boost::shared_ptr<Interpolation>(FCL_REAL, FCL_REAL)> CreateFunction;
+  std::vector<Bin> table_;
 
-public:
-  void registerClass(const InterpolationType type, const CreateFunction create_function);
+  HashFnc h_;
 
-  boost::shared_ptr<Interpolation> create(const InterpolationType type, FCL_REAL start_value, FCL_REAL end_value);
+  size_t table_size_;
 
-public:
-  static InterpolationFactory& instance();
+ public:
+  SimpleHashTable(const HashFnc& h);
 
-private:
-  InterpolationFactory();
+  /// @brief Init the number of bins in the hash table
+  void init(size_t size);
 
-  InterpolationFactory(const InterpolationFactory&)
-  {}
+  //// @brief Insert a key-value pair into the table
+  void insert(Key key, Data value);
 
-  InterpolationFactory& operator = (const InterpolationFactory&)
-  {
-    return *this;
-  }
+  /// @brief Find the elements in the hash table whose key is the same as query
+  /// key.
+  std::vector<Data> query(Key key) const;
 
-private:
-  std::map<InterpolationType, CreateFunction> creation_map_;
+  /// @brief remove the key-value pair from the table
+  void remove(Key key, Data value);
+
+  /// @brief clear the hash table
+  void clear();
 };
 
-}
+}  // namespace detail
+}  // namespace coal
+
+#include "coal/broadphase/detail/simple_hash_table-inl.h"
 
 #endif

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

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** @author Jia Pan */
+
+#ifndef COAL_BROADPHASE_DETAIL_SIMPLEINTERVAL_INL_H
+#define COAL_BROADPHASE_DETAIL_SIMPLEINTERVAL_INL_H
+
+#include "coal/broadphase/detail/simple_interval.h"
+#include <algorithm>
+
+namespace coal {
+namespace detail {
+
+//==============================================================================
+SimpleInterval::~SimpleInterval() {
+  // Do nothing
+}
+
+//==============================================================================
+void SimpleInterval::print() {
+  // Do nothing
+}
+
+}  // namespace detail
+}  // namespace coal
+
+#endif

include/coal/broadphase/detail/simple_interval.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** @author Jia Pan */
+
+#ifndef COAL_BROADPHASE_DETAIL_SIMPLEINTERVAL_H
+#define COAL_BROADPHASE_DETAIL_SIMPLEINTERVAL_H
+
+#include "coal/fwd.hh"
+#include "coal/data_types.h"
+
+namespace coal {
+namespace detail {
+
+/// @brief Interval trees implemented using red-black-trees as described in
+/// the book Introduction_To_Algorithms_ by Cormen, Leisserson, and Rivest.
+struct COAL_DLLAPI SimpleInterval {
+ public:
+  virtual ~SimpleInterval();
+
+  virtual void print();
+
+  /// @brief interval is defined as [low, high]
+  CoalScalar low, high;
+};
+
+}  // namespace detail
+}  // namespace coal
+
+#endif

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

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** @author Jia Pan */
+
+#ifndef COAL_BROADPHASE_SPARSEHASHTABLE_INL_H
+#define COAL_BROADPHASE_SPARSEHASHTABLE_INL_H
+
+#include "coal/broadphase/detail/sparse_hash_table.h"
+
+namespace coal {
+
+namespace detail {
+
+//==============================================================================
+template <typename Key, typename Data, typename HashFnc,
+          template <typename, typename> class TableT>
+SparseHashTable<Key, Data, HashFnc, TableT>::SparseHashTable(const HashFnc& h)
+    : h_(h) {
+  // Do nothing
+}
+
+//==============================================================================
+template <typename Key, typename Data, typename HashFnc,
+          template <typename, typename> class TableT>
+void SparseHashTable<Key, Data, HashFnc, TableT>::init(size_t) {
+  table_.clear();
+}
+
+//==============================================================================
+template <typename Key, typename Data, typename HashFnc,
+          template <typename, typename> class TableT>
+void SparseHashTable<Key, Data, HashFnc, TableT>::insert(Key key, Data value) {
+  std::vector<unsigned int> indices = h_(key);
+  for (size_t i = 0; i < indices.size(); ++i)
+    table_[indices[i]].push_back(value);
+}
+
+//==============================================================================
+template <typename Key, typename Data, typename HashFnc,
+          template <typename, typename> class TableT>
+std::vector<Data> SparseHashTable<Key, Data, HashFnc, TableT>::query(
+    Key key) const {
+  std::vector<unsigned int> indices = h_(key);
+  std::set<Data> result;
+  for (size_t i = 0; i < indices.size(); ++i) {
+    unsigned int index = indices[i];
+    typename Table::const_iterator p = table_.find(index);
+    if (p != table_.end()) {
+      std::copy((*p).second.begin(), (*p).second.end(),
+                std ::inserter(result, result.end()));
+    }
+  }
+
+  return std::vector<Data>(result.begin(), result.end());
+}
+
+//==============================================================================
+template <typename Key, typename Data, typename HashFnc,
+          template <typename, typename> class TableT>
+void SparseHashTable<Key, Data, HashFnc, TableT>::remove(Key key, Data value) {
+  std::vector<unsigned int> indices = h_(key);
+  for (size_t i = 0; i < indices.size(); ++i) {
+    unsigned int index = indices[i];
+    table_[index].remove(value);
+  }
+}
+
+//==============================================================================
+template <typename Key, typename Data, typename HashFnc,
+          template <typename, typename> class TableT>
+void SparseHashTable<Key, Data, HashFnc, TableT>::clear() {
+  table_.clear();
+}
+
+}  // namespace detail
+}  // namespace coal
+
+#endif

include/fcl/articulated_model/joint_config.h → include/coal/broadphase/detail/sparse_hash_table.h

@@ -2,7 +2,7 @@
  * Software License Agreement (BSD License)
  *
  *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, Open Source Robotics Foundation
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
  *  All rights reserved.
  *
  *  Redistribution and use in source and binary forms, with or without
@@ -33,69 +33,61 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  */
 
-/** \author Dalibor Matura, Jia Pan */
+/** @author Jia Pan */
 
-#ifndef FCL_ARTICULATED_MODEL_JOINT_CONFIG_H
-#define FCL_ARTICULATED_MODEL_JOINT_CONFIG_H
+#ifndef COAL_BROADPHASE_SPARSEHASHTABLE_H
+#define COAL_BROADPHASE_SPARSEHASHTABLE_H
 
-#include "fcl/data_types.h"
-#include <boost/shared_ptr.hpp>
-#include <boost/weak_ptr.hpp>
+#include <set>
 #include <vector>
+#include <list>
+#include <unordered_map>
 
-namespace fcl
-{
-
-class Joint;
-
-class JointConfig
-{
-public:
-  JointConfig();
-
-  JointConfig(const JointConfig& joint_cfg);
-
-  JointConfig(const boost::shared_ptr<Joint>& joint,
-              FCL_REAL default_value = 0,
-              FCL_REAL default_value_min = 0,
-              FCL_REAL default_value_max = 0);
-
-  std::size_t getDim() const;
-
-  inline FCL_REAL operator [] (std::size_t i) const
-  {
-    return values_[i];
-  }
-
-  inline FCL_REAL& operator [] (std::size_t i)
-  {
-    return values_[i];
-  }
-
-  FCL_REAL getValue(std::size_t i) const;
-
-  FCL_REAL& getValue(std::size_t i);
-  
-  FCL_REAL getLimitMin(std::size_t i) const;
-  
-  FCL_REAL& getLimitMin(std::size_t i);
-  
-  FCL_REAL getLimitMax(std::size_t i) const;
-  
-  FCL_REAL& getLimitMax(std::size_t i);
-  
-  boost::shared_ptr<Joint> getJoint() const;
-
-private:
-  boost::weak_ptr<Joint> joint_;
-  
-  std::vector<FCL_REAL> values_;
-  std::vector<FCL_REAL> limits_min_;
-  std::vector<FCL_REAL> limits_max_;
+namespace coal {
+
+namespace detail {
+
+template <typename U, typename V>
+class unordered_map_hash_table : public std::unordered_map<U, V> {
+  typedef std::unordered_map<U, V> Base;
+
+ public:
+  unordered_map_hash_table() : Base() {};
 };
 
-}
+/// @brief A hash table implemented using unordered_map
+template <typename Key, typename Data, typename HashFnc,
+          template <typename, typename> class TableT = unordered_map_hash_table>
+class SparseHashTable {
+ protected:
+  HashFnc h_;
+  typedef std::list<Data> Bin;
+  typedef TableT<size_t, Bin> Table;
+
+  Table table_;
+
+ public:
+  SparseHashTable(const HashFnc& h);
+
+  /// @brief Init the hash table. The bucket size is dynamically decided.
+  void init(size_t);
+
+  /// @brief insert one key-value pair into the hash table
+  void insert(Key key, Data value);
+
+  /// @brief find the elements whose key is the same as the query
+  std::vector<Data> query(Key key) const;
+
+  /// @brief remove one key-value pair from the hash table
+  void remove(Key key, Data value);
+
+  /// @brief clear the hash table
+  void clear();
+};
 
+}  // namespace detail
+}  // namespace coal
 
+#include "coal/broadphase/detail/sparse_hash_table-inl.h"
 
 #endif

include/fcl/data_types.h → include/coal/broadphase/detail/spatial_hash-inl.h

@@ -2,7 +2,7 @@
  * Software License Agreement (BSD License)
  *
  *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2015, Open Source Robotics Foundation
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
  *  All rights reserved.
  *
  *  Redistribution and use in source and binary forms, with or without
@@ -33,51 +33,48 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  */
 
-/** \author Jia Pan */
+/** @author Jia Pan */
 
-#ifndef FCL_DATA_TYPES_H
-#define FCL_DATA_TYPES_H
+#ifndef COAL_BROADPHASE_SPATIALHASH_INL_H
+#define COAL_BROADPHASE_SPATIALHASH_INL_H
 
-#include <cstddef>
-#include <boost/cstdint.hpp>
+#include "coal/broadphase/detail/spatial_hash.h"
+#include <algorithm>
 
-namespace fcl
-{
+namespace coal {
+namespace detail {
 
-typedef double FCL_REAL;
-typedef boost::uint64_t FCL_INT64;
-typedef boost::int64_t FCL_UINT64;
-typedef boost::uint32_t FCL_UINT32;
-typedef boost::int32_t FCL_INT32;
-
-/// @brief Triangle with 3 indices for points
-class Triangle
-{
-  /// @brief indices for each vertex of triangle
-  std::size_t vids[3];
-
-public:
-  /// @brief Default constructor
-  Triangle() {}
+//==============================================================================
+SpatialHash::SpatialHash(const AABB& scene_limit_, CoalScalar cell_size_)
+    : cell_size(cell_size_), scene_limit(scene_limit_) {
+  width[0] = std::ceil(scene_limit.width() / cell_size);
+  width[1] = std::ceil(scene_limit.height() / cell_size);
+  width[2] = std::ceil(scene_limit.depth() / cell_size);
+}
 
-  /// @brief Create a triangle with given vertex indices
-  Triangle(std::size_t p1, std::size_t p2, std::size_t p3)
-  {
-    set(p1, p2, p3);
-  }
+//==============================================================================
+std::vector<unsigned int> SpatialHash::operator()(const AABB& aabb) const {
+  int min_x = std::floor((aabb.min_[0] - scene_limit.min_[0]) / cell_size);
+  int max_x = std::ceil((aabb.max_[0] - scene_limit.min_[0]) / cell_size);
+  int min_y = std::floor((aabb.min_[1] - scene_limit.min_[1]) / cell_size);
+  int max_y = std::ceil((aabb.max_[1] - scene_limit.min_[1]) / cell_size);
+  int min_z = std::floor((aabb.min_[2] - scene_limit.min_[2]) / cell_size);
+  int max_z = std::ceil((aabb.max_[2] - scene_limit.min_[2]) / cell_size);
 
-  /// @brief Set the vertex indices of the triangle
-  inline void set(std::size_t p1, std::size_t p2, std::size_t p3)
-  {
-    vids[0] = p1; vids[1] = p2; vids[2] = p3;
+  std::vector<unsigned int> keys((max_x - min_x) * (max_y - min_y) *
+                                 (max_z - min_z));
+  int id = 0;
+  for (int x = min_x; x < max_x; ++x) {
+    for (int y = min_y; y < max_y; ++y) {
+      for (int z = min_z; z < max_z; ++z) {
+        keys[id++] = x + y * width[0] + z * width[0] * width[1];
+      }
+    }
   }
-
-  /// @access the triangle index
-  inline std::size_t operator[](int i) const { return vids[i]; }
-
-  inline std::size_t& operator[](int i) { return vids[i]; }
-};
-
+  return keys;
 }
 
+}  // namespace detail
+}  // namespace coal
+
 #endif

include/coal/broadphase/detail/spatial_hash.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** @author Jia Pan */
+
+#ifndef COAL_BROADPHASE_SPATIALHASH_H
+#define COAL_BROADPHASE_SPATIALHASH_H
+
+#include "coal/BV/AABB.h"
+#include <vector>
+
+namespace coal {
+
+namespace detail {
+
+/// @brief Spatial hash function: hash an AABB to a set of integer values
+struct COAL_DLLAPI SpatialHash {
+  SpatialHash(const AABB& scene_limit_, CoalScalar cell_size_);
+
+  std::vector<unsigned int> operator()(const AABB& aabb) const;
+
+ private:
+  CoalScalar cell_size;
+  AABB scene_limit;
+  unsigned int width[3];
+};
+
+}  // namespace detail
+}  // namespace coal
+
+#endif

include/fcl/collision.h → include/coal/collision.h

@@ -3,6 +3,7 @@
  *
  *  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
@@ -35,30 +36,85 @@
 
 /** \author Jia Pan */
 
+#ifndef COAL_COLLISION_H
+#define COAL_COLLISION_H
 
-#ifndef FCL_COLLISION_H
-#define FCL_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"
 
-#include "fcl/math/vec_3f.h"
-#include "fcl/collision_object.h"
-#include "fcl/collision_data.h"
+namespace coal {
 
-namespace fcl
-{
-
-/// @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. 
+/// @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;
 
-std::size_t collide(const CollisionObject* o1, const CollisionObject* o2,
-                    const CollisionRequest& request,
-                    CollisionResult& result);
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
 
-std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
-                    const CollisionGeometry* o2, const Transform3f& tf2,
-                    const CollisionRequest& request,
-                    CollisionResult& result);
-}
+}  // 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/fcl/collision_func_matrix.h → include/coal/collision_func_matrix.h

@@ -35,36 +35,43 @@
 
 /** \author Jia Pan */
 
+#ifndef COAL_COLLISION_FUNC_MATRIX_H
+#define COAL_COLLISION_FUNC_MATRIX_H
 
-#ifndef FCL_COLLISION_FUNC_MATRIX_H
-#define FCL_COLLISION_FUNC_MATRIX_H
+#include "coal/collision_object.h"
+#include "coal/collision_data.h"
+#include "coal/narrowphase/narrowphase.h"
 
+namespace coal {
 
-#include "fcl/collision_object.h"
-#include "fcl/collision_data.h"
+/// @brief collision matrix stores the functions for collision between different
+/// types of objects and provides a uniform call interface
 
-namespace fcl
-{
-
-/// @brief collision matrix stores the functions for collision between different types of objects and provides a uniform call interface
-template<typename NarrowPhaseSolver>
-struct 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);
+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 Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const CollisionRequest& request, CollisionResult& 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
+  /// @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/fcl/collision_object.h → include/coal/collision_object.h

@@ -35,38 +35,94 @@
 
 /** \author Jia Pan */
 
+#ifndef COAL_COLLISION_OBJECT_BASE_H
+#define COAL_COLLISION_OBJECT_BASE_H
 
-#ifndef FCL_COLLISION_OBJECT_BASE_H
-#define FCL_COLLISION_OBJECT_BASE_H
+#include <limits>
+#include <typeinfo>
 
-#include <fcl/deprecated.h>
-#include "fcl/BV/AABB.h"
-#include "fcl/math/transform.h"
-#include "fcl/ccd/motion_base.h"
-#include <boost/shared_ptr.hpp>
+#include "coal/deprecated.hh"
+#include "coal/fwd.hh"
+#include "coal/BV/AABB.h"
+#include "coal/math/transform.h"
 
-namespace fcl
-{
+namespace coal {
 
 /// @brief object type: BVH (mesh, points), basic geometry, octree
-enum OBJECT_TYPE {OT_UNKNOWN, OT_BVH, OT_GEOM, OT_OCTREE, OT_COUNT};
+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
+};
 
-/// @brief traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18, kDOP24), basic shape (box, sphere, 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, NODE_COUNT};
+/// @addtogroup Construction_Of_BVH
+/// @{
 
 /// @brief The geometry for the object for collision or distance computation
-class CollisionGeometry
-{
-public:
-  CollisionGeometry() : cost_density(1),
-                        threshold_occupied(1),
-                        threshold_free(0)
-  {
-  }
+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; }
 
@@ -77,16 +133,10 @@ public:
   virtual void computeLocalAABB() = 0;
 
   /// @brief get user data in geometry
-  void* getUserData() const
-  {
-    return user_data;
-  }
+  void* getUserData() const { return user_data; }
 
   /// @brief set user data in geometry
-  void setUserData(void *data)
-  {
-    user_data = data;
-  }
+  void setUserData(void* data) { user_data = data; }
 
   /// @brief whether the object is completely occupied
   inline bool isOccupied() const { return cost_density >= threshold_occupied; }
@@ -95,391 +145,216 @@ public:
   inline bool isFree() const { return cost_density <= threshold_free; }
 
   /// @brief whether the object has some uncertainty
-  inline bool isUncertain() const { return !isOccupied() && !isFree(); }
+  bool isUncertain() const;
 
   /// @brief AABB center in local coordinate
-  Vec3f aabb_center;
+  Vec3s aabb_center;
 
   /// @brief AABB radius
-  FCL_REAL aabb_radius;
+  CoalScalar aabb_radius;
 
-  /// @brief AABB in local coordinate, used for tight AABB when only translation transform
+  /// @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;
+  void* user_data;
 
   /// @brief collision cost for unit volume
-  FCL_REAL cost_density;
+  CoalScalar cost_density;
 
   /// @brief threshold for occupied ( >= is occupied)
-  FCL_REAL threshold_occupied;
+  CoalScalar threshold_occupied;
 
   /// @brief threshold for free (<= is free)
-  FCL_REAL threshold_free;
+  CoalScalar threshold_free;
 
   /// @brief compute center of mass
-  virtual Vec3f computeCOM() const { return Vec3f(); }
+  virtual Vec3s computeCOM() const { return Vec3s::Zero(); }
 
   /// @brief compute the inertia matrix, related to the origin
-  virtual Matrix3f computeMomentofInertia() const { return Matrix3f(); }
+  virtual Matrix3s computeMomentofInertia() const {
+    return Matrix3s::Constant(NAN);
+  }
 
   /// @brief compute the volume
-  virtual FCL_REAL computeVolume() const { return 0; }
+  virtual CoalScalar computeVolume() const { return 0; }
 
   /// @brief compute the inertia matrix, related to the com
-  virtual Matrix3f computeMomentofInertiaRelatedToCOM() const
-  {
-    Matrix3f C = computeMomentofInertia();
-    Vec3f com = computeCOM();
-    FCL_REAL V = computeVolume();
-
-    return Matrix3f(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]));
+  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 CollisionObject
-{
-public:
- CollisionObject(const boost::shared_ptr<CollisionGeometry> &cgeom_) :
-    cgeom(cgeom_), cgeom_const(cgeom_)
-  {
-    if (cgeom)
-    {
-      cgeom->computeLocalAABB();
-      computeAABB();
-    }
+/// @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 boost::shared_ptr<CollisionGeometry> &cgeom_, const Transform3f& tf) :
-    cgeom(cgeom_), cgeom_const(cgeom_), t(tf)
-  {
-    cgeom->computeLocalAABB();
-    computeAABB();
+  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 boost::shared_ptr<CollisionGeometry> &cgeom_, const Matrix3f& R, const Vec3f& T):
-      cgeom(cgeom_), cgeom_const(cgeom_), t(Transform3f(R, T))
-  {
-    cgeom->computeLocalAABB();
-    computeAABB();
+  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);
   }
 
-  ~CollisionObject()
-  {
+  bool operator==(const CollisionObject& other) const {
+    return cgeom == other.cgeom && t == other.t && user_data == other.user_data;
   }
 
-  /// @brief get the type of the object
-  OBJECT_TYPE getObjectType() const
-  {
-    return cgeom->getObjectType();
+  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();
-  }
+  NODE_TYPE getNodeType() const { return cgeom->getNodeType(); }
 
   /// @brief get the AABB in world space
-  inline const AABB& getAABB() const
-  {
-    return aabb;
-  }
+  const AABB& getAABB() const { return aabb; }
+
+  /// @brief get the AABB in world space
+  AABB& getAABB() { return aabb; }
 
   /// @brief compute the AABB in world space
-  inline void computeAABB()
-  {
-    if(t.getQuatRotation().isIdentity())
-    {
+  void computeAABB() {
+    if (t.getRotation().isIdentity()) {
       aabb = translate(cgeom->aabb_local, t.getTranslation());
-    }
-    else
-    {
-      Vec3f center = t.transform(cgeom->aabb_center);
-      Vec3f delta(cgeom->aabb_radius);
-      aabb.min_ = center - delta;
-      aabb.max_ = center + delta;
+    } 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;
-  }
+  void* getUserData() const { return user_data; }
 
   /// @brief set user data in object
-  void setUserData(void *data)
-  {
-    user_data = data;
-  }
+  void setUserData(void* data) { user_data = data; }
 
   /// @brief get translation of the object
-  inline const Vec3f& getTranslation() const
-  {
-    return t.getTranslation();
-  }
+  inline const Vec3s& getTranslation() const { return t.getTranslation(); }
 
   /// @brief get matrix rotation of the object
-  inline const Matrix3f& getRotation() const
-  {
-    return t.getRotation();
-  }
-
-  /// @brief get quaternion rotation of the object
-  inline const Quaternion3f& getQuatRotation() const
-  {
-    return t.getQuatRotation();
-  }
+  inline const Matrix3s& getRotation() const { return t.getRotation(); }
 
   /// @brief get object's transform
-  inline const Transform3f& getTransform() const
-  {
-    return t;
-  }
+  inline const Transform3s& getTransform() const { return t; }
 
   /// @brief set object's rotation matrix
-  void setRotation(const Matrix3f& R)
-  {
-    t.setRotation(R);
-  }
+  void setRotation(const Matrix3s& R) { t.setRotation(R); }
 
   /// @brief set object's translation
-  void setTranslation(const Vec3f& T)
-  {
-    t.setTranslation(T);
-  }
-
-  /// @brief set object's quatenrion rotation
-  void setQuatRotation(const Quaternion3f& q)
-  {
-    t.setQuatRotation(q);
-  }
+  void setTranslation(const Vec3s& T) { t.setTranslation(T); }
 
   /// @brief set object's transform
-  void setTransform(const Matrix3f& R, const Vec3f& T)
-  {
-    t.setTransform(R, T);
-  }
+  void setTransform(const Matrix3s& R, const Vec3s& T) { t.setTransform(R, T); }
 
   /// @brief set object's transform
-  void setTransform(const Quaternion3f& q, const Vec3f& T)
-  {
-    t.setTransform(q, T);
-  }
-
-  /// @brief set object's transform
-  void setTransform(const Transform3f& tf)
-  {
-    t = tf;
-  }
+  void setTransform(const Transform3s& tf) { t = tf; }
 
   /// @brief whether the object is in local coordinate
-  bool isIdentityTransform() const
-  {
-    return t.isIdentity();
-  }
+  bool isIdentityTransform() const { return t.isIdentity(); }
 
   /// @brief set the object in local coordinate
-  void setIdentityTransform()
-  {
-    t.setIdentity();
-  }
+  void setIdentityTransform() { t.setIdentity(); }
 
-  /// @brief get geometry from the object instance
-  FCL_DEPRECATED
-  const CollisionGeometry* getCollisionGeometry() const
-  {
-    return cgeom.get();
-  }
-
-  /// @brief get geometry from the object instance
-  const boost::shared_ptr<const CollisionGeometry>& collisionGeometry() const
-  {
-    return cgeom_const;
-  }
-
-  /// @brief get geometry from the object instance
-  const boost::shared_ptr<CollisionGeometry>& collisionGeometry()
-  {
+  /// @brief get shared pointer to collision geometry of the object instance
+  const shared_ptr<const CollisionGeometry> collisionGeometry() const {
     return cgeom;
   }
 
-  /// @brief get object's cost density
-  FCL_REAL getCostDensity() const
-  {
-    return cgeom->cost_density;
-  }
+  /// @brief get shared pointer to collision geometry of the object instance
+  const shared_ptr<CollisionGeometry>& collisionGeometry() { return cgeom; }
 
-  /// @brief set object's cost density
-  void setCostDensity(FCL_REAL c)
-  {
-    cgeom->cost_density = c;
-  }
+  /// @brief get raw pointer to collision geometry of the object instance
+  const CollisionGeometry* collisionGeometryPtr() const { return cgeom.get(); }
 
-  /// @brief whether the object is completely occupied
-  inline bool isOccupied() const
-  {
-    return cgeom->isOccupied();
-  }
+  /// @brief get raw pointer to collision geometry of the object instance
+  CollisionGeometry* collisionGeometryPtr() { return cgeom.get(); }
 
-  /// @brief whether the object is completely free
-  inline bool isFree() const
-  {
-    return cgeom->isFree();
+  /// @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);
+    }
   }
 
-  /// @brief whether the object is uncertain
-  inline bool isUncertain() const
-  {
-    return cgeom->isUncertain();
+ protected:
+  void init(bool compute_local_aabb = true) {
+    if (cgeom) {
+      if (compute_local_aabb) cgeom->computeLocalAABB();
+      computeAABB();
+    }
   }
 
-protected:
-
-  boost::shared_ptr<CollisionGeometry> cgeom;
-  boost::shared_ptr<const CollisionGeometry> cgeom_const;
+  shared_ptr<CollisionGeometry> cgeom;
 
-  Transform3f t;
+  Transform3s t;
 
   /// @brief AABB in global coordinate
   mutable AABB aabb;
 
-  /// @brief pointer to user defined data specific to this object
-  void *user_data;
-};
-
-
-/// @brief the object for continuous collision or distance computation, contains the geometry and the motion information
-class ContinuousCollisionObject
-{
-public:
-  ContinuousCollisionObject(const boost::shared_ptr<CollisionGeometry>& cgeom_) :
-    cgeom(cgeom_), cgeom_const(cgeom_)
-  {
-  }
-
-  ContinuousCollisionObject(const boost::shared_ptr<CollisionGeometry>& cgeom_, const boost::shared_ptr<MotionBase>& motion_) :
-    cgeom(cgeom_), cgeom_const(cgeom), motion(motion_)
-  {
-  }
-
-  ~ContinuousCollisionObject() {}
-
-  /// @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 the world space for the motion
-  inline const AABB& getAABB() const
-  {
-    return aabb;
-  }
-
-  /// @brief compute the AABB in the world space for the motion
-  inline void computeAABB()
-  {
-    IVector3 box;
-    TMatrix3 R;
-    TVector3 T;
-    motion->getTaylorModel(R, T);
-
-    Vec3f p = cgeom->aabb_local.min_;
-    box = (R * p + T).getTightBound();
-
-    p[2] = cgeom->aabb_local.max_[2];
-    box = bound(box, (R * p + T).getTightBound());
-
-    p[1] = cgeom->aabb_local.max_[1];
-    p[2] = cgeom->aabb_local.min_[2];
-    box = bound(box, (R * p + T).getTightBound());
-
-    p[2] = cgeom->aabb_local.max_[2];
-    box = bound(box, (R * p + T).getTightBound());
-
-    p[0] = cgeom->aabb_local.max_[0];
-    p[1] = cgeom->aabb_local.min_[1];
-    p[2] = cgeom->aabb_local.min_[2];
-    box = bound(box, (R * p + T).getTightBound());
-
-    p[2] = cgeom->aabb_local.max_[2];
-    box = bound(box, (R * p + T).getTightBound());
-
-    p[1] = cgeom->aabb_local.max_[1];
-    p[2] = cgeom->aabb_local.min_[2];
-    box = bound(box, (R * p + T).getTightBound());
-
-    p[2] = cgeom->aabb_local.max_[2];
-    box = bound(box, (R * p + T).getTightBound());
-
-    aabb.min_ = box.getLow();
-    aabb.max_ = box.getHigh();
-  }
-
-  /// @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 motion from the object instance
-  inline MotionBase* getMotion() const
-  {
-    return motion.get();
-  }
-
-  /// @brief get geometry from the object instance
-  FCL_DEPRECATED
-  inline const CollisionGeometry* getCollisionGeometry() const
-  {
-    return cgeom.get();
-  }
-
-  /// @brief get geometry from the object instance
-  inline const boost::shared_ptr<const CollisionGeometry>& collisionGeometry() const
-  {
-    return cgeom_const;
-  }
-
-protected:
-
-  boost::shared_ptr<CollisionGeometry> cgeom;
-  boost::shared_ptr<const CollisionGeometry> cgeom_const;
-
-  boost::shared_ptr<MotionBase> motion;
-
-  /// @brief AABB in the global coordinate for the motion
-  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/fcl/knn/nearest_neighbors_linear.h → include/coal/contact_patch.h

 /*
  * Software License Agreement (BSD License)
  *
- *  Copyright (c) 2008-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
@@ -15,7 +14,7 @@
  *     copyright notice, this list of conditions and the following
  *     disclaimer in the documentation and/or other materials provided
  *     with the distribution.
- *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *   * Neither the name of INRIA nor the names of its
  *     contributors may be used to endorse or promote products derived
  *     from this software without specific prior written permission.
  *
@@ -33,144 +32,91 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  */
 
-/* Author: Ioan Sucan */
-
-#ifndef FCL_KNN_NEAREST_NEIGHBORS_LINEAR_H
-#define FCL_KNN_NEAREST_NEIGHBORS_LINEAR_H
-
-#include "fcl/knn/nearest_neighbors.h"
-#include "fcl/exception.h"
-#include <algorithm>
-
-namespace fcl
-{
-/** \brief A nearest neighbors datastructure that uses linear
-    search.
-
-    \li Search for nearest neighbor is O(n).
-    \li Search for k-nearest neighbors is  O(n log(k)).
-    \li Search for neighbors within a range is O(n log(n)).
-    \li Adding an element to the datastructure is O(1).
-    \li Removing an element from the datastructure O(n).
-*/
-template<typename _T>
-class NearestNeighborsLinear : public NearestNeighbors<_T>
-{
-public:
-  NearestNeighborsLinear(void) : NearestNeighbors<_T>()
-  {
+/** \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;
   }
 
-  virtual ~NearestNeighborsLinear(void)
-  {
+  bool operator!=(const ComputeContactPatch& other) const {
+    return !(*this == other);
   }
 
-  virtual void clear(void)
-  {
-    data_.clear();
-  }
-
-  virtual void add(const _T &data)
-  {
-    data_.push_back(data);
-  }
-
-  virtual void add(const std::vector<_T> &data)
-  {
-    data_.reserve(data_.size() + data.size());
-    data_.insert(data_.end(), data.begin(), data.end());
-  }
-
-  virtual bool remove(const _T &data)
-  {
-    if (!data_.empty())
-      for (int i = data_.size() - 1 ; i >= 0 ; --i)
-        if (data_[i] == data)
-        {
-          data_.erase(data_.begin() + i);
-          return true;
-        }
-    return false;
-  }
-
-  virtual _T nearest(const _T &data) const
-  {
-    const std::size_t sz = data_.size();
-    std::size_t pos = sz;
-    double dmin = 0.0;
-    for (std::size_t i = 0 ; i < sz ; ++i)
-    {
-      double distance = NearestNeighbors<_T>::distFun_(data_[i], data);
-      if (pos == sz || dmin > distance)
-      {
-        pos = i;
-        dmin = distance;
-      }
-    }
-    if (pos != sz)
-      return data_[pos];
-
-    throw Exception("No elements found in nearest neighbors data structure");
-  }
-
-  virtual void nearestK(const _T &data, std::size_t k, std::vector<_T> &nbh) const
-  {
-    nbh = data_;
-    if (nbh.size() > k)
-    {
-      std::partial_sort(nbh.begin(), nbh.begin() + k, nbh.end(),
-                        ElemSort(data, NearestNeighbors<_T>::distFun_));
-      nbh.resize(k);
-    }
-    else
-    {
-      std::sort(nbh.begin(), nbh.end(), ElemSort(data, NearestNeighbors<_T>::distFun_));
-    }
-  }
-
-  virtual void nearestR(const _T &data, double radius, std::vector<_T> &nbh) const
-  {
-    nbh.clear();
-    for (std::size_t i = 0 ; i < data_.size() ; ++i)
-      if (NearestNeighbors<_T>::distFun_(data_[i], data) <= radius)
-        nbh.push_back(data_[i]);
-    std::sort(nbh.begin(), nbh.end(), ElemSort(data, NearestNeighbors<_T>::distFun_));
-  }
-
-  virtual std::size_t size(void) const
-  {
-    return data_.size();
-  }
-
-  virtual void list(std::vector<_T> &data) const
-  {
-    data = data_;
-  }
-
-protected:
-
-  /** \brief The data elements stored in this structure */
-  std::vector<_T>   data_;
+  virtual ~ComputeContactPatch() = default;
 
-private:
+ 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;
 
-  struct ElemSort
-  {
-    ElemSort(const _T &e, const typename NearestNeighbors<_T>::DistanceFunction &df) : e_(e), df_(df)
-    {
-    }
+  mutable ContactPatchSolver csolver;
 
-    bool operator()(const _T &a, const _T &b) const
-    {
-      return df_(a, e_) < df_(b, e_);
-    }
+  ContactPatchFunctionMatrix::ContactPatchFunc func;
+  bool swap_geoms;
 
-    const _T                                              &e_;
-    const typename NearestNeighbors<_T>::DistanceFunction &df_;
-  };
+  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/fcl/ccd/interval_vector.h → include/coal/distance.h

@@ -32,136 +32,84 @@
  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  *  POSSIBILITY OF SUCH DAMAGE.
  */
-// This code is based on code developed by Stephane Redon at UNC and Inria for the CATCH library: http://graphics.ewha.ac.kr/CATCH/
-/** \author Jia Pan */
 
-
-#ifndef FCL_CCD_INTERVAL_VECTOR_H
-#define FCL_CCD_INTERVAL_VECTOR_H
-
-#include "fcl/ccd/interval.h"
-#include "fcl/math/vec_3f.h"
-
-namespace fcl
-{
-
-struct IVector3
-{
-  Interval i_[3];
-
-  IVector3();
-  IVector3(FCL_REAL v);
-  IVector3(FCL_REAL x, FCL_REAL y, FCL_REAL z);
-  IVector3(FCL_REAL xl, FCL_REAL xu, FCL_REAL yl, FCL_REAL yu, FCL_REAL zl, FCL_REAL zu);
-  IVector3(Interval v[3]);
-  IVector3(FCL_REAL v[3][2]);
-  IVector3(const Interval& v1, const Interval& v2, const Interval& v3);
-  IVector3(const Vec3f& v);
-
-  inline void setValue(FCL_REAL v)
-  {
-    i_[0].setValue(v);
-    i_[1].setValue(v);
-    i_[2].setValue(v);
-  }
-
-  inline void setValue(FCL_REAL x, FCL_REAL y, FCL_REAL z)
-  {
-    i_[0].setValue(x);
-    i_[1].setValue(y);
-    i_[2].setValue(z);
+/** @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;
   }
 
-  inline void setValue(FCL_REAL xl, FCL_REAL xu, FCL_REAL yl, FCL_REAL yu, FCL_REAL zl, FCL_REAL zu)
-  {
-    i_[0].setValue(xl, xu);
-    i_[1].setValue(yl, yu);
-    i_[2].setValue(zl, zu);
+  bool operator!=(const ComputeDistance& other) const {
+    return !(*this == other);
   }
 
-  inline void setValue(FCL_REAL v[3][2])
-  {
-    i_[0].setValue(v[0][0], v[0][1]);
-    i_[1].setValue(v[1][0], v[1][1]);
-    i_[2].setValue(v[2][0], v[2][1]);
-  }
+  virtual ~ComputeDistance() {};
 
-  inline void setValue(Interval v[3])
-  {
-    i_[0] = v[0];
-    i_[1] = v[1];
-    i_[2] = v[2];
-  }
+ 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;
 
-  inline void setValue(const Interval& v1, const Interval& v2, const Interval& v3)
-  {
-    i_[0] = v1;
-    i_[1] = v2;
-    i_[2] = v3;
-  }
+  mutable GJKSolver solver;
 
-  inline void setValue(const Vec3f& v)
-  {
-    i_[0].setValue(v[0]);
-    i_[1].setValue(v[1]);
-    i_[2].setValue(v[2]);
-  }
-
-  inline void setValue(FCL_REAL v[3])
-  {
-    i_[0].setValue(v[0]);
-    i_[1].setValue(v[1]);
-    i_[2].setValue(v[2]);
-  }
-  
-  IVector3 operator + (const IVector3& other) const;
-  IVector3& operator += (const IVector3& other);
+  DistanceFunctionMatrix::DistanceFunc func;
+  bool swap_geoms;
 
-  IVector3 operator - (const IVector3& other) const;
-  IVector3& operator -= (const IVector3& other);
+  virtual CoalScalar run(const Transform3s& tf1, const Transform3s& tf2,
+                         const DistanceRequest& request,
+                         DistanceResult& result) const;
 
-  Interval dot(const IVector3& other) const;
-  IVector3 cross(const IVector3& other) const;
-
-  Interval dot(const Vec3f& other) const;
-  IVector3 cross(const Vec3f& other) const;
-
-  inline const Interval& operator [] (size_t i) const
-  {
-    return i_[i];
-  }
-
-  inline Interval& operator [] (size_t i)
-  {
-    return i_[i];
-  }
-
-  inline Vec3f getLow() const 
-  {
-    return Vec3f(i_[0][0], i_[1][0], i_[2][0]);
-  }
-  
-  inline Vec3f getHigh() const
-  {
-    return Vec3f(i_[0][1], i_[1][1], i_[2][1]);
-  }
-
-  void print() const;
-  Vec3f center() const;
-  FCL_REAL volumn() const;
-  void setZero();
-
-  void bound(const Vec3f& v);
-  void bound(const IVector3& v);
-
-  bool overlap(const IVector3& v) const;
-  bool contain(const IVector3& v) const;
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
-IVector3 bound(const IVector3& i, const Vec3f& v);
-
-IVector3 bound(const IVector3& i, const IVector3& v);
-
-}
+}  // namespace coal
 
 #endif