serialization: enhance support

CMakeLists.txt

@@ -176,10 +176,16 @@ SET(${PROJECT_NAME}_HEADERS
   include/hpp/fcl/internal/traversal.h
   include/hpp/fcl/serialization/fwd.h
   include/hpp/fcl/serialization/AABB.h
-  include/hpp/fcl/serialization/eigen.h
+  include/hpp/fcl/serialization/BV_node.h
+  include/hpp/fcl/serialization/BV_fitter.h
+  include/hpp/fcl/serialization/BV_splitter.h
+  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/BVH_model.h
+  include/hpp/fcl/serialization/eigen.h
+  include/hpp/fcl/serialization/OBB.h
+  include/hpp/fcl/serialization/RSS.h
+  include/hpp/fcl/serialization/OBBRSS.h
   )
 
 add_subdirectory(doc)

include/hpp/fcl/serialization/BVH_model.h

@@ -8,6 +8,9 @@
 #include "hpp/fcl/BVH/BVH_model.h"
 
 #include "hpp/fcl/serialization/fwd.h"
+#include "hpp/fcl/serialization/BV_node.h"
+#include "hpp/fcl/serialization/BV_splitter.h"
+#include "hpp/fcl/serialization/BV_fitter.h"
 #include "hpp/fcl/serialization/collision_object.h"
 
 namespace boost
@@ -15,6 +18,16 @@ namespace boost
   namespace serialization
   {
   
+    namespace internal
+    {
+      struct BVHModelBaseAccessor : hpp::fcl::BVHModelBase
+      {
+        typedef hpp::fcl::BVHModelBase Base;
+        using Base::num_tris_allocated;
+        using Base::num_vertices_allocated;
+      };
+    }
+  
     template <class Archive>
     void serialize(Archive & ar,
                    hpp::fcl::Triangle & triangle,
@@ -78,26 +91,39 @@ namespace boost
       using namespace hpp::fcl;
       
       ar >> make_nvp("base",boost::serialization::base_object<hpp::fcl::CollisionGeometry>(bvh_model));
+      
       ar >> make_nvp("num_tris",bvh_model.num_tris);
+      delete[] bvh_model.tri_indices;
       if(bvh_model.num_tris > 0)
         bvh_model.tri_indices = new Triangle[bvh_model.num_tris];
+      else
+        bvh_model.tri_indices = NULL;
+      
       ar >> make_nvp("num_vertices",bvh_model.num_vertices);
+      delete[] bvh_model.vertices;
       if(bvh_model.num_vertices > 0)
         bvh_model.vertices = new Vec3f[bvh_model.num_vertices];
+      else
+        bvh_model.vertices = NULL;
+      
       ar >> make_nvp("tri_indices",make_array(bvh_model.tri_indices,bvh_model.num_tris));
       ar >> make_nvp("vertices",make_array(bvh_model.vertices,bvh_model.num_vertices));
       ar >> make_nvp("build_state",bvh_model.build_state);
       
-      ar & make_nvp("num_tris_allocated",bvh_model.num_tris);
-      ar & make_nvp("num_vertices_allocated",bvh_model.num_vertices);
+      typedef internal::BVHModelBaseAccessor Accessor;
+      ar >> make_nvp("num_tris_allocated",reinterpret_cast<Accessor &>(bvh_model).num_tris_allocated);
+      ar >> make_nvp("num_vertices_allocated",reinterpret_cast<Accessor&>(bvh_model).num_vertices_allocated);
       
       bool has_prev_vertices;
       ar >> make_nvp("has_prev_vertices",has_prev_vertices);
+      delete[] bvh_model.prev_vertices;
       if(has_prev_vertices)
       {
         bvh_model.prev_vertices = new Vec3f[bvh_model.num_vertices];
         ar >> make_nvp("prev_vertices",make_array(bvh_model.prev_vertices,bvh_model.num_vertices));
       }
+      else
+        bvh_model.prev_vertices = NULL;
       
       bool has_convex = true;
       ar >> make_nvp("has_convex",has_convex);
@@ -105,6 +131,128 @@ namespace boost
     
     HPP_FCL_SERIALIZATION_SPLIT(hpp::fcl::BVHModelBase)
   
+    namespace internal
+    {
+      template<typename BV>
+      struct BVHModelAccessor : hpp::fcl::BVHModel<BV>
+      {
+        typedef hpp::fcl::BVHModel<BV> Base;
+        using Base::num_bvs_allocated;
+        using Base::primitive_indices;
+        using Base::bvs;
+        using Base::num_bvs;
+      };
+    }
+  
+    template <class Archive, typename BV>
+    void serialize(Archive & ar,
+                   hpp::fcl::BVHModel<BV> & bvh_model,
+                   const unsigned int version)
+    {
+      split_free(ar,bvh_model,version);
+    }
+  
+    template <class Archive, typename BV>
+    void save(Archive & ar,
+              const hpp::fcl::BVHModel<BV> & bvh_model_,
+              const unsigned int /*version*/)
+    {
+      using namespace hpp::fcl;
+      typedef internal::BVHModelAccessor<BV> Accessor;
+      
+      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)
+        {
+          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)
+      {
+        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(bvh_model.bvs,bvh_model.num_bvs));
+      }
+      else
+      {
+        const bool with_bvs = false;
+        ar & make_nvp("with_bvs",with_bvs);
+      }
+      
+    }
+  
+    template <class Archive, typename BV>
+    void load(Archive & ar,
+              hpp::fcl::BVHModel<BV> & bvh_model_,
+              const unsigned int /*version*/)
+    {
+      using namespace hpp::fcl;
+      typedef internal::BVHModelAccessor<BV> Accessor;
+      
+      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)
+      {
+        ar >> make_nvp("num_bvs",bvh_model.num_bvs);
+        delete[] bvh_model.bvs;
+        if(bvh_model.num_bvs > 0)
+        {
+          bvh_model.bvs = new BVNode<BV>[bvh_model.num_bvs];
+          ar >> make_nvp("bvs",make_array(bvh_model.bvs,bvh_model.num_bvs));
+        }
+        else
+          bvh_model.bvs = NULL;
+      }
+    }
+  
   }
 }
 

include/hpp/fcl/serialization/BV_fitter.h

+//
+// Copyright (c) 2021 INRIA
+//
+
+#ifndef HPP_FCL_SERIALIZATION_BV_FITTER_H
+#define HPP_FCL_SERIALIZATION_BV_FITTER_H
+
+#include "hpp/fcl/internal/BV_fitter.h"
+
+#include "hpp/fcl/serialization/fwd.h"
+
+namespace boost
+{
+  namespace serialization
+  {
+  
+  }
+}
+
+#endif // ifndef HPP_FCL_SERIALIZATION_BV_FITTER_H

include/hpp/fcl/serialization/BV_node.h

+//
+// Copyright (c) 2021 INRIA
+//
+
+#ifndef HPP_FCL_SERIALIZATION_BV_NODE_H
+#define HPP_FCL_SERIALIZATION_BV_NODE_H
+
+#include "hpp/fcl/BV/BV_node.h"
+
+#include "hpp/fcl/serialization/fwd.h"
+#include "hpp/fcl/serialization/OBBRSS.h"
+
+namespace boost
+{
+  namespace serialization
+  {
+  
+    template <class Archive>
+    void serialize(Archive & ar,
+                   hpp::fcl::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,
+                   hpp::fcl::BVNode<BV> & node,
+                   const unsigned int /*version*/)
+    {
+      ar & make_nvp("base",boost::serialization::base_object<hpp::fcl::BVNodeBase>(node));
+      ar & make_nvp("bv",node.bv);
+    }
+
+  }
+}
+
+#endif // ifndef HPP_FCL_SERIALIZATION_BV_NODE_H

include/hpp/fcl/serialization/BV_splitter.h

+//
+// Copyright (c) 2021 INRIA
+//
+
+#ifndef HPP_FCL_SERIALIZATION_BV_SPLITTER_H
+#define HPP_FCL_SERIALIZATION_BV_SPLITTER_H
+
+#include "hpp/fcl/internal/BV_splitter.h"
+
+#include "hpp/fcl/serialization/fwd.h"
+
+namespace boost
+{
+  namespace serialization
+  {
+  
+    namespace internal
+    {
+      template <typename BV>
+      struct BVSplitterAccessor : hpp::fcl::BVSplitter<BV>
+      {
+        typedef hpp::fcl::BVSplitter<BV> Base;
+        using Base::split_axis;
+        using Base::split_vector;
+        using Base::split_value;
+        using Base::vertices;
+        using Base::tri_indices;
+        using Base::type;
+        using Base::split_method;
+      };
+    }
+  
+    template <class Archive, typename BV>
+    void save(Archive & ar,
+              const hpp::fcl::BVSplitter<BV> & splitter_,
+              const unsigned int /*version*/)
+    {
+      using namespace hpp::fcl;
+      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,
+              hpp::fcl::BVSplitter<BV> & splitter_,
+              const unsigned int /*version*/)
+    {
+      using namespace hpp::fcl;
+      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;
+    }
+  }
+}
+
+#endif // ifndef HPP_FCL_SERIALIZATION_BV_SPLITTER_H

include/hpp/fcl/serialization/OBB.h

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

include/hpp/fcl/serialization/OBBRSS.h

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

include/hpp/fcl/serialization/RSS.h

+//
+// Copyright (c) 2021 INRIA
+//
+
+#ifndef HPP_FCL_SERIALIZATION_RSS_H
+#define HPP_FCL_SERIALIZATION_RSS_H
+
+#include "hpp/fcl/BV/RSS.h"
+
+#include "hpp/fcl/serialization/fwd.h"
+
+namespace boost
+{
+  namespace serialization
+  {
+  
+    template <class Archive>
+    void serialize(Archive & ar,
+                   hpp::fcl::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);
+    }
+
+  }
+}
+
+#endif // ifndef HPP_FCL_SERIALIZATION_RSS_H

include/hpp/fcl/serialization/fwd.h

@@ -6,6 +6,7 @@
 #define HPP_FCL_SERIALIZATION_FWD_H
 
 #include <boost/serialization/split_free.hpp>
+#include <boost/serialization/shared_ptr.hpp>
 
 #include "hpp/fcl/serialization/eigen.h"