hpp-fcl: include/hpp/fcl/serialization/BVH_model.h Source File

hpp-fcl 1.7.2 HPP fork of FCL -- The Flexible Collision Library

BVH_model.h
Go to the documentation of this file.
1 //
2 // Copyright (c) 2021 INRIA
3 //
4
5 #ifndef HPP_FCL_SERIALIZATION_BVH_MODEL_H
6 #define HPP_FCL_SERIALIZATION_BVH_MODEL_H
7
8 #include "hpp/fcl/BVH/BVH_model.h"
9
10 #include "hpp/fcl/serialization/fwd.h"
11 #include "hpp/fcl/serialization/BV_node.h"
12 #include "hpp/fcl/serialization/BV_splitter.h"
13 #include "hpp/fcl/serialization/collision_object.h"
14
15 namespace boost
16 {
17 namespace serialization
18 {
19
20 namespace internal
21 {
22 struct BVHModelBaseAccessor : hpp::fcl::BVHModelBase
23 {
24 typedef hpp::fcl::BVHModelBase Base;
25 using Base::num_tris_allocated;
26 using Base::num_vertices_allocated;
27 };
28 }
29
30 template <class Archive>
31 void serialize(Archive & ar,
32 hpp::fcl::Triangle & triangle,
33 const unsigned int /*version*/)
34 {
35 ar & make_nvp("p0",triangle[0]);
36 ar & make_nvp("p1",triangle[1]);
37 ar & make_nvp("p2",triangle[2]);
38 }
39
40 template <class Archive>
41 void save(Archive & ar,
42 const hpp::fcl::BVHModelBase & bvh_model,
43 const unsigned int /*version*/)
44 {
45 using namespace hpp::fcl;
46 if(!(bvh_model.build_state == BVH_BUILD_STATE_PROCESSED || bvh_model.build_state == BVH_BUILD_STATE_UPDATED)
47 && (bvh_model.getModelType() == BVH_MODEL_TRIANGLES))
48 {
49 throw std::invalid_argument("The BVH model is not in a BVH_BUILD_STATE_PROCESSED or BVH_BUILD_STATE_UPDATED state.\n"
50 "The BVHModel could not be serialized.");
51 }
52
53 ar & make_nvp("base",boost::serialization::base_object<hpp::fcl::CollisionGeometry>(bvh_model));
54
55 ar & make_nvp("num_vertices",bvh_model.num_vertices);
56 if(bvh_model.num_vertices > 0)
57 {
58 typedef Eigen::Matrix<FCL_REAL,3,Eigen::Dynamic> AsVertixMatrix;
59 const Eigen::Map<const AsVertixMatrix> vertices_map(reinterpret_cast<const double *>(bvh_model.vertices),3,bvh_model.num_vertices);
60 ar & make_nvp("vertices",vertices_map);
61 }
62
63 ar & make_nvp("num_tris",bvh_model.num_tris);
64 if(bvh_model.num_tris > 0)
65 {
66 typedef Eigen::Matrix<Triangle::index_type,3,Eigen::Dynamic> AsTriangleMatrix;
67 const Eigen::Map<const AsTriangleMatrix> tri_indices_map(reinterpret_cast<const Triangle::index_type *>(bvh_model.tri_indices),3,bvh_model.num_tris);
68 ar & make_nvp("tri_indices",tri_indices_map);
69 }
70 ar & make_nvp("build_state",bvh_model.build_state);
71
72 if(bvh_model.prev_vertices)
73 {
74 const bool has_prev_vertices = true;
75 ar << make_nvp("has_prev_vertices",has_prev_vertices);
76 typedef Eigen::Matrix<FCL_REAL,3,Eigen::Dynamic> AsVertixMatrix;
77 const Eigen::Map<const AsVertixMatrix> prev_vertices_map(reinterpret_cast<const double *>(bvh_model.prev_vertices),3,bvh_model.num_vertices);
78 ar & make_nvp("prev_vertices",prev_vertices_map);
79 }
80 else
81 {
82 const bool has_prev_vertices = false;
83 ar & make_nvp("has_prev_vertices",has_prev_vertices);
84 }
85
86 // if(bvh_model.convex)
87 // {
88 // const bool has_convex = true;
89 // ar << make_nvp("has_convex",has_convex);
90 // }
91 // else
92 // {
93 // const bool has_convex = false;
94 // ar << make_nvp("has_convex",has_convex);
95 // }
96 }
97
98 template <class Archive>
99 void load(Archive & ar,
100 hpp::fcl::BVHModelBase & bvh_model,
101 const unsigned int /*version*/)
102 {
103 using namespace hpp::fcl;
104
105 ar >> make_nvp("base",boost::serialization::base_object<hpp::fcl::CollisionGeometry>(bvh_model));
106
107 int num_vertices;
108 ar >> make_nvp("num_vertices",num_vertices);
109 if(num_vertices != bvh_model.num_vertices)
110 {
111 delete[] bvh_model.vertices;
112 bvh_model.vertices = NULL;
113 bvh_model.num_vertices = num_vertices;
114 if(num_vertices > 0)
115 bvh_model.vertices = new Vec3f[num_vertices];
116 }
117 if(num_vertices > 0)
118 {
119 typedef Eigen::Matrix<FCL_REAL,3,Eigen::Dynamic> AsVertixMatrix;
120 Eigen::Map<AsVertixMatrix> vertices_map(reinterpret_cast<double *>(bvh_model.vertices),3,bvh_model.num_vertices);
121 ar >> make_nvp("vertices",vertices_map);
122 }
123 else
124 bvh_model.vertices = NULL;
125
126 int num_tris;
127 ar >> make_nvp("num_tris",num_tris);
128
129 if(num_tris != bvh_model.num_tris)
130 {
131 delete[] bvh_model.tri_indices;
132 bvh_model.tri_indices = NULL;
133 bvh_model.num_tris = num_tris;
134 if(num_tris> 0)
135 bvh_model.tri_indices = new Triangle[num_tris];
136 }
137 if(num_tris > 0)
138 {
139 typedef Eigen::Matrix<Triangle::index_type,3,Eigen::Dynamic> AsTriangleMatrix;
140 Eigen::Map<AsTriangleMatrix> tri_indices_map(reinterpret_cast<Triangle::index_type *>(bvh_model.tri_indices),3,bvh_model.num_tris);
141 ar & make_nvp("tri_indices",tri_indices_map);
142 }
143 else
144 bvh_model.tri_indices = NULL;
145
146 ar >> make_nvp("build_state",bvh_model.build_state);
147
148 typedef internal::BVHModelBaseAccessor Accessor;
149 reinterpret_cast<Accessor &>(bvh_model).num_tris_allocated = num_tris;
150 reinterpret_cast<Accessor &>(bvh_model).num_vertices_allocated = num_vertices;
151
152 bool has_prev_vertices;
153 ar >> make_nvp("has_prev_vertices",has_prev_vertices);
154 if(has_prev_vertices)
155 {
156 if(num_vertices != bvh_model.num_vertices)
157 {
158 delete[] bvh_model.prev_vertices;
159 bvh_model.prev_vertices = NULL;
160 if(num_vertices > 0)
161 bvh_model.prev_vertices = new Vec3f[num_vertices];
162 }
163 if(num_vertices > 0)
164 {
165 typedef Eigen::Matrix<FCL_REAL,3,Eigen::Dynamic> AsVertixMatrix;
166 Eigen::Map<AsVertixMatrix> prev_vertices_map(reinterpret_cast<double *>(bvh_model.prev_vertices),3,bvh_model.num_vertices);
167 ar & make_nvp("prev_vertices",prev_vertices_map);
168 }
169 }
170 else
171 bvh_model.prev_vertices = NULL;
172
173 // bool has_convex = true;
174 // ar >> make_nvp("has_convex",has_convex);
175 }
176
177 HPP_FCL_SERIALIZATION_SPLIT(hpp::fcl::BVHModelBase)
178
179 namespace internal
180 {
181 template<typename BV>
182 struct BVHModelAccessor : hpp::fcl::BVHModel<BV>
183 {
184 typedef hpp::fcl::BVHModel<BV> Base;
185 using Base::num_bvs_allocated;
186 using Base::primitive_indices;
187 using Base::bvs;
188 using Base::num_bvs;
189 };
190 }
191
192 template <class Archive, typename BV>
193 void serialize(Archive & ar,
194 hpp::fcl::BVHModel<BV> & bvh_model,
195 const unsigned int version)
196 {
197 split_free(ar,bvh_model,version);
198 }
199
200 template <class Archive, typename BV>
201 void save(Archive & ar,
202 const hpp::fcl::BVHModel<BV> & bvh_model_,
203 const unsigned int /*version*/)
204 {
205 using namespace hpp::fcl;
206 typedef internal::BVHModelAccessor<BV> Accessor;
207 typedef BVNode<BV> Node;
208
209 const Accessor & bvh_model = reinterpret_cast<const Accessor &>(bvh_model_);
210 ar & make_nvp("base",boost::serialization::base_object<BVHModelBase>(bvh_model));
211
212 // if(bvh_model.primitive_indices)
213 // {
214 // const bool with_primitive_indices = true;
215 // ar & make_nvp("with_primitive_indices",with_primitive_indices);
216 //
217 // int num_primitives = 0;
218 // switch(bvh_model.getModelType())
219 // {
220 // case BVH_MODEL_TRIANGLES:
221 // num_primitives = bvh_model.num_tris;
222 // break;
223 // case BVH_MODEL_POINTCLOUD:
224 // num_primitives = bvh_model.num_vertices;
225 // break;
226 // default:
227 // ;
228 // }
229 //
230 // ar & make_nvp("num_primitives",num_primitives);
231 // if(num_primitives > 0)
232 // {
233 // typedef Eigen::Matrix<unsigned int,1,Eigen::Dynamic> AsPrimitiveIndexVector;
234 // const Eigen::Map<const AsPrimitiveIndexVector> primitive_indices_map(reinterpret_cast<const unsigned int *>(bvh_model.primitive_indices),1,num_primitives);
235 // ar & make_nvp("primitive_indices",primitive_indices_map);
237 // }
238 // }
239 // else
240 // {
241 // const bool with_primitive_indices = false;
242 // ar & make_nvp("with_primitive_indices",with_primitive_indices);
243 // }
244 //
245
246 if(bvh_model.bvs)
247 {
248 const bool with_bvs = true;
249 ar & make_nvp("with_bvs",with_bvs);
250 ar & make_nvp("num_bvs",bvh_model.num_bvs);
251 ar & make_nvp("bvs",make_array(reinterpret_cast<const char *>(bvh_model.bvs),sizeof(Node)*(std::size_t)bvh_model.num_bvs)); // Assuming BVs are POD.
252 }
253 else
254 {
255 const bool with_bvs = false;
256 ar & make_nvp("with_bvs",with_bvs);
257 }
258 }
259
260 template <class Archive, typename BV>
261 void load(Archive & ar,
262 hpp::fcl::BVHModel<BV> & bvh_model_,
263 const unsigned int /*version*/)
264 {
265 using namespace hpp::fcl;
266 typedef internal::BVHModelAccessor<BV> Accessor;
267 typedef BVNode<BV> Node;
268
269 Accessor & bvh_model = reinterpret_cast<Accessor &>(bvh_model_);
270
271 ar >> make_nvp("base",boost::serialization::base_object<BVHModelBase>(bvh_model));
272
273 // bool with_primitive_indices;
274 // ar >> make_nvp("with_primitive_indices",with_primitive_indices);
275 // if(with_primitive_indices)
276 // {
277 // int num_primitives;
278 // ar >> make_nvp("num_primitives",num_primitives);
279 //
280 // delete[] bvh_model.primitive_indices;
281 // if(num_primitives > 0)
282 // {
283 // bvh_model.primitive_indices = new unsigned int[num_primitives];
284 // ar & make_nvp("primitive_indices",make_array(bvh_model.primitive_indices,num_primitives));
285 // }
286 // else
287 // bvh_model.primitive_indices = NULL;
288 // }
289
290 bool with_bvs;
291 ar >> make_nvp("with_bvs",with_bvs);
292 if(with_bvs)
293 {
294 int num_bvs;
295 ar >> make_nvp("num_bvs",num_bvs);
296
297 if(num_bvs != bvh_model.num_bvs)
298 {
299 delete[] bvh_model.bvs;
300 bvh_model.bvs = NULL;
301 bvh_model.num_bvs = num_bvs;
302 if(num_bvs > 0)
303 bvh_model.bvs = new BVNode<BV>[num_bvs];
304 }
305 if(num_bvs > 0)
306 {
307 ar >> make_nvp("bvs",make_array(reinterpret_cast<char *>(bvh_model.bvs),sizeof(Node)*(std::size_t)num_bvs));
308 }
309 else
310 bvh_model.bvs = NULL;
311 }
312 }
313
314 }
315 }
316
317 #endif // ifndef HPP_FCL_SERIALIZATION_BVH_MODEL_H
BVH_model.h
hpp::fcl::BVHModelBase
A base class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewe...
Definition: BVH_model.h:64
hpp::fcl::BVHModelBase::prev_vertices
Vec3f * prev_vertices
Geometry point data in previous frame.
Definition: BVH_model.h:74
hpp::fcl::BVHModelBase::tri_indices
Triangle * tri_indices
Geometry triangle index data, will be NULL for point clouds.
Definition: BVH_model.h:71
hpp::fcl::BVHModelBase::build_state
BVHBuildState build_state
The state of BVH building process.
Definition: BVH_model.h:83
hpp::fcl::BVHModelBase::num_vertices_allocated
int num_vertices_allocated
Definition: BVH_model.h:269
hpp::fcl::BVHModelBase::getModelType
BVHModelType getModelType() const
Model type described by the instance.
Definition: BVH_model.h:89
hpp::fcl::BVHModelBase::num_tris
int num_tris
Number of triangles.
Definition: BVH_model.h:77
hpp::fcl::BVHModelBase::num_tris_allocated
int num_tris_allocated
Definition: BVH_model.h:268
hpp::fcl::BVHModelBase::vertices
Vec3f * vertices
Geometry point data.
Definition: BVH_model.h:68
hpp::fcl::BVHModelBase::num_vertices
int num_vertices
Number of points.
Definition: BVH_model.h:80
hpp::fcl::BVHModel
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Definition: BVH_model.h:278
hpp::fcl::BVHModel::num_bvs
int num_bvs
Number of BV nodes in bounding volume hierarchy.
Definition: BVH_model.h:418
hpp::fcl::BVHModel::primitive_indices
unsigned int * primitive_indices
Definition: BVH_model.h:412
hpp::fcl::BVHModel::bvs
BVNode< BV > * bvs
Bounding volume hierarchy.
Definition: BVH_model.h:415
hpp::fcl::BVHModel::num_bvs_allocated
int num_bvs_allocated
Definition: BVH_model.h:411
hpp::fcl::Triangle
Triangle with 3 indices for points.
Definition: data_types.h:75
hpp::fcl::Triangle::index_type
std::size_t index_type
Definition: data_types.h:77
fwd.h
HPP_FCL_SERIALIZATION_SPLIT
#define HPP_FCL_SERIALIZATION_SPLIT(Type)
Definition: fwd.h:13
boost::serialization::save
void save(Archive &ar, const hpp::fcl::BVSplitter< BV > &splitter_, const unsigned int)
Definition: BV_splitter.h:34
boost::serialization::load
void load(Archive &ar, hpp::fcl::BVSplitter< BV > &splitter_, const unsigned int)
Definition: BV_splitter.h:50
boost::serialization::serialize
void serialize(Archive &ar, hpp::fcl::AABB &aabb, const unsigned int)
Definition: AABB.h:17
boost
Definition: AABB.h:12
hpp::fcl
Definition: AABB.h:46
hpp::fcl::BVH_BUILD_STATE_PROCESSED
@ BVH_BUILD_STATE_PROCESSED
after beginModel(), state for adding geometry primitives
Definition: BVH_internal.h:56
hpp::fcl::BVH_BUILD_STATE_UPDATED
@ BVH_BUILD_STATE_UPDATED
after beginUpdateModel(), state for updating geometry primitives
Definition: BVH_internal.h:58
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
hpp::fcl::BVH_MODEL_TRIANGLES
@ BVH_MODEL_TRIANGLES
unknown model type
Definition: BVH_internal.h:80
BV_node.h
BV_splitter.h
collision_object.h
boost::serialization::internal::BVHModelAccessor
Definition: BVH_model.h:183
boost::serialization::internal::BVHModelAccessor::Base
hpp::fcl::BVHModel< BV > Base
Definition: BVH_model.h:184
boost::serialization::internal::BVHModelBaseAccessor
Definition: BVH_model.h:23
boost::serialization::internal::BVHModelBaseAccessor::Base
hpp::fcl::BVHModelBase Base
Definition: BVH_model.h:24
hpp::fcl::BVNode
A class describing a bounding volume node. It includes the tree structure providing in BVNodeBase and...
Definition: BV_node.h:110