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

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