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/doxygen_xml_parser.py

+#!/usr/bin/python3
+# ruff: noqa: E501
+
+from __future__ import print_function
+from lxml import etree
+from os import path
+from xml_docstring import XmlDocString
+import sys
+
+template_file_header = """#ifndef DOXYGEN_AUTODOC_{header_guard}
+#define DOXYGEN_AUTODOC_{header_guard}
+
+#include "{path}/doxygen.hh"
+"""
+template_file_footer = """
+#endif // DOXYGEN_AUTODOC_{header_guard}
+"""
+
+template_class_doc = """
+template <{tplargs}>
+struct class_doc_impl< {classname} >
+{{
+static inline const char* run ()
+{{
+  return "{docstring}";
+}}
+static inline const char* attribute (const char* attrib)
+{{{attributes}
+  (void)attrib; // turn off unused parameter warning.
+  return "";
+}}
+}};"""
+template_class_attribute_body = """
+  if (strcmp(attrib, "{attribute}") == 0)
+    return "{docstring}";"""
+template_constructor_doc = """
+template <{tplargs}>
+struct constructor_{nargs}_impl< {classname_prefix}{comma}{argsstring} >
+{{
+static inline const char* doc ()
+{{
+  return "{docstring}";
+}}
+static inline boost::python::detail::keywords<{nargs}+1> args ()
+{{
+  return ({argnamesstring});
+}}
+}};"""
+template_destructor_doc = """
+template <{tplargs}>
+struct destructor_doc_impl < {classname_prefix} >
+{{
+static inline const char* run ()
+{{
+  return "{docstring}";
+}}
+}};"""
+template_member_func_doc = """
+{template}inline const char* member_func_doc ({rettype} ({classname_prefix}*function_ptr) {argsstring})
+{{{body}
+  return "";
+}}"""
+template_member_func_doc_body = """
+  if (function_ptr == static_cast<{rettype} ({classname_prefix}*) {argsstring}>(&{classname_prefix}{membername}))
+    return "{docstring}";"""
+template_member_func_args = """
+{template}inline boost::python::detail::keywords<{n}> member_func_args ({rettype} ({classname_prefix}*function_ptr) {argsstring})
+{{{body}
+  return ({default_args});
+}}"""
+template_member_func_args_body = """
+  if (function_ptr == static_cast<{rettype} ({classname_prefix}*) {argsstring}>(&{classname_prefix}{membername}))
+    return ({args});"""
+template_static_func_doc = """
+{template}inline const char* member_func_doc ({rettype} (*function_ptr) {argsstring})
+{{{body}
+  return "";
+}}"""
+template_static_func_doc_body = """
+  if (function_ptr == static_cast<{rettype} (*) {argsstring}>(&{namespace}::{membername}))
+    return "{docstring}";"""
+template_open_namespace = """namespace {namespace} {{"""
+template_close_namespace = """}} // namespace {namespace}"""
+template_include_intern = """#include <doxygen_autodoc/{filename}>
+"""
+template_include_extern = """#include <{filename}>
+"""
+
+
+def _templateParamToDict(param):
+    type_ = param.find("type")
+    declname = param.find("declname")
+    defname = param.find("defname")
+    # FIXME type may contain references in two ways:
+    # - the real param type
+    # - the name of the template argument is recognized as the name of a type...
+    if defname is None and declname is None:
+        typetext = type_.text
+        if typetext is None:
+            typetext = ""
+        for c in type_.iter():
+            if c == type_:
+                continue
+            if c.text is not None:
+                typetext += c.text
+            if c.tail is not None:
+                typetext += c.tail
+        if typetext.startswith("typename") or typetext.startswith("class"):
+            if sys.version_info.major == 2:
+                s = typetext.split()
+                return {"type": s[0].strip(), "name": typetext[len(s[0]) :].strip()}
+            else:
+                s = typetext.split(maxsplit=1)
+                assert len(s) == 2
+                return {"type": s[0].strip(), "name": s[1].strip()}
+        else:
+            return {"type": type_.text, "name": ""}
+    else:
+        if type_.text is None:
+            # type_ is not a typename but an existing type
+            type_ = type_.find("ref")
+        assert defname.text == declname.text
+        return {"type": type_.text, "name": defname.text}
+
+
+def makeHeaderGuard(filename):
+    return filename.upper().replace(".", "_").replace("/", "_").replace("-", "_")
+
+
+def format_description(brief, detailed):
+    b = [el.text.strip() for el in brief.iter() if el.text] if brief is not None else []
+    d = (
+        [el.text.strip() for el in detailed.iter() if el.text]
+        if detailed is not None
+        else []
+    )
+    text = "".join(b)
+    if d:
+        text += "\n" + "".join(d)
+    return text
+
+
+class Reference(object):
+    def __init__(self, index, id=None, name=None):
+        self.id = id
+        self.name = name
+        self.index = index
+
+    def xmlToType(self, node, array=None, parentClass=None, tplargs=None):
+        """
+        - node:
+        - parentClass: a class
+        - tplargs: if one of the args is parentClass and no template arguments are provided,
+                   set the template arguments to this value
+        - array: content of the sibling tag 'array'
+        """
+        if node.text is not None:
+            t = node.text.strip()
+        else:
+            t = ""
+        for c in node.iterchildren():
+            if c.tag == "ref":
+                refid = c.attrib["refid"]
+                if parentClass is not None and refid == parentClass.id:
+                    t += " " + parentClass.name
+                    if c.tail is not None and c.tail.lstrip()[0] != "<":
+                        if tplargs is not None:
+                            t += tplargs
+                elif (
+                    parentClass is not None
+                    and isinstance(parentClass, ClassCompound)
+                    and parentClass.hasTypeDef(c.text.strip())
+                ):
+                    parent_has_templates = len(parentClass.template_params) > 0
+                    if parent_has_templates:
+                        t += " typename " + parentClass._className() + "::"
+                    else:
+                        t += " " + parentClass._className() + "::"
+                    self_has_templates = (
+                        c.tail is not None and c.tail.strip().find("<") != -1
+                    )
+                    if self_has_templates:
+                        t += " template "
+                    t += c.text.strip()
+                elif self.index.hasref(refid):
+                    t += " " + self.index.getref(refid).name
+                else:
+                    self.index.output.warn("Unknown reference: ", c.text, refid)
+                    t += " " + c.text.strip()
+            else:
+                if c.text is not None:
+                    t += " " + c.text.strip()
+            if c.tail is not None:
+                t += " " + c.tail.strip()
+        if array is not None:
+            t += array.text
+        return t
+
+
+# Only for function as of now.
+class MemberDef(Reference):
+    def __init__(self, index, memberdefxml, parent):
+        super(MemberDef, self).__init__(
+            index=index,
+            id=memberdefxml.attrib["id"],
+            name=memberdefxml.find("definition").text,
+        )
+        self.parent = parent
+
+        self.xml = memberdefxml
+        self.const = memberdefxml.attrib["const"] == "yes"
+        self.static = memberdefxml.attrib["static"] == "yes"
+        self.rettype = memberdefxml.find("type")
+        self.params = tuple(
+            [
+                (param.find("type"), param.find("declname"), param.find("array"))
+                for param in self.xml.findall("param")
+            ]
+        )
+        self.special = (
+            self.rettype.text is None and len(self.rettype.getchildren()) == 0
+        )
+        # assert self.special or len(self.rettype.text) > 0
+
+        self._templateParams(self.xml.find("templateparamlist"))
+
+    def _templateParams(self, tpl):
+        if tpl is not None:
+            self.template_params = tuple(
+                [_templateParamToDict(param) for param in tpl.iterchildren(tag="param")]
+            )
+        else:
+            self.template_params = tuple()
+
+    def prototypekey(self):
+        prototype = (
+            self.xmlToType(self.rettype, parentClass=self.parent),
+            tuple([tuple(t.items()) for t in self.template_params]),
+            tuple(
+                [
+                    self.xmlToType(param.find("type"), parentClass=self.parent)
+                    for param in self.xml.findall("param")
+                ]
+            ),
+            self.const,
+        )
+        return prototype
+
+    def s_prototypeArgs(self):
+        return "({0}){1}".format(self.s_args(), " const" if self.const else "")
+
+    def s_args(self):
+        # If the class is templated, check if one of the argument is the class itself.
+        # If so, we must add the template arguments to the class (if there is none)
+
+        if len(self.parent.template_params) > 0:
+            tplargs = (
+                " <"
+                + ", ".join([d["name"] for d in self.parent.template_params])
+                + " > "
+            )
+            args = ", ".join(
+                [
+                    self.xmlToType(
+                        type, array, parentClass=self.parent, tplargs=tplargs
+                    )
+                    for type, declname, array in self.params
+                ]
+            )
+        else:
+            args = ", ".join(
+                [
+                    self.xmlToType(type, array, parentClass=self.parent)
+                    for type, declname, array in self.params
+                ]
+            )
+        return args
+
+    def s_tpldecl(self):
+        if len(self.template_params) == 0:
+            return ""
+        return ", ".join([d["type"] + " " + d["name"] for d in self.template_params])
+
+    def s_rettype(self):
+        assert (
+            not self.special
+        ), "Member {} ({}) is a special function and no return type".format(
+            self.name, self.id
+        )
+        if len(self.parent.template_params) > 0:
+            tplargs = (
+                " <"
+                + ", ".join([d["name"] for d in self.parent.template_params])
+                + " > "
+            )
+        else:
+            tplargs = None
+        if isinstance(self.parent, ClassCompound):
+            return self.xmlToType(
+                self.rettype, parentClass=self.parent, tplargs=tplargs
+            )
+        else:
+            return self.xmlToType(self.rettype)
+
+    def s_name(self):
+        return self.xml.find("name").text.strip()
+
+    def s_docstring(self):
+        return self.index.xml_docstring.getDocString(
+            self.xml.find("briefdescription"),
+            self.xml.find("detaileddescription"),
+            self.index.output,
+        )
+
+    def n_args(self):
+        return len(self.params)
+
+    def s_argnamesstring(self):
+        def getdeclname(i, declname):
+            if declname is None or declname.text is None or declname.text.strip() == "":
+                return "arg{}".format(i)
+            return declname.text.strip()
+
+        arg = """boost::python::arg("{}")"""
+        argnames = [
+            "self",
+        ] + [getdeclname(i, declname) for i, (_, declname, _) in enumerate(self.params)]
+        return ", ".join([arg.format(n) for n in argnames])
+
+    def include(self):
+        loc = self.xml.find("location")
+        # The location is based on $CMAKE_SOURCE_DIR. Remove first directory.
+        return loc.attrib["file"].split("/", 1)[1]
+
+
+class CompoundBase(Reference):
+    def __init__(self, compound, index):
+        self.compound = compound
+        self.filename = path.join(index.directory, compound.attrib["refid"] + ".xml")
+        self.tree = etree.parse(self.filename)
+        self.definition = self.tree.getroot().find("compounddef")
+        super(CompoundBase, self).__init__(
+            index,
+            id=self.definition.attrib["id"],
+            name=self.definition.find("compoundname").text,
+        )
+
+
+class NamespaceCompound(CompoundBase):
+    def __init__(self, *args):
+        super(NamespaceCompound, self).__init__(*args)
+        self.typedefs = []
+        self.enums = []
+        self.static_funcs = []
+        self.template_params = tuple()
+
+        # Add references
+        for section in self.definition.iterchildren("sectiondef"):
+            assert "kind" in section.attrib
+            kind = section.attrib["kind"]
+            if kind == "enum":
+                self.parseEnumSection(section)
+            elif kind == "typedef":
+                self.parseTypedefSection(section)
+            elif kind == "func":
+                self.parseFuncSection(section)
+
+    def parseEnumSection(self, section):
+        for member in section.iterchildren("memberdef"):
+            ref = Reference(
+                index=self.index,
+                id=member.attrib["id"],
+                name=self.name + "::" + member.find("name").text,
+            )
+            self.index.registerReference(ref)
+            self.enums.append(member)
+            for value in member.iterchildren("enumvalue"):
+                ref = Reference(
+                    index=self.index,
+                    id=value.attrib["id"],
+                    name=self.name + "::" + member.find("name").text,
+                )
+
+    def parseTypedefSection(self, section):
+        for member in section.iterchildren("memberdef"):
+            ref = Reference(
+                index=self.index,
+                id=member.attrib["id"],
+                name=self.name + "::" + member.find("name").text,
+            )
+            self.index.registerReference(ref)
+            self.typedefs.append(member)
+
+    def parseFuncSection(self, section):
+        for member in section.iterchildren("memberdef"):
+            self.static_funcs.append(MemberDef(self.index, member, self))
+
+    def innerNamespace(self):
+        return self.name
+
+    def write(self, output):
+        pass
+
+
+class ClassCompound(CompoundBase):
+    def __init__(self, *args):
+        super(ClassCompound, self).__init__(*args)
+        self.member_funcs = list()
+        self.static_funcs = list()
+        self.special_funcs = list()
+        self.attributes = list()
+
+        self.struct = self.compound.attrib["kind"] == "struct"
+        self.public = self.definition.attrib["prot"] == "public"
+        self.template_specialization = self.name.find("<") > 0
+        self.typedef = dict()
+
+        # Handle templates
+        self._templateParams(self.definition.find("templateparamlist"))
+        for memberdef in self.definition.iter(tag="memberdef"):
+            if memberdef.attrib["prot"] != "public":
+                continue
+            if memberdef.attrib["kind"] == "variable":
+                self._attribute(memberdef)
+            elif memberdef.attrib["kind"] == "typedef":
+                ref = Reference(
+                    index=self.index,
+                    id=memberdef.attrib["id"],
+                    name=self._className() + "::" + memberdef.find("name").text,
+                )
+                self.index.registerReference(ref)
+                self.typedef[memberdef.find("name").text.strip()] = True
+
+            elif memberdef.attrib["kind"] == "enum":
+                if memberdef.find("name").text is None:
+                    ref_name = self._className() + "::" + "anonymous_enum"
+                else:
+                    ref_name = self._className() + "::" + memberdef.find("name").text
+                ref = Reference(
+                    index=self.index,
+                    id=memberdef.attrib["id"],
+                    name=ref_name,
+                )
+                self.index.registerReference(ref)
+                for value in memberdef.iterchildren("enumvalue"):
+                    value_ref = Reference(
+                        index=self.index,
+                        id=value.attrib["id"],
+                        name=ref.name,
+                    )
+                    self.index.registerReference(value_ref)
+            elif memberdef.attrib["kind"] == "function":
+                self._memberfunc(memberdef)
+
+    def _templateParams(self, tpl):
+        if tpl is not None:
+            self.template_params = tuple(
+                [_templateParamToDict(param) for param in tpl.iterchildren(tag="param")]
+            )
+        else:
+            self.template_params = tuple()
+
+    def _templateDecl(self):
+        if not hasattr(self, "template_params") or len(self.template_params) == 0:
+            return ""
+        return ", ".join([d["type"] + " " + d["name"] for d in self.template_params])
+
+    def _className(self):
+        if not hasattr(self, "template_params") or len(self.template_params) == 0:
+            return self.name
+        return (
+            self.name
+            + " <"
+            + ", ".join([d["name"] for d in self.template_params])
+            + " >"
+        )
+
+    def hasTypeDef(self, typename):
+        return typename in self.typedef
+
+    def innerNamespace(self):
+        return self._className()
+
+    def _memberfunc(self, member):
+        m = MemberDef(self.index, member, self)
+        if m.special:
+            self.special_funcs.append(m)
+        elif m.static:
+            self.static_funcs.append(m)
+        else:
+            self.member_funcs.append(m)
+
+    def _writeClassDoc(self, output):
+        docstring = self.index.xml_docstring.getDocString(
+            self.definition.find("briefdescription"),
+            self.definition.find("detaileddescription"),
+            self.index.output,
+        )
+        attribute_docstrings = ""
+        for member in self.attributes:
+            _dc = self.index.xml_docstring.getDocString(
+                member.find("briefdescription"),
+                member.find("detaileddescription"),
+                self.index.output,
+            )
+            if len(_dc) == 0:
+                continue
+            attribute_docstrings += template_class_attribute_body.format(
+                attribute=member.find("name").text,
+                docstring=_dc,
+            )
+        if len(docstring) == 0 and len(attribute_docstrings) == 0:
+            return
+        output.out(
+            template_class_doc.format(
+                tplargs=self._templateDecl(),
+                classname=self._className(),
+                docstring=docstring,
+                attributes=attribute_docstrings,
+            )
+        )
+
+    def write(self, output):
+        if not self.public:
+            return
+        if self.template_specialization:
+            output.warn(
+                "Disable class {} because template argument are not resolved for templated class specialization.".format(
+                    self.name
+                )
+            )
+            return
+
+        include = self.definition.find("includes")
+        if include is None:
+            output.err("Does not know where to write doc of", self.name)
+            return
+        output.open(include.text)
+        output.out(template_include_extern.format(filename=include.text))
+        output.out(template_open_namespace.format(namespace="doxygen"))
+
+        # Write class doc
+        self._writeClassDoc(output)
+
+        # Group member function by prototype
+        member_funcs = dict()
+        for m in self.member_funcs:
+            prototype = m.prototypekey()
+            if prototype in member_funcs:
+                member_funcs[prototype].append(m)
+            else:
+                member_funcs[prototype] = [
+                    m,
+                ]
+
+        classname_prefix = self._className() + "::"
+
+        for member in self.special_funcs:
+            docstring = member.s_docstring()
+            argnamesstring = member.s_argnamesstring()
+            if len(docstring) == 0 and len(argnamesstring) == 0:
+                continue
+            if member.s_name()[0] == "~":
+                output.out(
+                    template_destructor_doc.format(
+                        tplargs=self._templateDecl(),
+                        classname_prefix=self._className(),
+                        docstring=docstring,
+                    )
+                )
+            else:
+                output.out(
+                    template_constructor_doc.format(
+                        tplargs=", ".join(
+                            [
+                                d["type"] + " " + d["name"]
+                                for d in self.template_params + member.template_params
+                            ]
+                        ),
+                        nargs=len(member.params),
+                        comma=", " if len(member.params) > 0 else "",
+                        classname_prefix=self._className(),
+                        argsstring=member.s_args(),
+                        docstring=docstring,
+                        argnamesstring=argnamesstring,
+                    )
+                )
+
+        for prototype, members in member_funcs.items():
+            # remove undocumented members
+            documented_members = []
+            docstrings = []
+            argnamesstrings = []
+            for member in members:
+                docstring = member.s_docstring()
+                argnamesstring = member.s_argnamesstring()
+                if len(docstring) == 0 and len(argnamesstring) == 0:
+                    continue
+                documented_members.append(member)
+                docstrings.append(docstring)
+                argnamesstrings.append(argnamesstring)
+            if len(documented_members) == 0:
+                continue
+
+            # Write docstrings
+            body = "".join(
+                [
+                    template_member_func_doc_body.format(
+                        classname_prefix=classname_prefix,
+                        membername=member.s_name(),
+                        docstring=docstring,
+                        rettype=member.s_rettype(),
+                        argsstring=member.s_prototypeArgs(),
+                    )
+                    for member, docstring in zip(documented_members, docstrings)
+                ]
+            )
+
+            member = members[0]
+            tplargs = ", ".join(
+                [
+                    d["type"] + " " + d["name"]
+                    for d in self.template_params + member.template_params
+                ]
+            )
+            output.out(
+                template_member_func_doc.format(
+                    template=(
+                        "template <{}>\n".format(tplargs) if len(tplargs) > 0 else ""
+                    ),
+                    rettype=member.s_rettype(),
+                    classname_prefix=classname_prefix,
+                    argsstring=member.s_prototypeArgs(),
+                    body=body,
+                )
+            )
+
+            # Write argnamesstrings
+            body = "".join(
+                [
+                    template_member_func_args_body.format(
+                        classname_prefix=classname_prefix,
+                        membername=member.s_name(),
+                        args=argnamesstring,
+                        rettype=member.s_rettype(),
+                        argsstring=member.s_prototypeArgs(),
+                    )
+                    for member, argnamesstring in zip(
+                        documented_members, argnamesstrings
+                    )
+                ]
+            )
+
+            n_args = member.n_args()
+
+            default_args = ", ".join(
+                [
+                    """boost::python::arg("self")""",
+                ]
+                + ["""boost::python::arg("arg{}")""".format(i) for i in range(n_args)]
+            )
+            output.out(
+                template_member_func_args.format(
+                    template=(
+                        "template <{}>\n".format(tplargs) if len(tplargs) > 0 else ""
+                    ),
+                    rettype=member.s_rettype(),
+                    n=n_args + 1,
+                    default_args=default_args,
+                    classname_prefix=classname_prefix,
+                    argsstring=member.s_prototypeArgs(),
+                    body=body,
+                )
+            )
+
+        output.out(template_close_namespace.format(namespace="doxygen"))
+        output.close()
+
+    def _attribute(self, member):
+        self.attributes.append(member)
+
+
+class Index:
+    """
+    This class is responsible for generating the list of all C++-usable documented elements.
+    """
+
+    def __init__(self, input, output):
+        self.tree = etree.parse(input)
+        self.directory = path.dirname(input)
+        self.xml_docstring = XmlDocString(self)
+        self.compounds = list()
+        self.references = dict()
+        self.output = output
+
+    def parseCompound(self):
+        for compound in self.tree.getroot().iterchildren("compound"):
+            if compound.attrib["kind"] in ["class", "struct"]:
+                obj = ClassCompound(compound, self)
+            elif compound.attrib["kind"] == "namespace":
+                obj = NamespaceCompound(compound, self)
+            if obj.id not in self.compounds:
+                self.compounds.append(obj.id)
+            self.registerReference(obj)
+
+    def write(self):
+        # Header
+
+        self.output.open("doxygen_xml_parser_for_cmake.hh")
+        # self.output.out ("// Generated on {}".format (asctime()))
+        self.output.close()
+
+        # Implement template specialization for classes and member functions
+        for id in self.compounds:
+            compound = self.references[id]
+            compound.write(self.output)
+
+        self.output.open("functions.h")
+
+        # Implement template specialization for static functions
+        static_funcs = dict()
+        prototypes = list()
+        includes = list()
+        for id in self.compounds:
+            compound = self.references[id]
+            for m in compound.static_funcs:
+                include = m.include()
+                if include not in includes:
+                    includes.append(include)
+                docstring = m.s_docstring()
+                if len(docstring) == 0:
+                    continue
+                prototype = m.prototypekey()
+                if prototype in static_funcs:
+                    static_funcs[prototype].append((m, docstring))
+                else:
+                    static_funcs[prototype] = [
+                        (m, docstring),
+                    ]
+                    prototypes.append(prototype)
+
+        self.output.out(
+            "".join(
+                [
+                    template_include_extern.format(filename=filename)
+                    for filename in includes
+                ]
+            )
+        )
+
+        self.output.out(template_open_namespace.format(namespace="doxygen"))
+
+        for prototype in prototypes:
+            member_and_docstring_s = static_funcs[prototype]
+            body = "".join(
+                [
+                    template_static_func_doc_body.format(
+                        namespace=member.parent.innerNamespace(),
+                        membername=member.s_name(),
+                        docstring=docstring,
+                        rettype=member.s_rettype(),
+                        argsstring=member.s_prototypeArgs(),
+                    )
+                    for member, docstring in member_and_docstring_s
+                ]
+            )
+
+            member = member_and_docstring_s[0][0]
+            # TODO fix case of static method in templated class.
+            tplargs = ", ".join(
+                [
+                    d["type"] + " " + d["name"]
+                    for d in member.parent.template_params + member.template_params
+                ]
+            )
+            self.output.out(
+                template_static_func_doc.format(
+                    template=(
+                        "template <{}>\n".format(tplargs) if len(tplargs) > 0 else ""
+                    ),
+                    rettype=member.s_rettype(),
+                    argsstring=member.s_prototypeArgs(),
+                    body=body,
+                )
+            )
+
+        self.output.out(template_close_namespace.format(namespace="doxygen"))
+        self.output.close()
+
+    def registerReference(self, obj, overwrite=True):
+        if obj.id in self.references:
+            if obj.name != self.references[obj.id].name:
+                self.output.warn(
+                    "!!!! Compounds " + obj.id + " already exists.",
+                    obj.name,
+                    self.references[obj.id].name,
+                )
+            else:
+                self.output.warn("Reference " + obj.id + " already exists.", obj.name)
+            if not overwrite:
+                return
+        self.references[obj.id] = obj
+
+    def hasref(self, id):
+        return id in self.references
+
+    def getref(self, id):
+        return self.references[id]
+
+
+class OutputStreams(object):
+    def __init__(self, output_dir, warn, error, errorPrefix=""):
+        self.output_dir = output_dir
+        self._out = None
+        self._warn = warn
+        self._err = error
+        self.errorPrefix = errorPrefix
+
+        self._created_files = dict()
+
+    def open(self, name):
+        assert self._out is None, "You did not close the previous file"
+        import os
+
+        fullname = os.path.join(self.output_dir, name)
+        dirname = os.path.dirname(fullname)
+        if not os.path.isdir(dirname):
+            os.makedirs(dirname)
+
+        if name in self._created_files:
+            self._out = self._created_files[name]
+        else:
+            import codecs
+
+            if sys.version_info >= (3,):
+                encoding = "utf-8"
+            else:
+                encoding = "latin1"
+            self._out = codecs.open(fullname, mode="w", encoding=encoding)
+            self._created_files[name] = self._out
+
+            # Header
+            self.out(
+                template_file_header.format(
+                    path=os.path.dirname(os.path.abspath(__file__)),
+                    header_guard=makeHeaderGuard(name),
+                )
+            )
+
+    def close(self):
+        self._out = None
+
+    def writeFooterAndCloseFiles(self):
+        for n, f in self._created_files.items():
+            # Footer
+            self._out = f
+            self.out(
+                template_file_footer.format(
+                    header_guard=makeHeaderGuard(n),
+                )
+            )
+            f.close()
+        self._created_files.clear()
+        self._out = None
+
+    def out(self, *args):
+        if sys.version_info >= (3,):
+            print(*args, file=self._out)
+        else:
+            print(" ".join(str(arg) for arg in args).decode("latin1"), file=self._out)
+
+    def warn(self, *args):
+        print(self.errorPrefix, *args, file=self._warn)
+
+    def err(self, *args):
+        print(self.errorPrefix, *args, file=self._err)
+
+
+if __name__ == "__main__":
+    import argparse
+
+    parser = argparse.ArgumentParser(
+        description="Process Doxygen XML documentation and generate C++ code."
+    )
+    parser.add_argument("doxygen_index_xml", type=str, help="the Doxygen XML index.")
+    parser.add_argument("output_directory", type=str, help="the output directory.")
+    args = parser.parse_args()
+
+    index = Index(
+        input=sys.argv[1],
+        output=OutputStreams(args.output_directory, sys.stdout, sys.stderr),
+    )
+    index.parseCompound()
+    index.write()
+    index.output.writeFooterAndCloseFiles()
+    assert index.output._out is None

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

@@ -4,33 +4,33 @@ digraph CD  {
 	compound=true
         size = 11.7
 
-        "BVHShapeCollider<OBBRSS, Shape, NarrowPhaseSolver>::collide\n(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f& 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 Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f&tf2,\nNarrowPhaseSolver* nsolver, const CollisionRequest& request,\nCollisionResult& result)\ncollision_func_matrix.cpp" [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" [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,\nFCL_REAL& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp" [shape = box]
-        "virtual bool CollisionTraversalNodeBase::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const = 0\ntraversal/traversal_node_base.h" [shape = box]
-        "virtual void CollisionTraversalNodeBase::leafTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\ntraversal/traversal_node_base.h" [shape = box]
-        "bool MeshShapeCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [shape = box]
-        "bool MeshShapeCollisionTraversalNodeOBBRSS::leafTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [shape = box]
-        "bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1,\nconst OBBRSS& b2, const CollisionRequest& request,\nFCL_REAL& sqrDistLowerBound)\nBV/OBBRSS.cpp" [shape = box]
-        "bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2,\nconst CollisionRequest& request, FCL_REAL& sqrDistLowerBound)\nBV/OBB.cpp" [shape = box]
-        "bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,\nconst Vec3f& a, const Vec3f& b,\nconst CollisionRequest& request,\nFCL_REAL& squaredLowerBoundDistance)\nBV/OBB.cpp" [shape = box]
-        "void details::meshShapeCollisionOrientedNodeLeafTesting <OBBRSS, Shape, NarrowPhaseSolver>\n(int b1, int b2, const BVHModel<BV>* model1, const S& model2,\nVec3f* vertices, Triangle* tri_indices, const Transform3f& tf1,\nconst Transform3f& tf2, const NarrowPhaseSolver* nsolver,\nbool enable_statistics, int& num_leaf_tests,\nconst CollisionRequest& request, CollisionResult& result,\nFCL_REAL& sqrDistLowerBound)\ntraversal/traversal_node_bvh_shape.h:293" [shape = box]
-        "bool GJKSolver_indep::shapeTriangleIntersect<Shape>\n(const S& s, const Transform3f& tf,\nconst Vec3f& P1, const Vec3f& P2, const Vec3f& P3,\nFCL_REAL* distance, Vec3f* p1, Vec3f* p2) const\nnarrowphase/narrowphase.h:156" [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 Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f& 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 Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f&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 Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f&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"
+        "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,\nFCL_REAL& 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,\nFCL_REAL& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp"
-        "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nFCL_REAL& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp" -> "virtual bool CollisionTraversalNodeBase::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const = 0\ntraversal/traversal_node_base.h"
-        "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nFCL_REAL& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp" -> "virtual void CollisionTraversalNodeBase::leafTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\ntraversal/traversal_node_base.h"
-        "virtual bool CollisionTraversalNodeBase::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const = 0\ntraversal/traversal_node_base.h" -> "bool MeshShapeCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [color=red]
-        "virtual void CollisionTraversalNodeBase::leafTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\ntraversal/traversal_node_base.h" -> "bool MeshShapeCollisionTraversalNodeOBBRSS::leafTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [color = red]
-        "bool MeshShapeCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" -> "bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1,\nconst OBBRSS& b2, const CollisionRequest& request,\nFCL_REAL& sqrDistLowerBound)\nBV/OBBRSS.cpp"
-        "bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1,\nconst OBBRSS& b2, const CollisionRequest& request,\nFCL_REAL& sqrDistLowerBound)\nBV/OBBRSS.cpp" -> "bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2,\nconst CollisionRequest& request, FCL_REAL& sqrDistLowerBound)\nBV/OBB.cpp"
-        "bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2,\nconst CollisionRequest& request, FCL_REAL& sqrDistLowerBound)\nBV/OBB.cpp" -> "bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,\nconst Vec3f& a, const Vec3f& b,\nconst CollisionRequest& request,\nFCL_REAL& squaredLowerBoundDistance)\nBV/OBB.cpp"
-        "bool MeshShapeCollisionTraversalNodeOBBRSS::leafTesting\n(int b1, int b2, FCL_REAL& 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,\nVec3f* vertices, Triangle* tri_indices, const Transform3f& tf1,\nconst Transform3f& tf2, const NarrowPhaseSolver* nsolver,\nbool enable_statistics, int& num_leaf_tests,\nconst CollisionRequest& request, CollisionResult& result,\nFCL_REAL& 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,\nVec3f* vertices, Triangle* tri_indices, const Transform3f& tf1,\nconst Transform3f& tf2, const NarrowPhaseSolver* nsolver,\nbool enable_statistics, int& num_leaf_tests,\nconst CollisionRequest& request, CollisionResult& result,\nFCL_REAL& sqrDistLowerBound)\ntraversal/traversal_node_bvh_shape.h:293" -> "bool GJKSolver_indep::shapeTriangleIntersect<Shape>\n(const S& s, const Transform3f& tf,\nconst Vec3f& P1, const Vec3f& P2, const Vec3f& P3,\nFCL_REAL* distance, Vec3f* p1, Vec3f* p2) const\nnarrowphase/narrowphase.h:156"
+        "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

@@ -4,16 +4,16 @@ digraph CD  {
 	compound=true
         size = 11.7
 
-        "template<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>\nstd::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f& tf2,\nconst NarrowPhaseSolver* nsolver,const CollisionRequest& request,\nCollisionResult& result)" [shape = box]
-        "template<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>\nFCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f& tf2,\nconst NarrowPhaseSolver* nsolver, const DistanceRequest& request,\nDistanceResult& result)" [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)" [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 Transform3f& tf1, const S2& s2, const Transform3f& tf2,\nFCL_REAL* distance, Vec3f* p1, Vec3f* p2) const\nnarrowphase/narrowphase.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 Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f& tf2,\nconst NarrowPhaseSolver* nsolver,const CollisionRequest& request,\nCollisionResult& result)" -> "template<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>\nFCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f& tf2,\nconst NarrowPhaseSolver* nsolver, const DistanceRequest& request,\nDistanceResult& result)"
-        "template<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>\nFCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f& tf2,\nconst NarrowPhaseSolver* nsolver, const DistanceRequest& request,\nDistanceResult& result)" -> "void distance(DistanceTraversalNodeBase* node,\nBVHFrontList* front_list, int qsize)"
+        "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 Transform3f& tf1, const S2& s2, const Transform3f& tf2,\nFCL_REAL* distance, Vec3f* p1, Vec3f* p2) const\nnarrowphase/narrowphase.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"
 }

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/coal/BV/AABB.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Jia Pan */
+
+#ifndef COAL_AABB_H
+#define COAL_AABB_H
+
+#include "coal/data_types.h"
+
+namespace coal {
+
+struct CollisionRequest;
+class Plane;
+class Halfspace;
+
+/// @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
+  Vec3s min_;
+  /// @brief The max point in the AABB
+  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 Vec3s& v) : min_(v), max_(v) {}
+
+  /// @brief Creating an AABB with two endpoints a and 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 Vec3s& delta)
+      : min_(core.min_ - delta), max_(core.max_ + delta) {}
+
+  /// @brief Creating an AABB contains three points
+  AABB(const Vec3s& a, const Vec3s& b, const Vec3s& c)
+      : min_(a.cwiseMin(b).cwiseMin(c)), max_(a.cwiseMax(b).cwiseMax(c)) {}
+
+  AABB(const AABB& other) = default;
+
+  AABB& operator=(const AABB& other) = default;
+
+  AABB& update(const Vec3s& a, const Vec3s& b) {
+    min_ = a.cwiseMin(b);
+    max_ = a.cwiseMax(b);
+    return *this;
+  }
+
+  /// @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); }
+
+  /// @name Bounding volume API
+  /// Common API to BVs.
+  /// @{
+
+  /// @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
+  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 AABB overlaps a halfspace
+  bool overlap(const Halfspace& hs) const;
+
+  /// @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 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_ = 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 {
+    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 CoalScalar width() const { return max_[0] - min_[0]; }
+
+  /// @brief Height of the AABB
+  inline CoalScalar height() const { return max_[1] - min_[1]; }
+
+  /// @brief Depth of the AABB
+  inline CoalScalar depth() const { return max_[2] - min_[2]; }
+
+  /// @brief Volume of the AABB
+  inline CoalScalar volume() const { return width() * height() * depth(); }
+
+  /// @}
+
+  /// @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 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_ = min_.cwiseMax(other.min_);
+    overlap_part.max_ = max_.cwiseMin(other.max_);
+    return true;
+  }
+
+  /// @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;
+
+    return true;
+  }
+
+  /// @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, 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 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

include/coal/BV/BV.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Jia Pan */
+
+#ifndef COAL_BV_H
+#define COAL_BV_H
+
+#include "coal/BV/kDOP.h"
+#include "coal/BV/AABB.h"
+#include "coal/BV/OBB.h"
+#include "coal/BV/RSS.h"
+#include "coal/BV/OBBRSS.h"
+#include "coal/BV/kIOS.h"
+#include "coal/math/transform.h"
+
+/** @brief Main namespace */
+namespace coal {
+
+/// @cond IGNORE
+namespace details {
+
+/// @brief Convert a bounding volume of type BV1 in configuration tf1 to a
+/// bounding volume of type BV2 in I configuration.
+template <typename BV1, typename BV2>
+struct Converter {
+  static void convert(const BV1& bv1, const Transform3s& tf1, BV2& bv2);
+  static void convert(const BV1& bv1, BV2& bv2);
+};
+
+/// @brief Convert from AABB to AABB, not very tight but is fast.
+template <>
+struct Converter<AABB, AABB> {
+  static void convert(const AABB& bv1, const Transform3s& tf1, AABB& bv2) {
+    const Vec3s& center = bv1.center();
+    CoalScalar r = (bv1.max_ - bv1.min_).norm() * 0.5;
+    const Vec3s center2 = tf1.transform(center);
+    bv2.min_ = center2 - Vec3s::Constant(r);
+    bv2.max_ = center2 + Vec3s::Constant(r);
+  }
+
+  static void convert(const AABB& bv1, AABB& bv2) { bv2 = bv1; }
+};
+
+template <>
+struct Converter<AABB, OBB> {
+  static void convert(const AABB& bv1, const Transform3s& tf1, OBB& bv2) {
+    bv2.To = tf1.transform(bv1.center());
+    bv2.extent.noalias() = (bv1.max_ - bv1.min_) * 0.5;
+    bv2.axes = tf1.getRotation();
+  }
+
+  static void convert(const AABB& bv1, OBB& bv2) {
+    bv2.To = bv1.center();
+    bv2.extent.noalias() = (bv1.max_ - bv1.min_) * 0.5;
+    bv2.axes.setIdentity();
+  }
+};
+
+template <>
+struct Converter<OBB, OBB> {
+  static void convert(const OBB& bv1, const Transform3s& tf1, OBB& bv2) {
+    bv2.extent = bv1.extent;
+    bv2.To = tf1.transform(bv1.To);
+    bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
+  }
+
+  static void convert(const OBB& bv1, OBB& bv2) { bv2 = bv1; }
+};
+
+template <>
+struct Converter<OBBRSS, OBB> {
+  static void convert(const OBBRSS& bv1, const Transform3s& tf1, OBB& bv2) {
+    Converter<OBB, OBB>::convert(bv1.obb, tf1, bv2);
+  }
+
+  static void convert(const OBBRSS& bv1, OBB& bv2) {
+    Converter<OBB, OBB>::convert(bv1.obb, bv2);
+  }
+};
+
+template <>
+struct Converter<RSS, OBB> {
+  static void convert(const RSS& bv1, const Transform3s& tf1, OBB& bv2) {
+    bv2.extent = Vec3s(bv1.length[0] * 0.5 + bv1.radius,
+                       bv1.length[1] * 0.5 + bv1.radius, bv1.radius);
+    bv2.To = tf1.transform(bv1.Tr);
+    bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
+  }
+
+  static void convert(const RSS& bv1, OBB& bv2) {
+    bv2.extent = Vec3s(bv1.length[0] * 0.5 + bv1.radius,
+                       bv1.length[1] * 0.5 + bv1.radius, bv1.radius);
+    bv2.To = bv1.Tr;
+    bv2.axes = bv1.axes;
+  }
+};
+
+template <typename BV1>
+struct Converter<BV1, AABB> {
+  static void convert(const BV1& bv1, const Transform3s& tf1, AABB& bv2) {
+    const Vec3s& center = bv1.center();
+    CoalScalar r = Vec3s(bv1.width(), bv1.height(), bv1.depth()).norm() * 0.5;
+    const Vec3s center2 = tf1.transform(center);
+    bv2.min_ = center2 - Vec3s::Constant(r);
+    bv2.max_ = center2 + Vec3s::Constant(r);
+  }
+
+  static void convert(const BV1& bv1, AABB& bv2) {
+    const Vec3s& center = bv1.center();
+    CoalScalar r = Vec3s(bv1.width(), bv1.height(), bv1.depth()).norm() * 0.5;
+    bv2.min_ = center - Vec3s::Constant(r);
+    bv2.max_ = center + Vec3s::Constant(r);
+  }
+};
+
+template <typename BV1>
+struct Converter<BV1, OBB> {
+  static void convert(const BV1& bv1, const Transform3s& tf1, OBB& bv2) {
+    AABB bv;
+    Converter<BV1, AABB>::convert(bv1, bv);
+    Converter<AABB, OBB>::convert(bv, tf1, bv2);
+  }
+
+  static void convert(const BV1& bv1, OBB& bv2) {
+    AABB bv;
+    Converter<BV1, AABB>::convert(bv1, bv);
+    Converter<AABB, OBB>::convert(bv, bv2);
+  }
+};
+
+template <>
+struct Converter<OBB, RSS> {
+  static void convert(const OBB& bv1, const Transform3s& tf1, RSS& bv2) {
+    bv2.Tr = tf1.transform(bv1.To);
+    bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
+
+    bv2.radius = bv1.extent[2];
+    bv2.length[0] = 2 * (bv1.extent[0] - bv2.radius);
+    bv2.length[1] = 2 * (bv1.extent[1] - bv2.radius);
+  }
+
+  static void convert(const OBB& bv1, RSS& bv2) {
+    bv2.Tr = bv1.To;
+    bv2.axes = bv1.axes;
+
+    bv2.radius = bv1.extent[2];
+    bv2.length[0] = 2 * (bv1.extent[0] - bv2.radius);
+    bv2.length[1] = 2 * (bv1.extent[1] - bv2.radius);
+  }
+};
+
+template <>
+struct Converter<RSS, RSS> {
+  static void convert(const RSS& bv1, const Transform3s& tf1, RSS& bv2) {
+    bv2.Tr = tf1.transform(bv1.Tr);
+    bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
+
+    bv2.radius = bv1.radius;
+    bv2.length[0] = bv1.length[0];
+    bv2.length[1] = bv1.length[1];
+  }
+
+  static void convert(const RSS& bv1, RSS& bv2) { bv2 = bv1; }
+};
+
+template <>
+struct Converter<OBBRSS, RSS> {
+  static void convert(const OBBRSS& bv1, const Transform3s& tf1, RSS& bv2) {
+    Converter<RSS, RSS>::convert(bv1.rss, tf1, bv2);
+  }
+
+  static void convert(const OBBRSS& bv1, RSS& bv2) {
+    Converter<RSS, RSS>::convert(bv1.rss, bv2);
+  }
+};
+
+template <>
+struct Converter<AABB, RSS> {
+  static void convert(const AABB& bv1, const Transform3s& tf1, RSS& bv2) {
+    bv2.Tr = tf1.transform(bv1.center());
+
+    /// Sort the AABB edges so that AABB extents are ordered.
+    CoalScalar d[3] = {bv1.width(), bv1.height(), bv1.depth()};
+    Eigen::DenseIndex id[3] = {0, 1, 2};
+
+    for (Eigen::DenseIndex i = 1; i < 3; ++i) {
+      for (Eigen::DenseIndex j = i; j > 0; --j) {
+        if (d[j] > d[j - 1]) {
+          {
+            CoalScalar tmp = d[j];
+            d[j] = d[j - 1];
+            d[j - 1] = tmp;
+          }
+          {
+            Eigen::DenseIndex tmp = id[j];
+            id[j] = id[j - 1];
+            id[j - 1] = tmp;
+          }
+        }
+      }
+    }
+
+    const Vec3s extent = (bv1.max_ - bv1.min_) * 0.5;
+    bv2.radius = extent[id[2]];
+    bv2.length[0] = (extent[id[0]] - bv2.radius) * 2;
+    bv2.length[1] = (extent[id[1]] - bv2.radius) * 2;
+
+    const Matrix3s& R = tf1.getRotation();
+    const bool left_hand = (id[0] == (id[1] + 1) % 3);
+    if (left_hand)
+      bv2.axes.col(0) = -R.col(id[0]);
+    else
+      bv2.axes.col(0) = R.col(id[0]);
+    bv2.axes.col(1) = R.col(id[1]);
+    bv2.axes.col(2) = R.col(id[2]);
+  }
+
+  static void convert(const AABB& bv1, RSS& bv2) {
+    convert(bv1, Transform3s(), bv2);
+  }
+};
+
+template <>
+struct Converter<AABB, OBBRSS> {
+  static void convert(const AABB& bv1, const Transform3s& tf1, OBBRSS& bv2) {
+    Converter<AABB, OBB>::convert(bv1, tf1, bv2.obb);
+    Converter<AABB, RSS>::convert(bv1, tf1, bv2.rss);
+  }
+
+  static void convert(const AABB& bv1, OBBRSS& bv2) {
+    Converter<AABB, OBB>::convert(bv1, bv2.obb);
+    Converter<AABB, RSS>::convert(bv1, bv2.rss);
+  }
+};
+
+}  // namespace details
+
+/// @endcond
+
+/// @brief Convert a bounding volume of type BV1 in configuration tf1 to
+/// bounding volume of type BV2 in identity configuration.
+template <typename BV1, typename BV2>
+static inline void convertBV(const BV1& bv1, const Transform3s& tf1, BV2& bv2) {
+  details::Converter<BV1, BV2>::convert(bv1, tf1, bv2);
+}
+
+/// @brief Convert a bounding volume of type BV1 to bounding volume of type BV2
+/// in identity configuration.
+template <typename BV1, typename BV2>
+static inline void convertBV(const BV1& bv1, BV2& bv2) {
+  details::Converter<BV1, BV2>::convert(bv1, bv2);
+}
+
+}  // namespace coal
+
+#endif

include/coal/BV/BV_node.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Jia Pan */
+
+#ifndef COAL_BV_NODE_H
+#define COAL_BV_NODE_H
+
+#include "coal/data_types.h"
+#include "coal/BV/BV.h"
+
+namespace coal {
+
+/// @defgroup Construction_Of_BVH Construction of BVHModel
+/// Classes which are used to build a BVHModel (Bounding Volume Hierarchy)
+/// @{
+
+/// @brief BVNodeBase encodes the tree structure for BVH
+struct COAL_DLLAPI BVNodeBase {
+  /// @brief An index for first child node or primitive
+  /// If the value is positive, it is the index of the first child bv node
+  /// If the value is negative, it is -(primitive index + 1)
+  /// Zero is not used.
+  int first_child;
+
+  /// @brief The start id the primitive belonging to the current node. The index
+  /// is referred to the primitive_indices in BVHModel and from that we can
+  /// obtain the primitive's index in original data indirectly.
+  unsigned int first_primitive;
+
+  /// @brief The number of primitives belonging to the current node
+  unsigned int num_primitives;
+
+  /// @brief Default constructor
+  BVNodeBase()
+      : first_child(0),
+        first_primitive(
+            (std::numeric_limits<unsigned int>::max)())  // value we should help
+                                                         // to raise an issue
+        ,
+        num_primitives(0) {}
+
+  /// @brief Equality operator
+  bool operator==(const BVNodeBase& other) const {
+    return first_child == other.first_child &&
+           first_primitive == other.first_primitive &&
+           num_primitives == other.num_primitives;
+  }
+
+  /// @brief Difference operator
+  bool operator!=(const BVNodeBase& other) const { return !(*this == other); }
+
+  /// @brief Whether current node is a leaf node (i.e. contains a primitive
+  /// index
+  inline bool isLeaf() const { return first_child < 0; }
+
+  /// @brief Return the primitive index. The index is referred to the original
+  /// data (i.e. vertices or tri_indices) in BVHModel
+  inline int primitiveId() const { return -(first_child + 1); }
+
+  /// @brief Return the index of the first child. The index is referred to the
+  /// bounding volume array (i.e. bvs) in BVHModel
+  inline int leftChild() const { return first_child; }
+
+  /// @brief Return the index of the second child. The index is referred to the
+  /// bounding volume array (i.e. bvs) in BVHModel
+  inline int rightChild() const { return first_child + 1; }
+};
+
+/// @brief A class describing a bounding volume node. It includes the tree
+/// structure providing in BVNodeBase and also the geometry data provided in BV
+/// template parameter.
+template <typename BV>
+struct COAL_DLLAPI BVNode : public BVNodeBase {
+  typedef BVNodeBase Base;
+
+  /// @brief bounding volume storing the geometry
+  BV bv;
+
+  /// @brief Equality operator
+  bool operator==(const BVNode& other) const {
+    return Base::operator==(other) && bv == other.bv;
+  }
+
+  /// @brief Difference operator
+  bool operator!=(const BVNode& other) const { return !(*this == other); }
+
+  /// @brief Check whether two BVNode collide
+  bool overlap(const BVNode& other) const { return bv.overlap(other.bv); }
+  /// @brief Check whether two BVNode collide
+  bool overlap(const BVNode& other, const CollisionRequest& request,
+               CoalScalar& sqrDistLowerBound) const {
+    return bv.overlap(other.bv, request, sqrDistLowerBound);
+  }
+
+  /// @brief Compute the distance between two BVNode. P1 and P2, if not NULL and
+  /// the underlying BV supports distance, return the nearest points.
+  CoalScalar distance(const BVNode& other, Vec3s* P1 = NULL,
+                      Vec3s* P2 = NULL) const {
+    return bv.distance(other.bv, P1, P2);
+  }
+
+  /// @brief Access to the center of the BV
+  Vec3s getCenter() const { return bv.center(); }
+
+  /// @brief Access to the orientation of the BV
+  const Matrix3s& getOrientation() const {
+    static const Matrix3s id3 = Matrix3s::Identity();
+    return id3;
+  }
+
+  /// \cond
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  /// \endcond
+};
+
+template <>
+inline const Matrix3s& BVNode<OBB>::getOrientation() const {
+  return bv.axes;
+}
+
+template <>
+inline const Matrix3s& BVNode<RSS>::getOrientation() const {
+  return bv.axes;
+}
+
+template <>
+inline const Matrix3s& BVNode<OBBRSS>::getOrientation() const {
+  return bv.obb.axes;
+}
+
+}  // namespace coal
+
+#endif

include/coal/BV/OBB.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Jia Pan */
+
+#ifndef COAL_OBB_H
+#define COAL_OBB_H
+
+#include "coal/data_types.h"
+
+namespace coal {
+
+struct CollisionRequest;
+
+/// @addtogroup Bounding_Volume
+/// @{
+
+/// @brief Oriented bounding box class
+struct COAL_DLLAPI OBB {
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  /// @brief Orientation of OBB. axis[i] is the ith column of the orientation
+  /// matrix for the box; it is also the i-th principle direction of the box. We
+  /// assume that axis[0] corresponds to the axis with the longest box edge,
+  /// axis[1] corresponds to the shorter one and axis[2] corresponds to the
+  /// shortest one.
+  Matrix3s axes;
+
+  /// @brief Center of OBB
+  Vec3s To;
+
+  /// @brief Half dimensions of OBB
+  Vec3s extent;
+
+  OBB() : axes(Matrix3s::Zero()), To(Vec3s::Zero()), extent(Vec3s::Zero()) {}
+
+  /// @brief Equality operator
+  bool operator==(const OBB& other) const {
+    return axes == other.axes && To == other.To && extent == other.extent;
+  }
+
+  /// @brief Difference operator
+  bool operator!=(const OBB& other) const { return !(*this == other); }
+
+  /// @brief Check whether the OBB contains a point.
+  bool contain(const Vec3s& p) const;
+
+  /// Check collision between two OBB
+  /// @return true if collision happens.
+  bool overlap(const OBB& other) const;
+
+  /// Check collision between two OBB
+  /// @return true if collision happens.
+  /// @retval sqrDistLowerBound squared lower bound on distance between boxes if
+  ///         they do not overlap.
+  bool overlap(const OBB& other, const CollisionRequest& request,
+               CoalScalar& sqrDistLowerBound) const;
+
+  /// @brief Distance between two OBBs, not implemented.
+  CoalScalar distance(const OBB& other, Vec3s* P = NULL, Vec3s* Q = NULL) const;
+
+  /// @brief A simple way to merge the OBB and a point (the result is not
+  /// compact).
+  OBB& operator+=(const Vec3s& p);
+
+  /// @brief Merge the OBB and another OBB (the result is not compact).
+  OBB& operator+=(const OBB& other) {
+    *this = *this + other;
+    return *this;
+  }
+
+  /// @brief Return the merged OBB of current OBB and the other one (the result
+  /// is not compact).
+  OBB operator+(const OBB& other) const;
+
+  /// @brief Size of the OBB (used in BV_Splitter to order two OBBs)
+  inline CoalScalar size() const { return extent.squaredNorm(); }
+
+  /// @brief Center of the OBB
+  inline const Vec3s& center() const { return To; }
+
+  /// @brief Width of the OBB.
+  inline CoalScalar width() const { return 2 * extent[0]; }
+
+  /// @brief Height of the OBB.
+  inline CoalScalar height() const { return 2 * extent[1]; }
+
+  /// @brief Depth of the OBB
+  inline CoalScalar depth() const { return 2 * extent[2]; }
+
+  /// @brief Volume of the OBB
+  inline CoalScalar volume() const { return width() * height() * depth(); }
+};
+
+/// @brief Translate the OBB bv
+COAL_DLLAPI OBB translate(const OBB& bv, const Vec3s& t);
+
+/// @brief Check collision between two obbs, b1 is in configuration (R0, T0) and
+/// b2 is in identity.
+COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1,
+                         const OBB& b2);
+
+/// @brief Check collision between two obbs, b1 is in configuration (R0, T0) and
+/// b2 is in identity.
+COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1,
+                         const OBB& b2, const CollisionRequest& request,
+                         CoalScalar& sqrDistLowerBound);
+
+/// Check collision between two boxes
+/// @param B, T orientation and position of first box,
+/// @param a half dimensions of first box,
+/// @param b half dimensions of second box.
+/// The second box is in identity configuration.
+COAL_DLLAPI bool obbDisjoint(const Matrix3s& B, const Vec3s& T, const Vec3s& a,
+                             const Vec3s& b);
+}  // namespace coal
+
+#endif

include/coal/BV/OBBRSS.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Jia Pan */
+
+#ifndef COAL_OBBRSS_H
+#define COAL_OBBRSS_H
+
+#include "coal/BV/OBB.h"
+#include "coal/BV/RSS.h"
+
+namespace coal {
+
+struct CollisionRequest;
+
+/// @addtogroup Bounding_Volume
+/// @{
+
+/// @brief Class merging the OBB and RSS, can handle collision and distance
+/// simultaneously
+struct COAL_DLLAPI OBBRSS {
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  /// @brief OBB member, for rotation
+  OBB obb;
+
+  /// @brief RSS member, for distance
+  RSS rss;
+
+  /// @brief Equality operator
+  bool operator==(const OBBRSS& other) const {
+    return obb == other.obb && rss == other.rss;
+  }
+
+  /// @brief Difference operator
+  bool operator!=(const OBBRSS& other) const { return !(*this == other); }
+
+  /// @brief Check whether the OBBRSS contains a point
+  inline bool contain(const Vec3s& p) const { return obb.contain(p); }
+
+  /// @brief Check collision between two OBBRSS
+  bool overlap(const OBBRSS& other) const { return obb.overlap(other.obb); }
+
+  /// Check collision between two OBBRSS
+  /// @retval sqrDistLowerBound squared lower bound on distance between
+  ///         objects if they do not overlap.
+  bool overlap(const OBBRSS& other, const CollisionRequest& request,
+               CoalScalar& sqrDistLowerBound) const {
+    return obb.overlap(other.obb, request, sqrDistLowerBound);
+  }
+
+  /// @brief Distance between two OBBRSS; P and Q , is not NULL, returns the
+  /// nearest points
+  CoalScalar distance(const OBBRSS& other, Vec3s* P = NULL,
+                      Vec3s* Q = NULL) const {
+    return rss.distance(other.rss, P, Q);
+  }
+
+  /// @brief Merge the OBBRSS and a point
+  OBBRSS& operator+=(const Vec3s& p) {
+    obb += p;
+    rss += p;
+    return *this;
+  }
+
+  /// @brief Merge two OBBRSS
+  OBBRSS& operator+=(const OBBRSS& other) {
+    *this = *this + other;
+    return *this;
+  }
+
+  /// @brief Merge two OBBRSS
+  OBBRSS operator+(const OBBRSS& other) const {
+    OBBRSS result;
+    result.obb = obb + other.obb;
+    result.rss = rss + other.rss;
+    return result;
+  }
+
+  /// @brief Size of the OBBRSS (used in BV_Splitter to order two OBBRSS)
+  inline CoalScalar size() const { return obb.size(); }
+
+  /// @brief Center of the OBBRSS
+  inline const Vec3s& center() const { return obb.center(); }
+
+  /// @brief Width of the OBRSS
+  inline CoalScalar width() const { return obb.width(); }
+
+  /// @brief Height of the OBBRSS
+  inline CoalScalar height() const { return obb.height(); }
+
+  /// @brief Depth of the OBBRSS
+  inline CoalScalar depth() const { return obb.depth(); }
+
+  /// @brief Volume of the OBBRSS
+  inline CoalScalar volume() const { return obb.volume(); }
+};
+
+/// @brief Check collision between two OBBRSS, b1 is in configuration (R0, T0)
+/// and b2 is in indentity
+inline bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBBRSS& b1,
+                    const OBBRSS& b2) {
+  return overlap(R0, T0, b1.obb, b2.obb);
+}
+
+/// Check collision between two OBBRSS
+/// @param  R0, T0 configuration of b1
+/// @param  b1 first OBBRSS in configuration (R0, T0)
+/// @param  b2 second OBBRSS in identity position
+/// @retval sqrDistLowerBound squared lower bound on the distance if OBBRSS do
+/// not overlap.
+inline bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBBRSS& b1,
+                    const OBBRSS& b2, const CollisionRequest& request,
+                    CoalScalar& sqrDistLowerBound) {
+  return overlap(R0, T0, b1.obb, b2.obb, request, sqrDistLowerBound);
+}
+
+/// @brief Computate distance between two OBBRSS, b1 is in configuation (R0, T0)
+/// and b2 is in indentity; P and Q, is not NULL, returns the nearest points
+inline CoalScalar distance(const Matrix3s& R0, const Vec3s& T0,
+                           const OBBRSS& b1, const OBBRSS& b2, Vec3s* P = NULL,
+                           Vec3s* Q = NULL) {
+  return distance(R0, T0, b1.rss, b2.rss, P, Q);
+}
+
+}  // namespace coal
+
+#endif

include/coal/BV/RSS.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Jia Pan */
+
+#ifndef COAL_RSS_H
+#define COAL_RSS_H
+
+#include "coal/data_types.h"
+
+#include <boost/math/constants/constants.hpp>
+
+namespace coal {
+
+struct CollisionRequest;
+
+/// @addtogroup Bounding_Volume
+/// @{
+
+/// @brief A class for rectangle sphere-swept bounding volume
+struct COAL_DLLAPI RSS {
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  /// @brief Orientation of RSS. axis[i] is the ith column of the orientation
+  /// matrix for the RSS; it is also the i-th principle direction of the RSS. We
+  /// assume that axis[0] corresponds to the axis with the longest length,
+  /// axis[1] corresponds to the shorter one and axis[2] corresponds to the
+  /// shortest one.
+  Matrix3s axes;
+
+  /// @brief Origin of the rectangle in RSS
+  Vec3s Tr;
+
+  /// @brief Side lengths of rectangle
+  CoalScalar length[2];
+
+  /// @brief Radius of sphere summed with rectangle to form RSS
+  CoalScalar radius;
+
+  ///  @brief Default constructor with default values
+  RSS() : axes(Matrix3s::Zero()), Tr(Vec3s::Zero()), radius(-1) {
+    length[0] = 0;
+    length[1] = 0;
+  }
+
+  /// @brief Equality operator
+  bool operator==(const RSS& other) const {
+    return axes == other.axes && Tr == other.Tr &&
+           length[0] == other.length[0] && length[1] == other.length[1] &&
+           radius == other.radius;
+  }
+
+  /// @brief Difference operator
+  bool operator!=(const RSS& other) const { return !(*this == other); }
+
+  /// @brief Check whether the RSS contains a point
+  bool contain(const Vec3s& p) const;
+
+  /// @brief Check collision between two RSS
+  bool overlap(const RSS& other) const;
+
+  /// Not implemented
+  bool overlap(const RSS& other, const CollisionRequest&,
+               CoalScalar& sqrDistLowerBound) const {
+    sqrDistLowerBound = sqrt(-1);
+    return overlap(other);
+  }
+
+  /// @brief the distance between two RSS; P and Q, if not NULL, return the
+  /// nearest points
+  CoalScalar distance(const RSS& other, Vec3s* P = NULL, Vec3s* Q = NULL) const;
+
+  /// @brief A simple way to merge the RSS and a point, not compact.
+  /// @todo This function may have some bug.
+  RSS& operator+=(const Vec3s& p);
+
+  /// @brief Merge the RSS and another RSS
+  inline RSS& operator+=(const RSS& other) {
+    *this = *this + other;
+    return *this;
+  }
+
+  /// @brief Return the merged RSS of current RSS and the other one
+  RSS operator+(const RSS& other) const;
+
+  /// @brief Size of the RSS (used in BV_Splitter to order two RSSs)
+  inline CoalScalar size() const {
+    return (std::sqrt(length[0] * length[0] + length[1] * length[1]) +
+            2 * radius);
+  }
+
+  /// @brief The RSS center
+  inline const Vec3s& center() const { return Tr; }
+
+  /// @brief Width of the RSS
+  inline CoalScalar width() const { return length[0] + 2 * radius; }
+
+  /// @brief Height of the RSS
+  inline CoalScalar height() const { return length[1] + 2 * radius; }
+
+  /// @brief Depth of the RSS
+  inline CoalScalar depth() const { return 2 * radius; }
+
+  /// @brief Volume of the RSS
+  inline CoalScalar volume() const {
+    return (length[0] * length[1] * 2 * radius +
+            4 * boost::math::constants::pi<CoalScalar>() * radius * radius *
+                radius);
+  }
+
+  /// @brief Check collision between two RSS and return the overlap part.
+  /// For RSS, we return nothing, as the overlap part of two RSSs usually is not
+  /// a RSS.
+  bool overlap(const RSS& other, RSS& /*overlap_part*/) const {
+    return overlap(other);
+  }
+};
+
+/// @brief distance between two RSS bounding volumes
+/// P and Q (optional return values) are the closest points in the rectangles,
+/// not the RSS. But the direction P - Q is the correct direction for cloest
+/// points Notice that P and Q are both in the local frame of the first RSS (not
+/// global frame and not even the local frame of object 1)
+COAL_DLLAPI CoalScalar distance(const Matrix3s& R0, const Vec3s& T0,
+                                const RSS& b1, const RSS& b2, Vec3s* P = NULL,
+                                Vec3s* Q = NULL);
+
+/// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and
+/// b2 is in identity.
+COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const RSS& b1,
+                         const RSS& b2);
+
+/// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and
+/// b2 is in identity.
+COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const RSS& b1,
+                         const RSS& b2, const CollisionRequest& request,
+                         CoalScalar& sqrDistLowerBound);
+
+}  // namespace coal
+
+#endif

include/coal/BV/kDOP.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Jia Pan */
+
+#ifndef COAL_KDOP_H
+#define COAL_KDOP_H
+
+#include "coal/fwd.hh"
+#include "coal/data_types.h"
+
+namespace coal {
+
+struct CollisionRequest;
+
+/// @addtogroup Bounding_Volume
+/// @{
+
+/// @brief KDOP class describes the KDOP collision structures. K is set as the
+/// template parameter, which should be 16, 18, or 24
+///  The KDOP structure is defined by some pairs of parallel planes defined by
+///  some axes.
+/// For K = 16, the planes are 6 AABB planes and 10 diagonal planes that cut off
+/// some space of the edges:
+/// (-1,0,0) and (1,0,0)  -> indices 0 and 8
+/// (0,-1,0) and (0,1,0)  -> indices 1 and 9
+/// (0,0,-1) and (0,0,1)  -> indices 2 and 10
+/// (-1,-1,0) and (1,1,0) -> indices 3 and 11
+/// (-1,0,-1) and (1,0,1) -> indices 4 and 12
+/// (0,-1,-1) and (0,1,1) -> indices 5 and 13
+/// (-1,1,0) and (1,-1,0) -> indices 6 and 14
+/// (-1,0,1) and (1,0,-1) -> indices 7 and 15
+/// For K = 18, the planes are 6 AABB planes and 12 diagonal planes that cut off
+/// some space of the edges:
+/// (-1,0,0) and (1,0,0)  -> indices 0 and 9
+/// (0,-1,0) and (0,1,0)  -> indices 1 and 10
+/// (0,0,-1) and (0,0,1)  -> indices 2 and 11
+/// (-1,-1,0) and (1,1,0) -> indices 3 and 12
+/// (-1,0,-1) and (1,0,1) -> indices 4 and 13
+/// (0,-1,-1) and (0,1,1) -> indices 5 and 14
+/// (-1,1,0) and (1,-1,0) -> indices 6 and 15
+/// (-1,0,1) and (1,0,-1) -> indices 7 and 16
+/// (0,-1,1) and (0,1,-1) -> indices 8 and 17
+/// For K = 18, the planes are 6 AABB planes and 18 diagonal planes that cut off
+/// some space of the edges:
+/// (-1,0,0) and (1,0,0)  -> indices 0 and 12
+/// (0,-1,0) and (0,1,0)  -> indices 1 and 13
+/// (0,0,-1) and (0,0,1)  -> indices 2 and 14
+/// (-1,-1,0) and (1,1,0) -> indices 3 and 15
+/// (-1,0,-1) and (1,0,1) -> indices 4 and 16
+/// (0,-1,-1) and (0,1,1) -> indices 5 and 17
+/// (-1,1,0) and (1,-1,0) -> indices 6 and 18
+/// (-1,0,1) and (1,0,-1) -> indices 7 and 19
+/// (0,-1,1) and (0,1,-1) -> indices 8 and 20
+/// (-1, -1, 1) and (1, 1, -1) --> indices 9 and 21
+/// (-1, 1, -1) and (1, -1, 1) --> indices 10 and 22
+/// (1, -1, -1) and (-1, 1, 1) --> indices 11 and 23
+template <short N>
+class COAL_DLLAPI KDOP {
+ protected:
+  /// @brief Origin's distances to N KDOP planes
+  Eigen::Array<CoalScalar, N, 1> dist_;
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  /// @brief Creating kDOP containing nothing
+  KDOP();
+
+  /// @brief Creating kDOP containing only one point
+  KDOP(const Vec3s& v);
+
+  /// @brief Creating kDOP containing two points
+  KDOP(const Vec3s& a, const Vec3s& b);
+
+  /// @brief Equality operator
+  bool operator==(const KDOP& other) const {
+    return (dist_ == other.dist_).all();
+  }
+
+  /// @brief Difference operator
+  bool operator!=(const KDOP& other) const {
+    return (dist_ != other.dist_).any();
+  }
+
+  /// @brief Check whether two KDOPs overlap.
+  bool overlap(const KDOP<N>& other) const;
+
+  /// @brief Check whether two KDOPs overlap.
+  /// @return true if collision happens.
+  /// @retval sqrDistLowerBound squared lower bound on distance between boxes if
+  ///         they do not overlap.
+  bool overlap(const KDOP<N>& other, const CollisionRequest& request,
+               CoalScalar& sqrDistLowerBound) const;
+
+  /// @brief The distance between two KDOP<N>. Not implemented.
+  CoalScalar distance(const KDOP<N>& other, Vec3s* P = NULL,
+                      Vec3s* Q = NULL) const;
+
+  /// @brief Merge the point and the KDOP
+  KDOP<N>& operator+=(const Vec3s& p);
+
+  /// @brief Merge two KDOPs
+  KDOP<N>& operator+=(const KDOP<N>& other);
+
+  /// @brief Create a KDOP by mergin two KDOPs
+  KDOP<N> operator+(const KDOP<N>& other) const;
+
+  /// @brief Size of the kDOP (used in BV_Splitter to order two kDOPs)
+  inline CoalScalar size() const {
+    return width() * width() + height() * height() + depth() * depth();
+  }
+
+  /// @brief The (AABB) center
+  inline Vec3s center() const {
+    return (dist_.template head<3>() + dist_.template segment<3>(N / 2)) / 2;
+  }
+
+  /// @brief The (AABB) width
+  inline CoalScalar width() const { return dist_[N / 2] - dist_[0]; }
+
+  /// @brief The (AABB) height
+  inline CoalScalar height() const { return dist_[N / 2 + 1] - dist_[1]; }
+
+  /// @brief The (AABB) depth
+  inline CoalScalar depth() const { return dist_[N / 2 + 2] - dist_[2]; }
+
+  /// @brief The (AABB) volume
+  inline CoalScalar volume() const { return width() * height() * depth(); }
+
+  inline CoalScalar dist(short i) const { return dist_[i]; }
+
+  inline CoalScalar& dist(short i) { return dist_[i]; }
+
+  //// @brief Check whether one point is inside the KDOP
+  bool inside(const Vec3s& p) const;
+};
+
+template <short N>
+bool overlap(const Matrix3s& /*R0*/, const Vec3s& /*T0*/, const KDOP<N>& /*b1*/,
+             const KDOP<N>& /*b2*/) {
+  COAL_THROW_PRETTY("not implemented", std::logic_error);
+}
+
+template <short N>
+bool overlap(const Matrix3s& /*R0*/, const Vec3s& /*T0*/, const KDOP<N>& /*b1*/,
+             const KDOP<N>& /*b2*/, const CollisionRequest& /*request*/,
+             CoalScalar& /*sqrDistLowerBound*/) {
+  COAL_THROW_PRETTY("not implemented", std::logic_error);
+}
+
+/// @brief translate the KDOP BV
+template <short N>
+COAL_DLLAPI KDOP<N> translate(const KDOP<N>& bv, const Vec3s& t);
+
+}  // namespace coal
+
+#endif

include/coal/BV/kIOS.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Jia Pan */
+
+#ifndef COAL_KIOS_H
+#define COAL_KIOS_H
+
+#include "coal/BV/OBB.h"
+
+namespace coal {
+
+struct CollisionRequest;
+
+/// @addtogroup Bounding_Volume
+/// @{
+
+/// @brief A class describing the kIOS collision structure, which is a set of
+/// spheres.
+class COAL_DLLAPI kIOS {
+  /// @brief One sphere in kIOS
+  struct COAL_DLLAPI kIOS_Sphere {
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+    Vec3s o;
+    CoalScalar r;
+
+    bool operator==(const kIOS_Sphere& other) const {
+      return o == other.o && r == other.r;
+    }
+
+    bool operator!=(const kIOS_Sphere& other) const {
+      return !(*this == other);
+    }
+  };
+
+  /// @brief generate one sphere enclosing two spheres
+  static kIOS_Sphere encloseSphere(const kIOS_Sphere& s0,
+                                   const kIOS_Sphere& s1) {
+    Vec3s d = s1.o - s0.o;
+    CoalScalar dist2 = d.squaredNorm();
+    CoalScalar diff_r = s1.r - s0.r;
+
+    /** The sphere with the larger radius encloses the other */
+    if (diff_r * diff_r >= dist2) {
+      if (s1.r > s0.r)
+        return s1;
+      else
+        return s0;
+    } else /** spheres partially overlapping or disjoint */
+    {
+      float dist = (float)std::sqrt(dist2);
+      kIOS_Sphere s;
+      s.r = dist + s0.r + s1.r;
+      if (dist > 0)
+        s.o = s0.o + d * ((s.r - s0.r) / dist);
+      else
+        s.o = s0.o;
+      return s;
+    }
+  }
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  /// @brief Equality operator
+  bool operator==(const kIOS& other) const {
+    bool res = obb == other.obb && num_spheres == other.num_spheres;
+    if (!res) return false;
+
+    for (size_t k = 0; k < num_spheres; ++k) {
+      if (spheres[k] != other.spheres[k]) return false;
+    }
+
+    return true;
+  }
+
+  /// @brief Difference operator
+  bool operator!=(const kIOS& other) const { return !(*this == other); }
+
+  static constexpr size_t max_num_spheres = 5;
+
+  /// @brief The (at most) five spheres for intersection
+  kIOS_Sphere spheres[max_num_spheres];
+
+  /// @brief The number of spheres, no larger than 5
+  unsigned int num_spheres;
+
+  /// @ OBB related with kIOS
+  OBB obb;
+
+  /// @brief Check whether the kIOS contains a point
+  bool contain(const Vec3s& p) const;
+
+  /// @brief Check collision between two kIOS
+  bool overlap(const kIOS& other) const;
+
+  /// @brief Check collision between two kIOS
+  bool overlap(const kIOS& other, const CollisionRequest&,
+               CoalScalar& sqrDistLowerBound) const;
+
+  /// @brief The distance between two kIOS
+  CoalScalar distance(const kIOS& other, Vec3s* P = NULL,
+                      Vec3s* Q = NULL) const;
+
+  /// @brief A simple way to merge the kIOS and a point
+  kIOS& operator+=(const Vec3s& p);
+
+  /// @brief Merge the kIOS and another kIOS
+  kIOS& operator+=(const kIOS& other) {
+    *this = *this + other;
+    return *this;
+  }
+
+  /// @brief Return the merged kIOS of current kIOS and the other one
+  kIOS operator+(const kIOS& other) const;
+
+  /// @brief size of the kIOS (used in BV_Splitter to order two kIOSs)
+  CoalScalar size() const;
+
+  /// @brief Center of the kIOS
+  const Vec3s& center() const { return spheres[0].o; }
+
+  /// @brief Width of the kIOS
+  CoalScalar width() const;
+
+  /// @brief Height of the kIOS
+  CoalScalar height() const;
+
+  /// @brief Depth of the kIOS
+  CoalScalar depth() const;
+
+  /// @brief Volume of the kIOS
+  CoalScalar volume() const;
+};
+
+/// @brief Translate the kIOS BV
+COAL_DLLAPI kIOS translate(const kIOS& bv, const Vec3s& t);
+
+/// @brief Check collision between two kIOSs, b1 is in configuration (R0, T0)
+/// and b2 is in identity.
+/// @todo Not efficient
+COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const kIOS& b1,
+                         const kIOS& b2);
+
+/// @brief Check collision between two kIOSs, b1 is in configuration (R0, T0)
+/// and b2 is in identity.
+/// @todo Not efficient
+COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const kIOS& b1,
+                         const kIOS& b2, const CollisionRequest& request,
+                         CoalScalar& sqrDistLowerBound);
+
+/// @brief Approximate distance between two kIOS bounding volumes
+/// @todo P and Q is not returned, need implementation
+COAL_DLLAPI CoalScalar distance(const Matrix3s& R0, const Vec3s& T0,
+                                const kIOS& b1, const kIOS& b2, Vec3s* P = NULL,
+                                Vec3s* Q = NULL);
+
+}  // namespace coal
+
+#endif

include/coal/BVH/BVH_front.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Jia Pan */
+
+#ifndef COAL_BVH_FRONT_H
+#define COAL_BVH_FRONT_H
+
+#include <list>
+
+#include "coal/config.hh"
+
+namespace coal {
+
+/// @brief Front list acceleration for collision
+/// Front list is a set of internal and leaf nodes in the BVTT hierarchy, where
+/// the traversal terminates while performing a query during a given time
+/// instance. The front list reflects the subset of a BVTT that is traversed for
+/// that particular proximity query.
+struct COAL_DLLAPI BVHFrontNode {
+  /// @brief The nodes to start in the future, i.e. the wave front of the
+  /// traversal tree.
+  unsigned int left, right;
+
+  /// @brief The front node is not valid when collision is detected on the front
+  /// node.
+  bool valid;
+
+  BVHFrontNode(unsigned int left_, unsigned int right_)
+      : left(left_), right(right_), valid(true) {}
+};
+
+/// @brief BVH front list is a list of front nodes.
+typedef std::list<BVHFrontNode> BVHFrontList;
+
+/// @brief Add new front node into the front list
+inline void updateFrontList(BVHFrontList* front_list, unsigned int b1,
+                            unsigned int b2) {
+  if (front_list) front_list->push_back(BVHFrontNode(b1, b2));
+}
+
+}  // namespace coal
+
+#endif

test/eigen.cpp → include/coal/BVH/BVH_internal.h

@@ -33,83 +33,55 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  */
 
-#define BOOST_TEST_MODULE FCL_EIGEN
-#define BOOST_TEST_DYN_LINK
-#include <boost/test/unit_test.hpp>
-#include <boost/utility/binary.hpp>
-
-#include <hpp/fcl/eigen/vec_3fx.h>
-
-using namespace hpp::fcl;
-
-#define PRINT_VECTOR(a) std::cout << #a": " << a.base().transpose() << std::endl;
-#define PRINT_MATRIX(a) std::cout << #a": " << a << std::endl;
-
-BOOST_AUTO_TEST_CASE(fcl_eigen_vec3fx)
-{
-  typedef Eigen::FclMatrix <double, 1, 0> Vec3f;
-  Vec3f a (0, 1, 2);
-  Vec3f b (1, 2, 3);
-
-  std::cout << (Vec3f::Base&) a - (Vec3f::Base&) b << std::endl;
-  std::cout << a - b << std::endl;
-  Vec3f c = a - b;
-  std::cout << c << std::endl;
-  c.normalize ();
-
-  Vec3f l = (a - b).normalize ();
-
-  std::cout << l << std::endl;
-  std::cout << c << std::endl;
-
-  Vec3f d = -b;
-  std::cout << d << std::endl;
-
-  d += b;
-  std::cout << d << std::endl;
-
-  d += 1;
-  std::cout << d << std::endl;
-
-  d *= b;
-  std::cout << d << std::endl;
-
-  // std::cout << d * b << std::endl;
-  // std::cout << d / b << std::endl;
-  // std::cout << d + 3.4 << std::endl;
-  // std::cout << d - 3.4 << std::endl;
-  // std::cout << d * 2 << std::endl;
-  // std::cout << d / 3 << std::endl;
-  // std::cout << (d - 3.4).abs().squaredNorm() << std::endl;
-
-  // std::cout << (((d - 3.4).abs() + 1) + 3).squaredNorm() << std::endl;
-  PRINT_VECTOR(a)
-  PRINT_VECTOR(b)
-  PRINT_VECTOR(min(a,b))
-  PRINT_VECTOR(max(a,b))
-}
-
-BOOST_AUTO_TEST_CASE(fcl_eigen_matrix3fx)
-{
-  typedef Eigen::FclMatrix <double, 3, 0> Matrix3fX;
-  Matrix3fX a (0, 1, 2, 3, 4, 5, 6, 7, 8);
-
-  PRINT_MATRIX(a);
-  a.transpose ();
-  PRINT_MATRIX(a);
-  a += a;
-  PRINT_MATRIX(a);
-  a *= a;
-  PRINT_MATRIX(a);
-
-  Matrix3fX b = a.inverse();
-  a += a + a * b;
-  PRINT_MATRIX(a);
-
-  b = a.inverse();
-  a.transpose ();
-  PRINT_MATRIX(a);
-  PRINT_MATRIX(a.transposeTimes (b));
-  PRINT_MATRIX(a.timesTranspose (b));
-  PRINT_MATRIX(a.tensorTransform (b));
-}
+/** \author Jia Pan */
+
+#ifndef COAL_BVH_INTERNAL_H
+#define COAL_BVH_INTERNAL_H
+
+#include "coal/data_types.h"
+
+namespace coal {
+
+/// @brief States for BVH construction
+/// empty->begun->processed ->replace_begun->processed -> ......
+///                        |
+///                        |-> update_begun -> updated -> .....
+enum BVHBuildState {
+  BVH_BUILD_STATE_EMPTY,      /// empty state, immediately after constructor
+  BVH_BUILD_STATE_BEGUN,      /// after beginModel(), state for adding geometry
+                              /// primitives
+  BVH_BUILD_STATE_PROCESSED,  /// after tree has been build, ready for cd use
+  BVH_BUILD_STATE_UPDATE_BEGUN,  /// after beginUpdateModel(), state for
+                                 /// updating geometry primitives
+  BVH_BUILD_STATE_UPDATED,  /// after tree has been build for updated geometry,
+                            /// ready for ccd use
+  BVH_BUILD_STATE_REPLACE_BEGUN  /// after beginReplaceModel(), state for
+                                 /// replacing geometry primitives
+};
+
+/// @brief Error code for BVH
+enum BVHReturnCode {
+  BVH_OK = 0,  /// BVH is valid
+  BVH_ERR_MODEL_OUT_OF_MEMORY =
+      -1,  /// Cannot allocate memory for vertices and triangles
+  BVH_ERR_BUILD_OUT_OF_SEQUENCE =
+      -2,  /// BVH construction does not follow correct sequence
+  BVH_ERR_BUILD_EMPTY_MODEL = -3,  /// BVH geometry is not prepared
+  BVH_ERR_BUILD_EMPTY_PREVIOUS_FRAME =
+      -4,  /// BVH geometry in previous frame is not prepared
+  BVH_ERR_UNSUPPORTED_FUNCTION = -5,  /// BVH funtion is not supported
+  BVH_ERR_UNUPDATED_MODEL = -6,       /// BVH model update failed
+  BVH_ERR_INCORRECT_DATA = -7,        /// BVH data is not valid
+  BVH_ERR_UNKNOWN = -8                /// Unknown failure
+};
+
+/// @brief BVH model type
+enum BVHModelType {
+  BVH_MODEL_UNKNOWN,    /// @brief unknown model type
+  BVH_MODEL_TRIANGLES,  /// @brief triangle model
+  BVH_MODEL_POINTCLOUD  /// @brief point cloud model
+};
+
+}  // namespace coal
+
+#endif

include/coal/BVH/BVH_model.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, Open Source Robotics Foundation
+ *  Copyright (c) 2020-2022, 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_BVH_MODEL_H
+#define COAL_BVH_MODEL_H
+
+#include "coal/fwd.hh"
+#include "coal/collision_object.h"
+#include "coal/BVH/BVH_internal.h"
+#include "coal/BV/BV_node.h"
+
+#include <vector>
+#include <memory>
+#include <iostream>
+
+namespace coal {
+
+/// @addtogroup Construction_Of_BVH
+/// @{
+
+class ConvexBase;
+
+template <typename BV>
+class BVFitter;
+template <typename BV>
+class BVSplitter;
+
+/// @brief A base class describing the bounding hierarchy of a mesh model or a
+/// point cloud model (which is viewed as a degraded version of mesh)
+class COAL_DLLAPI BVHModelBase : public CollisionGeometry {
+ public:
+  /// @brief Geometry point data
+  std::shared_ptr<std::vector<Vec3s>> vertices;
+
+  /// @brief Geometry triangle index data, will be NULL for point clouds
+  std::shared_ptr<std::vector<Triangle>> tri_indices;
+
+  /// @brief Geometry point data in previous frame
+  std::shared_ptr<std::vector<Vec3s>> prev_vertices;
+
+  /// @brief Number of triangles
+  unsigned int num_tris;
+
+  /// @brief Number of points
+  unsigned int num_vertices;
+
+  /// @brief The state of BVH building process
+  BVHBuildState build_state;
+
+  /// @brief Convex<Triangle> representation of this object
+  shared_ptr<ConvexBase> convex;
+
+  /// @brief Model type described by the instance
+  BVHModelType getModelType() const {
+    if (num_tris && num_vertices)
+      return BVH_MODEL_TRIANGLES;
+    else if (num_vertices)
+      return BVH_MODEL_POINTCLOUD;
+    else
+      return BVH_MODEL_UNKNOWN;
+  }
+
+  /// @brief Constructing an empty BVH
+  BVHModelBase();
+
+  /// @brief copy from another BVH
+  BVHModelBase(const BVHModelBase& other);
+
+  /// @brief deconstruction, delete mesh data related.
+  virtual ~BVHModelBase() {}
+
+  /// @brief Get the object type: it is a BVH
+  OBJECT_TYPE getObjectType() const { return OT_BVH; }
+
+  /// @brief Compute the AABB for the BVH, used for broad-phase collision
+  void computeLocalAABB();
+
+  /// @brief Begin a new BVH model
+  int beginModel(unsigned int num_tris = 0, unsigned int num_vertices = 0);
+
+  /// @brief Add one point in the new BVH model
+  int addVertex(const Vec3s& p);
+
+  /// @brief Add points in the new BVH model
+  int addVertices(const MatrixX3s& points);
+
+  /// @brief Add triangles in the new BVH model
+  int addTriangles(const Matrixx3i& triangles);
+
+  /// @brief Add one triangle in the new BVH model
+  int addTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3);
+
+  /// @brief Add a set of triangles in the new BVH model
+  int addSubModel(const std::vector<Vec3s>& ps,
+                  const std::vector<Triangle>& ts);
+
+  /// @brief Add a set of points in the new BVH model
+  int addSubModel(const std::vector<Vec3s>& ps);
+
+  /// @brief End BVH model construction, will build the bounding volume
+  /// hierarchy
+  int endModel();
+
+  /// @brief Replace the geometry information of current frame (i.e. should have
+  /// the same mesh topology with the previous frame)
+  int beginReplaceModel();
+
+  /// @brief Replace one point in the old BVH model
+  int replaceVertex(const Vec3s& p);
+
+  /// @brief Replace one triangle in the old BVH model
+  int replaceTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3);
+
+  /// @brief Replace a set of points in the old BVH model
+  int replaceSubModel(const std::vector<Vec3s>& ps);
+
+  /// @brief End BVH model replacement, will also refit or rebuild the bounding
+  /// volume hierarchy
+  int endReplaceModel(bool refit = true, bool bottomup = true);
+
+  /// @brief Replace the geometry information of current frame (i.e. should have
+  /// the same mesh topology with the previous frame). The current frame will be
+  /// saved as the previous frame in prev_vertices.
+  int beginUpdateModel();
+
+  /// @brief Update one point in the old BVH model
+  int updateVertex(const Vec3s& p);
+
+  /// @brief Update one triangle in the old BVH model
+  int updateTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3);
+
+  /// @brief Update a set of points in the old BVH model
+  int updateSubModel(const std::vector<Vec3s>& ps);
+
+  /// @brief End BVH model update, will also refit or rebuild the bounding
+  /// volume hierarchy
+  int endUpdateModel(bool refit = true, bool bottomup = true);
+
+  /// @brief Build this \ref Convex "Convex<Triangle>" representation of this
+  /// model. The result is stored in attribute \ref convex. \note this only
+  /// takes the points of this model. It does not check that the
+  ///       object is convex. It does not compute a convex hull.
+  void buildConvexRepresentation(bool share_memory);
+
+  /// @brief Build a convex hull
+  /// and store it in attribute \ref convex.
+  /// \param keepTriangle whether the convex should be triangulated.
+  /// \param qhullCommand see \ref ConvexBase::convexHull.
+  /// \return \c true if this object is convex, hence the convex hull represents
+  ///         the same object.
+  /// \sa ConvexBase::convexHull
+  /// \warning At the moment, the return value only checks whether there are as
+  ///          many points in the convex hull as in the original object. This is
+  ///          neither necessary (duplicated vertices get merged) nor sufficient
+  ///          (think of a U with 4 vertices and 3 edges).
+  bool buildConvexHull(bool keepTriangle, const char* qhullCommand = NULL);
+
+  virtual int memUsage(const bool msg = false) const = 0;
+
+  /// @brief This is a special acceleration: BVH_model default stores the BV's
+  /// transform in world coordinate. However, we can also store each BV's
+  /// transform related to its parent BV node. When traversing the BVH, this can
+  /// save one matrix transformation.
+  virtual void makeParentRelative() = 0;
+
+  Vec3s computeCOM() const {
+    CoalScalar vol = 0;
+    Vec3s com(0, 0, 0);
+    if (!(vertices.get())) {
+      std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain "
+                   "vertices."
+                << std::endl;
+      return com;
+    }
+    const std::vector<Vec3s>& vertices_ = *vertices;
+    if (!(tri_indices.get())) {
+      std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain "
+                   "triangles."
+                << std::endl;
+      return com;
+    }
+    const std::vector<Triangle>& tri_indices_ = *tri_indices;
+
+    for (unsigned int i = 0; i < num_tris; ++i) {
+      const Triangle& tri = tri_indices_[i];
+      CoalScalar d_six_vol =
+          (vertices_[tri[0]].cross(vertices_[tri[1]])).dot(vertices_[tri[2]]);
+      vol += d_six_vol;
+      com += (vertices_[tri[0]] + vertices_[tri[1]] + vertices_[tri[2]]) *
+             d_six_vol;
+    }
+
+    return com / (vol * 4);
+  }
+
+  CoalScalar computeVolume() const {
+    CoalScalar vol = 0;
+    if (!(vertices.get())) {
+      std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain "
+                   "vertices."
+                << std::endl;
+      return vol;
+    }
+    const std::vector<Vec3s>& vertices_ = *vertices;
+    if (!(tri_indices.get())) {
+      std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain "
+                   "triangles."
+                << std::endl;
+      return vol;
+    }
+    const std::vector<Triangle>& tri_indices_ = *tri_indices;
+    for (unsigned int i = 0; i < num_tris; ++i) {
+      const Triangle& tri = tri_indices_[i];
+      CoalScalar d_six_vol =
+          (vertices_[tri[0]].cross(vertices_[tri[1]])).dot(vertices_[tri[2]]);
+      vol += d_six_vol;
+    }
+
+    return vol / 6;
+  }
+
+  Matrix3s computeMomentofInertia() const {
+    Matrix3s C = Matrix3s::Zero();
+
+    Matrix3s C_canonical;
+    C_canonical << 1 / 60.0, 1 / 120.0, 1 / 120.0, 1 / 120.0, 1 / 60.0,
+        1 / 120.0, 1 / 120.0, 1 / 120.0, 1 / 60.0;
+
+    if (!(vertices.get())) {
+      std::cerr << "BVH Error in `computeMomentofInertia`! The BVHModel does "
+                   "not contain vertices."
+                << std::endl;
+      return C;
+    }
+    const std::vector<Vec3s>& vertices_ = *vertices;
+    if (!(vertices.get())) {
+      std::cerr << "BVH Error in `computeMomentofInertia`! The BVHModel does "
+                   "not contain vertices."
+                << std::endl;
+      return C;
+    }
+    const std::vector<Triangle>& tri_indices_ = *tri_indices;
+    for (unsigned int i = 0; i < num_tris; ++i) {
+      const Triangle& tri = tri_indices_[i];
+      const Vec3s& v1 = vertices_[tri[0]];
+      const Vec3s& v2 = vertices_[tri[1]];
+      const Vec3s& v3 = vertices_[tri[2]];
+      Matrix3s A;
+      A << v1.transpose(), v2.transpose(), v3.transpose();
+      C += A.derived().transpose() * C_canonical * A * (v1.cross(v2)).dot(v3);
+    }
+
+    return C.trace() * Matrix3s::Identity() - C;
+  }
+
+ protected:
+  virtual void deleteBVs() = 0;
+  virtual bool allocateBVs() = 0;
+
+  /// @brief Build the bounding volume hierarchy
+  virtual int buildTree() = 0;
+
+  /// @brief Refit the bounding volume hierarchy
+  virtual int refitTree(bool bottomup) = 0;
+
+  unsigned int num_tris_allocated;
+  unsigned int num_vertices_allocated;
+  unsigned int num_vertex_updated;  /// for ccd vertex update
+
+ protected:
+  /// \brief Comparison operators
+  virtual bool isEqual(const CollisionGeometry& other) const;
+};
+
+/// @brief A class describing the bounding hierarchy of a mesh model or a point
+/// cloud model (which is viewed as a degraded version of mesh) \tparam BV one
+/// of the bounding volume class in \ref Bounding_Volume.
+template <typename BV>
+class COAL_DLLAPI BVHModel : public BVHModelBase {
+  typedef BVHModelBase Base;
+
+ public:
+  using bv_node_vector_t =
+      std::vector<BVNode<BV>, Eigen::aligned_allocator<BVNode<BV>>>;
+
+  /// @brief Split rule to split one BV node into two children
+  shared_ptr<BVSplitter<BV>> bv_splitter;
+
+  /// @brief Fitting rule to fit a BV node to a set of geometry primitives
+  shared_ptr<BVFitter<BV>> bv_fitter;
+
+  /// @brief Default constructor to build an empty BVH
+  BVHModel();
+
+  /// @brief Copy constructor from another BVH
+  ///
+  /// \param[in] other BVHModel to copy.
+  ///
+  BVHModel(const BVHModel& other);
+
+  /// @brief Clone *this into a new BVHModel
+  virtual BVHModel<BV>* clone() const { return new BVHModel(*this); }
+
+  /// @brief deconstruction, delete mesh data related.
+  ~BVHModel() {}
+
+  /// @brief We provide getBV() and getNumBVs() because BVH may be compressed
+  /// (in future), so we must provide some flexibility here
+
+  /// @brief Access the bv giving the its index
+  const BVNode<BV>& getBV(unsigned int i) const {
+    assert(i < num_bvs);
+    return (*bvs)[i];
+  }
+
+  /// @brief Access the bv giving the its index
+  BVNode<BV>& getBV(unsigned int i) {
+    assert(i < num_bvs);
+    return (*bvs)[i];
+  }
+
+  /// @brief Get the number of bv in the BVH
+  unsigned int getNumBVs() const { return num_bvs; }
+
+  /// @brief Get the BV type: default is unknown
+  NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
+
+  /// @brief Check the number of memory used
+  int memUsage(const bool msg) const;
+
+  /// @brief This is a special acceleration: BVH_model default stores the BV's
+  /// transform in world coordinate. However, we can also store each BV's
+  /// transform related to its parent BV node. When traversing the BVH, this can
+  /// save one matrix transformation.
+  void makeParentRelative() {
+    Matrix3s I(Matrix3s::Identity());
+    makeParentRelativeRecurse(0, I, Vec3s::Zero());
+  }
+
+ protected:
+  void deleteBVs();
+  bool allocateBVs();
+
+  unsigned int num_bvs_allocated;
+  std::shared_ptr<std::vector<unsigned int>> primitive_indices;
+
+  /// @brief Bounding volume hierarchy
+  std::shared_ptr<bv_node_vector_t> bvs;
+
+  /// @brief Number of BV nodes in bounding volume hierarchy
+  unsigned int num_bvs;
+
+  /// @brief Build the bounding volume hierarchy
+  int buildTree();
+
+  /// @brief Refit the bounding volume hierarchy
+  int refitTree(bool bottomup);
+
+  /// @brief Refit the bounding volume hierarchy in a top-down way (slow but
+  /// more compact)
+  int refitTree_topdown();
+
+  /// @brief Refit the bounding volume hierarchy in a bottom-up way (fast but
+  /// less compact)
+  int refitTree_bottomup();
+
+  /// @brief Recursive kernel for hierarchy construction
+  int recursiveBuildTree(int bv_id, unsigned int first_primitive,
+                         unsigned int num_primitives);
+
+  /// @brief Recursive kernel for bottomup refitting
+  int recursiveRefitTree_bottomup(int bv_id);
+
+  /// @ recursively compute each bv's transform related to its parent. For
+  /// default BV, only the translation works. For oriented BV (OBB, RSS,
+  /// OBBRSS), special implementation is provided.
+  void makeParentRelativeRecurse(int bv_id, Matrix3s& parent_axes,
+                                 const Vec3s& parent_c) {
+    bv_node_vector_t& bvs_ = *bvs;
+    if (!bvs_[static_cast<size_t>(bv_id)].isLeaf()) {
+      makeParentRelativeRecurse(bvs_[static_cast<size_t>(bv_id)].first_child,
+                                parent_axes,
+                                bvs_[static_cast<size_t>(bv_id)].getCenter());
+
+      makeParentRelativeRecurse(
+          bvs_[static_cast<size_t>(bv_id)].first_child + 1, parent_axes,
+          bvs_[static_cast<size_t>(bv_id)].getCenter());
+    }
+
+    bvs_[static_cast<size_t>(bv_id)].bv =
+        translate(bvs_[static_cast<size_t>(bv_id)].bv, -parent_c);
+  }
+
+ private:
+  virtual bool isEqual(const CollisionGeometry& _other) const {
+    const BVHModel* other_ptr = dynamic_cast<const BVHModel*>(&_other);
+    if (other_ptr == nullptr) return false;
+    const BVHModel& other = *other_ptr;
+
+    bool res = Base::isEqual(other);
+    if (!res) return false;
+
+    // unsigned int other_num_primitives = 0;
+    // if(other.primitive_indices)
+    // {
+
+    //   switch(other.getModelType())
+    //   {
+    //     case BVH_MODEL_TRIANGLES:
+    //       other_num_primitives = num_tris;
+    //       break;
+    //     case BVH_MODEL_POINTCLOUD:
+    //       other_num_primitives = num_vertices;
+    //       break;
+    //     default:
+    //       ;
+    //   }
+    // }
+
+    //    unsigned int num_primitives = 0;
+    //    if(primitive_indices)
+    //    {
+    //
+    //      switch(other.getModelType())
+    //      {
+    //        case BVH_MODEL_TRIANGLES:
+    //          num_primitives = num_tris;
+    //          break;
+    //        case BVH_MODEL_POINTCLOUD:
+    //          num_primitives = num_vertices;
+    //          break;
+    //        default:
+    //          ;
+    //      }
+    //    }
+    //
+    //    if(num_primitives != other_num_primitives)
+    //      return false;
+    //
+    //    for(int k = 0; k < num_primitives; ++k)
+    //    {
+    //      if(primitive_indices[k] != other.primitive_indices[k])
+    //        return false;
+    //    }
+
+    if (num_bvs != other.num_bvs) return false;
+
+    if ((!(bvs.get()) && other.bvs.get()) || (bvs.get() && !(other.bvs.get())))
+      return false;
+    if (bvs.get() && other.bvs.get()) {
+      const bv_node_vector_t& bvs_ = *bvs;
+      const bv_node_vector_t& other_bvs_ = *(other.bvs);
+      for (unsigned int k = 0; k < num_bvs; ++k) {
+        if (bvs_[k] != other_bvs_[k]) return false;
+      }
+    }
+
+    return true;
+  }
+};
+
+/// @}
+
+template <>
+void BVHModel<OBB>::makeParentRelativeRecurse(int bv_id, Matrix3s& parent_axes,
+                                              const Vec3s& parent_c);
+
+template <>
+void BVHModel<RSS>::makeParentRelativeRecurse(int bv_id, Matrix3s& parent_axes,
+                                              const Vec3s& parent_c);
+
+template <>
+void BVHModel<OBBRSS>::makeParentRelativeRecurse(int bv_id,
+                                                 Matrix3s& parent_axes,
+                                                 const Vec3s& parent_c);
+
+/// @brief Specialization of getNodeType() for BVHModel with different BV types
+template <>
+NODE_TYPE BVHModel<AABB>::getNodeType() const;
+
+template <>
+NODE_TYPE BVHModel<OBB>::getNodeType() const;
+
+template <>
+NODE_TYPE BVHModel<RSS>::getNodeType() const;
+
+template <>
+NODE_TYPE BVHModel<kIOS>::getNodeType() const;
+
+template <>
+NODE_TYPE BVHModel<OBBRSS>::getNodeType() const;
+
+template <>
+NODE_TYPE BVHModel<KDOP<16>>::getNodeType() const;
+
+template <>
+NODE_TYPE BVHModel<KDOP<18>>::getNodeType() const;
+
+template <>
+NODE_TYPE BVHModel<KDOP<24>>::getNodeType() const;
+
+}  // namespace coal
+
+#endif