python/hppfcl/viewer.py

+import warnings
+
+warnings.warn(
+    "Please update your 'hppfcl' imports to 'coal'", category=DeprecationWarning
+)
+
+from coal.viewer import *  # noqa

python/math.cc

 //
 // Software License Agreement (BSD License)
 //
-//  Copyright (c) 2019 CNRS-LAAS
+//  Copyright (c) 2019 CNRS-LAAS INRIA
 //  Author: Joseph Mirabel
 //  All rights reserved.
 //
@@ -32,76 +32,122 @@
 //  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 //  POSSIBILITY OF SUCH DAMAGE.
 
-#include <boost/python.hpp>
-
 #include <eigenpy/eigenpy.hpp>
 #include <eigenpy/geometry.hpp>
 
-#include <hpp/fcl/fwd.hh>
-#include <hpp/fcl/math/transform.h>
+#include "coal/fwd.hh"
+#include "coal/math/transform.h"
+#include "coal/serialization/transform.h"
+
+#include "coal.hh"
+#include "pickle.hh"
+#include "serializable.hh"
 
-#include "fcl.hh"
+#ifdef COAL_HAS_DOXYGEN_AUTODOC
+#include "doxygen_autodoc/coal/math/transform.h"
+#endif
 
 using namespace boost::python;
+using namespace coal;
+using namespace coal::python;
 
-using namespace hpp::fcl;
+namespace dv = doxygen::visitor;
 
-struct TriangleWrapper
-{
-  static Triangle::index_type getitem (const Triangle& t, int i)
-  {
-    if (i >= 3) throw std::out_of_range("index is out of range");
-    return t[i];
+struct TriangleWrapper {
+  static Triangle::index_type getitem(const Triangle& t, int i) {
+    if (i >= 3 || i <= -3)
+      PyErr_SetString(PyExc_IndexError, "Index out of range");
+    return t[static_cast<coal::Triangle::index_type>(i % 3)];
   }
-  static void setitem (Triangle& t, int i, Triangle::index_type v)
-  {
-    if (i >= 3) throw std::out_of_range("index is out of range");
-    t[i] = v;
+  static void setitem(Triangle& t, int i, Triangle::index_type v) {
+    if (i >= 3 || i <= -3)
+      PyErr_SetString(PyExc_IndexError, "Index out of range");
+    t[static_cast<coal::Triangle::index_type>(i % 3)] = v;
   }
 };
 
-void exposeMaths ()
-{
+void exposeMaths() {
   eigenpy::enableEigenPy();
 
-  if(!eigenpy::register_symbolic_link_to_registered_type<Eigen::Quaterniond>())
+  if (!eigenpy::register_symbolic_link_to_registered_type<Eigen::Quaterniond>())
     eigenpy::exposeQuaternion();
-  if(!eigenpy::register_symbolic_link_to_registered_type<Eigen::AngleAxisd>())
+  if (!eigenpy::register_symbolic_link_to_registered_type<Eigen::AngleAxisd>())
     eigenpy::exposeAngleAxis();
 
-  class_ <Transform3f> ("Transform3f", init<>())
-    .def (init<Matrix3f, Vec3f>())
-    .def (init<Quaternion3f, Vec3f>())
-    .def (init<Matrix3f>())
-    .def (init<Quaternion3f>())
-    .def (init<Vec3f>())
-
-    .def ("getQuatRotation", &Transform3f::getQuatRotation)
-    .def ("getTranslation", &Transform3f::getTranslation, return_value_policy<copy_const_reference>())
-    .def ("getRotation", &Transform3f::getRotation, return_value_policy<copy_const_reference>())
-    .def ("isIdentity", &Transform3f::setIdentity)
-
-    .def ("setQuatRotation", &Transform3f::setQuatRotation)
-    .def ("setTranslation", &Transform3f::setTranslation<Vec3f>)
-    .def ("setRotation", &Transform3f::setRotation<Matrix3f>)
-    .def ("setTransform", &Transform3f::setTransform<Matrix3f,Vec3f>)
-    .def ("setTransform", static_cast<void (Transform3f::*)(const Quaternion3f&, const Vec3f&)>(&Transform3f::setTransform))
-    .def ("setIdentity", &Transform3f::setIdentity)
-
-    .def ("transform", &Transform3f::transform<Vec3f>)
-    .def ("inverseInPlace", &Transform3f::inverseInPlace, return_internal_reference<>())
-    .def ("inverse", &Transform3f::inverse)
-    .def ("inverseTimes", &Transform3f::inverseTimes)
-
-    .def (self * self)
-    .def (self *= self)
-    .def (self == self)
-    .def (self != self)
-    ;
-
-  class_ <Triangle> ("Triangle", init<>())
-    .def (init <Triangle::index_type, Triangle::index_type, Triangle::index_type>())
-    .def ("__getitem__", &TriangleWrapper::getitem)
-    .def ("__setitem__", &TriangleWrapper::setitem)
-    ;
+  eigenpy::enableEigenPySpecific<Matrix3s>();
+  eigenpy::enableEigenPySpecific<Vec3s>();
+
+  class_<Transform3s>("Transform3s", doxygen::class_doc<Transform3s>(), no_init)
+      .def(dv::init<Transform3s>())
+      .def(dv::init<Transform3s, const Matrix3s::MatrixBase&,
+                    const Vec3s::MatrixBase&>())
+      .def(dv::init<Transform3s, const Quatf&, const Vec3s::MatrixBase&>())
+      .def(dv::init<Transform3s, const Matrix3s&>())
+      .def(dv::init<Transform3s, const Quatf&>())
+      .def(dv::init<Transform3s, const Vec3s&>())
+      .def(dv::init<Transform3s, const Transform3s&>())
+
+      .def(dv::member_func("getQuatRotation", &Transform3s::getQuatRotation))
+      .def("getTranslation", &Transform3s::getTranslation,
+           doxygen::member_func_doc(&Transform3s::getTranslation),
+           return_value_policy<copy_const_reference>())
+      .def("getRotation", &Transform3s::getRotation,
+           return_value_policy<copy_const_reference>())
+      .def("isIdentity", &Transform3s::isIdentity,
+           (bp::arg("self"),
+            bp::arg("prec") = Eigen::NumTraits<CoalScalar>::dummy_precision()),
+           doxygen::member_func_doc(&Transform3s::getTranslation))
+
+      .def(dv::member_func("setQuatRotation", &Transform3s::setQuatRotation))
+      .def("setTranslation", &Transform3s::setTranslation<Vec3s>)
+      .def("setRotation", &Transform3s::setRotation<Matrix3s>)
+      .def(dv::member_func("setTransform",
+                           &Transform3s::setTransform<Matrix3s, Vec3s>))
+      .def(dv::member_func(
+          "setTransform",
+          static_cast<void (Transform3s::*)(const Quatf&, const Vec3s&)>(
+              &Transform3s::setTransform)))
+      .def(dv::member_func("setIdentity", &Transform3s::setIdentity))
+      .def(dv::member_func("Identity", &Transform3s::Identity))
+      .staticmethod("Identity")
+
+      .def(dv::member_func("setRandom", &Transform3s::setRandom))
+      .def(dv::member_func("Random", &Transform3s::Random))
+      .staticmethod("Random")
+
+      .def(dv::member_func("transform", &Transform3s::transform<Vec3s>))
+      .def("inverseInPlace", &Transform3s::inverseInPlace,
+           return_internal_reference<>(),
+           doxygen::member_func_doc(&Transform3s::inverseInPlace))
+      .def(dv::member_func("inverse", &Transform3s::inverse))
+      .def(dv::member_func("inverseTimes", &Transform3s::inverseTimes))
+
+      .def(self * self)
+      .def(self *= self)
+      .def(self == self)
+      .def(self != self)
+      .def_pickle(PickleObject<Transform3s>())
+      .def(SerializableVisitor<Transform3s>());
+
+  class_<Triangle>("Triangle", no_init)
+      .def(dv::init<Triangle>())
+      .def(dv::init<Triangle, Triangle::index_type, Triangle::index_type,
+                    Triangle::index_type>())
+      .def("__getitem__", &TriangleWrapper::getitem)
+      .def("__setitem__", &TriangleWrapper::setitem)
+      .def(dv::member_func("set", &Triangle::set))
+      .def(dv::member_func("size", &Triangle::size))
+      .staticmethod("size")
+      .def(self == self);
+
+  if (!eigenpy::register_symbolic_link_to_registered_type<
+          std::vector<Vec3s> >()) {
+    class_<std::vector<Vec3s> >("StdVec_Vec3s")
+        .def(vector_indexing_suite<std::vector<Vec3s> >());
+  }
+  if (!eigenpy::register_symbolic_link_to_registered_type<
+          std::vector<Triangle> >()) {
+    class_<std::vector<Triangle> >("StdVec_Triangle")
+        .def(vector_indexing_suite<std::vector<Triangle> >());
+  }
 }

python/octree.cc

+
+#include "coal.hh"
+
+#include <eigenpy/std-vector.hpp>
+
+#include "coal/fwd.hh"
+#include "coal/octree.h"
+
+#ifdef COAL_HAS_DOXYGEN_AUTODOC
+#include "doxygen_autodoc/functions.h"
+#include "doxygen_autodoc/coal/octree.h"
+#endif
+
+bp::object toPyBytes(std::vector<uint8_t>& bytes) {
+#if PY_MAJOR_VERSION == 2
+  PyObject* py_buf =
+      PyBuffer_FromMemory((char*)bytes.data(), Py_ssize_t(bytes.size()));
+  return bp::object(bp::handle<>(py_buf));
+#else
+  PyObject* py_buf = PyMemoryView_FromMemory(
+      (char*)bytes.data(), Py_ssize_t(bytes.size()), PyBUF_READ);
+  return bp::object(bp::handle<>(PyBytes_FromObject(py_buf)));
+#endif
+}
+
+bp::object tobytes(const coal::OcTree& self) {
+  std::vector<uint8_t> bytes = self.tobytes();
+  return toPyBytes(bytes);
+}
+
+void exposeOctree() {
+  using namespace coal;
+  namespace bp = boost::python;
+  namespace dv = doxygen::visitor;
+
+  bp::class_<OcTree, bp::bases<CollisionGeometry>, shared_ptr<OcTree> >(
+      "OcTree", doxygen::class_doc<OcTree>(), bp::no_init)
+      .def(dv::init<OcTree, CoalScalar>())
+      .def("clone", &OcTree::clone, doxygen::member_func_doc(&OcTree::clone),
+           bp::return_value_policy<bp::manage_new_object>())
+      .def(dv::member_func("getTreeDepth", &OcTree::getTreeDepth))
+      .def(dv::member_func("size", &OcTree::size))
+      .def(dv::member_func("getResolution", &OcTree::getResolution))
+      .def(dv::member_func("getOccupancyThres", &OcTree::getOccupancyThres))
+      .def(dv::member_func("getFreeThres", &OcTree::getFreeThres))
+      .def(dv::member_func("getDefaultOccupancy", &OcTree::getDefaultOccupancy))
+      .def(dv::member_func("setCellDefaultOccupancy",
+                           &OcTree::setCellDefaultOccupancy))
+      .def(dv::member_func("setOccupancyThres", &OcTree::setOccupancyThres))
+      .def(dv::member_func("setFreeThres", &OcTree::setFreeThres))
+      .def(dv::member_func("getRootBV", &OcTree::getRootBV))
+      .def(dv::member_func("toBoxes", &OcTree::toBoxes))
+      .def("tobytes", tobytes, doxygen::member_func_doc(&OcTree::tobytes));
+
+  doxygen::def("makeOctree", &makeOctree);
+  eigenpy::enableEigenPySpecific<Vec6s>();
+  eigenpy::StdVectorPythonVisitor<std::vector<Vec6s>, true>::expose(
+      "StdVec_Vec6");
+}

python/pickle.hh

+//
+// Copyright (c) 2022 INRIA
+//
+
+#ifndef COAL_PYTHON_PICKLE_H
+#define COAL_PYTHON_PICKLE_H
+
+#include <boost/python.hpp>
+#include <eigenpy/eigenpy.hpp>
+
+#include <boost/archive/text_oarchive.hpp>
+#include <boost/archive/text_iarchive.hpp>
+#include <sstream>
+
+using namespace boost::python;
+using namespace coal;
+//
+template <typename T>
+struct PickleObject : boost::python::pickle_suite {
+  static boost::python::tuple getinitargs(const T&) {
+    return boost::python::make_tuple();
+  }
+
+  static boost::python::tuple getstate(const T& obj) {
+    std::stringstream ss;
+    boost::archive::text_oarchive oa(ss);
+    oa & obj;
+
+    return boost::python::make_tuple(boost::python::str(ss.str()));
+  }
+
+  static void setstate(T& obj, boost::python::tuple tup) {
+    if (boost::python::len(tup) == 0 || boost::python::len(tup) > 1) {
+      throw eigenpy::Exception(
+          "Pickle was not able to reconstruct the object from the loaded "
+          "data.\n"
+          "The pickle data structure contains too many elements.");
+    }
+
+    boost::python::object py_obj = tup[0];
+    boost::python::extract<std::string> obj_as_string(py_obj.ptr());
+    if (obj_as_string.check()) {
+      const std::string str = obj_as_string;
+      std::istringstream is(str);
+      boost::archive::text_iarchive ia(is, boost::archive::no_codecvt);
+      ia >> obj;
+    } else {
+      throw eigenpy::Exception(
+          "Pickle was not able to reconstruct the model from the loaded data.\n"
+          "The entry is not a string.");
+    }
+  }
+
+  static bool getstate_manages_dict() { return false; }
+};
+
+#endif  // ifndef COAL_PYTHON_PICKLE_H

python/serializable.hh

+//
+// Copyright (c) 2017-2024 CNRS INRIA
+// This file was borrowed from the Pinocchio library:
+// https://github.com/stack-of-tasks/pinocchio
+//
+
+#ifndef COAL_PYTHON_SERIALIZABLE_H
+#define COAL_PYTHON_SERIALIZABLE_H
+
+#include <boost/python.hpp>
+
+#include "coal/serialization/archive.h"
+#include "coal/serialization/serializer.h"
+
+namespace coal {
+namespace python {
+
+using Serializer = ::coal::serialization::Serializer;
+
+namespace bp = boost::python;
+
+template <typename Derived>
+struct SerializableVisitor
+    : public bp::def_visitor<SerializableVisitor<Derived>> {
+  template <class PyClass>
+  void visit(PyClass& cl) const {
+    cl.def("saveToText", &Serializer::saveToText<Derived>, bp::arg("filename"),
+           "Saves *this inside a text file.")
+        .def("loadFromText", &Serializer::loadFromText<Derived>,
+             bp::arg("filename"), "Loads *this from a text file.")
+
+        .def("saveToString", &Serializer::saveToString<Derived>,
+             bp::arg("self"), "Parses the current object to a string.")
+        .def("loadFromString", &Serializer::loadFromString<Derived>,
+             bp::args("self", "string"),
+             "Parses from the input string the content of the current object.")
+
+        .def("saveToXML", &Serializer::saveToXML<Derived>,
+             bp::args("filename", "tag_name"), "Saves *this inside a XML file.")
+        .def("loadFromXML", &Serializer::loadFromXML<Derived>,
+             bp::args("self", "filename", "tag_name"),
+             "Loads *this from a XML file.")
+
+        .def("saveToBinary", &Serializer::saveToBinary<Derived>,
+             bp::args("self", "filename"), "Saves *this inside a binary file.")
+        .def("loadFromBinary", &Serializer::loadFromBinary<Derived>,
+             bp::args("self", "filename"), "Loads *this from a binary file.")
+
+        .def("saveToBuffer", &Serializer::saveToBuffer<Derived>,
+             bp::args("self", "buffer"), "Saves *this inside a binary buffer.")
+        .def("loadFromBuffer", &Serializer::loadFromBuffer<Derived>,
+             bp::args("self", "buffer"), "Loads *this from a binary buffer.");
+  }
+};
+}  // namespace python
+}  // namespace coal
+
+#endif  // ifndef COAL_PYTHON_SERIALIZABLE_H

python/utils/std-pair.hh

+//
+// Copyright (c) 2023 INRIA
+//
+
+#ifndef COAL_PYTHON_UTILS_STD_PAIR_H
+#define COAL_PYTHON_UTILS_STD_PAIR_H
+
+#include <boost/python.hpp>
+#include <utility>
+
+template <typename pair_type>
+struct StdPairConverter {
+  typedef typename pair_type::first_type T1;
+  typedef typename pair_type::second_type T2;
+
+  static PyObject* convert(const pair_type& pair) {
+    return boost::python::incref(
+        boost::python::make_tuple(pair.first, pair.second).ptr());
+  }
+
+  static void* convertible(PyObject* obj) {
+    if (!PyTuple_CheckExact(obj)) return 0;
+    if (PyTuple_Size(obj) != 2) return 0;
+    {
+      boost::python::tuple tuple(boost::python::borrowed(obj));
+      boost::python::extract<T1> elt1(tuple[0]);
+      if (!elt1.check()) return 0;
+      boost::python::extract<T2> elt2(tuple[1]);
+      if (!elt2.check()) return 0;
+    }
+    return obj;
+  }
+
+  static void construct(
+      PyObject* obj,
+      boost::python::converter::rvalue_from_python_stage1_data* memory) {
+    boost::python::tuple tuple(boost::python::borrowed(obj));
+    void* storage =
+        reinterpret_cast<
+            boost::python::converter::rvalue_from_python_storage<pair_type>*>(
+            reinterpret_cast<void*>(memory))
+            ->storage.bytes;
+    new (storage) pair_type(boost::python::extract<T1>(tuple[0]),
+                            boost::python::extract<T2>(tuple[1]));
+    memory->convertible = storage;
+  }
+
+  static PyTypeObject const* get_pytype() {
+    PyTypeObject const* py_type = &PyTuple_Type;
+    return py_type;
+  }
+
+  static void registration() {
+    boost::python::converter::registry::push_back(
+        reinterpret_cast<void* (*)(_object*)>(&convertible), &construct,
+        boost::python::type_id<pair_type>()
+#ifndef BOOST_PYTHON_NO_PY_SIGNATURES
+            ,
+        get_pytype
+#endif
+    );
+  }
+};
+
+#endif  // ifndef COAL_PYTHON_UTILS_STD_PAIR_H

python/version.cc

 //
-// Copyright (c) 2019 CNRS INRIA
+// Copyright (c) 2019-2023 CNRS INRIA
 //
 
-#include "hpp/fcl/config.hh"
-#include <boost/python.hpp>
+#include "coal/config.hh"
+#include "coal.hh"
 #include <boost/preprocessor/stringize.hpp>
 
 namespace bp = boost::python;
 
-inline bool checkVersionAtLeast(unsigned int major,
-                                unsigned int minor,
-                                unsigned int patch)
-{
-  return HPP_FCL_VERSION_AT_LEAST(major, minor, patch);
+inline bool checkVersionAtLeast(int major, int minor, int patch) {
+  return COAL_VERSION_AT_LEAST(major, minor, patch);
 }
 
-inline bool checkVersionAtMost(unsigned int major,
-                               unsigned int minor,
-                               unsigned int patch)
-{
-  return HPP_FCL_VERSION_AT_MOST(major, minor, patch);
+inline bool checkVersionAtMost(int major, int minor, int patch) {
+  return COAL_VERSION_AT_MOST(major, minor, patch);
 }
 
-void exposeVersion()
-{
-  // Define release numbers of the current hpp-fcl version.
-  bp::scope().attr("__version__") = BOOST_PP_STRINGIZE(HPP_FCL_MAJOR_VERSION) "."
-    BOOST_PP_STRINGIZE(HPP_FCL_MINOR_VERSION) "."
-    BOOST_PP_STRINGIZE(HPP_FCL_PATCH_VERSION);
-  bp::scope().attr("__raw_version__") = HPP_FCL_VERSION;
-  bp::scope().attr("HPP_FCL_MAJOR_VERSION") = HPP_FCL_MAJOR_VERSION;
-  bp::scope().attr("HPP_FCL_MINOR_VERSION") = HPP_FCL_MINOR_VERSION;
-  bp::scope().attr("HPP_FCL_PATCH_VERSION") = HPP_FCL_PATCH_VERSION;
-
-  bp::def("checkVersionAtLeast",&checkVersionAtLeast,
-          bp::args("major","minor","patch"),
-          "Checks if the current version of hpp-fcl is at least"
+void exposeVersion() {
+  // Define release numbers of the current Coal version.
+  bp::scope().attr("__version__") =
+      BOOST_PP_STRINGIZE(COAL_MAJOR_VERSION) "." BOOST_PP_STRINGIZE(COAL_MINOR_VERSION) "." BOOST_PP_STRINGIZE(COAL_PATCH_VERSION);
+  bp::scope().attr("__raw_version__") = COAL_VERSION;
+  bp::scope().attr("COAL_MAJOR_VERSION") = COAL_MAJOR_VERSION;
+  bp::scope().attr("COAL_MINOR_VERSION") = COAL_MINOR_VERSION;
+  bp::scope().attr("COAL_PATCH_VERSION") = COAL_PATCH_VERSION;
+
+#if COAL_HAS_QHULL
+  bp::scope().attr("WITH_QHULL") = true;
+#else
+  bp::scope().attr("WITH_QHULL") = false;
+#endif
+
+#if COAL_HAS_OCTOMAP
+  bp::scope().attr("WITH_OCTOMAP") = true;
+#else
+  bp::scope().attr("WITH_OCTOMAP") = false;
+#endif
+
+  bp::def("checkVersionAtLeast", &checkVersionAtLeast,
+          bp::args("major", "minor", "patch"),
+          "Checks if the current version of coal is at least"
           " the version provided by the input arguments.");
 
-  bp::def("checkVersionAtMost",&checkVersionAtMost,
-          bp::args("major","minor","patch"),
-          "Checks if the current version of hpp-fcl is at most"
+  bp::def("checkVersionAtMost", &checkVersionAtMost,
+          bp::args("major", "minor", "patch"),
+          "Checks if the current version of coal is at most"
           " the version provided by the input arguments.");
 }

src/BV/AABB.cpp

@@ -35,79 +35,72 @@
 
 /** \author Jia Pan */
 
-#include <hpp/fcl/BV/AABB.h>
+#include "coal/BV/AABB.h"
+#include "coal/shape/geometric_shapes.h"
+#include "coal/collision_data.h"
 
 #include <limits>
-#include <hpp/fcl/collision_data.h>
 
-namespace hpp
-{
-namespace fcl
-{
+namespace coal {
 
-AABB::AABB() : min_(Vec3f::Constant(std::numeric_limits<FCL_REAL>::max())),
-               max_(Vec3f::Constant(-std::numeric_limits<FCL_REAL>::max()))
-{
-}
+AABB::AABB()
+    : min_(Vec3s::Constant((std::numeric_limits<CoalScalar>::max)())),
+      max_(Vec3s::Constant(-(std::numeric_limits<CoalScalar>::max)())) {}
 
 bool AABB::overlap(const AABB& other, const CollisionRequest& request,
-                    FCL_REAL& sqrDistLowerBound) const
-{
-  const FCL_REAL breakDistance (request.break_distance + request.security_margin);
-  const FCL_REAL breakDistance2 = breakDistance * breakDistance;
-
-  sqrDistLowerBound = (min_ - other.max_).array().max(0).matrix().squaredNorm();
-  if(sqrDistLowerBound > breakDistance2) return false;
-
-  sqrDistLowerBound = (other.min_ - max_).array().max(0).matrix().squaredNorm();
-  if(sqrDistLowerBound > breakDistance2) return false;
+                   CoalScalar& sqrDistLowerBound) const {
+  const CoalScalar break_distance_squared =
+      request.break_distance * request.break_distance;
+
+  sqrDistLowerBound =
+      (min_ - other.max_ - Vec3s::Constant(request.security_margin))
+          .array()
+          .max(CoalScalar(0))
+          .matrix()
+          .squaredNorm();
+  if (sqrDistLowerBound > break_distance_squared) return false;
+
+  sqrDistLowerBound =
+      (other.min_ - max_ - Vec3s::Constant(request.security_margin))
+          .array()
+          .max(CoalScalar(0))
+          .matrix()
+          .squaredNorm();
+  if (sqrDistLowerBound > break_distance_squared) return false;
 
   return true;
 }
 
-FCL_REAL AABB::distance(const AABB& other, Vec3f* P, Vec3f* Q) const
-{
-  FCL_REAL result = 0;
-  for(std::size_t i = 0; i < 3; ++i)
-  {
-    const FCL_REAL& amin = min_[i];
-    const FCL_REAL& amax = max_[i];
-    const FCL_REAL& bmin = other.min_[i];
-    const FCL_REAL& bmax = other.max_[i];
-    
-    if(amin > bmax)
-    {
-      FCL_REAL delta = bmax - amin;
+CoalScalar AABB::distance(const AABB& other, Vec3s* P, Vec3s* Q) const {
+  CoalScalar result = 0;
+  for (Eigen::DenseIndex i = 0; i < 3; ++i) {
+    const CoalScalar& amin = min_[i];
+    const CoalScalar& amax = max_[i];
+    const CoalScalar& bmin = other.min_[i];
+    const CoalScalar& bmax = other.max_[i];
+
+    if (amin > bmax) {
+      CoalScalar delta = bmax - amin;
       result += delta * delta;
-      if(P && Q)
-      {
+      if (P && Q) {
         (*P)[i] = amin;
         (*Q)[i] = bmax;
       }
-    }
-    else if(bmin > amax)
-    {
-      FCL_REAL delta = amax - bmin;
+    } else if (bmin > amax) {
+      CoalScalar delta = amax - bmin;
       result += delta * delta;
-      if(P && Q)
-      {
+      if (P && Q) {
         (*P)[i] = amax;
         (*Q)[i] = bmin;
       }
-    }
-    else
-    {
-      if(P && Q)
-      {
-        if(bmin >= amin)
-        {
-          FCL_REAL t = 0.5 * (amax + bmin);
+    } else {
+      if (P && Q) {
+        if (bmin >= amin) {
+          CoalScalar t = 0.5 * (amax + bmin);
           (*P)[i] = t;
           (*Q)[i] = t;
-        }
-        else
-        {
-          FCL_REAL t = 0.5 * (amin + bmax);
+        } else {
+          CoalScalar t = 0.5 * (amin + bmax);
           (*P)[i] = t;
           (*Q)[i] = t;
         }
@@ -118,24 +111,19 @@ FCL_REAL AABB::distance(const AABB& other, Vec3f* P, Vec3f* Q) const
   return std::sqrt(result);
 }
 
-FCL_REAL AABB::distance(const AABB& other) const
-{
-  FCL_REAL result = 0;
-  for(std::size_t i = 0; i < 3; ++i)
-  {
-    const FCL_REAL& amin = min_[i];
-    const FCL_REAL& amax = max_[i];
-    const FCL_REAL& bmin = other.min_[i];
-    const FCL_REAL& bmax = other.max_[i];
-    
-    if(amin > bmax)
-    {
-      FCL_REAL delta = bmax - amin;
+CoalScalar AABB::distance(const AABB& other) const {
+  CoalScalar result = 0;
+  for (Eigen::DenseIndex i = 0; i < 3; ++i) {
+    const CoalScalar& amin = min_[i];
+    const CoalScalar& amax = max_[i];
+    const CoalScalar& bmin = other.min_[i];
+    const CoalScalar& bmax = other.max_[i];
+
+    if (amin > bmax) {
+      CoalScalar delta = bmax - amin;
       result += delta * delta;
-    }
-    else if(bmin > amax)
-    {
-      FCL_REAL delta = amax - bmin;
+    } else if (bmin > amax) {
+      CoalScalar delta = amax - bmin;
       result += delta * delta;
     }
   }
@@ -143,20 +131,64 @@ FCL_REAL AABB::distance(const AABB& other) const
   return std::sqrt(result);
 }
 
-bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, const AABB& b2)
-{
-  AABB bb1 (translate (rotate (b1, R0), T0));
-  return bb1.overlap (b2);
+bool overlap(const Matrix3s& R0, const Vec3s& T0, const AABB& b1,
+             const AABB& b2) {
+  AABB bb1(translate(rotate(b1, R0), T0));
+  return bb1.overlap(b2);
+}
+
+bool overlap(const Matrix3s& R0, const Vec3s& T0, const AABB& b1,
+             const AABB& b2, const CollisionRequest& request,
+             CoalScalar& sqrDistLowerBound) {
+  AABB bb1(translate(rotate(b1, R0), T0));
+  return bb1.overlap(b2, request, sqrDistLowerBound);
 }
 
-bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1,
-	     const AABB& b2, const CollisionRequest& request,
-             FCL_REAL& sqrDistLowerBound)
-{
-  AABB bb1 (translate (rotate (b1, R0), T0));
-  return bb1.overlap (b2, request, sqrDistLowerBound);
+bool AABB::overlap(const Plane& p) const {
+  // Convert AABB to a (box, transform) representation and compute the support
+  // points in the directions normal and -normal.
+  // If both points lie on different sides of the plane, there is an overlap
+  // between the AABB and the plane. Otherwise, there is no overlap.
+  const Vec3s halfside = (this->max_ - this->min_) / 2;
+  const Vec3s center = (this->max_ + this->min_) / 2;
+
+  const Vec3s support1 = (p.n.array() > 0).select(halfside, -halfside) + center;
+  const Vec3s support2 =
+      ((-p.n).array() > 0).select(halfside, -halfside) + center;
+
+  const CoalScalar dist1 = p.n.dot(support1) - p.d;
+  const CoalScalar dist2 = p.n.dot(support2) - p.d;
+  const int sign1 = (dist1 > 0) ? 1 : -1;
+  const int sign2 = (dist2 > 0) ? 1 : -1;
+
+  if (p.getSweptSphereRadius() > 0) {
+    if (sign1 != sign2) {
+      // Supports are on different sides of the plane. There is an overlap.
+      return true;
+    }
+    // Both supports are on the same side of the plane.
+    // We now need to check if they are on the same side of the plane inflated
+    // by the swept-sphere radius.
+    const CoalScalar ssr_dist1 = std::abs(dist1) - p.getSweptSphereRadius();
+    const CoalScalar ssr_dist2 = std::abs(dist2) - p.getSweptSphereRadius();
+    const int ssr_sign1 = (ssr_dist1 > 0) ? 1 : -1;
+    const int ssr_sign2 = (ssr_dist2 > 0) ? 1 : -1;
+    return ssr_sign1 != ssr_sign2;
+  }
+
+  return (sign1 != sign2);
 }
 
+bool AABB::overlap(const Halfspace& hs) const {
+  // Convert AABB to a (box, transform) representation and compute the support
+  // points in the direction -normal.
+  // If the support is below the plane defined by the halfspace, there is an
+  // overlap between the AABB and the halfspace. Otherwise, there is no
+  // overlap.
+  Vec3s halfside = (this->max_ - this->min_) / 2;
+  Vec3s center = (this->max_ + this->min_) / 2;
+  Vec3s support = ((-hs.n).array() > 0).select(halfside, -halfside) + center;
+  return (hs.signedDistance(support) < 0);
 }
 
-} // namespace hpp
+}  // namespace coal

src/BV/OBB.cpp

@@ -35,68 +35,74 @@
 
 /** \author Jia Pan, Florent Lamiraux */
 
-#include <hpp/fcl/BV/OBB.h>
-#include <hpp/fcl/BVH/BVH_utility.h>
-#include <hpp/fcl/math/transform.h>
-#include <hpp/fcl/collision_data.h>
-#include <hpp/fcl/internal/tools.h>
+#include "coal/BV/OBB.h"
+#include "coal/BVH/BVH_utility.h"
+#include "coal/math/transform.h"
+#include "coal/collision_data.h"
+#include "coal/internal/tools.h"
 
 #include <iostream>
 #include <limits>
 
-namespace hpp
-{
-namespace fcl
-{
+namespace coal {
 
 /// @brief Compute the 8 vertices of a OBB
-inline void computeVertices(const OBB& b, Vec3f vertices[8])
-{
-  Matrix3f extAxes (b.axes * b.extent.asDiagonal());
-  vertices[0].noalias() = b.To + extAxes * Vec3f(-1,-1,-1);
-  vertices[1].noalias() = b.To + extAxes * Vec3f( 1,-1,-1);
-  vertices[2].noalias() = b.To + extAxes * Vec3f( 1, 1,-1);
-  vertices[3].noalias() = b.To + extAxes * Vec3f(-1, 1,-1);
-  vertices[4].noalias() = b.To + extAxes * Vec3f(-1,-1, 1);
-  vertices[5].noalias() = b.To + extAxes * Vec3f( 1,-1, 1);
-  vertices[6].noalias() = b.To + extAxes * Vec3f( 1, 1, 1);
-  vertices[7].noalias() = b.To + extAxes * Vec3f(-1, 1, 1);
+inline void computeVertices(const OBB& b, Vec3s vertices[8]) {
+  Matrix3s extAxes(b.axes * b.extent.asDiagonal());
+  vertices[0].noalias() = b.To + extAxes * Vec3s(-1, -1, -1);
+  vertices[1].noalias() = b.To + extAxes * Vec3s(1, -1, -1);
+  vertices[2].noalias() = b.To + extAxes * Vec3s(1, 1, -1);
+  vertices[3].noalias() = b.To + extAxes * Vec3s(-1, 1, -1);
+  vertices[4].noalias() = b.To + extAxes * Vec3s(-1, -1, 1);
+  vertices[5].noalias() = b.To + extAxes * Vec3s(1, -1, 1);
+  vertices[6].noalias() = b.To + extAxes * Vec3s(1, 1, 1);
+  vertices[7].noalias() = b.To + extAxes * Vec3s(-1, 1, 1);
 }
 
 /// @brief OBB merge method when the centers of two smaller OBB are far away
-inline OBB merge_largedist(const OBB& b1, const OBB& b2)
-{
+inline OBB merge_largedist(const OBB& b1, const OBB& b2) {
   OBB b;
-  Vec3f vertex[16];
+  Vec3s vertex[16];
   computeVertices(b1, vertex);
   computeVertices(b2, vertex + 8);
-  Matrix3f M;
-  Vec3f E[3];
-  Matrix3f::Scalar s[3] = {0, 0, 0};
+  Matrix3s M;
+  Vec3s E[3];
+  CoalScalar s[3] = {0, 0, 0};
 
   // obb axes
   b.axes.col(0).noalias() = (b1.To - b2.To).normalized();
 
-  Vec3f vertex_proj[16];
-  for(int i = 0; i < 16; ++i)
-    vertex_proj[i].noalias() = vertex[i] - b.axes.col(0) * vertex[i].dot(b.axes.col(0));
+  Vec3s vertex_proj[16];
+  for (int i = 0; i < 16; ++i)
+    vertex_proj[i].noalias() =
+        vertex[i] - b.axes.col(0) * vertex[i].dot(b.axes.col(0));
 
   getCovariance(vertex_proj, NULL, NULL, NULL, 16, M);
   eigen(M, s, E);
 
   int min, mid, max;
-  if (s[0] > s[1]) { max = 0; min = 1; }
-  else { min = 0; max = 1; }
-  if (s[2] < s[min]) { mid = min; min = 2; }
-  else if (s[2] > s[max]) { mid = max; max = 2; }
-  else { mid = 2; }
-
+  if (s[0] > s[1]) {
+    max = 0;
+    min = 1;
+  } else {
+    min = 0;
+    max = 1;
+  }
+  if (s[2] < s[min]) {
+    mid = min;
+    min = 2;
+  } else if (s[2] > s[max]) {
+    mid = max;
+    max = 2;
+  } else {
+    mid = 2;
+  }
 
   b.axes.col(1) << E[0][max], E[1][max], E[2][max];
   b.axes.col(2) << E[0][mid], E[1][mid], E[2][mid];
 
   // set obb centers and extensions
-  Vec3f center, extent;
+  Vec3s center, extent;
   getExtentAndCenter(vertex, NULL, NULL, NULL, 16, b.axes, center, extent);
 
   b.To.noalias() = center;
@@ -105,55 +111,46 @@ inline OBB merge_largedist(const OBB& b1, const OBB& b2)
   return b;
 }
 
-
 /// @brief OBB merge method when the centers of two smaller OBB are close
-inline OBB merge_smalldist(const OBB& b1, const OBB& b2)
-{
+inline OBB merge_smalldist(const OBB& b1, const OBB& b2) {
   OBB b;
   b.To = (b1.To + b2.To) * 0.5;
-  Quaternion3f q0 (b1.axes), q1 (b2.axes);
-  if(q0.dot(q1) < 0)
-    q1.coeffs() *= -1;
+  Quatf q0(b1.axes), q1(b2.axes);
+  if (q0.dot(q1) < 0) q1.coeffs() *= -1;
 
-  Quaternion3f q ((q0.coeffs() + q1.coeffs()).normalized());
+  Quatf q((q0.coeffs() + q1.coeffs()).normalized());
   b.axes = q.toRotationMatrix();
 
-
-  Vec3f vertex[8], diff;
-  FCL_REAL real_max = std::numeric_limits<FCL_REAL>::max();
-  Vec3f pmin(real_max, real_max, real_max);
-  Vec3f pmax(-real_max, -real_max, -real_max);
+  Vec3s vertex[8], diff;
+  CoalScalar real_max = (std::numeric_limits<CoalScalar>::max)();
+  Vec3s pmin(real_max, real_max, real_max);
+  Vec3s pmax(-real_max, -real_max, -real_max);
 
   computeVertices(b1, vertex);
-  for(int i = 0; i < 8; ++i)
-  {
+  for (int i = 0; i < 8; ++i) {
     diff = vertex[i] - b.To;
-    for(int j = 0; j < 3; ++j)
-    {
-      FCL_REAL dot = diff.dot(b.axes.col(j));
-      if(dot > pmax[j])
+    for (int j = 0; j < 3; ++j) {
+      CoalScalar dot = diff.dot(b.axes.col(j));
+      if (dot > pmax[j])
         pmax[j] = dot;
-      else if(dot < pmin[j])
+      else if (dot < pmin[j])
         pmin[j] = dot;
     }
   }
 
   computeVertices(b2, vertex);
-  for(int i = 0; i < 8; ++i)
-  {
+  for (int i = 0; i < 8; ++i) {
     diff = vertex[i] - b.To;
-    for(int j = 0; j < 3; ++j)
-    {
-      FCL_REAL dot = diff.dot(b.axes.col(j));
-      if(dot > pmax[j])
+    for (int j = 0; j < 3; ++j) {
+      CoalScalar dot = diff.dot(b.axes.col(j));
+      if (dot > pmax[j])
         pmax[j] = dot;
-      else if(dot < pmin[j])
+      else if (dot < pmin[j])
         pmin[j] = dot;
     }
   }
 
-  for(int j = 0; j < 3; ++j)
-  {
+  for (int j = 0; j < 3; ++j) {
     b.To.noalias() += (b.axes.col(j) * (0.5 * (pmax[j] + pmin[j])));
     b.extent[j] = 0.5 * (pmax[j] - pmin[j]);
   }
@@ -161,12 +158,12 @@ inline OBB merge_smalldist(const OBB& b1, const OBB& b2)
   return b;
 }
 
-bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b)
-{
-  register FCL_REAL t, s;
-  const FCL_REAL reps = 1e-6;
+bool obbDisjoint(const Matrix3s& B, const Vec3s& T, const Vec3s& a,
+                 const Vec3s& b) {
+  CoalScalar t, s;
+  const CoalScalar reps = 1e-6;
 
-  Matrix3f Bf (B.array().abs() + reps);
+  Matrix3s Bf(B.array().abs() + reps);
   // Bf += reps;
 
   // if any of these tests are one-sided, then the polyhedra are disjoint
@@ -175,31 +172,27 @@ bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f&
   t = ((T[0] < 0.0) ? -T[0] : T[0]);
 
   // if(t > (a[0] + Bf.dotX(b)))
-  if(t > (a[0] + Bf.row(0).dot(b)))
-    return true;
+  if (t > (a[0] + Bf.row(0).dot(b))) return true;
 
   // B1 x B2 = B0
   // s =  B.transposeDotX(T);
-  s =  B.col(0).dot(T);
+  s = B.col(0).dot(T);
   t = ((s < 0.0) ? -s : s);
 
   // if(t > (b[0] + Bf.transposeDotX(a)))
-  if(t > (b[0] + Bf.col(0).dot(a)))
-    return true;
+  if (t > (b[0] + Bf.col(0).dot(a))) return true;
 
   // A2 x A0 = A1
   t = ((T[1] < 0.0) ? -T[1] : T[1]);
 
   // if(t > (a[1] + Bf.dotY(b)))
-  if(t > (a[1] + Bf.row(1).dot(b)))
-    return true;
+  if (t > (a[1] + Bf.row(1).dot(b))) return true;
 
   // A0 x A1 = A2
-  t =((T[2] < 0.0) ? -T[2] : T[2]);
+  t = ((T[2] < 0.0) ? -T[2] : T[2]);
 
   // if(t > (a[2] + Bf.dotZ(b)))
-  if(t > (a[2] + Bf.row(2).dot(b)))
-    return true;
+  if (t > (a[2] + Bf.row(2).dot(b))) return true;
 
   // B2 x B0 = B1
   // s = B.transposeDotY(T);
@@ -207,8 +200,7 @@ bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f&
   t = ((s < 0.0) ? -s : s);
 
   // if(t > (b[1] + Bf.transposeDotY(a)))
-  if(t > (b[1] + Bf.col(1).dot(a)))
-    return true;
+  if (t > (b[1] + Bf.col(1).dot(a))) return true;
 
   // B0 x B1 = B2
   // s = B.transposeDotZ(T);
@@ -216,149 +208,131 @@ bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f&
   t = ((s < 0.0) ? -s : s);
 
   // if(t > (b[2] + Bf.transposeDotZ(a)))
-  if(t > (b[2] + Bf.col(2).dot(a)))
-    return true;
+  if (t > (b[2] + Bf.col(2).dot(a))) return true;
 
   // A0 x B0
   s = T[2] * B(1, 0) - T[1] * B(2, 0);
   t = ((s < 0.0) ? -s : s);
 
-  if(t > (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) +
-          b[1] * Bf(0, 2) + b[2] * Bf(0, 1)))
+  if (t >
+      (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + b[1] * Bf(0, 2) + b[2] * Bf(0, 1)))
     return true;
 
   // A0 x B1
   s = T[2] * B(1, 1) - T[1] * B(2, 1);
   t = ((s < 0.0) ? -s : s);
 
-  if(t > (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) +
-          b[0] * Bf(0, 2) + b[2] * Bf(0, 0)))
+  if (t >
+      (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) + b[0] * Bf(0, 2) + b[2] * Bf(0, 0)))
     return true;
 
   // A0 x B2
   s = T[2] * B(1, 2) - T[1] * B(2, 2);
   t = ((s < 0.0) ? -s : s);
 
-  if(t > (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) +
-          b[0] * Bf(0, 1) + b[1] * Bf(0, 0)))
+  if (t >
+      (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) + b[0] * Bf(0, 1) + b[1] * Bf(0, 0)))
     return true;
 
   // A1 x B0
   s = T[0] * B(2, 0) - T[2] * B(0, 0);
   t = ((s < 0.0) ? -s : s);
 
-  if(t > (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) +
-          b[1] * Bf(1, 2) + b[2] * Bf(1, 1)))
+  if (t >
+      (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) + b[1] * Bf(1, 2) + b[2] * Bf(1, 1)))
     return true;
 
   // A1 x B1
   s = T[0] * B(2, 1) - T[2] * B(0, 1);
   t = ((s < 0.0) ? -s : s);
 
-  if(t > (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) +
-          b[0] * Bf(1, 2) + b[2] * Bf(1, 0)))
+  if (t >
+      (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) + b[0] * Bf(1, 2) + b[2] * Bf(1, 0)))
     return true;
 
   // A1 x B2
   s = T[0] * B(2, 2) - T[2] * B(0, 2);
   t = ((s < 0.0) ? -s : s);
 
-  if(t > (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) +
-          b[0] * Bf(1, 1) + b[1] * Bf(1, 0)))
+  if (t >
+      (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) + b[0] * Bf(1, 1) + b[1] * Bf(1, 0)))
     return true;
 
   // A2 x B0
   s = T[1] * B(0, 0) - T[0] * B(1, 0);
   t = ((s < 0.0) ? -s : s);
 
-  if(t > (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) +
-          b[1] * Bf(2, 2) + b[2] * Bf(2, 1)))
+  if (t >
+      (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) + b[1] * Bf(2, 2) + b[2] * Bf(2, 1)))
     return true;
 
   // A2 x B1
   s = T[1] * B(0, 1) - T[0] * B(1, 1);
   t = ((s < 0.0) ? -s : s);
 
-  if(t > (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) +
-          b[0] * Bf(2, 2) + b[2] * Bf(2, 0)))
+  if (t >
+      (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) + b[0] * Bf(2, 2) + b[2] * Bf(2, 0)))
     return true;
 
   // A2 x B2
   s = T[1] * B(0, 2) - T[0] * B(1, 2);
   t = ((s < 0.0) ? -s : s);
 
-  if(t > (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) +
-          b[0] * Bf(2, 1) + b[1] * Bf(2, 0)))
+  if (t >
+      (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) + b[0] * Bf(2, 1) + b[1] * Bf(2, 0)))
     return true;
 
   return false;
+}
 
+namespace internal {
+inline CoalScalar obbDisjoint_check_A_axis(const Vec3s& T, const Vec3s& a,
+                                           const Vec3s& b, const Matrix3s& Bf) {
+  // |T| - |B| * b - a
+  Vec3s AABB_corner(T.cwiseAbs() - a);
+  AABB_corner.noalias() -= Bf * b;
+  return AABB_corner.array().max(CoalScalar(0)).matrix().squaredNorm();
 }
 
-namespace internal
-{
-  inline FCL_REAL obbDisjoint_check_A_axis (
-      const Vec3f& T, const Vec3f& a, const Vec3f& b, const Matrix3f& Bf)
-  {
-    // |T| - |B| * b - a
-    Vec3f AABB_corner (T.cwiseAbs () - a);
-    AABB_corner.noalias() -= Bf.col(0) * b[0];
-    AABB_corner.noalias() -= Bf.col(1) * b[1];
-    AABB_corner.noalias() -= Bf.col(2) * b[2];
-    return AABB_corner.array().max(0).matrix().squaredNorm ();
-  }
+inline CoalScalar obbDisjoint_check_B_axis(const Matrix3s& B, const Vec3s& T,
+                                           const Vec3s& a, const Vec3s& b,
+                                           const Matrix3s& Bf) {
+  // Bf = |B|
+  // | B^T T| - Bf^T * a - b
+  CoalScalar s, t = 0;
+  s = std::abs(B.col(0).dot(T)) - Bf.col(0).dot(a) - b[0];
+  if (s > 0) t += s * s;
+  s = std::abs(B.col(1).dot(T)) - Bf.col(1).dot(a) - b[1];
+  if (s > 0) t += s * s;
+  s = std::abs(B.col(2).dot(T)) - Bf.col(2).dot(a) - b[2];
+  if (s > 0) t += s * s;
+  return t;
+}
 
-  inline FCL_REAL obbDisjoint_check_B_axis (
-      const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b, const Matrix3f& Bf)
-  {
-    // Bf = |B|
-    // | B^T T| - Bf^T * a - b
-    register FCL_REAL s, t = 0;
-    s = std::abs(B.col(0).dot(T)) - Bf.col(0).dot(a) - b[0];
-    if (s > 0) t += s*s;
-    s = std::abs(B.col(1).dot(T)) - Bf.col(1).dot(a) - b[1];
-    if (s > 0) t += s*s;
-    s = std::abs(B.col(2).dot(T)) - Bf.col(2).dot(a) - b[2];
-    if (s > 0) t += s*s;
-    return t;
+template <int ib, int jb = (ib + 1) % 3, int kb = (ib + 2) % 3>
+struct COAL_LOCAL obbDisjoint_check_Ai_cross_Bi{static inline bool run(
+    int ia, int ja, int ka, const Matrix3s& B, const Vec3s& T, const Vec3s& a,
+    const Vec3s& b, const Matrix3s& Bf, const CoalScalar& breakDistance2,
+    CoalScalar& squaredLowerBoundDistance){CoalScalar sinus2 =
+                                               1 - Bf(ia, ib) * Bf(ia, ib);
+if (sinus2 < 1e-6) return false;
+
+const CoalScalar s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib);
+
+const CoalScalar diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) +
+                                   b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb));
+// We need to divide by the norm || Aia x Bib ||
+// As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2  = cosine^2
+if (diff > 0) {
+  squaredLowerBoundDistance = diff * diff / sinus2;
+  if (squaredLowerBoundDistance > breakDistance2) {
+    return true;
   }
-
-  template <int ib, int jb = (ib+1)%3, int kb = (ib+2)%3 >
-  struct obbDisjoint_check_Ai_cross_Bi
-  {
-    static inline bool run (int ia, int ja, int ka,
-        const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b,
-        const Matrix3f& Bf,
-        const FCL_REAL& breakDistance2, FCL_REAL& squaredLowerBoundDistance)
-    {
-      const FCL_REAL s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib);
-
-      const FCL_REAL diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) +
-                                       b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb));
-      // We need to divide by the norm || Aia x Bib ||
-      // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2  = cosine^2
-      if (diff > 0) {
-        FCL_REAL sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib);
-        if (sinus2 > 1e-6) {
-          squaredLowerBoundDistance = diff * diff / sinus2;
-          if (squaredLowerBoundDistance > breakDistance2) {
-            return true;
-          }
-        }
-        /* // or
-           FCL_REAL sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib);
-           squaredLowerBoundDistance = diff * diff;
-           if (squaredLowerBoundDistance > breakDistance2 * sinus2) {
-           squaredLowerBoundDistance /= sinus2;
-           return true;
-           }
-        // */
-      }
-      return false;
-    }
-  };
 }
-
+return false;
+}  // namespace internal
+};  // namespace coal
+}  // namespace internal
 
 // B, T orientation and position of 2nd OBB in frame of 1st OBB,
 // a extent of 1st OBB,
@@ -366,93 +340,102 @@ namespace internal
 //
 // This function tests whether bounding boxes should be broken down.
 //
-bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,
-                                       const Vec3f& a, const Vec3f& b,
-                                       const CollisionRequest& request,
-                                       FCL_REAL& squaredLowerBoundDistance)
-{
-  const FCL_REAL breakDistance (request.break_distance + request.security_margin);
-  const FCL_REAL breakDistance2 = breakDistance * breakDistance;
-
-  Matrix3f Bf (B.cwiseAbs());
+bool obbDisjointAndLowerBoundDistance(const Matrix3s& B, const Vec3s& T,
+                                      const Vec3s& a_, const Vec3s& b_,
+                                      const CollisionRequest& request,
+                                      CoalScalar& squaredLowerBoundDistance) {
+  assert(request.security_margin >
+             -2 * (std::min)(a_.minCoeff(), b_.minCoeff()) -
+                 10 * Eigen::NumTraits<CoalScalar>::epsilon() &&
+         "A negative security margin could not be lower than the OBB extent.");
+  //  const CoalScalar breakDistance(request.break_distance +
+  //                               request.security_margin);
+  const CoalScalar breakDistance2 =
+      request.break_distance * request.break_distance;
+
+  Matrix3s Bf(B.cwiseAbs());
+  const Vec3s a((a_ + Vec3s::Constant(request.security_margin / 2))
+                    .array()
+                    .max(CoalScalar(0)));
+  const Vec3s b((b_ + Vec3s::Constant(request.security_margin / 2))
+                    .array()
+                    .max(CoalScalar(0)));
 
   // Corner of b axis aligned bounding box the closest to the origin
-  squaredLowerBoundDistance = internal::obbDisjoint_check_A_axis (T, a, b, Bf);
-  if (squaredLowerBoundDistance > breakDistance2)
-    return true;
+  squaredLowerBoundDistance = internal::obbDisjoint_check_A_axis(T, a, b, Bf);
+  if (squaredLowerBoundDistance > breakDistance2) return true;
 
-  squaredLowerBoundDistance = internal::obbDisjoint_check_B_axis (B, T, a, b, Bf);
-  if (squaredLowerBoundDistance > breakDistance2)
-    return true;
+  squaredLowerBoundDistance =
+      internal::obbDisjoint_check_B_axis(B, T, a, b, Bf);
+  if (squaredLowerBoundDistance > breakDistance2) return true;
 
   // Ai x Bj
   int ja = 1, ka = 2;
   for (int ia = 0; ia < 3; ++ia) {
-    if (internal::obbDisjoint_check_Ai_cross_Bi<0>::run (ia, ja, ka,
-          B, T, a, b, Bf, breakDistance2, squaredLowerBoundDistance)) return true;
-    if (internal::obbDisjoint_check_Ai_cross_Bi<1>::run (ia, ja, ka,
-          B, T, a, b, Bf, breakDistance2, squaredLowerBoundDistance)) return true;
-    if (internal::obbDisjoint_check_Ai_cross_Bi<2>::run (ia, ja, ka,
-          B, T, a, b, Bf, breakDistance2, squaredLowerBoundDistance)) return true;
-    ja = ka; ka = ia;
+    if (internal::obbDisjoint_check_Ai_cross_Bi<0>::run(
+            ia, ja, ka, B, T, a, b, Bf, breakDistance2,
+            squaredLowerBoundDistance))
+      return true;
+    if (internal::obbDisjoint_check_Ai_cross_Bi<1>::run(
+            ia, ja, ka, B, T, a, b, Bf, breakDistance2,
+            squaredLowerBoundDistance))
+      return true;
+    if (internal::obbDisjoint_check_Ai_cross_Bi<2>::run(
+            ia, ja, ka, B, T, a, b, Bf, breakDistance2,
+            squaredLowerBoundDistance))
+      return true;
+    ja = ka;
+    ka = ia;
   }
 
   return false;
 }
 
-bool OBB::overlap(const OBB& other) const
-{
+bool OBB::overlap(const OBB& other) const {
   /// compute what transform [R,T] that takes us from cs1 to cs2.
   /// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)]
   /// First compute the rotation part, then translation part
-  Vec3f T (axes.transpose() * (other.To - To));
-  Matrix3f R (axes.transpose() * other.axes);
+  Vec3s T(axes.transpose() * (other.To - To));
+  Matrix3s R(axes.transpose() * other.axes);
 
   return !obbDisjoint(R, T, extent, other.extent);
 }
 
-  bool OBB::overlap(const OBB& other, const CollisionRequest& request,
-                    FCL_REAL& sqrDistLowerBound) const
-  {
-    /// compute what transform [R,T] that takes us from cs1 to cs2.
-    /// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)]
-    /// First compute the rotation part, then translation part
-    // Vec3f t = other.To - To; // T2 - T1
-    // Vec3f T(t.dot(axis[0]), t.dot(axis[1]), t.dot(axis[2])); // R1'(T2-T1)
-    // Matrix3f R(axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]),
-	       // axis[0].dot(other.axis[2]),
-	       // axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]),
-	       // axis[1].dot(other.axis[2]),
-	       // axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]),
-	       // axis[2].dot(other.axis[2]));
-  Vec3f T (axes.transpose() * (other.To - To));
-  Matrix3f R (axes.transpose() * other.axes);
-
-  return !obbDisjointAndLowerBoundDistance
-    (R, T, extent, other.extent, request, sqrDistLowerBound);
+bool OBB::overlap(const OBB& other, const CollisionRequest& request,
+                  CoalScalar& sqrDistLowerBound) const {
+  /// compute what transform [R,T] that takes us from cs1 to cs2.
+  /// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)]
+  /// First compute the rotation part, then translation part
+  // Vec3s t = other.To - To; // T2 - T1
+  // Vec3s T(t.dot(axis[0]), t.dot(axis[1]), t.dot(axis[2])); // R1'(T2-T1)
+  // Matrix3s R(axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]),
+  // axis[0].dot(other.axis[2]),
+  // axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]),
+  // axis[1].dot(other.axis[2]),
+  // axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]),
+  // axis[2].dot(other.axis[2]));
+  Vec3s T(axes.transpose() * (other.To - To));
+  Matrix3s R(axes.transpose() * other.axes);
+
+  return !obbDisjointAndLowerBoundDistance(R, T, extent, other.extent, request,
+                                           sqrDistLowerBound);
 }
 
-
-bool OBB::contain(const Vec3f& p) const
-{
-  Vec3f local_p (p - To);
-  FCL_REAL proj = local_p.dot(axes.col(0));
-  if((proj > extent[0]) || (proj < -extent[0]))
-    return false;
+bool OBB::contain(const Vec3s& p) const {
+  Vec3s local_p(p - To);
+  CoalScalar proj = local_p.dot(axes.col(0));
+  if ((proj > extent[0]) || (proj < -extent[0])) return false;
 
   proj = local_p.dot(axes.col(1));
-  if((proj > extent[1]) || (proj < -extent[1]))
-    return false;
+  if ((proj > extent[1]) || (proj < -extent[1])) return false;
 
   proj = local_p.dot(axes.col(2));
-  if((proj > extent[2]) || (proj < -extent[2]))
-    return false;
+  if ((proj > extent[2]) || (proj < -extent[2])) return false;
 
   return true;
 }
 
-OBB& OBB::operator += (const Vec3f& p)
-{
+OBB& OBB::operator+=(const Vec3s& p) {
   OBB bvp;
   bvp.To = p;
   bvp.axes.noalias() = axes;
@@ -462,56 +445,47 @@ OBB& OBB::operator += (const Vec3f& p)
   return *this;
 }
 
-OBB OBB::operator + (const OBB& other) const
-{
-  Vec3f center_diff = To - other.To;
-  FCL_REAL max_extent = std::max(std::max(extent[0], extent[1]), extent[2]);
-  FCL_REAL max_extent2 = std::max(std::max(other.extent[0], other.extent[1]), other.extent[2]);
-  if(center_diff.norm() > 2 * (max_extent + max_extent2))
-  {
+OBB OBB::operator+(const OBB& other) const {
+  Vec3s center_diff = To - other.To;
+  CoalScalar max_extent = std::max(std::max(extent[0], extent[1]), extent[2]);
+  CoalScalar max_extent2 =
+      std::max(std::max(other.extent[0], other.extent[1]), other.extent[2]);
+  if (center_diff.norm() > 2 * (max_extent + max_extent2)) {
     return merge_largedist(*this, other);
-  }
-  else
-  {
+  } else {
     return merge_smalldist(*this, other);
   }
 }
 
-
-FCL_REAL OBB::distance(const OBB& /*other*/, Vec3f* /*P*/, Vec3f* /*Q*/) const
-{
+CoalScalar OBB::distance(const OBB& /*other*/, Vec3s* /*P*/,
+                         Vec3s* /*Q*/) const {
   std::cerr << "OBB distance not implemented!" << std::endl;
   return 0.0;
 }
 
-
-bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2)
-{
-  Vec3f Ttemp (R0 * b2.To + T0 - b1.To);
-  Vec3f T (b1.axes.transpose() * Ttemp);
-  Matrix3f R (b1.axes.transpose() * R0 * b2.axes);
+bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1,
+             const OBB& b2) {
+  Vec3s Ttemp(R0.transpose() * (b2.To - T0) - b1.To);
+  Vec3s T(b1.axes.transpose() * Ttemp);
+  Matrix3s R(b1.axes.transpose() * R0.transpose() * b2.axes);
 
   return !obbDisjoint(R, T, b1.extent, b2.extent);
 }
 
-bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2,
-	     const CollisionRequest& request, FCL_REAL& sqrDistLowerBound)
-{
-  Vec3f Ttemp (R0 * b2.To + T0 - b1.To);
-  Vec3f T (b1.axes.transpose() * Ttemp);
-  Matrix3f R (b1.axes.transpose() * R0 * b2.axes);
+bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1, const OBB& b2,
+             const CollisionRequest& request, CoalScalar& sqrDistLowerBound) {
+  Vec3s Ttemp(R0.transpose() * (b2.To - T0) - b1.To);
+  Vec3s T(b1.axes.transpose() * Ttemp);
+  Matrix3s R(b1.axes.transpose() * R0.transpose() * b2.axes);
 
-  return !obbDisjointAndLowerBoundDistance (R, T, b1.extent, b2.extent,
-					    request, sqrDistLowerBound);
+  return !obbDisjointAndLowerBoundDistance(R, T, b1.extent, b2.extent, request,
+                                           sqrDistLowerBound);
 }
 
-OBB translate(const OBB& bv, const Vec3f& t)
-{
+OBB translate(const OBB& bv, const Vec3s& t) {
   OBB res(bv);
   res.To += t;
   return res;
 }
 
-}
-
-} // namespace hpp
+}  // namespace coal

src/BV/OBB.h

@@ -33,23 +33,18 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  */
 
-#ifndef HPP_FCL_SRC_OBB_H
-# define HPP_FCL_SRC_OBB_H
+#ifndef COAL_SRC_OBB_H
+#define COAL_SRC_OBB_H
 
-namespace hpp
-{
-namespace fcl
-{
+namespace coal {
 
-  bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,
-					 const Vec3f& a, const Vec3f& b,
-                                         const CollisionRequest& request,
-					 FCL_REAL& squaredLowerBoundDistance);
+bool obbDisjointAndLowerBoundDistance(const Matrix3s& B, const Vec3s& T,
+                                      const Vec3s& a, const Vec3s& b,
+                                      const CollisionRequest& request,
+                                      CoalScalar& squaredLowerBoundDistance);
 
-  bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a,
-		   const Vec3f& b);
-} // namespace fcl
+bool obbDisjoint(const Matrix3s& B, const Vec3s& T, const Vec3s& a,
+                 const Vec3s& b);
+}  // namespace coal
 
-} // namespace hpp
-
-#endif // HPP_FCL_SRC_OBB_H
+#endif  // COAL_SRC_OBB_H

src/BV/OBBRSS.cpp

@@ -35,22 +35,15 @@
 
 /** \author Jia Pan */
 
-#include <hpp/fcl/BV/OBBRSS.h>
+#include "coal/BV/OBBRSS.h"
 
-namespace hpp
-{
-namespace fcl
-{
+namespace coal {
 
-OBBRSS translate(const OBBRSS& bv, const Vec3f& t)
-{
+OBBRSS translate(const OBBRSS& bv, const Vec3s& t) {
   OBBRSS res(bv);
   res.obb.To += t;
   res.rss.Tr += t;
   return res;
 }
 
-
-}
-
-} // namespace hpp
+}  // namespace coal