include/coal/broadphase/detail/hierarchy_tree_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_HIERARCHY_TREE_ARRAY_H
+#define COAL_HIERARCHY_TREE_ARRAY_H
+
+#include <vector>
+#include <map>
+#include <functional>
+
+#include "coal/fwd.hh"
+#include "coal/BV/AABB.h"
+#include "coal/broadphase/detail/morton.h"
+#include "coal/broadphase/detail/node_base_array.h"
+
+namespace coal {
+
+namespace detail {
+
+namespace implementation_array {
+
+/// @brief Class for hierarchy tree structure
+template <typename BV>
+class HierarchyTree {
+ public:
+  typedef NodeBase<BV> Node;
+
+ private:
+  struct SortByMorton {
+    SortByMorton(Node* nodes_in) : nodes(nodes_in) {}
+    SortByMorton(Node* nodes_in, uint32_t split_in)
+        : nodes(nodes_in), split(split_in) {}
+    bool operator()(size_t a, size_t b) const {
+      if ((a != NULL_NODE) && (b != NULL_NODE))
+        return nodes[a].code < nodes[b].code;
+      else if (a == NULL_NODE)
+        return split < nodes[b].code;
+      else if (b == NULL_NODE)
+        return nodes[a].code < split;
+
+      return false;
+    }
+
+    Node* nodes{};
+    uint32_t split{};
+  };
+
+ public:
+  /// @brief Create hierarchy tree with suitable setting.
+  /// bu_threshold decides the height of tree node to start bottom-up
+  /// construction / optimization; topdown_level decides different methods to
+  /// construct tree in topdown manner. lower level method constructs tree with
+  /// better quality but is slower.
+  HierarchyTree(int bu_threshold_ = 16, int topdown_level_ = 0);
+
+  ~HierarchyTree();
+
+  /// @brief Initialize the tree by a set of leaves using algorithm with a given
+  /// level.
+  void init(Node* leaves, int n_leaves_, int level = 0);
+
+  /// @brief Initialize the tree by a set of leaves using algorithm with a given
+  /// level.
+  size_t insert(const BV& bv, void* data);
+
+  /// @brief Remove a leaf node
+  void remove(size_t leaf);
+
+  /// @brief Clear the tree
+  void clear();
+
+  /// @brief Whether the tree is empty
+  bool empty() const;
+
+  /// @brief update one leaf node
+  void update(size_t leaf, int lookahead_level = -1);
+
+  /// @brief update the tree when the bounding volume of a given leaf has
+  /// changed
+  bool update(size_t leaf, const BV& bv);
+
+  /// @brief update one leaf's bounding volume, with prediction
+  bool update(size_t leaf, const BV& bv, const Vec3s& vel, CoalScalar margin);
+
+  /// @brief update one leaf's bounding volume, with prediction
+  bool update(size_t leaf, const BV& bv, const Vec3s& vel);
+
+  /// @brief get the max height of the tree
+  size_t getMaxHeight() const;
+
+  /// @brief get the max depth of the tree
+  size_t getMaxDepth() const;
+
+  /// @brief balance the tree from bottom
+  void balanceBottomup();
+
+  /// @brief balance the tree from top
+  void balanceTopdown();
+
+  /// @brief balance the tree in an incremental way
+  void balanceIncremental(int iterations);
+
+  /// @brief refit the tree, i.e., when the leaf nodes' bounding volumes change,
+  /// update the entire tree in a bottom-up manner
+  void refit();
+
+  /// @brief extract all the leaves of the tree
+  void extractLeaves(size_t root, Node*& leaves) const;
+
+  /// @brief number of leaves in the tree
+  size_t size() const;
+
+  /// @brief get the root of the tree
+  size_t getRoot() const;
+
+  /// @brief get the pointer to the nodes array
+  Node* getNodes() const;
+
+  /// @brief print the tree in a recursive way
+  void print(size_t root, int depth);
+
+ private:
+  /// @brief construct a tree for a set of leaves from bottom -- very heavy way
+  void bottomup(size_t* lbeg, size_t* lend);
+
+  /// @brief construct a tree for a set of leaves from top
+  size_t topdown(size_t* lbeg, size_t* lend);
+
+  /// @brief compute the maximum height of a subtree rooted from a given node
+  size_t getMaxHeight(size_t node) const;
+
+  /// @brief compute the maximum depth of a subtree rooted from a given node
+  void getMaxDepth(size_t node, size_t depth, size_t& max_depth) const;
+
+  /// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a
+  /// topdown manner. During construction, first compute the best split axis as
+  /// the axis along with the longest AABB edge. Then compute the median of all
+  /// nodes' center projection onto the axis and using it as the split
+  /// threshold.
+  size_t topdown_0(size_t* lbeg, size_t* lend);
+
+  /// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a
+  /// topdown manner. During construction, first compute the best split
+  /// thresholds for different axes as the average of all nodes' center. Then
+  /// choose the split axis as the axis whose threshold can divide the nodes
+  /// into two parts with almost similar size. This construction is more
+  /// expensive then topdown_0, but also can provide tree with better quality.
+  size_t topdown_1(size_t* lbeg, size_t* lend);
+
+  /// @brief init tree from leaves in the topdown manner (topdown_0 or
+  /// topdown_1)
+  void init_0(Node* leaves, int n_leaves_);
+
+  /// @brief init tree from leaves using morton code. It uses morton_0, i.e.,
+  /// for nodes which is of depth more than the maximum bits of the morton code,
+  /// we use bottomup method to construct the subtree, which is slow but can
+  /// construct tree with high quality.
+  void init_1(Node* leaves, int n_leaves_);
+
+  /// @brief init tree from leaves using morton code. It uses morton_0, i.e.,
+  /// for nodes which is of depth more than the maximum bits of the morton code,
+  /// we split the leaves into two parts with the same size simply using the
+  /// node index.
+  void init_2(Node* leaves, int n_leaves_);
+
+  /// @brief init tree from leaves using morton code. It uses morton_2, i.e.,
+  /// for all nodes, we simply divide the leaves into parts with the same size
+  /// simply using the node index.
+  void init_3(Node* leaves, int n_leaves_);
+
+  size_t mortonRecurse_0(size_t* lbeg, size_t* lend, const uint32_t& split,
+                         int bits);
+
+  size_t mortonRecurse_1(size_t* lbeg, size_t* lend, const uint32_t& split,
+                         int bits);
+
+  size_t mortonRecurse_2(size_t* lbeg, size_t* lend);
+
+  /// @brief update one leaf node's bounding volume
+  void update_(size_t leaf, const BV& bv);
+
+  /// @brief Insert a leaf node and also update its ancestors
+  void insertLeaf(size_t root, size_t leaf);
+
+  /// @brief Remove a leaf. The leaf node itself is not deleted yet, but all the
+  /// unnecessary internal nodes are deleted. return the node with the smallest
+  /// depth and is influenced by the remove operation
+  size_t removeLeaf(size_t leaf);
+
+  /// @brief Delete all internal nodes and return all leaves nodes with given
+  /// depth from root
+  void fetchLeaves(size_t root, Node*& leaves, int depth = -1);
+
+  size_t indexOf(size_t node);
+
+  size_t allocateNode();
+
+  /// @brief create one node (leaf or internal)
+  size_t createNode(size_t parent, const BV& bv1, const BV& bv2, void* data);
+
+  size_t createNode(size_t parent, const BV& bv, void* data);
+
+  size_t createNode(size_t parent, void* data);
+
+  void deleteNode(size_t node);
+
+  void recurseRefit(size_t node);
+
+ protected:
+  size_t root_node;
+  Node* nodes;
+  size_t n_nodes;
+  size_t n_nodes_alloc;
+
+  size_t n_leaves;
+  size_t freelist;
+  unsigned int opath;
+
+  int max_lookahead_level;
+
+ public:
+  /// @brief decide which topdown algorithm to use
+  int topdown_level;
+
+  /// @brief decide the depth to use expensive bottom-up algorithm
+  int bu_threshold;
+
+ public:
+  static const size_t NULL_NODE = std::numeric_limits<size_t>::max();
+};
+
+template <typename BV>
+const size_t HierarchyTree<BV>::NULL_NODE;
+
+/// @brief Functor comparing two nodes
+template <typename BV>
+struct nodeBaseLess {
+  nodeBaseLess(const NodeBase<BV>* nodes_, size_t d_);
+
+  bool operator()(size_t i, size_t j) const;
+
+ private:
+  /// @brief the nodes array
+  const NodeBase<BV>* nodes;
+
+  /// @brief the dimension to compare
+  size_t d;
+};
+
+/// @brief select the node from node1 and node2 which is close to the query-th
+/// node in the nodes. 0 for node1 and 1 for node2.
+template <typename BV>
+size_t select(size_t query, size_t node1, size_t node2, NodeBase<BV>* nodes);
+
+/// @brief select the node from node1 and node2 which is close to the query
+/// AABB. 0 for node1 and 1 for node2.
+template <typename BV>
+size_t select(const BV& query, size_t node1, size_t node2, NodeBase<BV>* nodes);
+
+}  // namespace implementation_array
+}  // namespace detail
+}  // namespace coal
+
+#include "coal/broadphase/detail/hierarchy_tree_array-inl.h"
+
+#endif

include/coal/broadphase/detail/interval_tree.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_INTERVAL_TREE_H
+#define COAL_INTERVAL_TREE_H
+
+#include <deque>
+#include <limits>
+#include <cstdlib>
+
+#include "coal/broadphase/detail/interval_tree_node.h"
+
+namespace coal {
+namespace detail {
+
+/// @brief Class describes the information needed when we take the
+/// right branch in searching for intervals but possibly come back
+/// and check the left branch as well.
+struct COAL_DLLAPI it_recursion_node {
+ public:
+  IntervalTreeNode* start_node;
+
+  unsigned int parent_index;
+
+  bool try_right_branch;
+};
+
+/// @brief Interval tree
+class COAL_DLLAPI IntervalTree {
+ public:
+  IntervalTree();
+
+  ~IntervalTree();
+
+  /// @brief Print the whole interval tree
+  void print() const;
+
+  /// @brief Delete one node of the interval tree
+  SimpleInterval* deleteNode(IntervalTreeNode* node);
+
+  /// @brief delete node stored a given interval
+  void deleteNode(SimpleInterval* ivl);
+
+  /// @brief Insert one node of the interval tree
+  IntervalTreeNode* insert(SimpleInterval* new_interval);
+
+  /// @brief get the predecessor of a given node
+  IntervalTreeNode* getPredecessor(IntervalTreeNode* node) const;
+
+  /// @brief Get the successor of a given node
+  IntervalTreeNode* getSuccessor(IntervalTreeNode* node) const;
+
+  /// @brief Return result for a given query
+  std::deque<SimpleInterval*> query(CoalScalar low, CoalScalar high);
+
+ protected:
+  IntervalTreeNode* root;
+
+  IntervalTreeNode* invalid_node;
+
+  /// @brief left rotation of tree node
+  void leftRotate(IntervalTreeNode* node);
+
+  /// @brief right rotation of tree node
+  void rightRotate(IntervalTreeNode* node);
+
+  /// @brief Inserts node into the tree as if it were a regular binary tree
+  void recursiveInsert(IntervalTreeNode* node);
+
+  /// @brief recursively print a subtree
+  void recursivePrint(IntervalTreeNode* node) const;
+
+  /// @brief recursively find the node corresponding to the interval
+  IntervalTreeNode* recursiveSearch(IntervalTreeNode* node,
+                                    SimpleInterval* ivl) const;
+
+  /// @brief Travels up to the root fixing the max_high fields after an
+  /// insertion or deletion
+  void fixupMaxHigh(IntervalTreeNode* node);
+
+  void deleteFixup(IntervalTreeNode* node);
+
+ private:
+  unsigned int recursion_node_stack_size;
+  it_recursion_node* recursion_node_stack;
+  unsigned int current_parent;
+  unsigned int recursion_node_stack_top;
+};
+
+}  // namespace detail
+}  // namespace coal
+
+#endif

include/coal/broadphase/detail/interval_tree_node-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_INTERVALTREENODE_INL_H
+#define COAL_BROADPHASE_DETAIL_INTERVALTREENODE_INL_H
+
+#include "coal/broadphase/detail/interval_tree_node.h"
+
+#include <iostream>
+#include <algorithm>
+
+namespace coal {
+namespace detail {
+
+//==============================================================================
+IntervalTreeNode::IntervalTreeNode() {
+  // Do nothing
+}
+
+//==============================================================================
+IntervalTreeNode::IntervalTreeNode(SimpleInterval* new_interval)
+    : stored_interval(new_interval),
+      key(new_interval->low),
+      high(new_interval->high),
+      max_high(high) {
+  // Do nothing
+}
+
+//==============================================================================
+IntervalTreeNode::~IntervalTreeNode() {
+  // Do nothing
+}
+
+//==============================================================================
+void IntervalTreeNode::print(IntervalTreeNode* nil,
+                             IntervalTreeNode* root) const {
+  stored_interval->print();
+  std::cout << ", k = " << key << ", h = " << high << ", mH = " << max_high;
+  std::cout << "  l->key = ";
+  if (left == nil)
+    std::cout << "nullptr";
+  else
+    std::cout << left->key;
+  std::cout << "  r->key = ";
+  if (right == nil)
+    std::cout << "nullptr";
+  else
+    std::cout << right->key;
+  std::cout << "  p->key = ";
+  if (parent == root)
+    std::cout << "nullptr";
+  else
+    std::cout << parent->key;
+  std::cout << "  red = " << (int)red << std::endl;
+}
+
+}  // namespace detail
+}  // namespace coal
+
+#endif

include/coal/broadphase/detail/interval_tree_node.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_INTERVALTREENODE_H
+#define COAL_BROADPHASE_DETAIL_INTERVALTREENODE_H
+
+#include "coal/broadphase/detail/simple_interval.h"
+#include "coal/fwd.hh"
+
+namespace coal {
+
+namespace detail {
+
+class COAL_DLLAPI IntervalTree;
+
+/// @brief The node for interval tree
+class COAL_DLLAPI IntervalTreeNode {
+ public:
+  friend class IntervalTree;
+
+  /// @brief Create an empty node
+  IntervalTreeNode();
+
+  /// @brief Create an node storing the interval
+  IntervalTreeNode(SimpleInterval* new_interval);
+
+  ~IntervalTreeNode();
+
+  /// @brief Print the interval node information: set left = invalid_node and
+  /// right = root
+  void print(IntervalTreeNode* left, IntervalTreeNode* right) const;
+
+ protected:
+  /// @brief interval stored in the node
+  SimpleInterval* stored_interval;
+
+  CoalScalar key;
+
+  CoalScalar high;
+
+  CoalScalar max_high;
+
+  /// @brief red or black node: if red = false then the node is black
+  bool red;
+
+  IntervalTreeNode* left;
+
+  IntervalTreeNode* right;
+
+  IntervalTreeNode* parent;
+};
+
+}  // namespace detail
+}  // namespace coal
+
+#endif

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

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  Copyright (c) 2016, Toyota Research Institute
+ *  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_MORTON_INL_H
+#define COAL_MORTON_INL_H
+
+#include "coal/broadphase/detail/morton.h"
+
+namespace coal {  /// @cond IGNORE
+namespace detail {
+
+//==============================================================================
+template <typename S>
+uint32_t quantize(S x, uint32_t n) {
+  return std::max(std::min((uint32_t)(x * (S)n), uint32_t(n - 1)), uint32_t(0));
+}
+
+//==============================================================================
+template <typename S>
+morton_functor<S, uint32_t>::morton_functor(const AABB& bbox)
+    : base(bbox.min_),
+      inv(1.0 / (bbox.max_[0] - bbox.min_[0]),
+          1.0 / (bbox.max_[1] - bbox.min_[1]),
+          1.0 / (bbox.max_[2] - bbox.min_[2])) {
+  // Do nothing
+}
+
+//==============================================================================
+template <typename S>
+uint32_t morton_functor<S, uint32_t>::operator()(const Vec3s& point) const {
+  uint32_t x = detail::quantize((point[0] - base[0]) * inv[0], 1024u);
+  uint32_t y = detail::quantize((point[1] - base[1]) * inv[1], 1024u);
+  uint32_t z = detail::quantize((point[2] - base[2]) * inv[2], 1024u);
+
+  return detail::morton_code(x, y, z);
+}
+
+//==============================================================================
+template <typename S>
+morton_functor<S, uint64_t>::morton_functor(const AABB& bbox)
+    : base(bbox.min_),
+      inv(1.0 / (bbox.max_[0] - bbox.min_[0]),
+          1.0 / (bbox.max_[1] - bbox.min_[1]),
+          1.0 / (bbox.max_[2] - bbox.min_[2])) {
+  // Do nothing
+}
+
+//==============================================================================
+template <typename S>
+uint64_t morton_functor<S, uint64_t>::operator()(const Vec3s& point) const {
+  uint32_t x = detail::quantize((point[0] - base[0]) * inv[0], 1u << 20);
+  uint32_t y = detail::quantize((point[1] - base[1]) * inv[1], 1u << 20);
+  uint32_t z = detail::quantize((point[2] - base[2]) * inv[2], 1u << 20);
+
+  return detail::morton_code60(x, y, z);
+}
+
+//==============================================================================
+template <typename S>
+constexpr size_t morton_functor<S, uint64_t>::bits() {
+  return 60;
+}
+
+//==============================================================================
+template <typename S>
+constexpr size_t morton_functor<S, uint32_t>::bits() {
+  return 30;
+}
+
+//==============================================================================
+template <typename S, size_t N>
+morton_functor<S, std::bitset<N>>::morton_functor(const AABB& bbox)
+    : base(bbox.min_),
+      inv(1.0 / (bbox.max_[0] - bbox.min_[0]),
+          1.0 / (bbox.max_[1] - bbox.min_[1]),
+          1.0 / (bbox.max_[2] - bbox.min_[2])) {
+  // Do nothing
+}
+
+//==============================================================================
+template <typename S, size_t N>
+std::bitset<N> morton_functor<S, std::bitset<N>>::operator()(
+    const Vec3s& point) const {
+  S x = (point[0] - base[0]) * inv[0];
+  S y = (point[1] - base[1]) * inv[1];
+  S z = (point[2] - base[2]) * inv[2];
+  int start_bit = bits() - 1;
+  std::bitset<N> bset;
+
+  x *= 2;
+  y *= 2;
+  z *= 2;
+
+  for (size_t i = 0; i < bits() / 3; ++i) {
+    bset[start_bit--] = ((z < 1) ? 0 : 1);
+    bset[start_bit--] = ((y < 1) ? 0 : 1);
+    bset[start_bit--] = ((x < 1) ? 0 : 1);
+    x = ((x >= 1) ? 2 * (x - 1) : 2 * x);
+    y = ((y >= 1) ? 2 * (y - 1) : 2 * y);
+    z = ((z >= 1) ? 2 * (z - 1) : 2 * z);
+  }
+
+  return bset;
+}
+
+//==============================================================================
+template <typename S, size_t N>
+constexpr size_t morton_functor<S, std::bitset<N>>::bits() {
+  return N;
+}
+
+}  // namespace detail
+/// @endcond
+}  // namespace coal
+
+#endif

include/coal/broadphase/detail/morton.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2016, Open Source Robotics Foundation
+ *  Copyright (c) 2016, Toyota Research Institute
+ *  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_MORTON_H
+#define COAL_MORTON_H
+
+#include "coal/BV/AABB.h"
+#include <cstdint>
+#include <bitset>
+
+namespace coal {
+
+/// @cond IGNORE
+namespace detail {
+
+template <typename S>
+uint32_t quantize(S x, uint32_t n);
+
+/// @brief compute 30 bit morton code
+COAL_DLLAPI
+uint32_t morton_code(uint32_t x, uint32_t y, uint32_t z);
+
+/// @brief compute 60 bit morton code
+COAL_DLLAPI
+uint64_t morton_code60(uint32_t x, uint32_t y, uint32_t z);
+
+/// @brief Functor to compute the morton code for a given AABB
+/// This is specialized for 32- and 64-bit unsigned integers giving
+/// a 30- or 60-bit code, respectively, and for `std::bitset<N>` where
+/// N is the length of the code and must be a multiple of 3.
+template <typename S, typename T>
+struct morton_functor {};
+
+/// @brief Functor to compute 30 bit morton code for a given AABB
+template <typename S>
+struct morton_functor<S, uint32_t> {
+  morton_functor(const AABB& bbox);
+
+  uint32_t operator()(const Vec3s& point) const;
+
+  const Vec3s base;
+  const Vec3s inv;
+
+  static constexpr size_t bits();
+};
+
+using morton_functoru32f = morton_functor<float, uint32_t>;
+using morton_functoru32d = morton_functor<CoalScalar, uint32_t>;
+
+/// @brief Functor to compute 60 bit morton code for a given AABB
+template <typename S>
+struct morton_functor<S, uint64_t> {
+  morton_functor(const AABB& bbox);
+
+  uint64_t operator()(const Vec3s& point) const;
+
+  const Vec3s base;
+  const Vec3s inv;
+
+  static constexpr size_t bits();
+};
+
+/// @brief Functor to compute N bit morton code for a given AABB
+/// N must be a multiple of 3.
+template <typename S, size_t N>
+struct morton_functor<S, std::bitset<N>> {
+  static_assert(N % 3 == 0, "Number of bits must be a multiple of 3");
+
+  morton_functor(const AABB& bbox);
+
+  std::bitset<N> operator()(const Vec3s& point) const;
+
+  const Vec3s base;
+  const Vec3s inv;
+
+  static constexpr size_t bits();
+};
+
+}  // namespace detail
+/// @endcond
+}  // namespace coal
+
+#include "coal/broadphase/detail/morton-inl.h"
+
+#endif

include/coal/broadphase/detail/node_base-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_NODEBASE_INL_H
+#define COAL_BROADPHASE_DETAIL_NODEBASE_INL_H
+
+#include "coal/broadphase/detail/node_base.h"
+
+namespace coal {
+namespace detail {
+
+//============================================================================//
+//                                                                            //
+//                              Implementations                               //
+//                                                                            //
+//============================================================================//
+
+//==============================================================================
+template <typename BV>
+bool NodeBase<BV>::isLeaf() const {
+  return (children[1] == nullptr);
+}
+
+//==============================================================================
+template <typename BV>
+bool NodeBase<BV>::isInternal() const {
+  return !isLeaf();
+}
+
+//==============================================================================
+template <typename BV>
+NodeBase<BV>::NodeBase() {
+  parent = nullptr;
+  children[0] = nullptr;
+  children[1] = nullptr;
+}
+
+}  // namespace detail
+}  // namespace coal
+
+#endif

include/coal/broadphase/detail/node_base.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_NODEBASE_H
+#define COAL_BROADPHASE_DETAIL_NODEBASE_H
+
+#include "coal/data_types.h"
+
+namespace coal {
+
+namespace detail {
+
+/// @brief dynamic AABB tree node
+template <typename BV>
+struct NodeBase {
+  /// @brief the bounding volume for the node
+  BV bv;
+
+  /// @brief pointer to parent node
+  NodeBase<BV>* parent;
+
+  /// @brief whether is a leaf
+  bool isLeaf() const;
+
+  /// @brief whether is internal node
+  bool isInternal() const;
+
+  union {
+    /// @brief for leaf node, children nodes
+    NodeBase<BV>* children[2];
+    void* data;
+  };
+
+  /// @brief morton code for current BV
+  uint32_t code;
+
+  NodeBase();
+};
+
+}  // namespace detail
+}  // namespace coal
+
+#include "coal/broadphase/detail/node_base-inl.h"
+
+#endif

include/coal/broadphase/detail/node_base_array-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_NODEBASEARRAY_INL_H
+#define COAL_BROADPHASE_DETAIL_NODEBASEARRAY_INL_H
+
+#include "coal/broadphase/detail/node_base_array.h"
+
+namespace coal {
+
+namespace detail {
+
+namespace implementation_array {
+
+//==============================================================================
+template <typename BV>
+bool NodeBase<BV>::isLeaf() const {
+  return (children[1] == (size_t)(-1));
+}
+
+//==============================================================================
+template <typename BV>
+bool NodeBase<BV>::isInternal() const {
+  return !isLeaf();
+}
+
+}  // namespace implementation_array
+}  // namespace detail
+}  // namespace coal
+
+#endif

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/coal/broadphase/detail/simple_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_SIMPLEHASHTABLE_INL_H
+#define COAL_BROADPHASE_SIMPLEHASHTABLE_INL_H
+
+#include "coal/broadphase/detail/simple_hash_table.h"
+
+#include <iterator>
+namespace coal {
+
+namespace detail {
+
+//==============================================================================
+template <typename Key, typename Data, typename HashFnc>
+SimpleHashTable<Key, Data, HashFnc>::SimpleHashTable(const HashFnc& h) : h_(h) {
+  // Do nothing
+}
+
+//==============================================================================
+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);
+  }
+
+  table_.resize(size);
+  table_size_ = size;
+}
+
+//==============================================================================
+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);
+}
+
+//==============================================================================
+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()));
+  }
+
+  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/coal/broadphase/detail/simple_hash_table.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_SIMPLEHASHTABLE_H
+#define COAL_BROADPHASE_SIMPLEHASHTABLE_H
+
+#include <set>
+#include <vector>
+#include <list>
+
+namespace coal {
+
+namespace detail {
+
+/// @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;
+
+  std::vector<Bin> table_;
+
+  HashFnc h_;
+
+  size_t table_size_;
+
+ public:
+  SimpleHashTable(const HashFnc& h);
+
+  /// @brief Init the number of bins in the hash table
+  void init(size_t size);
+
+  //// @brief Insert a key-value pair into the table
+  void insert(Key key, Data value);
+
+  /// @brief Find the elements in the hash table whose key is the same as query
+  /// key.
+  std::vector<Data> query(Key key) const;
+
+  /// @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/coal/broadphase/detail/sparse_hash_table.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_H
+#define COAL_BROADPHASE_SPARSEHASHTABLE_H
+
+#include <set>
+#include <vector>
+#include <list>
+#include <unordered_map>
+
+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/coal/broadphase/detail/spatial_hash-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_SPATIALHASH_INL_H
+#define COAL_BROADPHASE_SPATIALHASH_INL_H
+
+#include "coal/broadphase/detail/spatial_hash.h"
+#include <algorithm>
+
+namespace coal {
+namespace detail {
+
+//==============================================================================
+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);
+}
+
+//==============================================================================
+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);
+
+  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];
+      }
+    }
+  }
+  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/coal/collision.h

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

include/coal/collision_data.h

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