doc/mesh-mesh-collision-call.dot

+digraph CD  {
+
+	rankdir = BT
+	compound=true
+        size = 11.7
+
+        "std::size_t BVHCollide(const CollisionGeometry* o1,\nconst Transform3s& tf1, const CollisionGeometry* o2,\nconst Transform3s& tf2, const CollisionRequest& request,\nCollisionResult& result)" [shape = box]
+        "bool OBB::overlap(const OBB& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" [shape = box]
+        "bool OBBRSS::overlap(const OBBRSS& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" [shape = box]
+        "bool overlap(const Matrix3s& R0, const Vec3s& T0,\n const OBB& b1, const OBB& b2,\n const CollisionRequest& request, CoalScalar& sqrDistLowerBound)" [shape = box]
+        "bool overlap(const Matrix3s& R0, const Vec3s& T0,\n const OBBRSS& b1, const OBBRSS& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)" [shape = box]
+        "bool BVNode::overlap(const BVNode& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" [shape = box]
+        "bool BVHCollisionTraversalNode::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n  -request\n  - result" [shape = box]
+        "bool MeshCollisionTraversalNode<T_BVH>::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n  -request\n  - result" [shape = box]
+        "void collide(MeshCollisionTraversalNode<T_BVH>* node,\n const CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)" [shape = box]
+        "void collisionRecurse(MeshCollisionTraversalNode<T_BVH>* node,\n int b1, int b2, BVHFrontList* front_list, CoalScalar& sqrDistLowerBound)" [shape = box]
+        "void propagateBVHFrontListCollisionRecurse(MeshCollisionTraversalNode<T_BVH>* node\n, BVHFrontList* front_list, const CollisionRequest& request, CollisionResult& result)" [shape = box]
+        "bool MeshCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n  -request\n  - result" [shape = box]
+        "MeshCollisionTraversalNode<T_BVH>::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n  -request\n  - result" [shape = box]
+        "bool obbDisjointAndLowerBoundDistance\n(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b,\n CoalScalar& squaredLowerBoundDistance)" [shape = box]
+
+         "std::size_t BVHCollide(const CollisionGeometry* o1,\nconst Transform3s& tf1, const CollisionGeometry* o2,\nconst Transform3s& tf2, const CollisionRequest& request,\nCollisionResult& result)" -> "void collide(MeshCollisionTraversalNode<T_BVH>* node,\n const CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)"
+        "bool OBB::overlap(const OBB& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" -> "bool obbDisjointAndLowerBoundDistance\n(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b,\n CoalScalar& squaredLowerBoundDistance)"
+        "bool overlap(const Matrix3s& R0, const Vec3s& T0,\n const OBB& b1, const OBB& b2,\n const CollisionRequest& request, CoalScalar& sqrDistLowerBound)" -> "bool obbDisjointAndLowerBoundDistance\n(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b,\n CoalScalar& squaredLowerBoundDistance)"
+        "bool overlap(const Matrix3s& R0, const Vec3s& T0,\n const OBBRSS& b1, const OBBRSS& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)" -> "bool overlap(const Matrix3s& R0, const Vec3s& T0,\n const OBB& b1, const OBB& b2,\n const CollisionRequest& request, CoalScalar& sqrDistLowerBound)"
+        "void collide(MeshCollisionTraversalNode<T_BVH>* node,\n const CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)"-> "void propagateBVHFrontListCollisionRecurse(MeshCollisionTraversalNode<T_BVH>* node\n, BVHFrontList* front_list, const CollisionRequest& request, CollisionResult& result)"
+        "void collide(MeshCollisionTraversalNode<T_BVH>* node,\n const CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)" -> "void collisionRecurse(MeshCollisionTraversalNode<T_BVH>* node,\n int b1, int b2, BVHFrontList* front_list, CoalScalar& sqrDistLowerBound)"
+        "void collisionRecurse(MeshCollisionTraversalNode<T_BVH>* node,\n int b1, int b2, BVHFrontList* front_list, CoalScalar& sqrDistLowerBound)" -> "bool MeshCollisionTraversalNode<T_BVH>::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n  -request\n  - result"
+        "void collisionRecurse(MeshCollisionTraversalNode<T_BVH>* node,\n int b1, int b2, BVHFrontList* front_list, CoalScalar& sqrDistLowerBound)" -> "MeshCollisionTraversalNode<T_BVH>::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n  -request\n  - result"
+        "void propagateBVHFrontListCollisionRecurse(MeshCollisionTraversalNode<T_BVH>* node\n, BVHFrontList* front_list, const CollisionRequest& request, CollisionResult& result)" -> "void collisionRecurse(MeshCollisionTraversalNode<T_BVH>* node,\n int b1, int b2, BVHFrontList* front_list, CoalScalar& sqrDistLowerBound)"
+        "void propagateBVHFrontListCollisionRecurse(MeshCollisionTraversalNode<T_BVH>* node\n, BVHFrontList* front_list, const CollisionRequest& request, CollisionResult& result)" -> "bool MeshCollisionTraversalNode<T_BVH>::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n  -request\n  - result"
+"bool MeshCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n  -request\n  - result" -> "bool overlap(const Matrix3s& R0, const Vec3s& T0,\n const OBBRSS& b1, const OBBRSS& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)"
+      "bool MeshCollisionTraversalNode<T_BVH>::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n  -request\n  - result" -> "bool MeshCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n  -request\n  - result" [color=red]
+      "bool MeshCollisionTraversalNode<T_BVH>::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n  -request\n  - result" -> "bool BVHCollisionTraversalNode::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n  -request\n  - result" [color = red]
+      "bool OBBRSS::overlap(const OBBRSS& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" -> "bool OBB::overlap(const OBB& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const"
+      "bool BVNode::overlap(const BVNode& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" -> "bool OBB::overlap(const OBB& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const"
+      "bool BVHCollisionTraversalNode::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n  -request\n  - result" -> "bool BVNode::overlap(const BVNode& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const"
+}
\ No newline at end of file

doc/notes

+In include/hpp/fcl/traversal:
+
+  - some initialize method compute the position of all triangles in
+    global frame before performing collision test,
+
+    - bool initialize(MeshShapeCollisionTraversalNode<BV, S, NarrowPhaseSolver>& node, ...
+    - bool initialize(ShapeMeshCollisionTraversalNode<S, BV, NarrowPhaseSolver>& node, ...
+    - bool initialize(MeshCollisionTraversalNode<BV>& node,...
+
+  - other do not.
+
+    - bool initialize(MeshOcTreeCollisionTraversalNode<BV, NarrowPhaseSolver>& node, ...
+    - bool initialize(OcTreeMeshCollisionTraversalNode<BV, NarrowPhaseSolver>& node, ...
+    - bool initialize(MeshShapeCollisionTraversalNodeOBB<S, NarrowPhaseSolver>& node, ...
+    - bool initialize(MeshShapeCollisionTraversalNodeRSS<S, NarrowPhaseSolver>& node, ...
+    - bool initialize(MeshShapeCollisionTraversalNodekIOS<S, NarrowPhaseSolver>& node, ...
+    - bool initialize(MeshShapeCollisionTraversalNodeOBBRSS<S, NarrowPhaseSolver>& node, ...
+    - bool initialize(ShapeMeshCollisionTraversalNodeOBB<S, NarrowPhaseSolver>& node, ...
+    - bool initialize(ShapeMeshCollisionTraversalNodeRSS<S, NarrowPhaseSolver>& node, ...
+    - bool initialize(ShapeMeshCollisionTraversalNodekIOS<S, NarrowPhaseSolver>& node, ...
+    - bool initialize(ShapeMeshCollisionTraversalNodeOBBRSS<S, NarrowPhaseSolver>& node, ...
+    - bool initialize(MeshCollisionTraversalNodeOBB& node, ...
+    - bool initialize(MeshCollisionTraversalNodeRSS& node, ...
+    - bool initialize(MeshCollisionTraversalNodekIOS& node, ...
+    - bool initialize(MeshCollisionTraversalNodeOBBRSS& node, ...
+
+-------------------------------------------------------------------------------
+
+classes
+  - MeshCollisionTraversalNodeRSS,
+  - MeshCollisionTraversalNodekIOS,
+  - MeshCollisionTraversalNodeOBBRSS
+derive from
+  - MeshCollisionTraversalNode <RSS>,
+  - MeshCollisionTraversalNode <kIOS>,
+  - MeshCollisionTraversalNode <OBBRSS>.
+They store the relative position and orientation between two objects to test for
+collision. before calling overlap function, this additional information computes
+the relative positions of the bounding volumes.
+
+-------------------------------------------------------------------------------
+
+For primitive shapes, collision and distance computation are very close.
+ShapeShapeCollide calls indeed ShapeShapeDistance. It would be convenient to
+merge CollisionRequest and DistanceRequest on the one hand and CollisionResult
+and DistanceResult on the other hand into two classes InteractionRequest and
+InteractionResult.
\ No newline at end of file

doc/python/doxygen-boost.hh

+#ifndef DOXYGEN_BOOST_DOC_HH
+#define DOXYGEN_BOOST_DOC_HH
+
+#ifndef DOXYGEN_DOC_HH
+#error "You should have included doxygen.hh first."
+#endif  // DOXYGEN_DOC_HH
+
+#include <boost/python.hpp>
+
+namespace doxygen {
+
+namespace visitor {
+
+template <typename function_type,
+          typename policy_type = boost::python::default_call_policies>
+struct member_func_impl : boost::python::def_visitor<
+                              member_func_impl<function_type, policy_type> > {
+  member_func_impl(const char* n, const function_type& f)
+      : name(n), function(f) {}
+
+  member_func_impl(const char* n, const function_type& f, policy_type p)
+      : name(n), function(f), policy(p) {}
+
+  template <class classT>
+  inline void visit(classT& c) const {
+    // Either a boost::python::keyword<N> object or a void_ object
+    call(c, member_func_args(function));
+  }
+
+  template <class classT, std::size_t nkeywords>
+  inline void call(
+      classT& c, const boost::python::detail::keywords<nkeywords>& args) const {
+    c.def(name, function, member_func_doc(function), args, policy);
+  }
+
+  template <class classT>
+  inline void call(classT& c, const void_&) const {
+    c.def(name, function, member_func_doc(function), policy);
+  }
+
+  const char* name;
+  const function_type& function;
+  policy_type policy;
+};
+
+// TODO surprisingly, this does not work when defined here but it works when
+// defined after the generated files are included.
+template <typename function_type>
+inline member_func_impl<function_type> member_func(
+    const char* name, const function_type& function) {
+  return member_func_impl<function_type>(name, function);
+}
+
+template <typename function_type, typename policy_type>
+inline member_func_impl<function_type, policy_type> member_func(
+    const char* name, const function_type& function,
+    const policy_type& policy) {
+  return member_func_impl<function_type, policy_type>(name, function, policy);
+}
+
+#define DOXYGEN_DOC_DECLARE_INIT_VISITOR(z, nargs, unused)                    \
+  template <typename Class BOOST_PP_COMMA_IF(nargs)                           \
+                BOOST_PP_ENUM_PARAMS(nargs, class Arg)>                       \
+  struct init_##nargs##_impl                                                  \
+      : boost::python::def_visitor<                                           \
+            init_##nargs##_impl<Class BOOST_PP_COMMA_IF(nargs)                \
+                                    BOOST_PP_ENUM_PARAMS(nargs, Arg)> > {     \
+    typedef constructor_##nargs##_impl<Class BOOST_PP_COMMA_IF(nargs)         \
+                                           BOOST_PP_ENUM_PARAMS(nargs, Arg)>  \
+        constructor;                                                          \
+    typedef boost::python::init<BOOST_PP_ENUM_PARAMS(nargs, Arg)> init_base;  \
+                                                                              \
+    template <class classT>                                                   \
+    inline void visit(classT& c) const {                                      \
+      call(c, constructor::args());                                           \
+    }                                                                         \
+                                                                              \
+    template <class classT>                                                   \
+    void call(classT& c,                                                      \
+              const boost::python::detail::keywords<nargs + 1>& args) const { \
+      c.def(init_base(constructor::doc(), args));                             \
+    }                                                                         \
+                                                                              \
+    template <class classT>                                                   \
+    void call(classT& c, const void_&) const {                                \
+      c.def(init_base(constructor::doc()));                                   \
+    }                                                                         \
+  };                                                                          \
+                                                                              \
+  template <typename Class BOOST_PP_COMMA_IF(nargs)                           \
+                BOOST_PP_ENUM_PARAMS(nargs, class Arg)>                       \
+  inline init_##nargs##_impl<Class BOOST_PP_COMMA_IF(nargs)                   \
+                                 BOOST_PP_ENUM_PARAMS(nargs, Arg)>            \
+  init() {                                                                    \
+    return init_##nargs##_impl<Class BOOST_PP_COMMA_IF(nargs)                 \
+                                   BOOST_PP_ENUM_PARAMS(nargs, Arg)>();       \
+  }
+
+BOOST_PP_REPEAT(DOXYGEN_DOC_MAX_NUMBER_OF_ARGUMENTS_IN_CONSTRUCTOR,
+                DOXYGEN_DOC_DECLARE_INIT_VISITOR, ~)
+#undef DOXYGEN_DOC_DECLARE_INIT_VISITOR
+
+}  // namespace visitor
+
+template <typename Func>
+void def(const char* name, Func func) {
+  boost::python::def(name, func, member_func_doc(func));
+}
+
+}  // namespace doxygen
+
+#endif  // DOXYGEN_BOOST_DOC_HH

doc/python/doxygen.hh

+#ifndef DOXYGEN_DOC_HH
+#define DOXYGEN_DOC_HH
+
+#include <boost/preprocessor/repetition.hpp>
+#include <boost/preprocessor/punctuation/comma_if.hpp>
+
+#include <boost/mpl/void.hpp>
+
+#ifndef DOXYGEN_DOC_MAX_NUMBER_OF_ARGUMENTS_IN_CONSTRUCTOR
+#define DOXYGEN_DOC_MAX_NUMBER_OF_ARGUMENTS_IN_CONSTRUCTOR 10
+#endif
+
+namespace doxygen {
+typedef boost::mpl::void_ void_;
+
+template <typename _class>
+struct class_doc_impl {
+  static inline const char* run() { return ""; }
+  static inline const char* attribute(const char*) { return ""; }
+};
+
+template <typename _class>
+inline const char* class_doc() {
+  return class_doc_impl<_class>::run();
+}
+
+template <typename _class>
+inline const char* class_attrib_doc(const char* name) {
+  return class_doc_impl<_class>::attribute(name);
+}
+
+template <typename FuncPtr>
+inline const char* member_func_doc(FuncPtr) {
+  return "";
+}
+
+template <typename FuncPtr>
+inline void_ member_func_args(FuncPtr) {
+  return void_();
+}
+
+#define DOXYGEN_DOC_DECLARE_CONSTRUCTOR(z, nargs, unused)      \
+  template <typename Class BOOST_PP_COMMA_IF(nargs)            \
+                BOOST_PP_ENUM_PARAMS(nargs, class Arg)>        \
+  struct constructor_##nargs##_impl {                          \
+    static inline const char* doc() { return ""; }             \
+    static inline void_ args() { return void_(); }             \
+  };                                                           \
+                                                               \
+  template <typename Class BOOST_PP_COMMA_IF(nargs)            \
+                BOOST_PP_ENUM_PARAMS(nargs, class Arg)>        \
+  inline const char* constructor_doc() {                       \
+    return constructor_##nargs##_impl<Class BOOST_PP_COMMA_IF( \
+        nargs) BOOST_PP_ENUM_PARAMS(nargs, Arg)>::doc();       \
+  }
+
+BOOST_PP_REPEAT(DOXYGEN_DOC_MAX_NUMBER_OF_ARGUMENTS_IN_CONSTRUCTOR,
+                DOXYGEN_DOC_DECLARE_CONSTRUCTOR, ~)
+#undef DOXYGEN_DOC_DECLARE_CONSTRUCTOR
+/*
+template <typename Class>
+inline const char* constructor_doc ()
+{
+  return "";
+}
+*/
+
+template <typename Class>
+struct destructor_doc_impl {
+  static inline const char* run() { return ""; }
+};
+
+template <typename Class>
+inline const char* destructor_doc() {
+  return destructor_doc_impl<Class>::run();
+}
+
+/* TODO class attribute can be handled by
+
+template <typename Class, typename AttributeType>
+const char* attribute_doc (AttributeType Class::* ptr)
+{
+  // Body looks like
+  // if (ptr == &Class::attributeName)
+  //   return "attrib documentation";
+  return "undocumented";
+}
+*/
+
+}  // namespace doxygen
+
+#endif  // DOXYGEN_DOC_HH

doc/python/xml_docstring.py

+class XmlDocString(object):
+    def __init__(self, index):
+        self.index = index
+        self.tags = {
+            "para": self.para,
+            "ref": self.ref,
+            "briefdescription": self.otherTags,
+            "detaileddescription": self.otherTags,
+            "parameterlist": self.parameterlist,
+            "parameterdescription": self.otherTags,
+            "emphasis": self.emphasis,
+            "simplesect": self.simplesect,
+            "formula": self.formula,
+            "itemizedlist": self.itemizedlist,
+            "listitem": self.listitem,
+        }
+        self.unkwownTags = set()
+        self.unkwownReferences = dict()
+        self._linesep = '\\n"\n"'
+
+        try:
+            from pylatexenc.latex2text import LatexNodes2Text
+
+            self.latex = LatexNodes2Text()
+        except ImportError:
+            self.latex = None
+
+    def clear(self):
+        self.lines = []
+        self.unkwownTags.clear()
+        self.unkwownReferences.clear()
+
+    def writeErrors(self, output):
+        ret = False
+        for t in self.unkwownTags:
+            output.warn("Unknown tag: ", t)
+            ret = True
+        for ref, node in self.unkwownReferences.items():
+            output.warn("Unknown reference: ", ref, node.text)
+            ret = True
+        return ret
+
+    def _write(self, str):
+        nlines = str.split("\n")
+        if len(self.lines) == 0:
+            self.lines += nlines
+        else:
+            self.lines[-1] += nlines[0]
+            self.lines += nlines[1:]
+            # self.lines += nlines[1:]
+
+    def _newline(self, n=1):
+        self.lines.extend(
+            [
+                "",
+            ]
+            * n
+        )
+
+    def _clean(self):
+        s = 0
+        for line in self.lines:
+            if len(line.strip()) == 0:
+                s += 1
+            else:
+                break
+        e = len(self.lines)
+        for line in reversed(self.lines):
+            if len(line.strip()) == 0:
+                e -= 1
+            else:
+                break
+        self.lines = self.lines[s:e]
+
+    def getDocString(self, brief, detailled, output):
+        self.clear()
+        if brief is not None:
+            self.visit(brief)
+        if detailled is not None and len(detailled.getchildren()) > 0:
+            if brief is not None:
+                self._newline()
+            self.visit(detailled)
+        from sys import version_info
+
+        self.writeErrors(output)
+        self._clean()
+        if version_info[0] == 2:
+            return self._linesep.join(self.lines).encode("utf-8")
+        else:
+            return self._linesep.join(self.lines)
+
+    def visit(self, node):
+        assert isinstance(node.tag, str)
+        tag = node.tag
+        if tag not in self.tags:
+            self.unknownTag(node)
+        else:
+            self.tags[tag](node)
+
+    def unknownTag(self, node):
+        self.unkwownTags.add(node.tag)
+        self.otherTags(node)
+
+    def otherTags(self, node):
+        if node.text:
+            self._write(node.text.strip().replace('"', r"\""))
+        for c in node.iterchildren():
+            self.visit(c)
+            if c.tail:
+                self._write(c.tail.strip().replace('"', r"\""))
+
+    def emphasis(self, node):
+        self._write("*")
+        self.otherTags(node)
+        self._write("*")
+
+    def simplesect(self, node):
+        self._write(node.attrib["kind"].title() + ": ")
+        self.otherTags(node)
+
+    def para(self, node):
+        if node.text:
+            self._write(node.text.replace('"', r"\""))
+        for c in node.iterchildren():
+            self.visit(c)
+            if c.tail:
+                self._write(c.tail.replace('"', r"\""))
+        self._newline()
+
+    def ref(self, node):
+        refid = node.attrib["refid"]
+        if self.index.hasref(refid):
+            self._write(self.index.getref(refid).name)
+        else:
+            self.unkwownReferences[refid] = node
+            self._write(node.text)
+        assert len(node.getchildren()) == 0
+
+    def parameterlist(self, node):
+        self._newline()
+        self._write(node.attrib["kind"].title())
+        self._newline()
+        for item in node.iterchildren("parameteritem"):
+            self.parameteritem(item)
+
+    def parameteritem(self, node):
+        indent = "  "
+        self._write(indent + "- ")
+        # should contain two children
+        assert len(node.getchildren()) == 2
+        namelist = node.find("parameternamelist")
+        desc = node.find("parameterdescription")
+        sep = ""
+        for name in namelist.iterchildren("parametername"):
+            self._write(sep + name.text)
+            sep = ", "
+        self._write(" ")
+        self.visit(desc)
+
+    def itemizedlist(self, node):
+        self._newline()
+        self.otherTags(node)
+
+    def listitem(self, node):
+        self._write("- ")
+        self.otherTags(node)
+
+    def formula(self, node):
+        if node.text:
+            if self.latex is None:
+                self._write(node.text.strip())
+            else:
+                self._write(self.latex.latex_to_text(node.text))

doc/shape-mesh-collision-call.dot

+digraph CD  {
+
+	rankdir = BT
+	compound=true
+        size = 11.7
+
+        "BVHShapeCollider<OBBRSS, Shape, NarrowPhaseSolver>::collide\n(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s& tf2,\nconst NarrowPhaseSolver* nsolver,\nconst CollisionRequest& request, CollisionResult& result)\ncollision_func_matrix.cpp" [shape = box]
+        "details::orientedBVHShapeCollide<MeshShapeCollisionTraversalNodeOBBRSS\n<T_SH, NarrowPhaseSolver>, OBBRSS, T_SH, NarrowPhaseSolver>\n(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s&tf2,\nNarrowPhaseSolver* nsolver, const CollisionRequest& request,\nCollisionResult& result)\ncollision_func_matrix.cpp" [shape = box]
+        "void collide(CollisionTraversalNodeBase* node,\nconst CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)\ncollision_node.cpp" [shape = box]
+        "void propagateBVHFrontListCollisionRecurse\n(CollisionTraversalNodeBase* node, const CollisionRequest& request,\nCollisionResult& result, BVHFrontList* front_list)\ntraversal/traversal_recurse.cpp" [shape = box]
+        "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp" [shape = box]
+        "virtual bool CollisionTraversalNodeBase::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const = 0\ntraversal/traversal_node_base.h" [shape = box]
+        "virtual void CollisionTraversalNodeBase::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_base.h" [shape = box]
+        "bool MeshShapeCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [shape = box]
+        "bool MeshShapeCollisionTraversalNodeOBBRSS::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [shape = box]
+        "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBBRSS& b1,\nconst OBBRSS& b2, const CollisionRequest& request,\nCoalScalar& sqrDistLowerBound)\nBV/OBBRSS.cpp" [shape = box]
+        "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1, const OBB& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)\nBV/OBB.cpp" [shape = box]
+        "bool obbDisjointAndLowerBoundDistance (const Matrix3s& B, const Vec3s& T,\nconst Vec3s& a, const Vec3s& b,\nconst CollisionRequest& request,\nCoalScalar& squaredLowerBoundDistance)\nBV/OBB.cpp" [shape = box]
+        "void details::meshShapeCollisionOrientedNodeLeafTesting <OBBRSS, Shape, NarrowPhaseSolver>\n(int b1, int b2, const BVHModel<BV>* model1, const S& model2,\nVec3s* vertices, Triangle* tri_indices, const Transform3s& tf1,\nconst Transform3s& tf2, const NarrowPhaseSolver* nsolver,\nbool enable_statistics, int& num_leaf_tests,\nconst CollisionRequest& request, CollisionResult& result,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_node_bvh_shape.h:293" [shape = box]
+        "bool GJKSolver_indep::shapeTriangleIntersect<Shape>\n(const S& s, const Transform3s& tf,\nconst Vec3s& P1, const Vec3s& P2, const Vec3s& P3,\nCoalScalar* distance, Vec3s* p1, Vec3s* p2) const\nnarrowphase/narrowphase.h:156" [shape = box]
+
+        "BVHShapeCollider<OBBRSS, Shape, NarrowPhaseSolver>::collide\n(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s& tf2,\nconst NarrowPhaseSolver* nsolver,\nconst CollisionRequest& request, CollisionResult& result)\ncollision_func_matrix.cpp" -> "details::orientedBVHShapeCollide<MeshShapeCollisionTraversalNodeOBBRSS\n<T_SH, NarrowPhaseSolver>, OBBRSS, T_SH, NarrowPhaseSolver>\n(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s&tf2,\nNarrowPhaseSolver* nsolver, const CollisionRequest& request,\nCollisionResult& result)\ncollision_func_matrix.cpp"
+        "details::orientedBVHShapeCollide<MeshShapeCollisionTraversalNodeOBBRSS\n<T_SH, NarrowPhaseSolver>, OBBRSS, T_SH, NarrowPhaseSolver>\n(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s&tf2,\nNarrowPhaseSolver* nsolver, const CollisionRequest& request,\nCollisionResult& result)\ncollision_func_matrix.cpp" -> "void collide(CollisionTraversalNodeBase* node,\nconst CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)\ncollision_node.cpp"
+        "void collide(CollisionTraversalNodeBase* node,\nconst CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)\ncollision_node.cpp" -> "void propagateBVHFrontListCollisionRecurse\n(CollisionTraversalNodeBase* node, const CollisionRequest& request,\nCollisionResult& result, BVHFrontList* front_list)\ntraversal/traversal_recurse.cpp"
+        "void collide(CollisionTraversalNodeBase* node,\nconst CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)\ncollision_node.cpp" -> "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp"
+        "void propagateBVHFrontListCollisionRecurse\n(CollisionTraversalNodeBase* node, const CollisionRequest& request,\nCollisionResult& result, BVHFrontList* front_list)\ntraversal/traversal_recurse.cpp" -> "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp"
+        "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp" -> "virtual bool CollisionTraversalNodeBase::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const = 0\ntraversal/traversal_node_base.h"
+        "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp" -> "virtual void CollisionTraversalNodeBase::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_base.h"
+        "virtual bool CollisionTraversalNodeBase::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const = 0\ntraversal/traversal_node_base.h" -> "bool MeshShapeCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [color=red]
+        "virtual void CollisionTraversalNodeBase::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_base.h" -> "bool MeshShapeCollisionTraversalNodeOBBRSS::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [color = red]
+        "bool MeshShapeCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" -> "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBBRSS& b1,\nconst OBBRSS& b2, const CollisionRequest& request,\nCoalScalar& sqrDistLowerBound)\nBV/OBBRSS.cpp"
+        "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBBRSS& b1,\nconst OBBRSS& b2, const CollisionRequest& request,\nCoalScalar& sqrDistLowerBound)\nBV/OBBRSS.cpp" -> "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1, const OBB& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)\nBV/OBB.cpp"
+        "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1, const OBB& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)\nBV/OBB.cpp" -> "bool obbDisjointAndLowerBoundDistance (const Matrix3s& B, const Vec3s& T,\nconst Vec3s& a, const Vec3s& b,\nconst CollisionRequest& request,\nCoalScalar& squaredLowerBoundDistance)\nBV/OBB.cpp"
+        "bool MeshShapeCollisionTraversalNodeOBBRSS::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" -> "void details::meshShapeCollisionOrientedNodeLeafTesting <OBBRSS, Shape, NarrowPhaseSolver>\n(int b1, int b2, const BVHModel<BV>* model1, const S& model2,\nVec3s* vertices, Triangle* tri_indices, const Transform3s& tf1,\nconst Transform3s& tf2, const NarrowPhaseSolver* nsolver,\nbool enable_statistics, int& num_leaf_tests,\nconst CollisionRequest& request, CollisionResult& result,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_node_bvh_shape.h:293"
+        "void details::meshShapeCollisionOrientedNodeLeafTesting <OBBRSS, Shape, NarrowPhaseSolver>\n(int b1, int b2, const BVHModel<BV>* model1, const S& model2,\nVec3s* vertices, Triangle* tri_indices, const Transform3s& tf1,\nconst Transform3s& tf2, const NarrowPhaseSolver* nsolver,\nbool enable_statistics, int& num_leaf_tests,\nconst CollisionRequest& request, CollisionResult& result,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_node_bvh_shape.h:293" -> "bool GJKSolver_indep::shapeTriangleIntersect<Shape>\n(const S& s, const Transform3s& tf,\nconst Vec3s& P1, const Vec3s& P2, const Vec3s& P3,\nCoalScalar* distance, Vec3s* p1, Vec3s* p2) const\nnarrowphase/narrowphase.h:156"
+}

doc/shape-shape-collision-call.dot

+digraph CD  {
+
+	rankdir = BT
+	compound=true
+        size = 11.7
+
+        "template<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>\nstd::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s& tf2,\nconst NarrowPhaseSolver* nsolver,const CollisionRequest& request,\nCollisionResult& result)" [shape = box]
+        "template<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>\nCoalScalar ShapeShapeDistance(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s& tf2,\nconst NarrowPhaseSolver* nsolver, const DistanceRequest& request,\nDistanceResult& result)" [shape = box]
+        "void distance(DistanceTraversalNodeBase* node,\nBVHFrontList* front_list, int qsize)" [shape = box]
+        "void distanceRecurse(DistanceTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list)" [shape = box]
+        "void ShapeDistanceTraversalNode::leafTesting(int, int) const\ntraversal/traversal_node_shapes.h" [shape = box]
+        "template<typename S1, typename S2> bool GJKSolver_indep::shapeDistance\n(const S1& s1, const Transform3s& tf1, const S2& s2, const Transform3s& tf2,\nCoalScalar* distance, Vec3s* p1, Vec3s* p2) const\nnarrowphase/narrowphase.h" [shape = box]
+
+        "template<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>\nstd::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s& tf2,\nconst NarrowPhaseSolver* nsolver,const CollisionRequest& request,\nCollisionResult& result)" -> "template<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>\nCoalScalar ShapeShapeDistance(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s& tf2,\nconst NarrowPhaseSolver* nsolver, const DistanceRequest& request,\nDistanceResult& result)"
+        "template<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>\nCoalScalar ShapeShapeDistance(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s& tf2,\nconst NarrowPhaseSolver* nsolver, const DistanceRequest& request,\nDistanceResult& result)" -> "void distance(DistanceTraversalNodeBase* node,\nBVHFrontList* front_list, int qsize)"
+        "void distance(DistanceTraversalNodeBase* node,\nBVHFrontList* front_list, int qsize)" -> "void distanceRecurse(DistanceTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list)"
+        "void distanceRecurse(DistanceTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list)" -> "void ShapeDistanceTraversalNode::leafTesting(int, int) const\ntraversal/traversal_node_shapes.h"
+        "void ShapeDistanceTraversalNode::leafTesting(int, int) const\ntraversal/traversal_node_shapes.h" -> "template<typename S1, typename S2> bool GJKSolver_indep::shapeDistance\n(const S1& s1, const Transform3s& tf1, const S2& s2, const Transform3s& tf2,\nCoalScalar* distance, Vec3s* p1, Vec3s* p2) const\nnarrowphase/narrowphase.h"
+}

fcl.pc.in

-# This file was generated by CMake for @PROJECT_NAME@
-prefix=@CMAKE_INSTALL_PREFIX@
-exec_prefix=${prefix}
-libdir=${prefix}/lib
-includedir=${prefix}/include
-
-Name: @PROJECT_NAME@
-Description: @PKG_DESC@
-Version: @FCL_VERSION@
-Requires: @PKG_EXTERNAL_DEPS@
-Libs: -L${libdir} -lfcl
-Cflags: -I${includedir}

flake.lock

+{
+  "nodes": {
+    "flake-parts": {
+      "inputs": {
+        "nixpkgs-lib": "nixpkgs-lib"
+      },
+      "locked": {
+        "lastModified": 1738453229,
+        "narHash": "sha256-7H9XgNiGLKN1G1CgRh0vUL4AheZSYzPm+zmZ7vxbJdo=",
+        "owner": "hercules-ci",
+        "repo": "flake-parts",
+        "rev": "32ea77a06711b758da0ad9bd6a844c5740a87abd",
+        "type": "github"
+      },
+      "original": {
+        "owner": "hercules-ci",
+        "repo": "flake-parts",
+        "type": "github"
+      }
+    },
+    "nixpkgs": {
+      "locked": {
+        "lastModified": 1734122394,
+        "narHash": "sha256-TmVqB5V9ZIn66jlyPcp4yzsC6uF46YQLH00MSBio42c=",
+        "owner": "NixOS",
+        "repo": "nixpkgs",
+        "rev": "eb1b38d147a53360c11f0d033196f94d844bd86c",
+        "type": "github"
+      },
+      "original": {
+        "owner": "NixOS",
+        "ref": "refs/pull/357705/head",
+        "repo": "nixpkgs",
+        "type": "github"
+      }
+    },
+    "nixpkgs-lib": {
+      "locked": {
+        "lastModified": 1738452942,
+        "narHash": "sha256-vJzFZGaCpnmo7I6i416HaBLpC+hvcURh/BQwROcGIp8=",
+        "type": "tarball",
+        "url": "https://github.com/NixOS/nixpkgs/archive/072a6db25e947df2f31aab9eccd0ab75d5b2da11.tar.gz"
+      },
+      "original": {
+        "type": "tarball",
+        "url": "https://github.com/NixOS/nixpkgs/archive/072a6db25e947df2f31aab9eccd0ab75d5b2da11.tar.gz"
+      }
+    },
+    "root": {
+      "inputs": {
+        "flake-parts": "flake-parts",
+        "nixpkgs": "nixpkgs"
+      }
+    }
+  },
+  "root": "root",
+  "version": 7
+}

flake.nix

+{
+  description = "An extension of the Flexible Collision Library";
+
+  inputs = {
+    flake-parts.url = "github:hercules-ci/flake-parts";
+    # TODO: switch back to nixos-unstable after
+    # https://github.com/NixOS/nixpkgs/pull/357705
+    nixpkgs.url = "github:NixOS/nixpkgs/refs/pull/357705/head";
+  };
+
+  outputs =
+    inputs:
+    inputs.flake-parts.lib.mkFlake { inherit inputs; } {
+      systems = inputs.nixpkgs.lib.systems.flakeExposed;
+      perSystem =
+        { pkgs, self', ... }:
+        {
+          apps.default = {
+            type = "app";
+            program = pkgs.python3.withPackages (_: [ self'.packages.default ]);
+          };
+          devShells.default = pkgs.mkShell { inputsFrom = [ self'.packages.default ]; };
+          packages = {
+            default = self'.packages.coal;
+            coal = pkgs.python3Packages.coal.overrideAttrs (_: {
+              src = pkgs.lib.fileset.toSource {
+                root = ./.;
+                fileset = pkgs.lib.fileset.unions [
+                  ./CMakeLists.txt
+                  ./doc
+                  ./hpp-fclConfig.cmake
+                  ./include
+                  ./package.xml
+                  ./python
+                  ./src
+                  ./test
+                ];
+              };
+            });
+          };
+        };
+    };
+}

hpp-fclConfig.cmake

+# This file provide bacward compatiblity for `find_package(hpp-fcl)`.
+
+message(WARNING "Please update your CMake from 'hpp-fcl' to 'coal'")
+
+find_package(coal REQUIRED)
+
+if(NOT TARGET hpp-fcl::hpp-fcl)
+  add_library(hpp-fcl::hpp-fcl INTERFACE IMPORTED)
+
+  # Compute the installation prefix relative to this file.
+  # This code is taken from generated cmake xxxTargets.cmake.
+  get_filename_component(_IMPORT_PREFIX "${CMAKE_CURRENT_LIST_FILE}" PATH)
+  get_filename_component(_IMPORT_PREFIX "${_IMPORT_PREFIX}" PATH)
+  get_filename_component(_IMPORT_PREFIX "${_IMPORT_PREFIX}" PATH)
+  get_filename_component(_IMPORT_PREFIX "${_IMPORT_PREFIX}" PATH)
+  if(_IMPORT_PREFIX STREQUAL "/")
+    set(_IMPORT_PREFIX "")
+  endif()
+
+  set_target_properties(
+    hpp-fcl::hpp-fcl
+    PROPERTIES INTERFACE_COMPILE_DEFINITIONS
+               "COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL"
+               INTERFACE_INCLUDE_DIRECTORIES "${_IMPORT_PREFIX}/include"
+               INTERFACE_LINK_LIBRARIES "coal::coal")
+endif()

include/fcl/BV/AABB.h → include/coal/BV/AABB.h

 /*
  * Software License Agreement (BSD License)
  *
- *  Copyright (c) 2011, Willow Garage, Inc.
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, Open Source Robotics Foundation
  *  All rights reserved.
  *
  *  Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
  *     copyright notice, this list of conditions and the following
  *     disclaimer in the documentation and/or other materials provided
  *     with the distribution.
- *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
  *     contributors may be used to endorse or promote products derived
  *     from this software without specific prior written permission.
  *
@@ -34,208 +35,232 @@
 
 /** \author Jia Pan */
 
-#ifndef FCL_AABB_H
-#define FCL_AABB_H
+#ifndef COAL_AABB_H
+#define COAL_AABB_H
 
+#include "coal/data_types.h"
 
-#include "fcl/math/vec_3f.h"
+namespace coal {
 
-namespace fcl
-{
+struct CollisionRequest;
+class Plane;
+class Halfspace;
 
-/// @brief A class describing the AABB collision structure, which is a box in 3D space determined by two diagonal points
-class AABB
-{
-public:
+/// @defgroup Bounding_Volume Bounding volumes
+/// Classes of different types of bounding volume.
+/// @{
+
+/// @brief A class describing the AABB collision structure, which is a box in 3D
+/// space determined by two diagonal points
+class COAL_DLLAPI AABB {
+ public:
   /// @brief The min point in the AABB
-  Vec3f min_;
+  Vec3s min_;
   /// @brief The max point in the AABB
-  Vec3f max_;
+  Vec3s max_;
 
   /// @brief Creating an AABB with zero size (low bound +inf, upper bound -inf)
   AABB();
 
   /// @brief Creating an AABB at position v with zero size
-  AABB(const Vec3f& v) : min_(v), max_(v)
-  {
-  }
+  AABB(const Vec3s& v) : min_(v), max_(v) {}
 
   /// @brief Creating an AABB with two endpoints a and b
-  AABB(const Vec3f& a, const Vec3f&b) : min_(min(a, b)),
-                                        max_(max(a, b))
-  {
-  }
+  AABB(const Vec3s& a, const Vec3s& b)
+      : min_(a.cwiseMin(b)), max_(a.cwiseMax(b)) {}
 
   /// @brief Creating an AABB centered as core and is of half-dimension delta
-  AABB(const AABB& core, const Vec3f& delta) : min_(core.min_ - delta),
-                                               max_(core.max_ + delta)
-  {
-  }
+  AABB(const AABB& core, const Vec3s& delta)
+      : min_(core.min_ - delta), max_(core.max_ + delta) {}
 
   /// @brief Creating an AABB contains three points
-  AABB(const Vec3f& a, const Vec3f& b, const Vec3f& c) : min_(min(min(a, b), c)),
-                                                         max_(max(max(a, b), c))
-  {
-  }
+  AABB(const Vec3s& a, const Vec3s& b, const Vec3s& c)
+      : min_(a.cwiseMin(b).cwiseMin(c)), max_(a.cwiseMax(b).cwiseMax(c)) {}
 
-  /// @brief Check whether two AABB are overlap
-  inline bool overlap(const AABB& other) const
-  {
-    if(min_[0] > other.max_[0]) return false;
-    if(min_[1] > other.max_[1]) return false;
-    if(min_[2] > other.max_[2]) return false;
+  AABB(const AABB& other) = default;
 
-    if(max_[0] < other.min_[0]) return false;
-    if(max_[1] < other.min_[1]) return false;
-    if(max_[2] < other.min_[2]) return false;
+  AABB& operator=(const AABB& other) = default;
 
-    return true;
-  }    
+  AABB& update(const Vec3s& a, const Vec3s& b) {
+    min_ = a.cwiseMin(b);
+    max_ = a.cwiseMax(b);
+    return *this;
+  }
 
-  /// @brief Check whether the AABB contains another AABB
-  inline bool contain(const AABB& other) const
-  {
-    return (other.min_[0] >= min_[0]) && (other.max_[0] <= max_[0]) && (other.min_[1] >= min_[1]) && (other.max_[1] <= max_[1]) && (other.min_[2] >= min_[2]) && (other.max_[2] <= max_[2]);
+  /// @brief Comparison operator
+  bool operator==(const AABB& other) const {
+    return min_ == other.min_ && max_ == other.max_;
   }
 
+  bool operator!=(const AABB& other) const { return !(*this == other); }
 
-  /// @brief Check whether two AABB are overlapped along specific axis
-  inline bool axisOverlap(const AABB& other, int axis_id) const
-  {
-    if(min_[axis_id] > other.max_[axis_id]) return false;
+  /// @name Bounding volume API
+  /// Common API to BVs.
+  /// @{
 
-    if(max_[axis_id] < other.min_[axis_id]) return false;
+  /// @brief Check whether the AABB contains a point
+  inline bool contain(const Vec3s& p) const {
+    if (p[0] < min_[0] || p[0] > max_[0]) return false;
+    if (p[1] < min_[1] || p[1] > max_[1]) return false;
+    if (p[2] < min_[2] || p[2] > max_[2]) return false;
 
     return true;
   }
 
-  /// @brief Check whether two AABB are overlap and return the overlap part
-  inline bool overlap(const AABB& other, AABB& overlap_part) const
-  {
-    if(!overlap(other))
-    {
-      return false;
-    }
-    
-    overlap_part.min_ = max(min_, other.min_);
-    overlap_part.max_ = min(max_, other.max_);
+  /// @brief Check whether two AABB are overlap
+  inline bool overlap(const AABB& other) const {
+    if (min_[0] > other.max_[0]) return false;
+    if (min_[1] > other.max_[1]) return false;
+    if (min_[2] > other.max_[2]) return false;
+
+    if (max_[0] < other.min_[0]) return false;
+    if (max_[1] < other.min_[1]) return false;
+    if (max_[2] < other.min_[2]) return false;
+
     return true;
   }
 
+  /// @brief Check whether AABB overlaps a plane
+  bool overlap(const Plane& p) const;
 
-  /// @brief Check whether the AABB contains a point
-  inline bool contain(const Vec3f& p) const
-  {
-    if(p[0] < min_[0] || p[0] > max_[0]) return false;
-    if(p[1] < min_[1] || p[1] > max_[1]) return false;
-    if(p[2] < min_[2] || p[2] > max_[2]) return false;
+  /// @brief Check whether AABB overlaps a halfspace
+  bool overlap(const Halfspace& hs) const;
 
-    return true;
-  }
+  /// @brief Check whether two AABB are overlap
+  bool overlap(const AABB& other, const CollisionRequest& request,
+               CoalScalar& sqrDistLowerBound) const;
+
+  /// @brief Distance between two AABBs
+  CoalScalar distance(const AABB& other) const;
+
+  /// @brief Distance between two AABBs; P and Q, should not be NULL, return the
+  /// nearest points
+  CoalScalar distance(const AABB& other, Vec3s* P, Vec3s* Q) const;
 
   /// @brief Merge the AABB and a point
-  inline AABB& operator += (const Vec3f& p)
-  {
-    min_.ubound(p);
-    max_.lbound(p);
+  inline AABB& operator+=(const Vec3s& p) {
+    min_ = min_.cwiseMin(p);
+    max_ = max_.cwiseMax(p);
     return *this;
   }
 
   /// @brief Merge the AABB and another AABB
-  inline AABB& operator += (const AABB& other)
-  {
-    min_.ubound(other.min_);
-    max_.lbound(other.max_);
+  inline AABB& operator+=(const AABB& other) {
+    min_ = min_.cwiseMin(other.min_);
+    max_ = max_.cwiseMax(other.max_);
     return *this;
   }
 
   /// @brief Return the merged AABB of current AABB and the other one
-  inline AABB operator + (const AABB& other) const
-  {
+  inline AABB operator+(const AABB& other) const {
     AABB res(*this);
     return res += other;
   }
 
+  /// @brief Size of the AABB (used in BV_Splitter to order two AABBs)
+  inline CoalScalar size() const { return (max_ - min_).squaredNorm(); }
+
+  /// @brief Center of the AABB
+  inline Vec3s center() const { return (min_ + max_) * 0.5; }
+
   /// @brief Width of the AABB
-  inline FCL_REAL width() const
-  {
-    return max_[0] - min_[0];
-  }
+  inline CoalScalar width() const { return max_[0] - min_[0]; }
 
   /// @brief Height of the AABB
-  inline FCL_REAL height() const
-  {
-    return max_[1] - min_[1];
-  }
+  inline CoalScalar height() const { return max_[1] - min_[1]; }
 
   /// @brief Depth of the AABB
-  inline FCL_REAL depth() const
-  {
-    return max_[2] - min_[2];
-  }
+  inline CoalScalar depth() const { return max_[2] - min_[2]; }
 
   /// @brief Volume of the AABB
-  inline FCL_REAL volume() const
-  {
-    return width() * height() * depth();
-  }  
+  inline CoalScalar volume() const { return width() * height() * depth(); }
 
-  /// @brief Size of the AABB (used in BV_Splitter to order two AABBs)
-  inline FCL_REAL size() const
-  {
-    return (max_ - min_).sqrLength();
-  }
+  /// @}
 
-  /// @brief Radius of the AABB
-  inline FCL_REAL radius() const
-  {
-    return (max_ - min_).length() / 2;
+  /// @brief Check whether the AABB contains another AABB
+  inline bool contain(const AABB& other) const {
+    return (other.min_[0] >= min_[0]) && (other.max_[0] <= max_[0]) &&
+           (other.min_[1] >= min_[1]) && (other.max_[1] <= max_[1]) &&
+           (other.min_[2] >= min_[2]) && (other.max_[2] <= max_[2]);
   }
 
-  /// @brief Center of the AABB
-  inline  Vec3f center() const
-  {
-    return (min_ + max_) * 0.5;
-  }
+  /// @brief Check whether two AABB are overlap and return the overlap part
+  inline bool overlap(const AABB& other, AABB& overlap_part) const {
+    if (!overlap(other)) {
+      return false;
+    }
 
-  /// @brief Distance between two AABBs; P and Q, should not be NULL, return the nearest points 
-  FCL_REAL distance(const AABB& other, Vec3f* P, Vec3f* Q) const;
+    overlap_part.min_ = min_.cwiseMax(other.min_);
+    overlap_part.max_ = max_.cwiseMin(other.max_);
+    return true;
+  }
 
-  /// @brief Distance between two AABBs
-  FCL_REAL distance(const AABB& other) const;
+  /// @brief Check whether two AABB are overlapped along specific axis
+  inline bool axisOverlap(const AABB& other, int axis_id) const {
+    if (min_[axis_id] > other.max_[axis_id]) return false;
+    if (max_[axis_id] < other.min_[axis_id]) return false;
 
-  /// @brief whether two AABB are equal
-  inline bool equal(const AABB& other) const
-  {
-    return min_.equal(other.min_) && max_.equal(other.max_);
+    return true;
   }
 
-  /// @brief expand the half size of the AABB by delta, and keep the center unchanged.
-  inline AABB& expand(const Vec3f& delta)
-  {
+  /// @brief expand the half size of the AABB by delta, and keep the center
+  /// unchanged.
+  inline AABB& expand(const Vec3s& delta) {
     min_ -= delta;
     max_ += delta;
     return *this;
   }
 
+  /// @brief expand the half size of the AABB by a scalar delta, and keep the
+  /// center unchanged.
+  inline AABB& expand(const CoalScalar delta) {
+    min_.array() -= delta;
+    max_.array() += delta;
+    return *this;
+  }
+
   /// @brief expand the aabb by increase the thickness of the plate by a ratio
-  inline AABB& expand(const AABB& core, FCL_REAL ratio)
-  {
+  inline AABB& expand(const AABB& core, CoalScalar ratio) {
     min_ = min_ * ratio - core.min_;
     max_ = max_ * ratio - core.max_;
     return *this;
-  }  
+  }
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 /// @brief translate the center of AABB by t
-static inline AABB translate(const AABB& aabb, const Vec3f& t)
-{
+static inline AABB translate(const AABB& aabb, const Vec3s& t) {
   AABB res(aabb);
   res.min_ += t;
   res.max_ += t;
   return res;
 }
 
+static inline AABB rotate(const AABB& aabb, const Matrix3s& R) {
+  AABB res(R * aabb.min_);
+  Vec3s corner(aabb.min_);
+  const Eigen::DenseIndex bit[3] = {1, 2, 4};
+  for (Eigen::DenseIndex ic = 1; ic < 8;
+       ++ic) {  // ic = 0 corresponds to aabb.min_. Skip it.
+    for (Eigen::DenseIndex i = 0; i < 3; ++i) {
+      corner[i] = (ic & bit[i]) ? aabb.max_[i] : aabb.min_[i];
+    }
+    res += R * corner;
+  }
+  return res;
 }
 
+/// @brief Check collision between two aabbs, b1 is in configuration (R0, T0)
+/// and b2 is in identity.
+COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const AABB& b1,
+                         const AABB& b2);
+
+/// @brief Check collision between two aabbs, b1 is in configuration (R0, T0)
+/// and b2 is in identity.
+COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const AABB& b1,
+                         const AABB& b2, const CollisionRequest& request,
+                         CoalScalar& sqrDistLowerBound);
+}  // namespace coal
+
 #endif