Merge pull request #363 from jcarpent/topic/devel

Add support of Pickling

CMakeLists.txt

@@ -1,7 +1,7 @@
 #
 # Software License Agreement (BSD License)
 #
-#  Copyright (c) 2014-2020 CNRS-LAAS, INRIA
+#  Copyright (c) 2014-2022 CNRS-LAAS, INRIA
 #  Author: Florent Lamiraux, Joseph Mirabel
 #  All rights reserved.
 #
@@ -249,13 +249,16 @@ SET(${PROJECT_NAME}_HEADERS
   include/hpp/fcl/serialization/BVH_model.h
   include/hpp/fcl/serialization/collision_data.h
   include/hpp/fcl/serialization/collision_object.h
+  include/hpp/fcl/serialization/convex.h
   include/hpp/fcl/serialization/eigen.h
   include/hpp/fcl/serialization/geometric_shapes.h
   include/hpp/fcl/serialization/memory.h
   include/hpp/fcl/serialization/OBB.h
   include/hpp/fcl/serialization/RSS.h
   include/hpp/fcl/serialization/OBBRSS.h
   include/hpp/fcl/serialization/hfield.h
+  include/hpp/fcl/serialization/quadrilateral.h
+  include/hpp/fcl/serialization/triangle.h
   include/hpp/fcl/timings.h
   )
 

include/hpp/fcl/serialization/BVH_model.h

@@ -1,5 +1,5 @@
 //
-// Copyright (c) 2021 INRIA
+// Copyright (c) 2021-2022 INRIA
 //
 
 #ifndef HPP_FCL_SERIALIZATION_BVH_MODEL_H
@@ -12,6 +12,7 @@
 #include "hpp/fcl/serialization/BV_splitter.h"
 #include "hpp/fcl/serialization/collision_object.h"
 #include "hpp/fcl/serialization/memory.h"
+#include "hpp/fcl/serialization/triangle.h"
 
 namespace boost {
 namespace serialization {
@@ -24,14 +25,6 @@ struct BVHModelBaseAccessor : hpp::fcl::BVHModelBase {
 };
 }  // namespace internal
 
-template <class Archive>
-void serialize(Archive &ar, hpp::fcl::Triangle &triangle,
-               const unsigned int /*version*/) {
-  ar &make_nvp("p0", triangle[0]);
-  ar &make_nvp("p1", triangle[1]);
-  ar &make_nvp("p2", triangle[2]);
-}
-
 template <class Archive>
 void save(Archive &ar, const hpp::fcl::BVHModelBase &bvh_model,
           const unsigned int /*version*/) {

include/hpp/fcl/serialization/convex.h

@@ -0,0 +1,113 @@
+//
+// Copyright (c) 2022 INRIA
+//
+
+#ifndef HPP_FCL_SERIALIZATION_CONVEX_H
+#define HPP_FCL_SERIALIZATION_CONVEX_H
+
+#include "hpp/fcl/shape/convex.h"
+
+#include "hpp/fcl/serialization/fwd.h"
+#include "hpp/fcl/serialization/geometric_shapes.h"
+#include "hpp/fcl/serialization/memory.h"
+#include "hpp/fcl/serialization/triangle.h"
+#include "hpp/fcl/serialization/quadrilateral.h"
+
+namespace boost {
+namespace serialization {
+
+namespace internal {
+struct ConvexBaseAccessor : hpp::fcl::ConvexBase {
+  typedef hpp::fcl::ConvexBase Base;
+  using Base::own_storage_;
+};
+
+}  // namespace internal
+
+template <class Archive>
+void serialize(Archive &ar, hpp::fcl::ConvexBase &convex_base,
+               const unsigned int /*version*/) {
+  using namespace hpp::fcl;
+
+  typedef internal::ConvexBaseAccessor Accessor;
+  Accessor &accessor = reinterpret_cast<Accessor &>(convex_base);
+
+  ar &make_nvp("base", boost::serialization::base_object<hpp::fcl::ShapeBase>(
+                           convex_base));
+  const unsigned int num_points_previous = convex_base.num_points;
+  ar &make_nvp("num_points", convex_base.num_points);
+
+  if (Archive::is_loading::value) {
+    if (num_points_previous != convex_base.num_points ||
+        !accessor.own_storage_) {
+      delete[] convex_base.points;
+      convex_base.points = new hpp::fcl::Vec3f[convex_base.num_points];
+      accessor.own_storage_ = true;
+    }
+  }
+
+  {
+    typedef Eigen::Matrix<FCL_REAL, 3, Eigen::Dynamic> MatrixPoints;
+    Eigen::Map<MatrixPoints> points_map(
+        reinterpret_cast<double *>(convex_base.points), 3,
+        convex_base.num_points);
+    ar &make_nvp("points", points_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 : hpp::fcl::Convex<PolygonT> {
+  typedef hpp::fcl::Convex<PolygonT> Base;
+  using Base::fillNeighbors;
+};
+
+}  // namespace internal
+
+template <class Archive, typename PolygonT>
+void serialize(Archive &ar, hpp::fcl::Convex<PolygonT> &convex_,
+               const unsigned int /*version*/) {
+  using namespace hpp::fcl;
+  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) {
+      delete[] convex.polygons;
+      convex.polygons = new PolygonT[convex.num_polygons];
+    }
+  }
+
+  ar &make_array<PolygonT>(convex.polygons, convex.num_polygons);
+
+  if (Archive::is_loading::value) convex.fillNeighbors();
+}
+
+}  // namespace serialization
+}  // namespace boost
+
+namespace hpp {
+namespace fcl {
+
+// namespace internal {
+// template <typename BV>
+// struct memory_footprint_evaluator< ::hpp::fcl::BVHModel<BV> > {
+//   static size_t run(const ::hpp::fcl::BVHModel<BV> &bvh_model) {
+//     return static_cast<size_t>(bvh_model.memUsage(false));
+//   }
+// };
+// }  // namespace internal
+
+}  // namespace fcl
+}  // namespace hpp
+
+#endif  // ifndef HPP_FCL_SERIALIZATION_CONVEX_H

include/hpp/fcl/serialization/quadrilateral.h

@@ -0,0 +1,26 @@
+//
+// Copyright (c) 2022 INRIA
+//
+
+#ifndef HPP_FCL_SERIALIZATION_QUADRILATERAL_H
+#define HPP_FCL_SERIALIZATION_QUADRILATERAL_H
+
+#include "hpp/fcl/data_types.h"
+#include "hpp/fcl/serialization/fwd.h"
+
+namespace boost {
+namespace serialization {
+
+template <class Archive>
+void serialize(Archive &ar, hpp::fcl::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 HPP_FCL_SERIALIZATION_QUADRILATERAL_H

include/hpp/fcl/serialization/triangle.h

@@ -0,0 +1,25 @@
+//
+// Copyright (c) 2021-2022 INRIA
+//
+
+#ifndef HPP_FCL_SERIALIZATION_TRIANGLE_H
+#define HPP_FCL_SERIALIZATION_TRIANGLE_H
+
+#include "hpp/fcl/data_types.h"
+#include "hpp/fcl/serialization/fwd.h"
+
+namespace boost {
+namespace serialization {
+
+template <class Archive>
+void serialize(Archive &ar, hpp::fcl::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 HPP_FCL_SERIALIZATION_TRIANGLE_H

include/hpp/fcl/shape/details/convex.hxx

@@ -202,6 +202,7 @@ FCL_REAL Convex<PolygonT>::computeVolume() const {
 
 template <typename PolygonT>
 void Convex<PolygonT>::fillNeighbors() {
+  if (neighbors) delete[] neighbors;
   neighbors = new Neighbors[num_points];
 
   typedef typename PolygonT::size_type size_type;
@@ -230,7 +231,9 @@ void Convex<PolygonT>::fillNeighbors() {
     }
   }
 
+  if (nneighbors_) delete[] nneighbors_;
   nneighbors_ = new unsigned int[c_nneighbors];
+
   unsigned int* p_nneighbors = nneighbors_;
   for (unsigned int i = 0; i < num_points; ++i) {
     Neighbors& n = neighbors[i];

include/hpp/fcl/shape/geometric_shapes.h

@@ -70,6 +70,8 @@ class HPP_FCL_DLLAPI ShapeBase : public CollisionGeometry {
 /// @brief Triangle stores the points instead of only indices of points
 class HPP_FCL_DLLAPI TriangleP : public ShapeBase {
  public:
+  TriangleP(){};
+
   TriangleP(const Vec3f& a_, const Vec3f& b_, const Vec3f& c_)
       : ShapeBase(), a(a_), b(b_), c(c_) {}
 
@@ -193,7 +195,10 @@ class HPP_FCL_DLLAPI Box : public ShapeBase {
 /// @brief Center at zero point sphere
 class HPP_FCL_DLLAPI Sphere : public ShapeBase {
  public:
-  Sphere(FCL_REAL radius_) : ShapeBase(), radius(radius_) {}
+  /// @brief Default constructor
+  Sphere() {}
+
+  explicit Sphere(FCL_REAL radius_) : ShapeBase(), radius(radius_) {}
 
   Sphere(const Sphere& other) : ShapeBase(other), radius(other.radius) {}
 
@@ -252,10 +257,13 @@ class HPP_FCL_DLLAPI Sphere : public ShapeBase {
 /// @brief Ellipsoid centered at point zero
 class HPP_FCL_DLLAPI Ellipsoid : public ShapeBase {
  public:
+  /// @brief Default constructor
+  Ellipsoid() {}
+
   Ellipsoid(FCL_REAL rx, FCL_REAL ry, FCL_REAL rz)
       : ShapeBase(), radii(rx, ry, rz) {}
 
-  Ellipsoid(const Vec3f& radii) : radii(radii) {}
+  explicit Ellipsoid(const Vec3f& radii) : radii(radii) {}
 
   Ellipsoid(const Ellipsoid& other) : ShapeBase(other), radii(other.radii) {}
 
@@ -324,6 +332,9 @@ class HPP_FCL_DLLAPI Ellipsoid : public ShapeBase {
 /// segment AB, with \f$ A = (0,0,-halfLength), B = (0,0,halfLength) \f$.
 class HPP_FCL_DLLAPI Capsule : public ShapeBase {
  public:
+  /// @brief Default constructor
+  Capsule() {}
+
   Capsule(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_) {
     halfLength = lz_ / 2;
   }
@@ -402,6 +413,9 @@ class HPP_FCL_DLLAPI Capsule : public ShapeBase {
 /// \f$ z = halfLength \f$.
 class HPP_FCL_DLLAPI Cone : public ShapeBase {
  public:
+  /// @brief Default constructor
+  Cone() {}
+
   Cone(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_) {
     halfLength = lz_ / 2;
   }
@@ -486,6 +500,9 @@ class HPP_FCL_DLLAPI Cone : public ShapeBase {
 /// The cylinder is defined at its centroid.
 class HPP_FCL_DLLAPI Cylinder : public ShapeBase {
  public:
+  /// @brief Default constructor
+  Cylinder() {}
+
   Cylinder(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_) {
     halfLength = lz_ / 2;
   }

python/CMakeLists.txt

@@ -1,7 +1,7 @@
 #
 # Software License Agreement (BSD License)
 #
-#  Copyright (c) 2019-2021 CNRS-LAAS INRIA
+#  Copyright (c) 2019-2022 CNRS-LAAS INRIA
 #  Author: Joseph Mirabel
 #  All rights reserved.
 #
@@ -47,6 +47,7 @@ SET(${LIBRARY_NAME}_HEADERS
   broadphase/fwd.hh
   broadphase/broadphase_collision_manager.hh
   broadphase/broadphase_callbacks.hh
+  pickle.hh
   )
 
 SET(ENABLE_PYTHON_DOXYGEN_AUTODOC TRUE CACHE BOOL "Enable automatic documentation of Python bindings from Doxygen documentation")