include/coal/narrowphase/support_functions.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, Open Source Robotics Foundation
+ *  Copyright (c) 2021-2024, INRIA
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \authors Jia Pan, Florent Lamiraux, Josef Mirabel, Louis Montaut */
+
+#ifndef COAL_SUPPORT_FUNCTIONS_H
+#define COAL_SUPPORT_FUNCTIONS_H
+
+#include "coal/shape/geometric_shapes.h"
+#include "coal/math/transform.h"
+#include "coal/collision_data.h"
+
+namespace coal {
+
+namespace details {
+
+/// @brief Options for the computation of support points.
+/// `NoSweptSphere` option is used when the support function is called
+/// by GJK or EPA. In this case, the swept sphere radius is not taken into
+/// account in the support function. It is used by GJK and EPA after they have
+/// converged to correct the solution.
+/// `WithSweptSphere` option is used when the support function is called
+/// directly by the user. In this case, the swept sphere radius is taken into
+/// account in the support function.
+enum SupportOptions {
+  NoSweptSphere = 0,
+  WithSweptSphere = 0x1,
+};
+
+// ============================================================================
+// ============================ SUPPORT FUNCTIONS =============================
+// ============================================================================
+/// @brief the support function for shape.
+/// The output support point is expressed in the local frame of the shape.
+/// @return a point which belongs to the set {argmax_{v in shape} v.dot(dir)}.
+/// @param shape the shape.
+/// @param dir support direction.
+/// @param hint used to initialize the search when shape is a ConvexBase object.
+/// @tparam SupportOptions is a value of the SupportOptions enum. If set to
+/// `WithSweptSphere`, the support functions take into account the shapes' swept
+/// sphere radii. Please see `MinkowskiDiff::set(const ShapeBase*, const
+/// ShapeBase*)` for more details.
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+Vec3s getSupport(const ShapeBase* shape, const Vec3s& dir, int& hint);
+
+/// @brief Stores temporary data for the computation of support points.
+struct COAL_DLLAPI ShapeSupportData {
+  // @brief Tracks which points have been visited in a ConvexBase.
+  std::vector<int8_t> visited;
+
+  // @brief Tracks the last support direction used on this shape; used to
+  // warm-start the ConvexBase support function.
+  Vec3s last_dir = Vec3s::Zero();
+
+  // @brief Temporary set used to compute the convex-hull of a support set.
+  // Only used for ConvexBase and Box.
+  SupportSet::Polygon polygon;
+};
+
+/// @brief Triangle support function.
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+void getShapeSupport(const TriangleP* triangle, const Vec3s& dir,
+                     Vec3s& support, int& /*unused*/,
+                     ShapeSupportData& /*unused*/);
+
+/// @brief Box support function.
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+void getShapeSupport(const Box* box, const Vec3s& dir, Vec3s& support,
+                     int& /*unused*/, ShapeSupportData& /*unused*/);
+
+/// @brief Sphere support function.
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+void getShapeSupport(const Sphere* sphere, const Vec3s& dir, Vec3s& support,
+                     int& /*unused*/, ShapeSupportData& /*unused*/);
+
+/// @brief Ellipsoid support function.
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+void getShapeSupport(const Ellipsoid* ellipsoid, const Vec3s& dir,
+                     Vec3s& support, int& /*unused*/,
+                     ShapeSupportData& /*unused*/);
+
+/// @brief Capsule support function.
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+void getShapeSupport(const Capsule* capsule, const Vec3s& dir, Vec3s& support,
+                     int& /*unused*/, ShapeSupportData& /*unused*/);
+
+/// @brief Cone support function.
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+void getShapeSupport(const Cone* cone, const Vec3s& dir, Vec3s& support,
+                     int& /*unused*/, ShapeSupportData& /*unused*/);
+
+/// @brief Cylinder support function.
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+void getShapeSupport(const Cylinder* cylinder, const Vec3s& dir, Vec3s& support,
+                     int& /*unused*/, ShapeSupportData& /*unused*/);
+
+/// @brief ConvexBase support function.
+/// @note See @ref LargeConvex and SmallConvex to see how to optimize
+/// ConvexBase's support computation.
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+void getShapeSupport(const ConvexBase* convex, const Vec3s& dir, Vec3s& support,
+                     int& hint, ShapeSupportData& /*unused*/);
+
+/// @brief Cast a `ConvexBase` to a `LargeConvex` to use the log version of
+/// `getShapeSupport`. This is **much** faster than the linear version of
+/// `getShapeSupport` when a `ConvexBase` has more than a few dozen of vertices.
+/// @note WARNING: when using a LargeConvex, the neighbors in `ConvexBase` must
+/// have been constructed! Otherwise the support function will segfault.
+struct LargeConvex : ShapeBase {};
+/// @brief See @ref LargeConvex.
+struct SmallConvex : ShapeBase {};
+
+/// @brief Support function for large ConvexBase (>32 vertices).
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+void getShapeSupport(const SmallConvex* convex, const Vec3s& dir,
+                     Vec3s& support, int& hint, ShapeSupportData& data);
+
+/// @brief Support function for small ConvexBase (<32 vertices).
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+void getShapeSupport(const LargeConvex* convex, const Vec3s& dir,
+                     Vec3s& support, int& hint, ShapeSupportData& support_data);
+
+// ============================================================================
+// ========================== SUPPORT SET FUNCTIONS ===========================
+// ============================================================================
+/// @brief Computes the support set for shape.
+/// This function assumes the frame of the support set has already been
+/// computed and that this frame is expressed w.r.t the local frame of the
+/// shape (i.e. the local frame of the shape is the WORLD frame of the support
+/// set). The support direction used to compute the support set is the positive
+/// z-axis if the support set has the DEFAULT direction; negative z-axis if it
+/// has the INVERTED direction. (In short, a shape's support set is has the
+/// DEFAULT direction if the shape is the first shape in a collision pair. It
+/// has the INVERTED direction if the shape is the second one in the collision
+/// pair).
+/// @return an approximation of the set {argmax_{v in shape} v.dot(dir)}, where
+/// dir is the support set's support direction.
+/// The support set is a plane passing by the origin of the support set frame
+/// and supported by the direction dir. As a consequence, any point added to the
+/// set is automatically projected onto this plane.
+/// @param[in] shape the shape.
+/// @param[in/out] support_set of shape.
+/// @param[in/out] hint used to initialize the search when shape is a ConvexBase
+/// object.
+/// @param[in] num_sampled_supports is only used for shapes with smooth
+/// non-strictly convex bases like cones and cylinders (their bases are
+/// circles). In such a case, if the support direction points to their base, we
+/// have to choose which points we want to add to the set. This is not needed
+/// for boxes or ConvexBase for example. Indeed, because their support sets are
+/// always polygons, we can characterize the entire support set with the
+/// vertices of the polygon.
+/// @param[in] tol given a point v on the shape, if
+/// `max_{p in shape}(p.dot(dir)) - v.dot(dir) <= tol`, where dir is the set's
+/// support direction, then v is added to the support set.
+/// Otherwise said, if a point p of the shape is at a distance `tol` from the
+/// support plane, it is added to the set. Thus, `tol` can be seen as the
+/// "thickness" of the support plane.
+/// @tparam SupportOptions is a value of the SupportOptions enum. If set to
+/// `WithSweptSphere`, the support functions take into account the shapes' swept
+/// sphere radii.
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+void getSupportSet(const ShapeBase* shape, SupportSet& support_set, int& hint,
+                   size_t num_sampled_supports = 6, CoalScalar tol = 1e-3);
+
+/// @brief Same as @ref getSupportSet(const ShapeBase*, const CoalScalar,
+/// SupportSet&, const int) but also constructs the support set frame from
+/// `dir`.
+/// @note The support direction `dir` is expressed in the local frame of the
+/// shape.
+/// @note This function automatically deals with the `direction` of the
+/// SupportSet.
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+void getSupportSet(const ShapeBase* shape, const Vec3s& dir,
+                   SupportSet& support_set, int& hint,
+                   size_t num_sampled_supports = 6, CoalScalar tol = 1e-3) {
+  support_set.tf.rotation() = constructOrthonormalBasisFromVector(dir);
+  const Vec3s& support_dir = support_set.getNormal();
+  const Vec3s support = getSupport<_SupportOptions>(shape, support_dir, hint);
+  getSupportSet<_SupportOptions>(shape, support_set, hint, num_sampled_supports,
+                                 tol);
+}
+
+/// @brief Triangle support set function.
+/// Assumes the support set frame has already been computed.
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+void getShapeSupportSet(const TriangleP* triangle, SupportSet& support_set,
+                        int& /*unused*/, ShapeSupportData& /*unused*/,
+                        size_t /*unused*/ num_sampled_supports = 6,
+                        CoalScalar tol = 1e-3);
+
+/// @brief Box support set function.
+/// Assumes the support set frame has already been computed.
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+void getShapeSupportSet(const Box* box, SupportSet& support_set,
+                        int& /*unused*/, ShapeSupportData& support_data,
+                        size_t /*unused*/ num_sampled_supports = 6,
+                        CoalScalar tol = 1e-3);
+
+/// @brief Sphere support set function.
+/// Assumes the support set frame has already been computed.
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+void getShapeSupportSet(const Sphere* sphere, SupportSet& support_set,
+                        int& /*unused*/, ShapeSupportData& /*unused*/,
+                        size_t /*unused*/ num_sampled_supports = 6,
+                        CoalScalar /*unused*/ tol = 1e-3);
+
+/// @brief Ellipsoid support set function.
+/// Assumes the support set frame has already been computed.
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+void getShapeSupportSet(const Ellipsoid* ellipsoid, SupportSet& support_set,
+                        int& /*unused*/, ShapeSupportData& /*unused*/,
+                        size_t /*unused*/ num_sampled_supports = 6,
+                        CoalScalar /*unused*/ tol = 1e-3);
+
+/// @brief Capsule support set function.
+/// Assumes the support set frame has already been computed.
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+void getShapeSupportSet(const Capsule* capsule, SupportSet& support_set,
+                        int& /*unused*/, ShapeSupportData& /*unused*/,
+                        size_t /*unused*/ num_sampled_supports = 6,
+                        CoalScalar tol = 1e-3);
+
+/// @brief Cone support set function.
+/// Assumes the support set frame has already been computed.
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+void getShapeSupportSet(const Cone* cone, SupportSet& support_set,
+                        int& /*unused*/, ShapeSupportData& /*unused*/,
+                        size_t num_sampled_supports = 6, CoalScalar tol = 1e-3);
+
+/// @brief Cylinder support set function.
+/// Assumes the support set frame has already been computed.
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+void getShapeSupportSet(const Cylinder* cylinder, SupportSet& support_set,
+                        int& /*unused*/, ShapeSupportData& /*unused*/,
+                        size_t num_sampled_supports = 6, CoalScalar tol = 1e-3);
+
+/// @brief ConvexBase support set function.
+/// Assumes the support set frame has already been computed.
+/// @note See @ref LargeConvex and SmallConvex to see how to optimize
+/// ConvexBase's support computation.
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+void getShapeSupportSet(const ConvexBase* convex, SupportSet& support_set,
+                        int& hint, ShapeSupportData& support_data,
+                        size_t /*unused*/ num_sampled_supports = 6,
+                        CoalScalar tol = 1e-3);
+
+/// @brief Support set function for large ConvexBase (>32 vertices).
+/// Assumes the support set frame has already been computed.
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+void getShapeSupportSet(const SmallConvex* convex, SupportSet& support_set,
+                        int& /*unused*/, ShapeSupportData& /*unused*/,
+                        size_t /*unused*/ num_sampled_supports = 6,
+                        CoalScalar tol = 1e-3);
+
+/// @brief Support set function for small ConvexBase (<32 vertices).
+/// Assumes the support set frame has already been computed.
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+void getShapeSupportSet(const LargeConvex* convex, SupportSet& support_set,
+                        int& hint, ShapeSupportData& support_data,
+                        size_t /*unused*/ num_sampled_supports = 6,
+                        CoalScalar tol = 1e-3);
+
+/// @brief Computes the convex-hull of support_set. For now, this function is
+/// only needed for Box and ConvexBase.
+/// @param[in] cloud data which contains the 2d points of the support set which
+/// convex-hull we want to compute.
+/// @param[out] 2d points of the the support set's convex-hull.
+COAL_DLLAPI void computeSupportSetConvexHull(SupportSet::Polygon& cloud,
+                                             SupportSet::Polygon& cvx_hull);
+
+}  // namespace details
+
+}  // namespace coal
+
+#endif  // COAL_SUPPORT_FUNCTIONS_H

include/coal/octree.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, Open Source Robotics Foundation
+ *  Copyright (c) 2022-2024, Inria
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Jia Pan */
+
+#ifndef COAL_OCTREE_H
+#define COAL_OCTREE_H
+
+#include <algorithm>
+
+#include <octomap/octomap.h>
+#include "coal/fwd.hh"
+#include "coal/BV/AABB.h"
+#include "coal/collision_object.h"
+
+namespace coal {
+
+/// @brief Octree is one type of collision geometry which can encode uncertainty
+/// information in the sensor data.
+class COAL_DLLAPI OcTree : public CollisionGeometry {
+ protected:
+  shared_ptr<const octomap::OcTree> tree;
+
+  CoalScalar default_occupancy;
+
+  CoalScalar occupancy_threshold;
+  CoalScalar free_threshold;
+
+ public:
+  typedef octomap::OcTreeNode OcTreeNode;
+
+  /// @brief construct octree with a given resolution
+  explicit OcTree(CoalScalar resolution)
+      : tree(shared_ptr<const octomap::OcTree>(
+            new octomap::OcTree(resolution))) {
+    default_occupancy = tree->getOccupancyThres();
+
+    // default occupancy/free threshold is consistent with default setting from
+    // octomap
+    occupancy_threshold = tree->getOccupancyThres();
+    free_threshold = 0;
+  }
+
+  /// @brief construct octree from octomap
+  explicit OcTree(const shared_ptr<const octomap::OcTree>& tree_)
+      : tree(tree_) {
+    default_occupancy = tree->getOccupancyThres();
+
+    // default occupancy/free threshold is consistent with default setting from
+    // octomap
+    occupancy_threshold = tree->getOccupancyThres();
+    free_threshold = 0;
+  }
+
+  ///  \brief Copy constructor
+  OcTree(const OcTree& other)
+      : CollisionGeometry(other),
+        tree(other.tree),
+        default_occupancy(other.default_occupancy),
+        occupancy_threshold(other.occupancy_threshold),
+        free_threshold(other.free_threshold) {}
+
+  /// \brief Clone *this into a new Octree
+  OcTree* clone() const { return new OcTree(*this); }
+
+  /// \brief Returns the tree associated to the underlying octomap OcTree.
+  shared_ptr<const octomap::OcTree> getTree() const { return tree; }
+
+  void exportAsObjFile(const std::string& filename) const;
+
+  /// @brief compute the AABB for the octree in its local coordinate system
+  void computeLocalAABB() {
+    typedef Eigen::Matrix<float, 3, 1> Vec3sloat;
+    Vec3sloat max_extent, min_extent;
+
+    octomap::OcTree::iterator it =
+        tree->begin((unsigned char)tree->getTreeDepth());
+    octomap::OcTree::iterator end = tree->end();
+
+    if (it == end) return;
+
+    {
+      const octomap::point3d& coord =
+          it.getCoordinate();  // getCoordinate returns a copy
+      max_extent = min_extent = Eigen::Map<const Vec3sloat>(&coord.x());
+      for (++it; it != end; ++it) {
+        const octomap::point3d& coord = it.getCoordinate();
+        const Vec3sloat pos = Eigen::Map<const Vec3sloat>(&coord.x());
+        max_extent = max_extent.array().max(pos.array());
+        min_extent = min_extent.array().min(pos.array());
+      }
+    }
+
+    // Account for the size of the boxes.
+    const CoalScalar resolution = tree->getResolution();
+    max_extent.array() += float(resolution / 2.);
+    min_extent.array() -= float(resolution / 2.);
+
+    aabb_local =
+        AABB(min_extent.cast<CoalScalar>(), max_extent.cast<CoalScalar>());
+    aabb_center = aabb_local.center();
+    aabb_radius = (aabb_local.min_ - aabb_center).norm();
+  }
+
+  /// @brief get the bounding volume for the root
+  AABB getRootBV() const {
+    CoalScalar delta = (1 << tree->getTreeDepth()) * tree->getResolution() / 2;
+
+    // std::cout << "octree size " << delta << std::endl;
+    return AABB(Vec3s(-delta, -delta, -delta), Vec3s(delta, delta, delta));
+  }
+
+  /// @brief Returns the depth of the octree
+  unsigned int getTreeDepth() const { return tree->getTreeDepth(); }
+
+  /// @brief Returns the size of the octree
+  unsigned long size() const { return tree->size(); }
+
+  /// @brief Returns the resolution of the octree
+  CoalScalar getResolution() const { return tree->getResolution(); }
+
+  /// @brief get the root node of the octree
+  OcTreeNode* getRoot() const { return tree->getRoot(); }
+
+  /// @brief whether one node is completely occupied
+  bool isNodeOccupied(const OcTreeNode* node) const {
+    // return tree->isNodeOccupied(node);
+    return node->getOccupancy() >= occupancy_threshold;
+  }
+
+  /// @brief whether one node is completely free
+  bool isNodeFree(const OcTreeNode* node) const {
+    // return false; // default no definitely free node
+    return node->getOccupancy() <= free_threshold;
+  }
+
+  /// @brief whether one node is uncertain
+  bool isNodeUncertain(const OcTreeNode* node) const {
+    return (!isNodeOccupied(node)) && (!isNodeFree(node));
+  }
+
+  /// @brief transform the octree into a bunch of boxes; uncertainty information
+  /// is kept in the boxes. However, we only keep the occupied boxes (i.e., the
+  /// boxes whose occupied probability is higher enough).
+  std::vector<Vec6s> toBoxes() const {
+    std::vector<Vec6s> boxes;
+    boxes.reserve(tree->size() / 2);
+    for (octomap::OcTree::iterator
+             it = tree->begin((unsigned char)tree->getTreeDepth()),
+             end = tree->end();
+         it != end; ++it) {
+      // if(tree->isNodeOccupied(*it))
+      if (isNodeOccupied(&*it)) {
+        CoalScalar x = it.getX();
+        CoalScalar y = it.getY();
+        CoalScalar z = it.getZ();
+        CoalScalar size = it.getSize();
+        CoalScalar c = (*it).getOccupancy();
+        CoalScalar t = tree->getOccupancyThres();
+
+        Vec6s box;
+        box << x, y, z, size, c, t;
+        boxes.push_back(box);
+      }
+    }
+    return boxes;
+  }
+
+  /// \brief Returns a byte description of *this
+  std::vector<uint8_t> tobytes() const {
+    typedef Eigen::Matrix<float, 3, 1> Vec3sloat;
+    const size_t total_size = (tree->size() * sizeof(CoalScalar) * 3) / 2;
+    std::vector<uint8_t> bytes;
+    bytes.reserve(total_size);
+
+    for (octomap::OcTree::iterator
+             it = tree->begin((unsigned char)tree->getTreeDepth()),
+             end = tree->end();
+         it != end; ++it) {
+      const Vec3s box_pos =
+          Eigen::Map<Vec3sloat>(&it.getCoordinate().x()).cast<CoalScalar>();
+      if (isNodeOccupied(&*it))
+        std::copy(box_pos.data(), box_pos.data() + sizeof(CoalScalar) * 3,
+                  std::back_inserter(bytes));
+    }
+
+    return bytes;
+  }
+
+  /// @brief the threshold used to decide whether one node is occupied, this is
+  /// NOT the octree occupied_thresold
+  CoalScalar getOccupancyThres() const { return occupancy_threshold; }
+
+  /// @brief the threshold used to decide whether one node is free, this is NOT
+  /// the octree free_threshold
+  CoalScalar getFreeThres() const { return free_threshold; }
+
+  CoalScalar getDefaultOccupancy() const { return default_occupancy; }
+
+  void setCellDefaultOccupancy(CoalScalar d) { default_occupancy = d; }
+
+  void setOccupancyThres(CoalScalar d) { occupancy_threshold = d; }
+
+  void setFreeThres(CoalScalar d) { free_threshold = d; }
+
+  /// @return ptr to child number childIdx of node
+  OcTreeNode* getNodeChild(OcTreeNode* node, unsigned int childIdx) {
+#if OCTOMAP_VERSION_AT_LEAST(1, 8, 0)
+    return tree->getNodeChild(node, childIdx);
+#else
+    return node->getChild(childIdx);
+#endif
+  }
+
+  /// @return const ptr to child number childIdx of node
+  const OcTreeNode* getNodeChild(const OcTreeNode* node,
+                                 unsigned int childIdx) const {
+#if OCTOMAP_VERSION_AT_LEAST(1, 8, 0)
+    return tree->getNodeChild(node, childIdx);
+#else
+    return node->getChild(childIdx);
+#endif
+  }
+
+  /// @brief return true if the child at childIdx exists
+  bool nodeChildExists(const OcTreeNode* node, unsigned int childIdx) const {
+#if OCTOMAP_VERSION_AT_LEAST(1, 8, 0)
+    return tree->nodeChildExists(node, childIdx);
+#else
+    return node->childExists(childIdx);
+#endif
+  }
+
+  /// @brief return true if node has at least one child
+  bool nodeHasChildren(const OcTreeNode* node) const {
+#if OCTOMAP_VERSION_AT_LEAST(1, 8, 0)
+    return tree->nodeHasChildren(node);
+#else
+    return node->hasChildren();
+#endif
+  }
+
+  /// @brief return object type, it is an octree
+  OBJECT_TYPE getObjectType() const { return OT_OCTREE; }
+
+  /// @brief return node type, it is an octree
+  NODE_TYPE getNodeType() const { return GEOM_OCTREE; }
+
+ private:
+  virtual bool isEqual(const CollisionGeometry& _other) const {
+    const OcTree* other_ptr = dynamic_cast<const OcTree*>(&_other);
+    if (other_ptr == nullptr) return false;
+    const OcTree& other = *other_ptr;
+
+    return (tree.get() == other.tree.get() || toBoxes() == other.toBoxes()) &&
+           default_occupancy == other.default_occupancy &&
+           occupancy_threshold == other.occupancy_threshold &&
+           free_threshold == other.free_threshold;
+  }
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+/// @brief compute the bounding volume of an octree node's i-th child
+static inline void computeChildBV(const AABB& root_bv, unsigned int i,
+                                  AABB& child_bv) {
+  if (i & 1) {
+    child_bv.min_[0] = (root_bv.min_[0] + root_bv.max_[0]) * 0.5;
+    child_bv.max_[0] = root_bv.max_[0];
+  } else {
+    child_bv.min_[0] = root_bv.min_[0];
+    child_bv.max_[0] = (root_bv.min_[0] + root_bv.max_[0]) * 0.5;
+  }
+
+  if (i & 2) {
+    child_bv.min_[1] = (root_bv.min_[1] + root_bv.max_[1]) * 0.5;
+    child_bv.max_[1] = root_bv.max_[1];
+  } else {
+    child_bv.min_[1] = root_bv.min_[1];
+    child_bv.max_[1] = (root_bv.min_[1] + root_bv.max_[1]) * 0.5;
+  }
+
+  if (i & 4) {
+    child_bv.min_[2] = (root_bv.min_[2] + root_bv.max_[2]) * 0.5;
+    child_bv.max_[2] = root_bv.max_[2];
+  } else {
+    child_bv.min_[2] = root_bv.min_[2];
+    child_bv.max_[2] = (root_bv.min_[2] + root_bv.max_[2]) * 0.5;
+  }
+}
+
+///
+/// \brief Build an OcTree from a point cloud and a given resolution
+///
+/// \param[in] point_cloud The input points to insert in the OcTree
+/// \param[in] resolution of the octree.
+///
+/// \returns An OcTree that can be used for collision checking and more.
+///
+COAL_DLLAPI OcTreePtr_t
+makeOctree(const Eigen::Matrix<CoalScalar, Eigen::Dynamic, 3>& point_cloud,
+           const CoalScalar resolution);
+
+}  // namespace coal
+
+#endif

include/coal/serialization/AABB.h

+//
+// Copyright (c) 2021 INRIA
+//
+
+#ifndef COAL_SERIALIZATION_AABB_H
+#define COAL_SERIALIZATION_AABB_H
+
+#include "coal/BV/AABB.h"
+#include "coal/serialization/fwd.h"
+
+namespace boost {
+namespace serialization {
+
+template <class Archive>
+void serialize(Archive& ar, coal::AABB& aabb, const unsigned int /*version*/) {
+  ar& make_nvp("min_", aabb.min_);
+  ar& make_nvp("max_", aabb.max_);
+}
+
+}  // namespace serialization
+}  // namespace boost
+
+#endif  // ifndef COAL_SERIALIZATION_AABB_H

include/coal/serialization/BVH_model.h

+//
+// Copyright (c) 2021-2022 INRIA
+//
+
+#ifndef COAL_SERIALIZATION_BVH_MODEL_H
+#define COAL_SERIALIZATION_BVH_MODEL_H
+
+#include "coal/BVH/BVH_model.h"
+
+#include "coal/serialization/fwd.h"
+#include "coal/serialization/BV_node.h"
+#include "coal/serialization/BV_splitter.h"
+#include "coal/serialization/collision_object.h"
+#include "coal/serialization/memory.h"
+#include "coal/serialization/triangle.h"
+
+namespace boost {
+namespace serialization {
+
+namespace internal {
+struct BVHModelBaseAccessor : coal::BVHModelBase {
+  typedef coal::BVHModelBase Base;
+  using Base::num_tris_allocated;
+  using Base::num_vertices_allocated;
+};
+}  // namespace internal
+
+template <class Archive>
+void save(Archive &ar, const coal::BVHModelBase &bvh_model,
+          const unsigned int /*version*/) {
+  using namespace coal;
+  if (!(bvh_model.build_state == BVH_BUILD_STATE_PROCESSED ||
+        bvh_model.build_state == BVH_BUILD_STATE_UPDATED) &&
+      (bvh_model.getModelType() == BVH_MODEL_TRIANGLES)) {
+    COAL_THROW_PRETTY(
+        "The BVH model is not in a BVH_BUILD_STATE_PROCESSED or "
+        "BVH_BUILD_STATE_UPDATED state.\n"
+        "The BVHModel could not be serialized.",
+        std::invalid_argument);
+  }
+
+  ar &make_nvp(
+      "base",
+      boost::serialization::base_object<coal::CollisionGeometry>(bvh_model));
+
+  ar &make_nvp("num_vertices", bvh_model.num_vertices);
+  ar &make_nvp("vertices", bvh_model.vertices);
+
+  ar &make_nvp("num_tris", bvh_model.num_tris);
+  ar &make_nvp("tri_indices", bvh_model.tri_indices);
+  ar &make_nvp("build_state", bvh_model.build_state);
+
+  ar &make_nvp("prev_vertices", bvh_model.prev_vertices);
+
+  //      if(bvh_model.convex)
+  //      {
+  //        const bool has_convex = true;
+  //        ar << make_nvp("has_convex",has_convex);
+  //      }
+  //      else
+  //      {
+  //        const bool has_convex = false;
+  //        ar << make_nvp("has_convex",has_convex);
+  //      }
+}
+
+template <class Archive>
+void load(Archive &ar, coal::BVHModelBase &bvh_model,
+          const unsigned int /*version*/) {
+  using namespace coal;
+
+  ar >> make_nvp("base",
+                 boost::serialization::base_object<coal::CollisionGeometry>(
+                     bvh_model));
+
+  ar >> make_nvp("num_vertices", bvh_model.num_vertices);
+  ar >> make_nvp("vertices", bvh_model.vertices);
+
+  ar >> make_nvp("num_tris", bvh_model.num_tris);
+  ar >> make_nvp("tri_indices", bvh_model.tri_indices);
+  ar >> make_nvp("build_state", bvh_model.build_state);
+
+  ar >> make_nvp("prev_vertices", bvh_model.prev_vertices);
+
+  //      bool has_convex = true;
+  //      ar >> make_nvp("has_convex",has_convex);
+}
+
+COAL_SERIALIZATION_SPLIT(coal::BVHModelBase)
+
+namespace internal {
+template <typename BV>
+struct BVHModelAccessor : coal::BVHModel<BV> {
+  typedef coal::BVHModel<BV> Base;
+  using Base::bvs;
+  using Base::num_bvs;
+  using Base::num_bvs_allocated;
+  using Base::primitive_indices;
+};
+}  // namespace internal
+
+template <class Archive, typename BV>
+void serialize(Archive &ar, coal::BVHModel<BV> &bvh_model,
+               const unsigned int version) {
+  split_free(ar, bvh_model, version);
+}
+
+template <class Archive, typename BV>
+void save(Archive &ar, const coal::BVHModel<BV> &bvh_model_,
+          const unsigned int /*version*/) {
+  using namespace coal;
+  typedef internal::BVHModelAccessor<BV> Accessor;
+  typedef BVNode<BV> Node;
+
+  const Accessor &bvh_model = reinterpret_cast<const Accessor &>(bvh_model_);
+  ar &make_nvp("base",
+               boost::serialization::base_object<BVHModelBase>(bvh_model));
+
+  //      if(bvh_model.primitive_indices)
+  //      {
+  //        const bool with_primitive_indices = true;
+  //        ar & make_nvp("with_primitive_indices",with_primitive_indices);
+  //
+  //        int num_primitives = 0;
+  //        switch(bvh_model.getModelType())
+  //        {
+  //          case BVH_MODEL_TRIANGLES:
+  //            num_primitives = bvh_model.num_tris;
+  //            break;
+  //          case BVH_MODEL_POINTCLOUD:
+  //            num_primitives = bvh_model.num_vertices;
+  //            break;
+  //          default:
+  //            ;
+  //        }
+  //
+  //        ar & make_nvp("num_primitives",num_primitives);
+  //        if(num_primitives > 0)
+  //        {
+  //          typedef Eigen::Matrix<unsigned int,1,Eigen::Dynamic>
+  //          AsPrimitiveIndexVector; const Eigen::Map<const
+  //          AsPrimitiveIndexVector>
+  //          primitive_indices_map(reinterpret_cast<const unsigned int
+  //          *>(bvh_model.primitive_indices),1,num_primitives); ar &
+  //          make_nvp("primitive_indices",primitive_indices_map);
+  ////          ar &
+  /// make_nvp("primitive_indices",make_array(bvh_model.primitive_indices,num_primitives));
+  //        }
+  //      }
+  //      else
+  //      {
+  //        const bool with_primitive_indices = false;
+  //        ar & make_nvp("with_primitive_indices",with_primitive_indices);
+  //      }
+  //
+
+  if (bvh_model.bvs.get()) {
+    const bool with_bvs = true;
+    ar &make_nvp("with_bvs", with_bvs);
+    ar &make_nvp("num_bvs", bvh_model.num_bvs);
+    ar &make_nvp(
+        "bvs",
+        make_array(
+            reinterpret_cast<const char *>(bvh_model.bvs->data()),
+            sizeof(Node) *
+                (std::size_t)bvh_model.num_bvs));  // Assuming BVs are POD.
+  } else {
+    const bool with_bvs = false;
+    ar &make_nvp("with_bvs", with_bvs);
+  }
+}
+
+template <class Archive, typename BV>
+void load(Archive &ar, coal::BVHModel<BV> &bvh_model_,
+          const unsigned int /*version*/) {
+  using namespace coal;
+  typedef internal::BVHModelAccessor<BV> Accessor;
+  typedef BVNode<BV> Node;
+
+  Accessor &bvh_model = reinterpret_cast<Accessor &>(bvh_model_);
+
+  ar >> make_nvp("base",
+                 boost::serialization::base_object<BVHModelBase>(bvh_model));
+
+  //      bool with_primitive_indices;
+  //      ar >> make_nvp("with_primitive_indices",with_primitive_indices);
+  //      if(with_primitive_indices)
+  //      {
+  //        int num_primitives;
+  //        ar >> make_nvp("num_primitives",num_primitives);
+  //
+  //        delete[] bvh_model.primitive_indices;
+  //        if(num_primitives > 0)
+  //        {
+  //          bvh_model.primitive_indices = new unsigned int[num_primitives];
+  //          ar &
+  //          make_nvp("primitive_indices",make_array(bvh_model.primitive_indices,num_primitives));
+  //        }
+  //        else
+  //          bvh_model.primitive_indices = NULL;
+  //      }
+
+  bool with_bvs;
+  ar >> make_nvp("with_bvs", with_bvs);
+  if (with_bvs) {
+    unsigned int num_bvs;
+    ar >> make_nvp("num_bvs", num_bvs);
+
+    if (num_bvs != bvh_model.num_bvs) {
+      bvh_model.bvs.reset();
+      bvh_model.num_bvs = num_bvs;
+      if (num_bvs > 0)
+        bvh_model.bvs.reset(new
+                            typename BVHModel<BV>::bv_node_vector_t(num_bvs));
+    }
+    if (num_bvs > 0) {
+      ar >> make_nvp("bvs",
+                     make_array(reinterpret_cast<char *>(bvh_model.bvs->data()),
+                                sizeof(Node) * (std::size_t)num_bvs));
+    } else
+      bvh_model.bvs.reset();
+  }
+}
+
+}  // namespace serialization
+}  // namespace boost
+
+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
+
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::AABB>)
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::OBB>)
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::RSS>)
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::OBBRSS>)
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::kIOS>)
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::KDOP<16>>)
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::KDOP<18>>)
+COAL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::KDOP<24>>)
+
+#endif  // ifndef COAL_SERIALIZATION_BVH_MODEL_H

include/coal/serialization/BV_node.h

+//
+// Copyright (c) 2021 INRIA
+//
+
+#ifndef COAL_SERIALIZATION_BV_NODE_H
+#define COAL_SERIALIZATION_BV_NODE_H
+
+#include "coal/BV/BV_node.h"
+
+#include "coal/serialization/fwd.h"
+#include "coal/serialization/OBBRSS.h"
+
+namespace boost {
+namespace serialization {
+
+template <class Archive>
+void serialize(Archive& ar, coal::BVNodeBase& node,
+               const unsigned int /*version*/) {
+  ar& make_nvp("first_child", node.first_child);
+  ar& make_nvp("first_primitive", node.first_primitive);
+  ar& make_nvp("num_primitives", node.num_primitives);
+}
+
+template <class Archive, typename BV>
+void serialize(Archive& ar, coal::BVNode<BV>& node,
+               const unsigned int /*version*/) {
+  ar& make_nvp("base",
+               boost::serialization::base_object<coal::BVNodeBase>(node));
+  ar& make_nvp("bv", node.bv);
+}
+
+}  // namespace serialization
+}  // namespace boost
+
+#endif  // ifndef COAL_SERIALIZATION_BV_NODE_H

include/coal/serialization/BV_splitter.h

+//
+// Copyright (c) 2021 INRIA
+//
+
+#ifndef COAL_SERIALIZATION_BV_SPLITTER_H
+#define COAL_SERIALIZATION_BV_SPLITTER_H
+
+#include "coal/internal/BV_splitter.h"
+
+#include "coal/serialization/fwd.h"
+
+namespace boost {
+namespace serialization {
+
+namespace internal {
+template <typename BV>
+struct BVSplitterAccessor : coal::BVSplitter<BV> {
+  typedef coal::BVSplitter<BV> Base;
+  using Base::split_axis;
+  using Base::split_method;
+  using Base::split_value;
+  using Base::split_vector;
+  using Base::tri_indices;
+  using Base::type;
+  using Base::vertices;
+};
+}  // namespace internal
+
+template <class Archive, typename BV>
+void save(Archive &ar, const coal::BVSplitter<BV> &splitter_,
+          const unsigned int /*version*/) {
+  using namespace coal;
+  typedef internal::BVSplitterAccessor<BV> Accessor;
+  const Accessor &splitter = reinterpret_cast<const Accessor &>(splitter_);
+
+  ar &make_nvp("split_axis", splitter.split_axis);
+  ar &make_nvp("split_vector", splitter.split_vector);
+  ar &make_nvp("split_value", splitter.split_value);
+  ar &make_nvp("type", splitter.type);
+  ar &make_nvp("split_method", splitter.split_method);
+}
+
+template <class Archive, typename BV>
+void load(Archive &ar, coal::BVSplitter<BV> &splitter_,
+          const unsigned int /*version*/) {
+  using namespace coal;
+  typedef internal::BVSplitterAccessor<BV> Accessor;
+  Accessor &splitter = reinterpret_cast<Accessor &>(splitter_);
+
+  ar >> make_nvp("split_axis", splitter.split_axis);
+  ar >> make_nvp("split_vector", splitter.split_vector);
+  ar >> make_nvp("split_value", splitter.split_value);
+  ar >> make_nvp("type", splitter.type);
+  ar >> make_nvp("split_method", splitter.split_method);
+
+  splitter.vertices = NULL;
+  splitter.tri_indices = NULL;
+}
+}  // namespace serialization
+}  // namespace boost
+
+#endif  // ifndef COAL_SERIALIZATION_BV_SPLITTER_H

include/coal/serialization/OBB.h

+//
+// Copyright (c) 2021 INRIA
+//
+
+#ifndef COAL_SERIALIZATION_OBB_H
+#define COAL_SERIALIZATION_OBB_H
+
+#include "coal/BV/OBB.h"
+
+#include "coal/serialization/fwd.h"
+
+namespace boost {
+namespace serialization {
+
+template <class Archive>
+void serialize(Archive& ar, coal::OBB& bv, const unsigned int /*version*/) {
+  ar& make_nvp("axes", bv.axes);
+  ar& make_nvp("To", bv.To);
+  ar& make_nvp("extent", bv.extent);
+}
+
+}  // namespace serialization
+}  // namespace boost
+
+#endif  // ifndef COAL_SERIALIZATION_OBB_H

include/coal/serialization/OBBRSS.h

+//
+// Copyright (c) 2021 INRIA
+//
+
+#ifndef COAL_SERIALIZATION_OBBRSS_H
+#define COAL_SERIALIZATION_OBBRSS_H
+
+#include "coal/BV/OBBRSS.h"
+
+#include "coal/serialization/fwd.h"
+#include "coal/serialization/OBB.h"
+#include "coal/serialization/RSS.h"
+
+namespace boost {
+namespace serialization {
+
+template <class Archive>
+void serialize(Archive& ar, coal::OBBRSS& bv, const unsigned int /*version*/) {
+  ar& make_nvp("obb", bv.obb);
+  ar& make_nvp("rss", bv.rss);
+}
+
+}  // namespace serialization
+}  // namespace boost
+
+#endif  // ifndef COAL_SERIALIZATION_OBBRSS_H

include/coal/serialization/RSS.h

+//
+// Copyright (c) 2021 INRIA
+//
+
+#ifndef COAL_SERIALIZATION_RSS_H
+#define COAL_SERIALIZATION_RSS_H
+
+#include "coal/BV/RSS.h"
+
+#include "coal/serialization/fwd.h"
+
+namespace boost {
+namespace serialization {
+
+template <class Archive>
+void serialize(Archive& ar, coal::RSS& bv, const unsigned int /*version*/) {
+  ar& make_nvp("axes", bv.axes);
+  ar& make_nvp("Tr", bv.Tr);
+  ar& make_nvp("length", make_array(bv.length, 2));
+  ar& make_nvp("radius", bv.radius);
+}
+
+}  // namespace serialization
+}  // namespace boost
+
+#endif  // ifndef COAL_SERIALIZATION_RSS_H

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