include/coal/serialization/archive.h

+//
+// Copyright (c) 2017-2024 CNRS INRIA
+// This file was borrowed from the Pinocchio library:
+// https://github.com/stack-of-tasks/pinocchio
+//
+
+#ifndef COAL_SERIALIZATION_ARCHIVE_H
+#define COAL_SERIALIZATION_ARCHIVE_H
+
+#include "coal/fwd.hh"
+
+#include <boost/serialization/nvp.hpp>
+#include <boost/archive/text_iarchive.hpp>
+#include <boost/archive/text_oarchive.hpp>
+#include <boost/archive/xml_iarchive.hpp>
+#include <boost/archive/xml_oarchive.hpp>
+#include <boost/archive/binary_iarchive.hpp>
+#include <boost/archive/binary_oarchive.hpp>
+
+#include <fstream>
+#include <string>
+#include <sstream>
+#include <stdexcept>
+
+#if BOOST_VERSION / 100 % 1000 == 78 && __APPLE__
+// See https://github.com/qcscine/utilities/issues/5#issuecomment-1246897049 for
+// further details
+
+#ifndef BOOST_ASIO_DISABLE_STD_ALIGNED_ALLOC
+#define DEFINE_BOOST_ASIO_DISABLE_STD_ALIGNED_ALLOC
+#define BOOST_ASIO_DISABLE_STD_ALIGNED_ALLOC
+#endif
+
+#include <boost/asio/streambuf.hpp>
+
+#ifdef DEFINE_BOOST_ASIO_DISABLE_STD_ALIGNED_ALLOC
+#undef BOOST_ASIO_DISABLE_STD_ALIGNED_ALLOC
+#endif
+
+#else
+#include <boost/asio/streambuf.hpp>
+#endif
+
+#include <boost/iostreams/device/array.hpp>
+#include <boost/iostreams/stream.hpp>
+#include <boost/iostreams/stream_buffer.hpp>
+
+// Handle NAN inside TXT or XML archives
+#include <boost/math/special_functions/nonfinite_num_facets.hpp>
+
+namespace coal {
+namespace serialization {
+
+///
+/// \brief Loads an object from a TXT file.
+///
+/// \tparam T Type of the object to deserialize.
+///
+/// \param[out] object Object in which the loaded data are copied.
+/// \param[in]  filename Name of the file containing the serialized data.
+///
+template <typename T>
+inline void loadFromText(T& object, const std::string& filename) {
+  std::ifstream ifs(filename.c_str());
+  if (ifs) {
+    std::locale const new_loc(ifs.getloc(),
+                              new boost::math::nonfinite_num_get<char>);
+    ifs.imbue(new_loc);
+    boost::archive::text_iarchive ia(ifs, boost::archive::no_codecvt);
+    ia >> object;
+  } else {
+    const std::string exception_message(filename +
+                                        " does not seem to be a valid file.");
+    throw std::invalid_argument(exception_message);
+  }
+}
+
+///
+/// \brief Saves an object inside a TXT file.
+///
+/// \tparam T Type of the object to deserialize.
+///
+/// \param[in]  object Object in which the loaded data are copied.
+/// \param[in]  filename Name of the file containing the serialized data.
+///
+template <typename T>
+inline void saveToText(const T& object, const std::string& filename) {
+  std::ofstream ofs(filename.c_str());
+  if (ofs) {
+    boost::archive::text_oarchive oa(ofs);
+    oa & object;
+  } else {
+    const std::string exception_message(filename +
+                                        " does not seem to be a valid file.");
+    throw std::invalid_argument(exception_message);
+  }
+}
+
+///
+/// \brief Loads an object from a std::stringstream.
+///
+/// \tparam T Type of the object to deserialize.
+///
+/// \param[out] object Object in which the loaded data are copied.
+/// \param[in]  is  string stream constaining the serialized content of the
+/// object.
+///
+template <typename T>
+inline void loadFromStringStream(T& object, std::istringstream& is) {
+  std::locale const new_loc(is.getloc(),
+                            new boost::math::nonfinite_num_get<char>);
+  is.imbue(new_loc);
+  boost::archive::text_iarchive ia(is, boost::archive::no_codecvt);
+  ia >> object;
+}
+
+///
+/// \brief Saves an object inside a std::stringstream.
+///
+/// \tparam T Type of the object to deserialize.
+///
+/// \param[in]   object Object in which the loaded data are copied.
+/// \param[out]  ss String stream constaining the serialized content of the
+/// object.
+///
+template <typename T>
+inline void saveToStringStream(const T& object, std::stringstream& ss) {
+  boost::archive::text_oarchive oa(ss);
+  oa & object;
+}
+
+///
+/// \brief Loads an object from a std::string
+///
+/// \tparam T Type of the object to deserialize.
+///
+/// \param[out] object Object in which the loaded data are copied.
+/// \param[in]  str  string constaining the serialized content of the object.
+///
+template <typename T>
+inline void loadFromString(T& object, const std::string& str) {
+  std::istringstream is(str);
+  loadFromStringStream(object, is);
+}
+
+///
+/// \brief Saves an object inside a std::string
+///
+/// \tparam T Type of the object to deserialize.
+///
+/// \param[in] object Object in which the loaded data are copied.
+///
+/// \returns a string  constaining the serialized content of the object.
+///
+template <typename T>
+inline std::string saveToString(const T& object) {
+  std::stringstream ss;
+  saveToStringStream(object, ss);
+  return ss.str();
+}
+
+///
+/// \brief Loads an object from a XML file.
+///
+/// \tparam T Type of the object to deserialize.
+///
+/// \param[out] object Object in which the loaded data are copied.
+/// \param[in] filename Name of the file containing the serialized data.
+/// \param[in] tag_name XML Tag for the given object.
+///
+template <typename T>
+inline void loadFromXML(T& object, const std::string& filename,
+                        const std::string& tag_name) {
+  if (filename.empty()) {
+    COAL_THROW_PRETTY("Tag name should not be empty.", std::invalid_argument);
+  }
+
+  std::ifstream ifs(filename.c_str());
+  if (ifs) {
+    std::locale const new_loc(ifs.getloc(),
+                              new boost::math::nonfinite_num_get<char>);
+    ifs.imbue(new_loc);
+    boost::archive::xml_iarchive ia(ifs, boost::archive::no_codecvt);
+    ia >> boost::serialization::make_nvp(tag_name.c_str(), object);
+  } else {
+    const std::string exception_message(filename +
+                                        " does not seem to be a valid file.");
+    throw std::invalid_argument(exception_message);
+  }
+}
+
+///
+/// \brief Saves an object inside a XML file.
+///
+/// \tparam T Type of the object to deserialize.
+///
+/// \param[in] object Object in which the loaded data are copied.
+/// \param[in] filename Name of the file containing the serialized data.
+/// \param[in] tag_name XML Tag for the given object.
+///
+template <typename T>
+inline void saveToXML(const T& object, const std::string& filename,
+                      const std::string& tag_name) {
+  if (filename.empty()) {
+    COAL_THROW_PRETTY("Tag name should not be empty.", std::invalid_argument);
+  }
+
+  std::ofstream ofs(filename.c_str());
+  if (ofs) {
+    boost::archive::xml_oarchive oa(ofs);
+    oa& boost::serialization::make_nvp(tag_name.c_str(), object);
+  } else {
+    const std::string exception_message(filename +
+                                        " does not seem to be a valid file.");
+    throw std::invalid_argument(exception_message);
+  }
+}
+
+///
+/// \brief Loads an object from a binary file.
+///
+/// \tparam T Type of the object to deserialize.
+///
+/// \param[out] object Object in which the loaded data are copied.
+/// \param[in] filename Name of the file containing the serialized data.
+///
+template <typename T>
+inline void loadFromBinary(T& object, const std::string& filename) {
+  std::ifstream ifs(filename.c_str(), std::ios::binary);
+  if (ifs) {
+    boost::archive::binary_iarchive ia(ifs);
+    ia >> object;
+  } else {
+    const std::string exception_message(filename +
+                                        " does not seem to be a valid file.");
+    throw std::invalid_argument(exception_message);
+  }
+}
+
+///
+/// \brief Saves an object inside a binary file.
+///
+/// \tparam T Type of the object to deserialize.
+///
+/// \param[in] object Object in which the loaded data are copied.
+/// \param[in] filename Name of the file containing the serialized data.
+///
+template <typename T>
+void saveToBinary(const T& object, const std::string& filename) {
+  std::ofstream ofs(filename.c_str(), std::ios::binary);
+  if (ofs) {
+    boost::archive::binary_oarchive oa(ofs);
+    oa & object;
+  } else {
+    const std::string exception_message(filename +
+                                        " does not seem to be a valid file.");
+    throw std::invalid_argument(exception_message);
+  }
+}
+
+///
+/// \brief Loads an object from a binary buffer.
+///
+/// \tparam T Type of the object to deserialize.
+///
+/// \param[out] object Object in which the loaded data are copied.
+/// \param[in] buffer Input buffer containing the serialized data.
+///
+template <typename T>
+inline void loadFromBuffer(T& object, boost::asio::streambuf& buffer) {
+  boost::archive::binary_iarchive ia(buffer);
+  ia >> object;
+}
+
+///
+/// \brief Saves an object to a binary buffer.
+///
+/// \tparam T Type of the object to serialize.
+///
+/// \param[in]  object Object in which the loaded data are copied.
+/// \param[out] buffer Output buffer containing the serialized data.
+///
+template <typename T>
+void saveToBuffer(const T& object, boost::asio::streambuf& buffer) {
+  boost::archive::binary_oarchive oa(buffer);
+  oa & object;
+}
+
+}  // namespace serialization
+}  // namespace coal
+
+#endif  // ifndef COAL_SERIALIZATION_ARCHIVE_H

include/coal/serialization/collision_data.h

+//
+// Copyright (c) 2021 INRIA
+//
+
+#ifndef COAL_SERIALIZATION_COLLISION_DATA_H
+#define COAL_SERIALIZATION_COLLISION_DATA_H
+
+#include "coal/collision_data.h"
+#include "coal/serialization/fwd.h"
+
+namespace boost {
+namespace serialization {
+
+template <class Archive>
+void save(Archive& ar, const coal::Contact& contact,
+          const unsigned int /*version*/) {
+  ar& make_nvp("b1", contact.b1);
+  ar& make_nvp("b2", contact.b2);
+  ar& make_nvp("normal", contact.normal);
+  ar& make_nvp("nearest_points", contact.nearest_points);
+  ar& make_nvp("pos", contact.pos);
+  ar& make_nvp("penetration_depth", contact.penetration_depth);
+}
+
+template <class Archive>
+void load(Archive& ar, coal::Contact& contact, const unsigned int /*version*/) {
+  ar >> make_nvp("b1", contact.b1);
+  ar >> make_nvp("b2", contact.b2);
+  ar >> make_nvp("normal", contact.normal);
+  std::array<coal::Vec3s, 2> nearest_points;
+  ar >> make_nvp("nearest_points", nearest_points);
+  contact.nearest_points[0] = nearest_points[0];
+  contact.nearest_points[1] = nearest_points[1];
+  ar >> make_nvp("pos", contact.pos);
+  ar >> make_nvp("penetration_depth", contact.penetration_depth);
+  contact.o1 = NULL;
+  contact.o2 = NULL;
+}
+
+COAL_SERIALIZATION_SPLIT(coal::Contact)
+
+template <class Archive>
+void serialize(Archive& ar, coal::QueryRequest& query_request,
+               const unsigned int /*version*/) {
+  ar& make_nvp("gjk_initial_guess", query_request.gjk_initial_guess);
+  // TODO: use gjk_initial_guess instead
+  COAL_COMPILER_DIAGNOSTIC_PUSH
+  COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+  ar& make_nvp("enable_cached_gjk_guess",
+               query_request.enable_cached_gjk_guess);
+  COAL_COMPILER_DIAGNOSTIC_POP
+  ar& make_nvp("cached_gjk_guess", query_request.cached_gjk_guess);
+  ar& make_nvp("cached_support_func_guess",
+               query_request.cached_support_func_guess);
+  ar& make_nvp("gjk_max_iterations", query_request.gjk_max_iterations);
+  ar& make_nvp("gjk_tolerance", query_request.gjk_tolerance);
+  ar& make_nvp("gjk_variant", query_request.gjk_variant);
+  ar& make_nvp("gjk_convergence_criterion",
+               query_request.gjk_convergence_criterion);
+  ar& make_nvp("gjk_convergence_criterion_type",
+               query_request.gjk_convergence_criterion_type);
+  ar& make_nvp("epa_max_iterations", query_request.epa_max_iterations);
+  ar& make_nvp("epa_tolerance", query_request.epa_tolerance);
+  ar& make_nvp("collision_distance_threshold",
+               query_request.collision_distance_threshold);
+  ar& make_nvp("enable_timings", query_request.enable_timings);
+}
+
+template <class Archive>
+void serialize(Archive& ar, coal::QueryResult& query_result,
+               const unsigned int /*version*/) {
+  ar& make_nvp("cached_gjk_guess", query_result.cached_gjk_guess);
+  ar& make_nvp("cached_support_func_guess",
+               query_result.cached_support_func_guess);
+}
+
+template <class Archive>
+void serialize(Archive& ar, coal::CollisionRequest& collision_request,
+               const unsigned int /*version*/) {
+  ar& make_nvp("base", boost::serialization::base_object<coal::QueryRequest>(
+                           collision_request));
+  ar& make_nvp("num_max_contacts", collision_request.num_max_contacts);
+  ar& make_nvp("enable_contact", collision_request.enable_contact);
+  COAL_COMPILER_DIAGNOSTIC_PUSH
+  COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+  ar& make_nvp("enable_distance_lower_bound",
+               collision_request.enable_distance_lower_bound);
+  COAL_COMPILER_DIAGNOSTIC_POP
+  ar& make_nvp("security_margin", collision_request.security_margin);
+  ar& make_nvp("break_distance", collision_request.break_distance);
+  ar& make_nvp("distance_upper_bound", collision_request.distance_upper_bound);
+}
+
+template <class Archive>
+void save(Archive& ar, const coal::CollisionResult& collision_result,
+          const unsigned int /*version*/) {
+  ar& make_nvp("base", boost::serialization::base_object<coal::QueryResult>(
+                           collision_result));
+  ar& make_nvp("contacts", collision_result.getContacts());
+  ar& make_nvp("distance_lower_bound", collision_result.distance_lower_bound);
+  ar& make_nvp("nearest_points", collision_result.nearest_points);
+  ar& make_nvp("normal", collision_result.normal);
+}
+
+template <class Archive>
+void load(Archive& ar, coal::CollisionResult& collision_result,
+          const unsigned int /*version*/) {
+  ar >> make_nvp("base", boost::serialization::base_object<coal::QueryResult>(
+                             collision_result));
+  std::vector<coal::Contact> contacts;
+  ar >> make_nvp("contacts", contacts);
+  collision_result.clear();
+  for (size_t k = 0; k < contacts.size(); ++k)
+    collision_result.addContact(contacts[k]);
+  ar >> make_nvp("distance_lower_bound", collision_result.distance_lower_bound);
+  std::array<coal::Vec3s, 2> nearest_points;
+  ar >> make_nvp("nearest_points", nearest_points);
+  collision_result.nearest_points[0] = nearest_points[0];
+  collision_result.nearest_points[1] = nearest_points[1];
+  ar >> make_nvp("normal", collision_result.normal);
+}
+
+COAL_SERIALIZATION_SPLIT(coal::CollisionResult)
+
+template <class Archive>
+void serialize(Archive& ar, coal::DistanceRequest& distance_request,
+               const unsigned int /*version*/) {
+  ar& make_nvp("base", boost::serialization::base_object<coal::QueryRequest>(
+                           distance_request));
+  COAL_COMPILER_DIAGNOSTIC_PUSH
+  COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+  ar& make_nvp("enable_nearest_points", distance_request.enable_nearest_points);
+  COAL_COMPILER_DIAGNOSTIC_POP
+  ar& make_nvp("enable_signed_distance",
+               distance_request.enable_signed_distance);
+  ar& make_nvp("rel_err", distance_request.rel_err);
+  ar& make_nvp("abs_err", distance_request.abs_err);
+}
+
+template <class Archive>
+void save(Archive& ar, const coal::DistanceResult& distance_result,
+          const unsigned int /*version*/) {
+  ar& make_nvp("base", boost::serialization::base_object<coal::QueryResult>(
+                           distance_result));
+  ar& make_nvp("min_distance", distance_result.min_distance);
+  ar& make_nvp("nearest_points", distance_result.nearest_points);
+  ar& make_nvp("normal", distance_result.normal);
+  ar& make_nvp("b1", distance_result.b1);
+  ar& make_nvp("b2", distance_result.b2);
+}
+
+template <class Archive>
+void load(Archive& ar, coal::DistanceResult& distance_result,
+          const unsigned int /*version*/) {
+  ar >> make_nvp("base", boost::serialization::base_object<coal::QueryResult>(
+                             distance_result));
+  ar >> make_nvp("min_distance", distance_result.min_distance);
+  std::array<coal::Vec3s, 2> nearest_points;
+  ar >> make_nvp("nearest_points", nearest_points);
+  distance_result.nearest_points[0] = nearest_points[0];
+  distance_result.nearest_points[1] = nearest_points[1];
+  ar >> make_nvp("normal", distance_result.normal);
+  ar >> make_nvp("b1", distance_result.b1);
+  ar >> make_nvp("b2", distance_result.b2);
+  distance_result.o1 = NULL;
+  distance_result.o2 = NULL;
+}
+
+COAL_SERIALIZATION_SPLIT(coal::DistanceResult)
+
+}  // namespace serialization
+}  // namespace boost
+
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::CollisionRequest)
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::CollisionResult)
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::DistanceRequest)
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::DistanceResult)
+
+#endif  // ifndef COAL_SERIALIZATION_COLLISION_DATA_H

include/coal/serialization/collision_object.h

+//
+// Copyright (c) 2021 INRIA
+//
+
+#ifndef COAL_SERIALIZATION_COLLISION_OBJECT_H
+#define COAL_SERIALIZATION_COLLISION_OBJECT_H
+
+#include "coal/collision_object.h"
+
+#include "coal/serialization/fwd.h"
+#include "coal/serialization/AABB.h"
+
+namespace boost {
+namespace serialization {
+
+template <class Archive>
+void save(Archive& ar, const coal::CollisionGeometry& collision_geometry,
+          const unsigned int /*version*/) {
+  ar& make_nvp("aabb_center", collision_geometry.aabb_center);
+  ar& make_nvp("aabb_radius", collision_geometry.aabb_radius);
+  ar& make_nvp("aabb_local", collision_geometry.aabb_local);
+  ar& make_nvp("cost_density", collision_geometry.cost_density);
+  ar& make_nvp("threshold_occupied", collision_geometry.threshold_occupied);
+  ar& make_nvp("threshold_free", collision_geometry.threshold_free);
+}
+
+template <class Archive>
+void load(Archive& ar, coal::CollisionGeometry& collision_geometry,
+          const unsigned int /*version*/) {
+  ar >> make_nvp("aabb_center", collision_geometry.aabb_center);
+  ar >> make_nvp("aabb_radius", collision_geometry.aabb_radius);
+  ar >> make_nvp("aabb_local", collision_geometry.aabb_local);
+  ar >> make_nvp("cost_density", collision_geometry.cost_density);
+  ar >> make_nvp("threshold_occupied", collision_geometry.threshold_occupied);
+  ar >> make_nvp("threshold_free", collision_geometry.threshold_free);
+  collision_geometry.user_data = NULL;  // no way to recover this
+}
+
+COAL_SERIALIZATION_SPLIT(coal::CollisionGeometry)
+
+}  // namespace serialization
+}  // namespace boost
+
+namespace coal {
+
+// fwd declaration
+template <typename BV>
+class HeightField;
+
+template <typename PolygonT>
+class Convex;
+
+struct OBB;
+struct OBBRSS;
+class AABB;
+
+class OcTree;
+class Box;
+class Sphere;
+class Ellipsoid;
+class Capsule;
+class Cone;
+class TriangleP;
+class Cylinder;
+class Halfspace;
+class Plane;
+
+namespace serialization {
+template <>
+struct register_type<CollisionGeometry> {
+  template <class Archive>
+  static void on(Archive& ar) {
+    ar.template register_type<Box>();
+    ar.template register_type<Sphere>();
+    ar.template register_type<Ellipsoid>();
+    ar.template register_type<TriangleP>();
+    ar.template register_type<Capsule>();
+    ar.template register_type<Cone>();
+    ar.template register_type<Cylinder>();
+    ar.template register_type<Halfspace>();
+    ar.template register_type<Plane>();
+    ar.template register_type<OcTree>();
+    ar.template register_type<HeightField<OBB>>();
+    ar.template register_type<HeightField<OBBRSS>>();
+    ar.template register_type<HeightField<AABB>>();
+    ar.template register_type<Convex<Triangle>>();
+    ;
+  }
+};
+}  // namespace serialization
+}  // namespace coal
+
+#endif  // ifndef COAL_SERIALIZATION_COLLISION_OBJECT_H

include/coal/serialization/contact_patch.h

+//
+// Copyright (c) 2024 INRIA
+//
+
+#ifndef COAL_SERIALIZATION_CONTACT_PATCH_H
+#define COAL_SERIALIZATION_CONTACT_PATCH_H
+
+#include "coal/collision_data.h"
+#include "coal/serialization/fwd.h"
+#include "coal/serialization/transform.h"
+
+namespace boost {
+namespace serialization {
+
+template <class Archive>
+void serialize(Archive& ar, coal::ContactPatch& contact_patch,
+               const unsigned int /*version*/) {
+  using namespace coal;
+  typedef Eigen::Matrix<CoalScalar, 2, Eigen::Dynamic> PolygonPoints;
+
+  size_t patch_size = contact_patch.size();
+  ar& make_nvp("patch_size", patch_size);
+  if (patch_size > 0) {
+    if (Archive::is_loading::value) {
+      contact_patch.points().resize(patch_size);
+    }
+    Eigen::Map<PolygonPoints> points_map(
+        reinterpret_cast<CoalScalar*>(contact_patch.points().data()), 2,
+        static_cast<Eigen::Index>(patch_size));
+    ar& make_nvp("points", points_map);
+  }
+
+  ar& make_nvp("penetration_depth", contact_patch.penetration_depth);
+  ar& make_nvp("direction", contact_patch.direction);
+  ar& make_nvp("tf", contact_patch.tf);
+}
+
+template <class Archive>
+void serialize(Archive& ar, coal::ContactPatchRequest& request,
+               const unsigned int /*version*/) {
+  using namespace coal;
+
+  ar& make_nvp("max_num_patch", request.max_num_patch);
+
+  size_t num_samples_curved_shapes = request.getNumSamplesCurvedShapes();
+  CoalScalar patch_tolerance = request.getPatchTolerance();
+  ar& make_nvp("num_samples_curved_shapes", num_samples_curved_shapes);
+  ar& make_nvp("patch_tolerance", num_samples_curved_shapes);
+
+  if (Archive::is_loading::value) {
+    request.setNumSamplesCurvedShapes(num_samples_curved_shapes);
+    request.setPatchTolerance(patch_tolerance);
+  }
+}
+
+template <class Archive>
+void serialize(Archive& ar, coal::ContactPatchResult& result,
+               const unsigned int /*version*/) {
+  using namespace coal;
+
+  size_t num_patches = result.numContactPatches();
+  ar& make_nvp("num_patches", num_patches);
+
+  std::vector<ContactPatch> patches;
+  patches.resize(num_patches);
+  if (Archive::is_loading::value) {
+    ar& make_nvp("patches", patches);
+
+    const ContactPatchRequest request(num_patches);
+    result.set(request);
+    for (size_t i = 0; i < num_patches; ++i) {
+      ContactPatch& patch = result.getUnusedContactPatch();
+      patch = patches[i];
+    }
+  } else {
+    patches.clear();
+    for (size_t i = 0; i < num_patches; ++i) {
+      patches.emplace_back(result.getContactPatch(i));
+    }
+    ar& make_nvp("patches", patches);
+  }
+}
+
+}  // namespace serialization
+}  // namespace boost
+
+#endif  // COAL_SERIALIZATION_CONTACT_PATCH_H

include/coal/serialization/convex.h

+//
+// Copyright (c) 2022-2024 INRIA
+//
+
+#ifndef COAL_SERIALIZATION_CONVEX_H
+#define COAL_SERIALIZATION_CONVEX_H
+
+#include "coal/shape/convex.h"
+
+#include "coal/serialization/fwd.h"
+#include "coal/serialization/geometric_shapes.h"
+#include "coal/serialization/memory.h"
+#include "coal/serialization/triangle.h"
+#include "coal/serialization/quadrilateral.h"
+
+namespace boost {
+namespace serialization {
+
+namespace internal {
+struct ConvexBaseAccessor : coal::ConvexBase {
+  typedef coal::ConvexBase Base;
+};
+
+}  // namespace internal
+
+template <class Archive>
+void serialize(Archive& ar, coal::ConvexBase& convex_base,
+               const unsigned int /*version*/) {
+  using namespace coal;
+
+  ar& make_nvp("base",
+               boost::serialization::base_object<coal::ShapeBase>(convex_base));
+
+  const unsigned int num_points_previous = convex_base.num_points;
+  ar& make_nvp("num_points", convex_base.num_points);
+
+  const unsigned int num_normals_and_offsets_previous =
+      convex_base.num_normals_and_offsets;
+  ar& make_nvp("num_normals_and_offsets", convex_base.num_normals_and_offsets);
+
+  const int num_warm_start_supports_previous =
+      static_cast<int>(convex_base.support_warm_starts.points.size());
+  assert(num_warm_start_supports_previous ==
+         static_cast<int>(convex_base.support_warm_starts.indices.size()));
+  int num_warm_start_supports = num_warm_start_supports_previous;
+  ar& make_nvp("num_warm_start_supports", num_warm_start_supports);
+
+  if (Archive::is_loading::value) {
+    if (num_points_previous != convex_base.num_points) {
+      convex_base.points.reset();
+      if (convex_base.num_points > 0)
+        convex_base.points.reset(
+            new std::vector<Vec3s>(convex_base.num_points));
+    }
+
+    if (num_normals_and_offsets_previous !=
+        convex_base.num_normals_and_offsets) {
+      convex_base.normals.reset();
+      convex_base.offsets.reset();
+      if (convex_base.num_normals_and_offsets > 0) {
+        convex_base.normals.reset(
+            new std::vector<Vec3s>(convex_base.num_normals_and_offsets));
+        convex_base.offsets.reset(
+            new std::vector<CoalScalar>(convex_base.num_normals_and_offsets));
+      }
+    }
+
+    if (num_warm_start_supports_previous != num_warm_start_supports) {
+      convex_base.support_warm_starts.points.resize(
+          static_cast<size_t>(num_warm_start_supports));
+      convex_base.support_warm_starts.indices.resize(
+          static_cast<size_t>(num_warm_start_supports));
+    }
+  }
+
+  typedef Eigen::Matrix<CoalScalar, 3, Eigen::Dynamic> MatrixPoints;
+  if (convex_base.num_points > 0) {
+    Eigen::Map<MatrixPoints> points_map(
+        reinterpret_cast<CoalScalar*>(convex_base.points->data()), 3,
+        convex_base.num_points);
+    ar& make_nvp("points", points_map);
+  }
+
+  typedef Eigen::Matrix<CoalScalar, 1, Eigen::Dynamic> VecOfReals;
+  if (convex_base.num_normals_and_offsets > 0) {
+    Eigen::Map<MatrixPoints> normals_map(
+        reinterpret_cast<CoalScalar*>(convex_base.normals->data()), 3,
+        convex_base.num_normals_and_offsets);
+    ar& make_nvp("normals", normals_map);
+
+    Eigen::Map<VecOfReals> offsets_map(
+        reinterpret_cast<CoalScalar*>(convex_base.offsets->data()), 1,
+        convex_base.num_normals_and_offsets);
+    ar& make_nvp("offsets", offsets_map);
+  }
+
+  typedef Eigen::Matrix<int, 1, Eigen::Dynamic> VecOfInts;
+  if (num_warm_start_supports > 0) {
+    Eigen::Map<MatrixPoints> warm_start_support_points_map(
+        reinterpret_cast<CoalScalar*>(
+            convex_base.support_warm_starts.points.data()),
+        3, num_warm_start_supports);
+    ar& make_nvp("warm_start_support_points", warm_start_support_points_map);
+
+    Eigen::Map<VecOfInts> warm_start_support_indices_map(
+        reinterpret_cast<int*>(convex_base.support_warm_starts.indices.data()),
+        1, num_warm_start_supports);
+    ar& make_nvp("warm_start_support_indices", warm_start_support_indices_map);
+  }
+
+  ar& make_nvp("center", convex_base.center);
+  // We don't save neighbors as they will be computed directly by calling
+  // fillNeighbors.
+}
+
+namespace internal {
+template <typename PolygonT>
+struct ConvexAccessor : coal::Convex<PolygonT> {
+  typedef coal::Convex<PolygonT> Base;
+  using Base::fillNeighbors;
+};
+
+}  // namespace internal
+
+template <class Archive, typename PolygonT>
+void serialize(Archive& ar, coal::Convex<PolygonT>& convex_,
+               const unsigned int /*version*/) {
+  using namespace coal;
+  typedef internal::ConvexAccessor<PolygonT> Accessor;
+
+  Accessor& convex = reinterpret_cast<Accessor&>(convex_);
+  ar& make_nvp("base", boost::serialization::base_object<ConvexBase>(convex_));
+
+  const unsigned int num_polygons_previous = convex.num_polygons;
+  ar& make_nvp("num_polygons", convex.num_polygons);
+
+  if (Archive::is_loading::value) {
+    if (num_polygons_previous != convex.num_polygons) {
+      convex.polygons.reset(new std::vector<PolygonT>(convex.num_polygons));
+    }
+  }
+
+  ar& make_array<PolygonT>(convex.polygons->data(), convex.num_polygons);
+
+  if (Archive::is_loading::value) convex.fillNeighbors();
+}
+
+}  // namespace serialization
+}  // namespace boost
+
+COAL_SERIALIZATION_DECLARE_EXPORT(coal::Convex<coal::Triangle>)
+COAL_SERIALIZATION_DECLARE_EXPORT(coal::Convex<coal::Quadrilateral>)
+
+namespace coal {
+
+// namespace internal {
+// template <typename BV>
+// struct memory_footprint_evaluator< ::coal::BVHModel<BV> > {
+//   static size_t run(const ::coal::BVHModel<BV> &bvh_model) {
+//     return static_cast<size_t>(bvh_model.memUsage(false));
+//   }
+// };
+// }  // namespace internal
+
+}  // namespace coal
+
+#endif  // ifndef COAL_SERIALIZATION_CONVEX_H

include/coal/serialization/eigen.h

+//
+// Copyright (c) 2017-2021 CNRS INRIA
+//
+
+/*
+ Code adapted from Pinocchio and
+ https://gist.githubusercontent.com/mtao/5798888/raw/5be9fa9b66336c166dba3a92c0e5b69ffdb81501/eigen_boost_serialization.hpp
+ Copyright (c) 2015 Michael Tao
+*/
+
+#ifndef COAL_SERIALIZATION_EIGEN_H
+#define COAL_SERIALIZATION_EIGEN_H
+
+#ifdef COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL
+#ifdef HPP_FCL_SKIP_EIGEN_BOOST_SERIALIZATION
+#define COAL_SKIP_EIGEN_BOOST_SERIALIZATION
+#endif
+#endif
+
+#ifndef COAL_SKIP_EIGEN_BOOST_SERIALIZATION
+
+#include <Eigen/Dense>
+
+#include <boost/serialization/split_free.hpp>
+#include <boost/serialization/vector.hpp>
+#include <boost/serialization/array.hpp>
+
+// Workaround a bug in GCC >= 7 and C++17
+// ref. https://gitlab.com/libeigen/eigen/-/issues/1676
+#ifdef __GNUC__
+#if __GNUC__ >= 7 && __cplusplus >= 201703L
+namespace boost {
+namespace serialization {
+struct U;
+}
+}  // namespace boost
+namespace Eigen {
+namespace internal {
+template <>
+struct traits<boost::serialization::U> {
+  enum { Flags = 0 };
+};
+}  // namespace internal
+}  // namespace Eigen
+#endif
+#endif
+
+namespace boost {
+namespace serialization {
+
+template <class Archive, typename S, int Rows, int Cols, int Options,
+          int MaxRows, int MaxCols>
+void save(Archive& ar,
+          const Eigen::Matrix<S, Rows, Cols, Options, MaxRows, MaxCols>& m,
+          const unsigned int /*version*/) {
+  Eigen::DenseIndex rows(m.rows()), cols(m.cols());
+  if (Rows == Eigen::Dynamic) ar& BOOST_SERIALIZATION_NVP(rows);
+  if (Cols == Eigen::Dynamic) ar& BOOST_SERIALIZATION_NVP(cols);
+  ar& make_nvp("data", make_array(m.data(), (size_t)m.size()));
+}
+
+template <class Archive, typename S, int Rows, int Cols, int Options,
+          int MaxRows, int MaxCols>
+void load(Archive& ar,
+          Eigen::Matrix<S, Rows, Cols, Options, MaxRows, MaxCols>& m,
+          const unsigned int /*version*/) {
+  Eigen::DenseIndex rows = Rows, cols = Cols;
+  if (Rows == Eigen::Dynamic) ar >> BOOST_SERIALIZATION_NVP(rows);
+  if (Cols == Eigen::Dynamic) ar >> BOOST_SERIALIZATION_NVP(cols);
+  m.resize(rows, cols);
+  ar >> make_nvp("data", make_array(m.data(), (size_t)m.size()));
+}
+
+template <class Archive, typename S, int Rows, int Cols, int Options,
+          int MaxRows, int MaxCols>
+void serialize(Archive& ar,
+               Eigen::Matrix<S, Rows, Cols, Options, MaxRows, MaxCols>& m,
+               const unsigned int version) {
+  split_free(ar, m, version);
+}
+
+template <class Archive, typename PlainObjectBase, int MapOptions,
+          typename StrideType>
+void save(Archive& ar,
+          const Eigen::Map<PlainObjectBase, MapOptions, StrideType>& m,
+          const unsigned int /*version*/) {
+  Eigen::DenseIndex rows(m.rows()), cols(m.cols());
+  if (PlainObjectBase::RowsAtCompileTime == Eigen::Dynamic)
+    ar& BOOST_SERIALIZATION_NVP(rows);
+  if (PlainObjectBase::ColsAtCompileTime == Eigen::Dynamic)
+    ar& BOOST_SERIALIZATION_NVP(cols);
+  ar& make_nvp("data", make_array(m.data(), (size_t)m.size()));
+}
+
+template <class Archive, typename PlainObjectBase, int MapOptions,
+          typename StrideType>
+void load(Archive& ar, Eigen::Map<PlainObjectBase, MapOptions, StrideType>& m,
+          const unsigned int /*version*/) {
+  Eigen::DenseIndex rows = PlainObjectBase::RowsAtCompileTime,
+                    cols = PlainObjectBase::ColsAtCompileTime;
+  if (PlainObjectBase::RowsAtCompileTime == Eigen::Dynamic)
+    ar >> BOOST_SERIALIZATION_NVP(rows);
+  if (PlainObjectBase::ColsAtCompileTime == Eigen::Dynamic)
+    ar >> BOOST_SERIALIZATION_NVP(cols);
+  m.resize(rows, cols);
+  ar >> make_nvp("data", make_array(m.data(), (size_t)m.size()));
+}
+
+template <class Archive, typename PlainObjectBase, int MapOptions,
+          typename StrideType>
+void serialize(Archive& ar,
+               Eigen::Map<PlainObjectBase, MapOptions, StrideType>& m,
+               const unsigned int version) {
+  split_free(ar, m, version);
+}
+
+}  // namespace serialization
+}  // namespace boost
+//
+#endif  // ifned COAL_SKIP_EIGEN_BOOST_SERIALIZATION
+
+#endif  // ifndef COAL_SERIALIZATION_EIGEN_H

include/coal/serialization/fwd.h

+//
+// Copyright (c) 2021-2024 INRIA
+//
+
+#ifndef COAL_SERIALIZATION_FWD_H
+#define COAL_SERIALIZATION_FWD_H
+
+#include <type_traits>
+
+#include <boost/archive/text_iarchive.hpp>
+#include <boost/archive/text_oarchive.hpp>
+#include <boost/archive/xml_iarchive.hpp>
+#include <boost/archive/xml_oarchive.hpp>
+#include <boost/archive/binary_iarchive.hpp>
+#include <boost/archive/binary_oarchive.hpp>
+
+#include <boost/serialization/split_free.hpp>
+#include <boost/serialization/shared_ptr.hpp>
+#include <boost/serialization/export.hpp>
+
+#include "coal/fwd.hh"
+#include "coal/serialization/eigen.h"
+
+#define COAL_SERIALIZATION_SPLIT(Type)                                   \
+  template <class Archive>                                               \
+  void serialize(Archive& ar, Type& value, const unsigned int version) { \
+    split_free(ar, value, version);                                      \
+  }
+
+#define COAL_SERIALIZATION_DECLARE_EXPORT(T) \
+  BOOST_CLASS_EXPORT_KEY(T)                  \
+  namespace boost {                          \
+  namespace archive {                        \
+  namespace detail {                         \
+  namespace extra_detail {                   \
+  template <>                                \
+  struct init_guid<T> {                      \
+    static guid_initializer<T> const& g;     \
+  };                                         \
+  }                                          \
+  }                                          \
+  }                                          \
+  }                                          \
+  /**/
+
+#define COAL_SERIALIZATION_DEFINE_EXPORT(T)             \
+  namespace boost {                                     \
+  namespace archive {                                   \
+  namespace detail {                                    \
+  namespace extra_detail {                              \
+  guid_initializer<T> const& init_guid<T>::g =          \
+      ::boost::serialization::singleton<                \
+          guid_initializer<T> >::get_mutable_instance() \
+          .export_guid();                               \
+  }                                                     \
+  }                                                     \
+  }                                                     \
+  }                                                     \
+  /**/
+
+namespace coal {
+namespace serialization {
+namespace detail {
+
+template <class Derived, class Base>
+struct init_cast_register {};
+
+template <class Derived, class Base>
+struct cast_register_initializer {
+  void init(std::true_type) const {
+    boost::serialization::void_cast_register<Derived, Base>();
+  }
+
+  void init(std::false_type) const {}
+
+  cast_register_initializer const& init() const {
+    COAL_COMPILER_DIAGNOSTIC_PUSH
+    _Pragma("GCC diagnostic ignored \"-Wconversion\"")
+        BOOST_STATIC_WARNING((std::is_base_of<Base, Derived>::value));
+    COAL_COMPILER_DIAGNOSTIC_POP
+    init(std::is_base_of<Base, Derived>());
+    return *this;
+  }
+};
+
+}  // namespace detail
+
+template <typename T>
+struct register_type {
+  template <class Archive>
+  static void on(Archive& /*ar*/) {}
+};
+}  // namespace serialization
+}  // namespace coal
+
+#define COAL_SERIALIZATION_CAST_REGISTER(Derived, Base)                      \
+  namespace coal {                                                           \
+  namespace serialization {                                                  \
+  namespace detail {                                                         \
+  template <>                                                                \
+  struct init_cast_register<Derived, Base> {                                 \
+    static cast_register_initializer<Derived, Base> const& g;                \
+  };                                                                         \
+  cast_register_initializer<Derived, Base> const& init_cast_register<        \
+      Derived, Base>::g =                                                    \
+      ::boost::serialization::singleton<                                     \
+          cast_register_initializer<Derived, Base> >::get_mutable_instance() \
+          .init();                                                           \
+  }                                                                          \
+  }                                                                          \
+  }
+
+#endif  // ifndef COAL_SERIALIZATION_FWD_H

include/coal/serialization/geometric_shapes.h

+//
+// Copyright (c) 2021-2024 INRIA
+//
+
+#ifndef COAL_SERIALIZATION_GEOMETRIC_SHAPES_H
+#define COAL_SERIALIZATION_GEOMETRIC_SHAPES_H
+
+#include "coal/shape/geometric_shapes.h"
+#include "coal/serialization/fwd.h"
+#include "coal/serialization/collision_object.h"
+
+namespace boost {
+namespace serialization {
+
+template <class Archive>
+void serialize(Archive& ar, coal::ShapeBase& shape_base,
+               const unsigned int /*version*/) {
+  ar& make_nvp(
+      "base",
+      boost::serialization::base_object<coal::CollisionGeometry>(shape_base));
+  ::coal::CoalScalar radius = shape_base.getSweptSphereRadius();
+  ar& make_nvp("swept_sphere_radius", radius);
+
+  if (Archive::is_loading::value) {
+    shape_base.setSweptSphereRadius(radius);
+  }
+}
+
+template <class Archive>
+void serialize(Archive& ar, coal::TriangleP& triangle,
+               const unsigned int /*version*/) {
+  ar& make_nvp("base",
+               boost::serialization::base_object<coal::ShapeBase>(triangle));
+  ar& make_nvp("a", triangle.a);
+  ar& make_nvp("b", triangle.b);
+  ar& make_nvp("c", triangle.c);
+}
+
+template <class Archive>
+void serialize(Archive& ar, coal::Box& box, const unsigned int /*version*/) {
+  ar& make_nvp("base", boost::serialization::base_object<coal::ShapeBase>(box));
+  ar& make_nvp("halfSide", box.halfSide);
+}
+
+template <class Archive>
+void serialize(Archive& ar, coal::Sphere& sphere,
+               const unsigned int /*version*/) {
+  ar& make_nvp("base",
+               boost::serialization::base_object<coal::ShapeBase>(sphere));
+  ar& make_nvp("radius", sphere.radius);
+}
+
+template <class Archive>
+void serialize(Archive& ar, coal::Ellipsoid& ellipsoid,
+               const unsigned int /*version*/) {
+  ar& make_nvp("base",
+               boost::serialization::base_object<coal::ShapeBase>(ellipsoid));
+  ar& make_nvp("radii", ellipsoid.radii);
+}
+
+template <class Archive>
+void serialize(Archive& ar, coal::Capsule& capsule,
+               const unsigned int /*version*/) {
+  ar& make_nvp("base",
+               boost::serialization::base_object<coal::ShapeBase>(capsule));
+  ar& make_nvp("radius", capsule.radius);
+  ar& make_nvp("halfLength", capsule.halfLength);
+}
+
+template <class Archive>
+void serialize(Archive& ar, coal::Cone& cone, const unsigned int /*version*/) {
+  ar& make_nvp("base",
+               boost::serialization::base_object<coal::ShapeBase>(cone));
+  ar& make_nvp("radius", cone.radius);
+  ar& make_nvp("halfLength", cone.halfLength);
+}
+
+template <class Archive>
+void serialize(Archive& ar, coal::Cylinder& cylinder,
+               const unsigned int /*version*/) {
+  ar& make_nvp("base",
+               boost::serialization::base_object<coal::ShapeBase>(cylinder));
+  ar& make_nvp("radius", cylinder.radius);
+  ar& make_nvp("halfLength", cylinder.halfLength);
+}
+
+template <class Archive>
+void serialize(Archive& ar, coal::Halfspace& half_space,
+               const unsigned int /*version*/) {
+  ar& make_nvp("base",
+               boost::serialization::base_object<coal::ShapeBase>(half_space));
+  ar& make_nvp("n", half_space.n);
+  ar& make_nvp("d", half_space.d);
+}
+
+template <class Archive>
+void serialize(Archive& ar, coal::Plane& plane,
+               const unsigned int /*version*/) {
+  ar& make_nvp("base",
+               boost::serialization::base_object<coal::ShapeBase>(plane));
+  ar& make_nvp("n", plane.n);
+  ar& make_nvp("d", plane.d);
+}
+
+}  // namespace serialization
+}  // namespace boost
+
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::ShapeBase)
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::CollisionGeometry)
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::TriangleP)
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::Box)
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::Sphere)
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::Ellipsoid)
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::Capsule)
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::Cone)
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::Cylinder)
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::Halfspace)
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::Plane)
+
+#endif  // ifndef COAL_SERIALIZATION_GEOMETRIC_SHAPES_H

include/coal/serialization/hfield.h

+//
+// Copyright (c) 2021-2024 INRIA
+//
+
+#ifndef COAL_SERIALIZATION_HFIELD_H
+#define COAL_SERIALIZATION_HFIELD_H
+
+#include "coal/hfield.h"
+
+#include "coal/serialization/fwd.h"
+#include "coal/serialization/OBBRSS.h"
+
+namespace boost {
+namespace serialization {
+
+template <class Archive>
+void serialize(Archive &ar, coal::HFNodeBase &node,
+               const unsigned int /*version*/) {
+  ar &make_nvp("first_child", node.first_child);
+  ar &make_nvp("x_id", node.x_id);
+  ar &make_nvp("x_size", node.x_size);
+  ar &make_nvp("y_id", node.y_id);
+  ar &make_nvp("y_size", node.y_size);
+  ar &make_nvp("max_height", node.max_height);
+  ar &make_nvp("contact_active_faces", node.contact_active_faces);
+}
+
+template <class Archive, typename BV>
+void serialize(Archive &ar, coal::HFNode<BV> &node,
+               const unsigned int /*version*/) {
+  ar &make_nvp("base",
+               boost::serialization::base_object<coal::HFNodeBase>(node));
+  ar &make_nvp("bv", node.bv);
+}
+
+namespace internal {
+template <typename BV>
+struct HeightFieldAccessor : coal::HeightField<BV> {
+  typedef coal::HeightField<BV> Base;
+  using Base::bvs;
+  using Base::heights;
+  using Base::max_height;
+  using Base::min_height;
+  using Base::num_bvs;
+  using Base::x_dim;
+  using Base::x_grid;
+  using Base::y_dim;
+  using Base::y_grid;
+};
+}  // namespace internal
+
+template <class Archive, typename BV>
+void serialize(Archive &ar, coal::HeightField<BV> &hf_model,
+               const unsigned int /*version*/) {
+  ar &make_nvp(
+      "base",
+      boost::serialization::base_object<coal::CollisionGeometry>(hf_model));
+
+  typedef internal::HeightFieldAccessor<BV> Accessor;
+  Accessor &access = reinterpret_cast<Accessor &>(hf_model);
+
+  ar &make_nvp("x_dim", access.x_dim);
+  ar &make_nvp("y_dim", access.y_dim);
+  ar &make_nvp("heights", access.heights);
+  ar &make_nvp("min_height", access.min_height);
+  ar &make_nvp("max_height", access.max_height);
+  ar &make_nvp("x_grid", access.x_grid);
+  ar &make_nvp("y_grid", access.y_grid);
+
+  ar &make_nvp("bvs", access.bvs);
+  ar &make_nvp("num_bvs", access.num_bvs);
+}
+}  // namespace serialization
+}  // namespace boost
+
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::HeightField<::coal::AABB>)
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::HeightField<::coal::OBB>)
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::HeightField<::coal::OBBRSS>)
+
+#endif  // ifndef COAL_SERIALIZATION_HFIELD_H

include/coal/serialization/kDOP.h

+//
+// Copyright (c) 2024 INRIA
+//
+
+#ifndef COAL_SERIALIZATION_kDOP_H
+#define COAL_SERIALIZATION_kDOP_H
+
+#include "coal/BV/kDOP.h"
+
+#include "coal/serialization/fwd.h"
+
+namespace boost {
+namespace serialization {
+
+namespace internal {
+template <short N>
+struct KDOPAccessor : coal::KDOP<N> {
+  typedef coal::KDOP<N> Base;
+  using Base::dist_;
+};
+}  // namespace internal
+
+template <class Archive, short N>
+void serialize(Archive& ar, coal::KDOP<N>& bv_,
+               const unsigned int /*version*/) {
+  typedef internal::KDOPAccessor<N> Accessor;
+  Accessor& access = reinterpret_cast<Accessor&>(bv_);
+  ar& make_nvp("distances", make_array(access.dist_.data(), N));
+}
+
+}  // namespace serialization
+}  // namespace boost
+
+#endif  // COAL_SERIALIZATION_kDOP_H

include/coal/serialization/kIOS.h

+//
+// Copyright (c) 2024 INRIA
+//
+
+#ifndef COAL_SERIALIZATION_kIOS_H
+#define COAL_SERIALIZATION_kIOS_H
+
+#include "coal/BV/kIOS.h"
+
+#include "coal/serialization/OBB.h"
+#include "coal/serialization/fwd.h"
+
+namespace boost {
+namespace serialization {
+
+template <class Archive>
+void serialize(Archive& ar, coal::kIOS& bv, const unsigned int version) {
+  split_free(ar, bv, version);
+}
+
+template <class Archive>
+void save(Archive& ar, const coal::kIOS& bv, const unsigned int /*version*/) {
+  // Number of spheres in kIOS is never larger than kIOS::kios_max_num_spheres
+  ar& make_nvp("num_spheres", bv.num_spheres);
+
+  std::array<coal::Vec3s, coal::kIOS::max_num_spheres> centers{};
+  std::array<coal::CoalScalar, coal::kIOS::max_num_spheres> radii;
+  for (std::size_t i = 0; i < coal::kIOS::max_num_spheres; ++i) {
+    centers[i] = bv.spheres[i].o;
+    radii[i] = bv.spheres[i].r;
+  }
+  ar& make_nvp("centers", make_array(centers.data(), centers.size()));
+  ar& make_nvp("radii", make_array(radii.data(), radii.size()));
+
+  ar& make_nvp("obb", bv.obb);
+}
+
+template <class Archive>
+void load(Archive& ar, coal::kIOS& bv, const unsigned int /*version*/) {
+  ar >> make_nvp("num_spheres", bv.num_spheres);
+
+  std::array<coal::Vec3s, coal::kIOS::max_num_spheres> centers;
+  std::array<coal::CoalScalar, coal::kIOS::max_num_spheres> radii;
+  ar >> make_nvp("centers", make_array(centers.data(), centers.size()));
+  ar >> make_nvp("radii", make_array(radii.data(), radii.size()));
+  for (std::size_t i = 0; i < coal::kIOS::max_num_spheres; ++i) {
+    bv.spheres[i].o = centers[i];
+    bv.spheres[i].r = radii[i];
+  }
+
+  ar >> make_nvp("obb", bv.obb);
+}
+
+}  // namespace serialization
+}  // namespace boost
+
+#endif  // COAL_SERIALIZATION_kIOS_H

include/coal/serialization/memory.h

+//
+// Copyright (c) 2021 INRIA
+//
+
+#ifndef COAL_SERIALIZATION_MEMORY_H
+#define COAL_SERIALIZATION_MEMORY_H
+
+namespace coal {
+
+namespace internal {
+template <typename T>
+struct memory_footprint_evaluator {
+  static size_t run(const T &) { return sizeof(T); }
+};
+}  // namespace internal
+
+/// \brief Returns the memory footpring of the input object.
+/// For POD objects, this function returns the result of sizeof(T)
+///
+/// \param[in] object whose memory footprint needs to be evaluated.
+///
+/// \return the memory footprint of the input object.
+template <typename T>
+size_t computeMemoryFootprint(const T &object) {
+  return internal::memory_footprint_evaluator<T>::run(object);
+}
+
+}  // namespace coal
+
+#endif  // ifndef COAL_SERIALIZATION_MEMORY_H

include/coal/serialization/octree.h

+//
+// Copyright (c) 2023-2024 INRIA
+//
+
+#ifndef COAL_SERIALIZATION_OCTREE_H
+#define COAL_SERIALIZATION_OCTREE_H
+
+#include <sstream>
+#include <iostream>
+
+#include <boost/serialization/string.hpp>
+
+#include "coal/octree.h"
+#include "coal/serialization/fwd.h"
+
+namespace boost {
+namespace serialization {
+
+namespace internal {
+struct OcTreeAccessor : coal::OcTree {
+  typedef coal::OcTree Base;
+  using Base::default_occupancy;
+  using Base::free_threshold;
+  using Base::occupancy_threshold;
+  using Base::tree;
+};
+}  // namespace internal
+
+template <class Archive>
+void save_construct_data(Archive &ar, const coal::OcTree *octree_ptr,
+                         const unsigned int /*version*/) {
+  const double resolution = octree_ptr->getResolution();
+  ar << make_nvp("resolution", resolution);
+}
+
+template <class Archive>
+void save(Archive &ar, const coal::OcTree &octree,
+          const unsigned int /*version*/) {
+  typedef internal::OcTreeAccessor Accessor;
+  const Accessor &access = reinterpret_cast<const Accessor &>(octree);
+
+  std::ostringstream stream;
+  access.tree->write(stream);
+  const std::string stream_str = stream.str();
+  auto size = stream_str.size();
+  // We can't directly serialize stream_str because it contains binary data.
+  // This create a bug on Windows with text_archive
+  ar << make_nvp("tree_data_size", size);
+  ar << make_nvp("tree_data",
+                 make_array(stream_str.c_str(), stream_str.size()));
+
+  ar << make_nvp("base", base_object<coal::CollisionGeometry>(octree));
+  ar << make_nvp("default_occupancy", access.default_occupancy);
+  ar << make_nvp("occupancy_threshold", access.occupancy_threshold);
+  ar << make_nvp("free_threshold", access.free_threshold);
+}
+
+template <class Archive>
+void load_construct_data(Archive &ar, coal::OcTree *octree_ptr,
+                         const unsigned int /*version*/) {
+  double resolution;
+  ar >> make_nvp("resolution", resolution);
+  new (octree_ptr) coal::OcTree(resolution);
+}
+
+template <class Archive>
+void load(Archive &ar, coal::OcTree &octree, const unsigned int /*version*/) {
+  typedef internal::OcTreeAccessor Accessor;
+  Accessor &access = reinterpret_cast<Accessor &>(octree);
+
+  std::size_t tree_data_size;
+  ar >> make_nvp("tree_data_size", tree_data_size);
+
+  std::string stream_str;
+  stream_str.resize(tree_data_size);
+  /// TODO use stream_str.data in C++17
+  assert(tree_data_size > 0 && "tree_data_size should be greater than 0");
+  ar >> make_nvp("tree_data", make_array(&stream_str[0], tree_data_size));
+  std::istringstream stream(stream_str);
+
+  octomap::AbstractOcTree *new_tree = octomap::AbstractOcTree::read(stream);
+  access.tree = std::shared_ptr<const octomap::OcTree>(
+      dynamic_cast<octomap::OcTree *>(new_tree));
+
+  ar >> make_nvp("base", base_object<coal::CollisionGeometry>(octree));
+  ar >> make_nvp("default_occupancy", access.default_occupancy);
+  ar >> make_nvp("occupancy_threshold", access.occupancy_threshold);
+  ar >> make_nvp("free_threshold", access.free_threshold);
+}
+
+template <class Archive>
+void serialize(Archive &ar, coal::OcTree &octree, const unsigned int version) {
+  split_free(ar, octree, version);
+}
+
+}  // namespace serialization
+}  // namespace boost
+
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::OcTree)
+
+#endif  // ifndef COAL_SERIALIZATION_OCTREE_H

include/coal/serialization/quadrilateral.h

+//
+// Copyright (c) 2022 INRIA
+//
+
+#ifndef COAL_SERIALIZATION_QUADRILATERAL_H
+#define COAL_SERIALIZATION_QUADRILATERAL_H
+
+#include "coal/data_types.h"
+#include "coal/serialization/fwd.h"
+
+namespace boost {
+namespace serialization {
+
+template <class Archive>
+void serialize(Archive &ar, coal::Quadrilateral &quadrilateral,
+               const unsigned int /*version*/) {
+  ar &make_nvp("p0", quadrilateral[0]);
+  ar &make_nvp("p1", quadrilateral[1]);
+  ar &make_nvp("p2", quadrilateral[2]);
+  ar &make_nvp("p3", quadrilateral[3]);
+}
+
+}  // namespace serialization
+}  // namespace boost
+
+#endif  // ifndef COAL_SERIALIZATION_QUADRILATERAL_H

include/coal/serialization/serializer.h

+//
+// Copyright (c) 2024 INRIA
+//
+
+#ifndef COAL_SERIALIZATION_SERIALIZER_H
+#define COAL_SERIALIZATION_SERIALIZER_H
+
+#include "coal/serialization/archive.h"
+
+namespace coal {
+namespace serialization {
+
+struct Serializer {
+  /// \brief Loads an object from a text file.
+  template <typename T>
+  static void loadFromText(T& object, const std::string& filename) {
+    ::coal::serialization::loadFromText(object, filename);
+  }
+
+  /// \brief Saves an object as a text file.
+  template <typename T>
+  static void saveToText(const T& object, const std::string& filename) {
+    ::coal::serialization::saveToText(object, filename);
+  }
+
+  /// \brief Loads an object from a stream string.
+  template <typename T>
+  static void loadFromStringStream(T& object, std::istringstream& is) {
+    ::coal::serialization::loadFromStringStream(object, is);
+  }
+
+  /// \brief Saves an object to a string stream.
+  template <typename T>
+  static void saveToStringStream(const T& object, std::stringstream& ss) {
+    ::coal::serialization::saveToStringStream(object, ss);
+  }
+
+  /// \brief Loads an object from a string.
+  template <typename T>
+  static void loadFromString(T& object, const std::string& str) {
+    ::coal::serialization::loadFromString(object, str);
+  }
+
+  /// \brief Saves a Derived object to a string.
+  template <typename T>
+  static std::string saveToString(const T& object) {
+    return ::coal::serialization::saveToString(object);
+  }
+
+  /// \brief Loads an object from an XML file.
+  template <typename T>
+  static void loadFromXML(T& object, const std::string& filename,
+                          const std::string& tag_name) {
+    ::coal::serialization::loadFromXML(object, filename, tag_name);
+  }
+
+  /// \brief Saves an object as an XML file.
+  template <typename T>
+  static void saveToXML(const T& object, const std::string& filename,
+                        const std::string& tag_name) {
+    ::coal::serialization::saveToXML(object, filename, tag_name);
+  }
+
+  /// \brief Loads a Derived object from an binary file.
+  template <typename T>
+  static void loadFromBinary(T& object, const std::string& filename) {
+    ::coal::serialization::loadFromBinary(object, filename);
+  }
+
+  /// \brief Saves a Derived object as an binary file.
+  template <typename T>
+  static void saveToBinary(const T& object, const std::string& filename) {
+    ::coal::serialization::saveToBinary(object, filename);
+  }
+
+  /// \brief Loads an object from a binary buffer.
+  template <typename T>
+  static void loadFromBuffer(T& object, boost::asio::streambuf& container) {
+    ::coal::serialization::loadFromBuffer(object, container);
+  }
+
+  /// \brief Saves an object as a binary buffer.
+  template <typename T>
+  static void saveToBuffer(const T& object, boost::asio::streambuf& container) {
+    ::coal::serialization::saveToBuffer(object, container);
+  }
+};
+
+}  // namespace serialization
+}  // namespace coal
+
+#endif  // ifndef COAL_SERIALIZATION_SERIALIZER_H

include/coal/serialization/transform.h

+//
+// Copyright (c) 2024 INRIA
+//
+
+#ifndef COAL_SERIALIZATION_TRANSFORM_H
+#define COAL_SERIALIZATION_TRANSFORM_H
+
+#include "coal/math/transform.h"
+#include "coal/serialization/fwd.h"
+
+namespace boost {
+namespace serialization {
+
+template <class Archive>
+void serialize(Archive& ar, coal::Transform3s& tf,
+               const unsigned int /*version*/) {
+  ar& make_nvp("R", tf.rotation());
+  ar& make_nvp("T", tf.translation());
+}
+
+}  // namespace serialization
+}  // namespace boost
+
+#endif  // COAL_SERIALIZATION_TRANSFORM_H

include/coal/serialization/triangle.h

+//
+// Copyright (c) 2021-2022 INRIA
+//
+
+#ifndef COAL_SERIALIZATION_TRIANGLE_H
+#define COAL_SERIALIZATION_TRIANGLE_H
+
+#include "coal/data_types.h"
+#include "coal/serialization/fwd.h"
+
+namespace boost {
+namespace serialization {
+
+template <class Archive>
+void serialize(Archive &ar, coal::Triangle &triangle,
+               const unsigned int /*version*/) {
+  ar &make_nvp("p0", triangle[0]);
+  ar &make_nvp("p1", triangle[1]);
+  ar &make_nvp("p2", triangle[2]);
+}
+
+}  // namespace serialization
+}  // namespace boost
+
+#endif  // ifndef COAL_SERIALIZATION_TRIANGLE_H

include/fcl/ccd/taylor_vector.h → include/coal/shape/convex.h

@@ -32,83 +32,81 @@
  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  *  POSSIBILITY OF SUCH DAMAGE.
  */
-// This code is based on code developed by Stephane Redon at UNC and Inria for the CATCH library: http://graphics.ewha.ac.kr/CATCH/
-/** \author Jia Pan */
-
-#ifndef FCL_CCD_TAYLOR_VECTOR_H
-#define FCL_CCD_TAYLOR_VECTOR_H
-
-#include "fcl/ccd/interval_vector.h"
-#include "fcl/ccd/taylor_model.h"
-
-namespace fcl
-{
-
-class TVector3
-{
-  TaylorModel i_[3];
-
-public:
-  
-  TVector3();
-  TVector3(const boost::shared_ptr<TimeInterval>& time_interval);
-  TVector3(TaylorModel v[3]);
-  TVector3(const TaylorModel& v0, const TaylorModel& v1, const TaylorModel& v2);
-  TVector3(const Vec3f& v, const boost::shared_ptr<TimeInterval>& time_interval);
-  
-  TVector3 operator + (const TVector3& other) const;
-  TVector3& operator += (const TVector3& other);
-
-  TVector3 operator + (const Vec3f& other) const;
-  TVector3& operator += (const Vec3f& other);
-
-  TVector3 operator - (const TVector3& other) const;
-  TVector3& operator -= (const TVector3& other);
-
-  TVector3 operator - (const Vec3f& other) const;
-  TVector3& operator -= (const Vec3f& other);
-
-  TVector3 operator - () const;
 
-  TVector3 operator * (const TaylorModel& d) const;
-  TVector3& operator *= (const TaylorModel& d);
-  TVector3 operator * (FCL_REAL d) const;
-  TVector3& operator *= (FCL_REAL d);
-
-  const TaylorModel& operator [] (size_t i) const;
-  TaylorModel& operator [] (size_t i);
-
-  TaylorModel dot(const TVector3& other) const;
-  TVector3 cross(const TVector3& other) const;
-  TaylorModel dot(const Vec3f& other) const;
-  TVector3 cross(const Vec3f& other) const;
-
-  IVector3 getBound() const;
-  IVector3 getBound(FCL_REAL l, FCL_REAL r) const;
-  IVector3 getBound(FCL_REAL t) const;
-
-  IVector3 getTightBound() const;
-  IVector3 getTightBound(FCL_REAL l, FCL_REAL r) const;
-
-  void print() const;
-  FCL_REAL volumn() const;
-  void setZero();
-
-  TaylorModel squareLength() const;
-
-  void setTimeInterval(const boost::shared_ptr<TimeInterval>& time_interval);
-  void setTimeInterval(FCL_REAL l, FCL_REAL r);
+/** \author Jia Pan */
 
-  const boost::shared_ptr<TimeInterval>& getTimeInterval() const;
+#ifndef COAL_SHAPE_CONVEX_H
+#define COAL_SHAPE_CONVEX_H
+
+#include "coal/shape/geometric_shapes.h"
+
+namespace coal {
+
+/// @brief Convex polytope
+/// @tparam PolygonT the polygon class. It must have method \c size() and
+///         \c operator[](int i)
+template <typename PolygonT>
+class Convex : public ConvexBase {
+ public:
+  /// @brief Construct an uninitialized convex object
+  Convex() : ConvexBase(), num_polygons(0) {}
+
+  /// @brief Constructing a convex, providing normal and offset of each polytype
+  /// surface, and the points and shape topology information \param ownStorage
+  /// whether this class owns the pointers of points and
+  ///                    polygons. If owned, they are deleted upon destruction.
+  /// \param points_ list of 3D points
+  /// \param num_points_ number of 3D points
+  /// \param polygons_ \copydoc Convex::polygons
+  /// \param num_polygons_ the number of polygons.
+  /// \note num_polygons_ is not the allocated size of polygons_.
+  Convex(std::shared_ptr<std::vector<Vec3s>> points_, unsigned int num_points_,
+         std::shared_ptr<std::vector<PolygonT>> polygons_,
+         unsigned int num_polygons_);
+
+  /// @brief Copy constructor
+  /// Only the list of neighbors is copied.
+  Convex(const Convex& other);
+
+  ~Convex();
+
+  /// based on http://number-none.com/blow/inertia/bb_inertia.doc
+  Matrix3s computeMomentofInertia() const;
+
+  Vec3s computeCOM() const;
+
+  CoalScalar computeVolume() const;
+
+  ///
+  /// @brief Set the current Convex from a list of points and polygons.
+  ///
+  /// \param ownStorage whether this class owns the pointers of points and
+  ///                    polygons. If owned, they are deleted upon destruction.
+  /// \param points list of 3D points
+  /// \param num_points number of 3D points
+  /// \param polygons \copydoc Convex::polygons
+  /// \param num_polygons the number of polygons.
+  /// \note num_polygons is not the allocated size of polygons.
+  ///
+  void set(std::shared_ptr<std::vector<Vec3s>> points, unsigned int num_points,
+           std::shared_ptr<std::vector<PolygonT>> polygons,
+           unsigned int num_polygons);
+
+  /// @brief Clone (deep copy)
+  virtual Convex<PolygonT>* clone() const;
+
+  /// @brief An array of PolygonT object.
+  /// PolygonT should contains a list of vertices for each polygon,
+  /// in counter clockwise order.
+  std::shared_ptr<std::vector<PolygonT>> polygons;
+  unsigned int num_polygons;
+
+ protected:
+  void fillNeighbors();
 };
 
-void generateTVector3ForLinearFunc(TVector3& v, const Vec3f& position, const Vec3f& velocity);
-
-
-TVector3 operator * (const Vec3f& v, const TaylorModel& a);
-TVector3 operator + (const Vec3f& v1, const TVector3& v2);
-TVector3 operator - (const Vec3f& v1, const TVector3& v2);
+}  // namespace coal
 
-}
+#include "coal/shape/details/convex.hxx"
 
 #endif

include/coal/shape/details/convex.hxx

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2019, CNRS - LAAS
+ *  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 Joseph Mirabel */
+
+#ifndef COAL_SHAPE_CONVEX_HXX
+#define COAL_SHAPE_CONVEX_HXX
+
+#include <set>
+#include <vector>
+#include <iostream>
+
+namespace coal {
+
+template <typename PolygonT>
+Convex<PolygonT>::Convex(std::shared_ptr<std::vector<Vec3s>> points_,
+                         unsigned int num_points_,
+                         std::shared_ptr<std::vector<PolygonT>> polygons_,
+                         unsigned int num_polygons_)
+    : ConvexBase(), polygons(polygons_), num_polygons(num_polygons_) {
+  this->initialize(points_, num_points_);
+  this->fillNeighbors();
+  this->buildSupportWarmStart();
+}
+
+template <typename PolygonT>
+Convex<PolygonT>::Convex(const Convex<PolygonT>& other)
+    : ConvexBase(other), num_polygons(other.num_polygons) {
+  if (other.polygons.get()) {
+    polygons.reset(new std::vector<PolygonT>(*(other.polygons)));
+  } else
+    polygons.reset();
+}
+
+template <typename PolygonT>
+Convex<PolygonT>::~Convex() {}
+
+template <typename PolygonT>
+void Convex<PolygonT>::set(std::shared_ptr<std::vector<Vec3s>> points_,
+                           unsigned int num_points_,
+                           std::shared_ptr<std::vector<PolygonT>> polygons_,
+                           unsigned int num_polygons_) {
+  ConvexBase::set(points_, num_points_);
+
+  this->num_polygons = num_polygons_;
+  this->polygons = polygons_;
+
+  this->fillNeighbors();
+  this->buildSupportWarmStart();
+}
+
+template <typename PolygonT>
+Convex<PolygonT>* Convex<PolygonT>::clone() const {
+  return new Convex(*this);
+}
+
+template <typename PolygonT>
+Matrix3s Convex<PolygonT>::computeMomentofInertia() const {
+  typedef typename PolygonT::size_type size_type;
+  typedef typename PolygonT::index_type index_type;
+
+  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 (!(points.get())) {
+    std::cerr
+        << "Error in `Convex::computeMomentofInertia`! Convex has no vertices."
+        << std::endl;
+    return C;
+  }
+  const std::vector<Vec3s>& points_ = *points;
+  if (!(polygons.get())) {
+    std::cerr
+        << "Error in `Convex::computeMomentofInertia`! Convex has no polygons."
+        << std::endl;
+    return C;
+  }
+  const std::vector<PolygonT>& polygons_ = *polygons;
+  for (unsigned int i = 0; i < num_polygons; ++i) {
+    const PolygonT& polygon = polygons_[i];
+
+    // compute the center of the polygon
+    Vec3s plane_center(0, 0, 0);
+    for (size_type j = 0; j < polygon.size(); ++j)
+      plane_center += points_[polygon[(index_type)j]];
+    plane_center /= polygon.size();
+
+    // compute the volume of tetrahedron making by neighboring two points, the
+    // plane center and the reference point (zero) of the convex shape
+    const Vec3s& v3 = plane_center;
+    for (size_type j = 0; j < polygon.size(); ++j) {
+      index_type e_first = polygon[static_cast<index_type>(j)];
+      index_type e_second =
+          polygon[static_cast<index_type>((j + 1) % polygon.size())];
+      const Vec3s& v1 = points_[e_first];
+      const Vec3s& v2 = points_[e_second];
+      Matrix3s A;
+      A << v1.transpose(), v2.transpose(),
+          v3.transpose();  // this is A' in the original document
+      C += A.transpose() * C_canonical * A * (v1.cross(v2)).dot(v3);
+    }
+  }
+
+  return C.trace() * Matrix3s::Identity() - C;
+}
+
+template <typename PolygonT>
+Vec3s Convex<PolygonT>::computeCOM() const {
+  typedef typename PolygonT::size_type size_type;
+  typedef typename PolygonT::index_type index_type;
+
+  Vec3s com(0, 0, 0);
+  CoalScalar vol = 0;
+  if (!(points.get())) {
+    std::cerr << "Error in `Convex::computeCOM`! Convex has no vertices."
+              << std::endl;
+    return com;
+  }
+  const std::vector<Vec3s>& points_ = *points;
+  if (!(polygons.get())) {
+    std::cerr << "Error in `Convex::computeCOM`! Convex has no polygons."
+              << std::endl;
+    return com;
+  }
+  const std::vector<PolygonT>& polygons_ = *polygons;
+  for (unsigned int i = 0; i < num_polygons; ++i) {
+    const PolygonT& polygon = polygons_[i];
+    // compute the center of the polygon
+    Vec3s plane_center(0, 0, 0);
+    for (size_type j = 0; j < polygon.size(); ++j)
+      plane_center += points_[polygon[(index_type)j]];
+    plane_center /= polygon.size();
+
+    // compute the volume of tetrahedron making by neighboring two points, the
+    // plane center and the reference point (zero) of the convex shape
+    const Vec3s& v3 = plane_center;
+    for (size_type j = 0; j < polygon.size(); ++j) {
+      index_type e_first = polygon[static_cast<index_type>(j)];
+      index_type e_second =
+          polygon[static_cast<index_type>((j + 1) % polygon.size())];
+      const Vec3s& v1 = points_[e_first];
+      const Vec3s& v2 = points_[e_second];
+      CoalScalar d_six_vol = (v1.cross(v2)).dot(v3);
+      vol += d_six_vol;
+      com += (points_[e_first] + points_[e_second] + plane_center) * d_six_vol;
+    }
+  }
+
+  return com / (vol * 4);  // here we choose zero as the reference
+}
+
+template <typename PolygonT>
+CoalScalar Convex<PolygonT>::computeVolume() const {
+  typedef typename PolygonT::size_type size_type;
+  typedef typename PolygonT::index_type index_type;
+
+  CoalScalar vol = 0;
+  if (!(points.get())) {
+    std::cerr << "Error in `Convex::computeVolume`! Convex has no vertices."
+              << std::endl;
+    return vol;
+  }
+  const std::vector<Vec3s>& points_ = *points;
+  if (!(polygons.get())) {
+    std::cerr << "Error in `Convex::computeVolume`! Convex has no polygons."
+              << std::endl;
+    return vol;
+  }
+  const std::vector<PolygonT>& polygons_ = *polygons;
+  for (unsigned int i = 0; i < num_polygons; ++i) {
+    const PolygonT& polygon = polygons_[i];
+
+    // compute the center of the polygon
+    Vec3s plane_center(0, 0, 0);
+    for (size_type j = 0; j < polygon.size(); ++j)
+      plane_center += points_[polygon[(index_type)j]];
+    plane_center /= polygon.size();
+
+    // compute the volume of tetrahedron making by neighboring two points, the
+    // plane center and the reference point (zero point) of the convex shape
+    const Vec3s& v3 = plane_center;
+    for (size_type j = 0; j < polygon.size(); ++j) {
+      index_type e_first = polygon[static_cast<index_type>(j)];
+      index_type e_second =
+          polygon[static_cast<index_type>((j + 1) % polygon.size())];
+      const Vec3s& v1 = points_[e_first];
+      const Vec3s& v2 = points_[e_second];
+      CoalScalar d_six_vol = (v1.cross(v2)).dot(v3);
+      vol += d_six_vol;
+    }
+  }
+
+  return vol / 6;
+}
+
+template <typename PolygonT>
+void Convex<PolygonT>::fillNeighbors() {
+  neighbors.reset(new std::vector<Neighbors>(num_points));
+
+  typedef typename PolygonT::size_type size_type;
+  typedef typename PolygonT::index_type index_type;
+  std::vector<std::set<index_type>> nneighbors(num_points);
+  unsigned int c_nneighbors = 0;
+
+  if (!(polygons.get())) {
+    std::cerr << "Error in `Convex::fillNeighbors`! Convex has no polygons."
+              << std::endl;
+  }
+  const std::vector<PolygonT>& polygons_ = *polygons;
+  for (unsigned int l = 0; l < num_polygons; ++l) {
+    const PolygonT& polygon = polygons_[l];
+    const size_type n = polygon.size();
+
+    for (size_type j = 0; j < polygon.size(); ++j) {
+      size_type i = (j == 0) ? n - 1 : j - 1;
+      size_type k = (j == n - 1) ? 0 : j + 1;
+      index_type pi = polygon[(index_type)i], pj = polygon[(index_type)j],
+                 pk = polygon[(index_type)k];
+      // Update neighbors of pj;
+      if (nneighbors[pj].count(pi) == 0) {
+        c_nneighbors++;
+        nneighbors[pj].insert(pi);
+      }
+      if (nneighbors[pj].count(pk) == 0) {
+        c_nneighbors++;
+        nneighbors[pj].insert(pk);
+      }
+    }
+  }
+
+  nneighbors_.reset(new std::vector<unsigned int>(c_nneighbors));
+
+  unsigned int* p_nneighbors = nneighbors_->data();
+  std::vector<Neighbors>& neighbors_ = *neighbors;
+  for (unsigned int i = 0; i < num_points; ++i) {
+    Neighbors& n = neighbors_[i];
+    if (nneighbors[i].size() >= (std::numeric_limits<unsigned char>::max)())
+      COAL_THROW_PRETTY("Too many neighbors.", std::logic_error);
+    n.count_ = (unsigned char)nneighbors[i].size();
+    n.n_ = p_nneighbors;
+    p_nneighbors =
+        std::copy(nneighbors[i].begin(), nneighbors[i].end(), p_nneighbors);
+  }
+  assert(p_nneighbors == nneighbors_->data() + c_nneighbors);
+}
+
+}  // namespace coal
+
+#endif

include/fcl/shape/geometric_shape_to_BVH_model.h → include/coal/shape/geometric_shape_to_BVH_model.h

@@ -35,34 +35,32 @@
 
 /** \author Jia Pan */
 
+#ifndef COAL_GEOMETRIC_SHAPE_TO_BVH_MODEL_H
+#define COAL_GEOMETRIC_SHAPE_TO_BVH_MODEL_H
 
-#ifndef GEOMETRIC_SHAPE_TO_BVH_MODEL_H
-#define GEOMETRIC_SHAPE_TO_BVH_MODEL_H
-
-#include "fcl/shape/geometric_shapes.h"
-#include "fcl/BVH/BVH_model.h"
+#include "coal/shape/geometric_shapes.h"
+#include "coal/BVH/BVH_model.h"
 #include <boost/math/constants/constants.hpp>
 
-namespace fcl
-{
+namespace coal {
 
 /// @brief Generate BVH model from box
-template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Box& shape, const Transform3f& pose)
-{
-  double a = shape.side[0];
-  double b = shape.side[1];
-  double c = shape.side[2];
-  std::vector<Vec3f> points(8);
+template <typename BV>
+void generateBVHModel(BVHModel<BV>& model, const Box& shape,
+                      const Transform3s& pose) {
+  CoalScalar a = shape.halfSide[0];
+  CoalScalar b = shape.halfSide[1];
+  CoalScalar c = shape.halfSide[2];
+  std::vector<Vec3s> points(8);
   std::vector<Triangle> tri_indices(12);
-  points[0].setValue(0.5 * a, -0.5 * b, 0.5 * c);
-  points[1].setValue(0.5 * a, 0.5 * b, 0.5 * c);
-  points[2].setValue(-0.5 * a, 0.5 * b, 0.5 * c);
-  points[3].setValue(-0.5 * a, -0.5 * b, 0.5 * c);
-  points[4].setValue(0.5 * a, -0.5 * b, -0.5 * c);
-  points[5].setValue(0.5 * a, 0.5 * b, -0.5 * c);
-  points[6].setValue(-0.5 * a, 0.5 * b, -0.5 * c);
-  points[7].setValue(-0.5 * a, -0.5 * b, -0.5 * c);
+  points[0] = Vec3s(a, -b, c);
+  points[1] = Vec3s(a, b, c);
+  points[2] = Vec3s(-a, b, c);
+  points[3] = Vec3s(-a, -b, c);
+  points[4] = Vec3s(a, -b, -c);
+  points[5] = Vec3s(a, b, -c);
+  points[6] = Vec3s(-a, b, -c);
+  points[7] = Vec3s(-a, -b, -c);
 
   tri_indices[0].set(0, 4, 1);
   tri_indices[1].set(1, 4, 5);
@@ -77,9 +75,8 @@ void generateBVHModel(BVHModel<BV>& model, const Box& shape, const Transform3f&
   tri_indices[10].set(3, 7, 0);
   tri_indices[11].set(0, 7, 4);
 
-  for(unsigned int i = 0; i < points.size(); ++i)
-  {
-    points[i] = pose.transform(points[i]);
+  for (unsigned int i = 0; i < points.size(); ++i) {
+    points[i] = pose.transform(points[i]).eval();
   }
 
   model.beginModel();
@@ -88,50 +85,49 @@ void generateBVHModel(BVHModel<BV>& model, const Box& shape, const Transform3f&
   model.computeLocalAABB();
 }
 
-/// @brief Generate BVH model from sphere, given the number of segments along longitude and number of rings along latitude.
-template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, const Transform3f& pose, unsigned int seg, unsigned int ring)
-{
-  std::vector<Vec3f> points;
+/// @brief Generate BVH model from sphere, given the number of segments along
+/// longitude and number of rings along latitude.
+template <typename BV>
+void generateBVHModel(BVHModel<BV>& model, const Sphere& shape,
+                      const Transform3s& pose, unsigned int seg,
+                      unsigned int ring) {
+  std::vector<Vec3s> points;
   std::vector<Triangle> tri_indices;
 
-  double r = shape.radius;
-  double phi, phid;
-  const double pi = boost::math::constants::pi<double>();
+  CoalScalar r = shape.radius;
+  CoalScalar phi, phid;
+  const CoalScalar pi = boost::math::constants::pi<CoalScalar>();
   phid = pi * 2 / seg;
   phi = 0;
 
-  double theta, thetad;
+  CoalScalar theta, thetad;
   thetad = pi / (ring + 1);
   theta = 0;
 
-  for(unsigned int i = 0; i < ring; ++i)
-  {
-    double theta_ = theta + thetad * (i + 1);
-    for(unsigned int j = 0; j < seg; ++j)
-    {
-      points.push_back(Vec3f(r * sin(theta_) * cos(phi + j * phid), r * sin(theta_) * sin(phi + j * phid), r * cos(theta_)));
+  for (unsigned int i = 0; i < ring; ++i) {
+    CoalScalar theta_ = theta + thetad * (i + 1);
+    for (unsigned int j = 0; j < seg; ++j) {
+      points.push_back(Vec3s(r * sin(theta_) * cos(phi + j * phid),
+                             r * sin(theta_) * sin(phi + j * phid),
+                             r * cos(theta_)));
     }
   }
-  points.push_back(Vec3f(0, 0, r));
-  points.push_back(Vec3f(0, 0, -r));
-
-  for(unsigned int i = 0; i < ring - 1; ++i)
-  {
-    for(unsigned int j = 0; j < seg; ++j)
-    {
-       unsigned int a, b, c, d;
-       a = i * seg + j;
-       b = (j == seg - 1) ? (i * seg) : (i * seg + j + 1);
-       c = (i + 1) * seg + j;
-       d = (j == seg - 1) ? ((i + 1) * seg) : ((i + 1) * seg + j + 1);
-       tri_indices.push_back(Triangle(a, c, b));
-       tri_indices.push_back(Triangle(b, c, d));
+  points.push_back(Vec3s(0, 0, r));
+  points.push_back(Vec3s(0, 0, -r));
+
+  for (unsigned int i = 0; i < ring - 1; ++i) {
+    for (unsigned int j = 0; j < seg; ++j) {
+      unsigned int a, b, c, d;
+      a = i * seg + j;
+      b = (j == seg - 1) ? (i * seg) : (i * seg + j + 1);
+      c = (i + 1) * seg + j;
+      d = (j == seg - 1) ? ((i + 1) * seg) : ((i + 1) * seg + j + 1);
+      tri_indices.push_back(Triangle(a, c, b));
+      tri_indices.push_back(Triangle(b, c, d));
     }
   }
 
-  for(unsigned int j = 0; j < seg; ++j)
-  {
+  for (unsigned int j = 0; j < seg; ++j) {
     unsigned int a, b;
     a = j;
     b = (j == seg - 1) ? 0 : (j + 1);
@@ -142,8 +138,7 @@ void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, const Transform3
     tri_indices.push_back(Triangle(a, ring * seg + 1, b));
   }
 
-  for(unsigned int i = 0; i < points.size(); ++i)
-  {
+  for (unsigned int i = 0; i < points.size(); ++i) {
     points[i] = pose.transform(points[i]);
   }
 
@@ -154,83 +149,85 @@ void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, const Transform3
 }
 
 /// @brief Generate BVH model from sphere
-/// The difference between generateBVHModel is that it gives the number of triangles faces N for a sphere with unit radius. For sphere of radius r,
-/// then the number of triangles is r * r * N so that the area represented by a single triangle is approximately the same.s
-template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, const Transform3f& pose, unsigned int n_faces_for_unit_sphere)
-{
-  double r = shape.radius;
-  double n_low_bound = sqrtf(n_faces_for_unit_sphere / 2.0) * r * r;
-  unsigned int ring = ceil(n_low_bound);
-  unsigned int seg = ceil(n_low_bound);
-
-  generateBVHModel(model, shape, pose, seg, ring);  
+/// The difference between generateBVHModel is that it gives the number of
+/// triangles faces N for a sphere with unit radius. For sphere of radius r,
+/// then the number of triangles is r * r * N so that the area represented by a
+/// single triangle is approximately the same.s
+template <typename BV>
+void generateBVHModel(BVHModel<BV>& model, const Sphere& shape,
+                      const Transform3s& pose,
+                      unsigned int n_faces_for_unit_sphere) {
+  CoalScalar r = shape.radius;
+  CoalScalar n_low_bound =
+      std::sqrt((CoalScalar)n_faces_for_unit_sphere / CoalScalar(2.)) * r * r;
+  unsigned int ring = (unsigned int)ceil(n_low_bound);
+  unsigned int seg = (unsigned int)ceil(n_low_bound);
+
+  generateBVHModel(model, shape, pose, seg, ring);
 }
 
-
-/// @brief Generate BVH model from cylinder, given the number of segments along circle and the number of segments along axis.
-template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, const Transform3f& pose, unsigned int tot, unsigned int h_num)
-{
-  std::vector<Vec3f> points;
+/// @brief Generate BVH model from cylinder, given the number of segments along
+/// circle and the number of segments along axis.
+template <typename BV>
+void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape,
+                      const Transform3s& pose, unsigned int tot,
+                      unsigned int h_num) {
+  std::vector<Vec3s> points;
   std::vector<Triangle> tri_indices;
 
-  double r = shape.radius;
-  double h = shape.lz;
-  double phi, phid;
-  const double pi = boost::math::constants::pi<double>();
+  CoalScalar r = shape.radius;
+  CoalScalar h = shape.halfLength;
+  CoalScalar phi, phid;
+  const CoalScalar pi = boost::math::constants::pi<CoalScalar>();
   phid = pi * 2 / tot;
   phi = 0;
 
-  double hd = h / h_num;
+  CoalScalar hd = 2 * h / h_num;
 
-  for(unsigned int i = 0; i < tot; ++i)
-    points.push_back(Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), h / 2));
+  for (unsigned int i = 0; i < tot; ++i)
+    points.push_back(
+        Vec3s(r * cos(phi + phid * i), r * sin(phi + phid * i), h));
 
-  for(unsigned int i = 0; i < h_num - 1; ++i)
-  {
-    for(unsigned int j = 0; j < tot; ++j)
-    {
-      points.push_back(Vec3f(r * cos(phi + phid * j), r * sin(phi + phid * j), h / 2 - (i + 1) * hd));
+  for (unsigned int i = 0; i < h_num - 1; ++i) {
+    for (unsigned int j = 0; j < tot; ++j) {
+      points.push_back(Vec3s(r * cos(phi + phid * j), r * sin(phi + phid * j),
+                             h - (i + 1) * hd));
     }
   }
 
-  for(unsigned int i = 0; i < tot; ++i)
-    points.push_back(Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), - h / 2));
+  for (unsigned int i = 0; i < tot; ++i)
+    points.push_back(
+        Vec3s(r * cos(phi + phid * i), r * sin(phi + phid * i), -h));
 
-  points.push_back(Vec3f(0, 0, h / 2));
-  points.push_back(Vec3f(0, 0, -h / 2));
+  points.push_back(Vec3s(0, 0, h));
+  points.push_back(Vec3s(0, 0, -h));
 
-  for(unsigned int i = 0; i < tot; ++i)
-  {
+  for (unsigned int i = 0; i < tot; ++i) {
     Triangle tmp((h_num + 1) * tot, i, ((i == tot - 1) ? 0 : (i + 1)));
     tri_indices.push_back(tmp);
   }
 
-  for(unsigned int i = 0; i < tot; ++i)
-  {
-    Triangle tmp((h_num + 1) * tot + 1, h_num * tot + ((i == tot - 1) ? 0 : (i + 1)), h_num * tot + i);
+  for (unsigned int i = 0; i < tot; ++i) {
+    Triangle tmp((h_num + 1) * tot + 1,
+                 h_num * tot + ((i == tot - 1) ? 0 : (i + 1)), h_num * tot + i);
     tri_indices.push_back(tmp);
   }
 
-  for(unsigned int i = 0; i < h_num; ++i)
-  {
-    for(unsigned int j = 0; j < tot; ++j)
-    {
-      int a, b, c, d;
+  for (unsigned int i = 0; i < h_num; ++i) {
+    for (unsigned int j = 0; j < tot; ++j) {
+      unsigned int a, b, c, d;
       a = j;
       b = (j == tot - 1) ? 0 : (j + 1);
       c = j + tot;
       d = (j == tot - 1) ? tot : (j + 1 + tot);
 
-      int start = i * tot;
+      unsigned int start = i * tot;
       tri_indices.push_back(Triangle(start + b, start + a, start + c));
       tri_indices.push_back(Triangle(start + b, start + c, start + d));
     }
   }
 
-  for(unsigned int i = 0; i < points.size(); ++i)
-  {
+  for (unsigned int i = 0; i < points.size(); ++i) {
     points[i] = pose.transform(points[i]);
   }
 
@@ -241,88 +238,88 @@ void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, const Transfor
 }
 
 /// @brief Generate BVH model from cylinder
-/// Difference from generateBVHModel: is that it gives the circle split number tot for a cylinder with unit radius. For cylinder with
-/// larger radius, the number of circle split number is r * tot.
-template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, const Transform3f& pose, unsigned int tot_for_unit_cylinder)
-{
-  double r = shape.radius;
-  double h = shape.lz;
-
-  const double pi = boost::math::constants::pi<double>();
-  unsigned int tot = tot_for_unit_cylinder * r;
-  double phid = pi * 2 / tot;
-
-  double circle_edge = phid * r;
-  unsigned int h_num = ceil(h / circle_edge);
-  
+/// Difference from generateBVHModel: is that it gives the circle split number
+/// tot for a cylinder with unit radius. For cylinder with larger radius, the
+/// number of circle split number is r * tot.
+template <typename BV>
+void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape,
+                      const Transform3s& pose,
+                      unsigned int tot_for_unit_cylinder) {
+  CoalScalar r = shape.radius;
+  CoalScalar h = 2 * shape.halfLength;
+
+  const CoalScalar pi = boost::math::constants::pi<CoalScalar>();
+  unsigned int tot = (unsigned int)(tot_for_unit_cylinder * r);
+  CoalScalar phid = pi * 2 / tot;
+
+  CoalScalar circle_edge = phid * r;
+  unsigned int h_num = (unsigned int)ceil(h / circle_edge);
+
   generateBVHModel(model, shape, pose, tot, h_num);
 }
 
-
-/// @brief Generate BVH model from cone, given the number of segments along circle and the number of segments along axis.
-template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cone& shape, const Transform3f& pose, unsigned int tot, unsigned int h_num)
-{
-  std::vector<Vec3f> points;
+/// @brief Generate BVH model from cone, given the number of segments along
+/// circle and the number of segments along axis.
+template <typename BV>
+void generateBVHModel(BVHModel<BV>& model, const Cone& shape,
+                      const Transform3s& pose, unsigned int tot,
+                      unsigned int h_num) {
+  std::vector<Vec3s> points;
   std::vector<Triangle> tri_indices;
 
-  double r = shape.radius;
-  double h = shape.lz;
+  CoalScalar r = shape.radius;
+  CoalScalar h = shape.halfLength;
 
-  double phi, phid;
-  const double pi = boost::math::constants::pi<double>();
+  CoalScalar phi, phid;
+  const CoalScalar pi = boost::math::constants::pi<CoalScalar>();
   phid = pi * 2 / tot;
   phi = 0;
 
-  double hd = h / h_num;
+  CoalScalar hd = 2 * h / h_num;
 
-  for(unsigned int i = 0; i < h_num - 1; ++i)
-  {
-    double h_i = h / 2 - (i + 1) * hd;
-    double rh = r * (0.5 - h_i / h);
-    for(unsigned int j = 0; j < tot; ++j)
-    {
-      points.push_back(Vec3f(rh * cos(phi + phid * j), rh * sin(phi + phid * j), h_i));
+  for (unsigned int i = 0; i < h_num - 1; ++i) {
+    CoalScalar h_i = h - (i + 1) * hd;
+    CoalScalar rh = r * (0.5 - h_i / h / 2);
+    for (unsigned int j = 0; j < tot; ++j) {
+      points.push_back(
+          Vec3s(rh * cos(phi + phid * j), rh * sin(phi + phid * j), h_i));
     }
   }
 
-  for(unsigned int i = 0; i < tot; ++i)
-    points.push_back(Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), - h / 2));
+  for (unsigned int i = 0; i < tot; ++i)
+    points.push_back(
+        Vec3s(r * cos(phi + phid * i), r * sin(phi + phid * i), -h));
 
-  points.push_back(Vec3f(0, 0, h / 2));
-  points.push_back(Vec3f(0, 0, -h / 2));
+  points.push_back(Vec3s(0, 0, h));
+  points.push_back(Vec3s(0, 0, -h));
 
-  for(unsigned int i = 0; i < tot; ++i)
-  {
+  for (unsigned int i = 0; i < tot; ++i) {
     Triangle tmp(h_num * tot, i, (i == tot - 1) ? 0 : (i + 1));
     tri_indices.push_back(tmp);
   }
 
-  for(unsigned int i = 0; i < tot; ++i)
-  {
-    Triangle tmp(h_num * tot + 1, (h_num - 1) * tot + ((i == tot - 1) ? 0 : (i + 1)), (h_num - 1) * tot + i);
+  for (unsigned int i = 0; i < tot; ++i) {
+    Triangle tmp(h_num * tot + 1,
+                 (h_num - 1) * tot + ((i == tot - 1) ? 0 : (i + 1)),
+                 (h_num - 1) * tot + i);
     tri_indices.push_back(tmp);
   }
 
-  for(unsigned int i = 0; i < h_num - 1; ++i)
-  {
-    for(unsigned int j = 0; j < tot; ++j)
-    {
-      int a, b, c, d;
+  for (unsigned int i = 0; i < h_num - 1; ++i) {
+    for (unsigned int j = 0; j < tot; ++j) {
+      unsigned int a, b, c, d;
       a = j;
       b = (j == tot - 1) ? 0 : (j + 1);
       c = j + tot;
       d = (j == tot - 1) ? tot : (j + 1 + tot);
 
-      int start = i * tot;
+      unsigned int start = i * tot;
       tri_indices.push_back(Triangle(start + b, start + a, start + c));
       tri_indices.push_back(Triangle(start + b, start + c, start + d));
     }
   }
 
-  for(unsigned int i = 0; i < points.size(); ++i)
-  {
+  for (unsigned int i = 0; i < points.size(); ++i) {
     points[i] = pose.transform(points[i]);
   }
 
@@ -333,24 +330,25 @@ void generateBVHModel(BVHModel<BV>& model, const Cone& shape, const Transform3f&
 }
 
 /// @brief Generate BVH model from cone
-/// Difference from generateBVHModel: is that it gives the circle split number tot for a cylinder with unit radius. For cone with
-/// larger radius, the number of circle split number is r * tot.
-template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cone& shape, const Transform3f& pose, unsigned int tot_for_unit_cone)
-{
-  double r = shape.radius;
-  double h = shape.lz;
-
-  const double pi = boost::math::constants::pi<double>();
-  unsigned int tot = tot_for_unit_cone * r;
-  double phid = pi * 2 / tot;
-
-  double circle_edge = phid * r;
-  unsigned int h_num = ceil(h / circle_edge);
+/// Difference from generateBVHModel: is that it gives the circle split number
+/// tot for a cylinder with unit radius. For cone with larger radius, the number
+/// of circle split number is r * tot.
+template <typename BV>
+void generateBVHModel(BVHModel<BV>& model, const Cone& shape,
+                      const Transform3s& pose, unsigned int tot_for_unit_cone) {
+  CoalScalar r = shape.radius;
+  CoalScalar h = 2 * shape.halfLength;
+
+  const CoalScalar pi = boost::math::constants::pi<CoalScalar>();
+  unsigned int tot = (unsigned int)(tot_for_unit_cone * r);
+  CoalScalar phid = pi * 2 / tot;
+
+  CoalScalar circle_edge = phid * r;
+  unsigned int h_num = (unsigned int)ceil(h / circle_edge);
 
   generateBVHModel(model, shape, pose, tot, h_num);
 }
 
-}
+}  // namespace coal
 
 #endif