GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/hpp/fcl/serialization/BVH_model.h Lines: 88 110 80.0 %
Date: 2024-02-09 12:57:42 Branches: 84 210 40.0 %

Line Branch Exec Source
1
//
2
// Copyright (c) 2021-2022 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
#include "hpp/fcl/serialization/triangle.h"
16
17
namespace boost {
18
namespace serialization {
19
20
namespace internal {
21
struct BVHModelBaseAccessor : hpp::fcl::BVHModelBase {
22
  typedef hpp::fcl::BVHModelBase Base;
23
  using Base::num_tris_allocated;
24
  using Base::num_vertices_allocated;
25
};
26
}  // namespace internal
27
28
template <class Archive>
29
8
void save(Archive &ar, const hpp::fcl::BVHModelBase &bvh_model,
30
          const unsigned int /*version*/) {
31
  using namespace hpp::fcl;
32
16
  if (!(bvh_model.build_state == BVH_BUILD_STATE_PROCESSED ||
33


8
        bvh_model.build_state == BVH_BUILD_STATE_UPDATED) &&
34
      (bvh_model.getModelType() == BVH_MODEL_TRIANGLES)) {
35
    throw std::invalid_argument(
36
        "The BVH model is not in a BVH_BUILD_STATE_PROCESSED or "
37
        "BVH_BUILD_STATE_UPDATED state.\n"
38
        "The BVHModel could not be serialized.");
39
  }
40
41
8
  ar &make_nvp("base",
42
               boost::serialization::base_object<hpp::fcl::CollisionGeometry>(
43
8
                   bvh_model));
44
45
8
  ar &make_nvp("num_vertices", bvh_model.num_vertices);
46
8
  if (bvh_model.num_vertices > 0) {
47
    typedef Eigen::Matrix<FCL_REAL, 3, Eigen::Dynamic> AsVertixMatrix;
48
8
    const Eigen::Map<const AsVertixMatrix> vertices_map(
49
8
        reinterpret_cast<const double *>(bvh_model.vertices), 3,
50
8
        bvh_model.num_vertices);
51

8
    ar &make_nvp("vertices", vertices_map);
52
  }
53
54
8
  ar &make_nvp("num_tris", bvh_model.num_tris);
55
8
  if (bvh_model.num_tris > 0) {
56
    typedef Eigen::Matrix<Triangle::index_type, 3, Eigen::Dynamic>
57
        AsTriangleMatrix;
58
8
    const Eigen::Map<const AsTriangleMatrix> tri_indices_map(
59
8
        reinterpret_cast<const Triangle::index_type *>(bvh_model.tri_indices),
60
8
        3, bvh_model.num_tris);
61

8
    ar &make_nvp("tri_indices", tri_indices_map);
62
  }
63
8
  ar &make_nvp("build_state", bvh_model.build_state);
64
65
8
  if (bvh_model.prev_vertices) {
66
    const bool has_prev_vertices = true;
67
    ar << make_nvp("has_prev_vertices", has_prev_vertices);
68
    typedef Eigen::Matrix<FCL_REAL, 3, Eigen::Dynamic> AsVertixMatrix;
69
    const Eigen::Map<const AsVertixMatrix> prev_vertices_map(
70
        reinterpret_cast<const double *>(bvh_model.prev_vertices), 3,
71
        bvh_model.num_vertices);
72
    ar &make_nvp("prev_vertices", prev_vertices_map);
73
  } else {
74
8
    const bool has_prev_vertices = false;
75

8
    ar &make_nvp("has_prev_vertices", has_prev_vertices);
76
  }
77
78
  //      if(bvh_model.convex)
79
  //      {
80
  //        const bool has_convex = true;
81
  //        ar << make_nvp("has_convex",has_convex);
82
  //      }
83
  //      else
84
  //      {
85
  //        const bool has_convex = false;
86
  //        ar << make_nvp("has_convex",has_convex);
87
  //      }
88
8
}
89
90
template <class Archive>
91
8
void load(Archive &ar, hpp::fcl::BVHModelBase &bvh_model,
92
          const unsigned int /*version*/) {
93
  using namespace hpp::fcl;
94
95
8
  ar >> make_nvp("base",
96
                 boost::serialization::base_object<hpp::fcl::CollisionGeometry>(
97

8
                     bvh_model));
98
99
  unsigned int num_vertices;
100

8
  ar >> make_nvp("num_vertices", num_vertices);
101
8
  if (num_vertices != bvh_model.num_vertices) {
102
4
    delete[] bvh_model.vertices;
103
4
    bvh_model.vertices = NULL;
104
4
    bvh_model.num_vertices = num_vertices;
105


26164
    if (num_vertices > 0) bvh_model.vertices = new Vec3f[num_vertices];
106
  }
107
8
  if (num_vertices > 0) {
108
    typedef Eigen::Matrix<FCL_REAL, 3, Eigen::Dynamic> AsVertixMatrix;
109
8
    Eigen::Map<AsVertixMatrix> vertices_map(
110
8
        reinterpret_cast<double *>(bvh_model.vertices), 3,
111
8
        bvh_model.num_vertices);
112

8
    ar >> make_nvp("vertices", vertices_map);
113
  } else
114
    bvh_model.vertices = NULL;
115
116
  unsigned int num_tris;
117

8
  ar >> make_nvp("num_tris", num_tris);
118
119
8
  if (num_tris != bvh_model.num_tris) {
120
4
    delete[] bvh_model.tri_indices;
121
4
    bvh_model.tri_indices = NULL;
122
4
    bvh_model.num_tris = num_tris;
123


8724
    if (num_tris > 0) bvh_model.tri_indices = new Triangle[num_tris];
124
  }
125
8
  if (num_tris > 0) {
126
    typedef Eigen::Matrix<Triangle::index_type, 3, Eigen::Dynamic>
127
        AsTriangleMatrix;
128
8
    Eigen::Map<AsTriangleMatrix> tri_indices_map(
129
8
        reinterpret_cast<Triangle::index_type *>(bvh_model.tri_indices), 3,
130
8
        bvh_model.num_tris);
131

8
    ar &make_nvp("tri_indices", tri_indices_map);
132
  } else
133
    bvh_model.tri_indices = NULL;
134
135

8
  ar >> make_nvp("build_state", bvh_model.build_state);
136
137
  typedef internal::BVHModelBaseAccessor Accessor;
138
8
  reinterpret_cast<Accessor &>(bvh_model).num_tris_allocated = num_tris;
139
8
  reinterpret_cast<Accessor &>(bvh_model).num_vertices_allocated = num_vertices;
140
141
  bool has_prev_vertices;
142

8
  ar >> make_nvp("has_prev_vertices", has_prev_vertices);
143
8
  if (has_prev_vertices) {
144
    if (num_vertices != bvh_model.num_vertices) {
145
      delete[] bvh_model.prev_vertices;
146
      bvh_model.prev_vertices = NULL;
147
      if (num_vertices > 0) bvh_model.prev_vertices = new Vec3f[num_vertices];
148
    }
149
    if (num_vertices > 0) {
150
      typedef Eigen::Matrix<FCL_REAL, 3, Eigen::Dynamic> AsVertixMatrix;
151
      Eigen::Map<AsVertixMatrix> prev_vertices_map(
152
          reinterpret_cast<double *>(bvh_model.prev_vertices), 3,
153
          bvh_model.num_vertices);
154
      ar &make_nvp("prev_vertices", prev_vertices_map);
155
    }
156
  } else
157
8
    bvh_model.prev_vertices = NULL;
158
159
  //      bool has_convex = true;
160
  //      ar >> make_nvp("has_convex",has_convex);
161
8
}
162
163
16
HPP_FCL_SERIALIZATION_SPLIT(hpp::fcl::BVHModelBase)
164
165
namespace internal {
166
template <typename BV>
167
struct BVHModelAccessor : hpp::fcl::BVHModel<BV> {
168
  typedef hpp::fcl::BVHModel<BV> Base;
169
  using Base::bvs;
170
  using Base::num_bvs;
171
  using Base::num_bvs_allocated;
172
  using Base::primitive_indices;
173
};
174
}  // namespace internal
175
176
template <class Archive, typename BV>
177
16
void serialize(Archive &ar, hpp::fcl::BVHModel<BV> &bvh_model,
178
               const unsigned int version) {
179
16
  split_free(ar, bvh_model, version);
180
16
}
181
182
template <class Archive, typename BV>
183
8
void save(Archive &ar, const hpp::fcl::BVHModel<BV> &bvh_model_,
184
          const unsigned int /*version*/) {
185
  using namespace hpp::fcl;
186
  typedef internal::BVHModelAccessor<BV> Accessor;
187
  typedef BVNode<BV> Node;
188
189
8
  const Accessor &bvh_model = reinterpret_cast<const Accessor &>(bvh_model_);
190
8
  ar &make_nvp("base",
191
8
               boost::serialization::base_object<BVHModelBase>(bvh_model));
192
193
  //      if(bvh_model.primitive_indices)
194
  //      {
195
  //        const bool with_primitive_indices = true;
196
  //        ar & make_nvp("with_primitive_indices",with_primitive_indices);
197
  //
198
  //        int num_primitives = 0;
199
  //        switch(bvh_model.getModelType())
200
  //        {
201
  //          case BVH_MODEL_TRIANGLES:
202
  //            num_primitives = bvh_model.num_tris;
203
  //            break;
204
  //          case BVH_MODEL_POINTCLOUD:
205
  //            num_primitives = bvh_model.num_vertices;
206
  //            break;
207
  //          default:
208
  //            ;
209
  //        }
210
  //
211
  //        ar & make_nvp("num_primitives",num_primitives);
212
  //        if(num_primitives > 0)
213
  //        {
214
  //          typedef Eigen::Matrix<unsigned int,1,Eigen::Dynamic>
215
  //          AsPrimitiveIndexVector; const Eigen::Map<const
216
  //          AsPrimitiveIndexVector>
217
  //          primitive_indices_map(reinterpret_cast<const unsigned int
218
  //          *>(bvh_model.primitive_indices),1,num_primitives); ar &
219
  //          make_nvp("primitive_indices",primitive_indices_map);
220
  ////          ar &
221
  /// make_nvp("primitive_indices",make_array(bvh_model.primitive_indices,num_primitives));
222
  //        }
223
  //      }
224
  //      else
225
  //      {
226
  //        const bool with_primitive_indices = false;
227
  //        ar & make_nvp("with_primitive_indices",with_primitive_indices);
228
  //      }
229
  //
230
231
8
  if (bvh_model.bvs) {
232
8
    const bool with_bvs = true;
233

8
    ar &make_nvp("with_bvs", with_bvs);
234

8
    ar &make_nvp("num_bvs", bvh_model.num_bvs);
235

8
    ar &make_nvp(
236
        "bvs",
237
        make_array(
238
8
            reinterpret_cast<const char *>(bvh_model.bvs),
239
            sizeof(Node) *
240
8
                (std::size_t)bvh_model.num_bvs));  // Assuming BVs are POD.
241
  } else {
242
    const bool with_bvs = false;
243
    ar &make_nvp("with_bvs", with_bvs);
244
  }
245
8
}
246
247
template <class Archive, typename BV>
248
8
void load(Archive &ar, hpp::fcl::BVHModel<BV> &bvh_model_,
249
          const unsigned int /*version*/) {
250
  using namespace hpp::fcl;
251
  typedef internal::BVHModelAccessor<BV> Accessor;
252
  typedef BVNode<BV> Node;
253
254
8
  Accessor &bvh_model = reinterpret_cast<Accessor &>(bvh_model_);
255
256
8
  ar >> make_nvp("base",
257

8
                 boost::serialization::base_object<BVHModelBase>(bvh_model));
258
259
  //      bool with_primitive_indices;
260
  //      ar >> make_nvp("with_primitive_indices",with_primitive_indices);
261
  //      if(with_primitive_indices)
262
  //      {
263
  //        int num_primitives;
264
  //        ar >> make_nvp("num_primitives",num_primitives);
265
  //
266
  //        delete[] bvh_model.primitive_indices;
267
  //        if(num_primitives > 0)
268
  //        {
269
  //          bvh_model.primitive_indices = new unsigned int[num_primitives];
270
  //          ar &
271
  //          make_nvp("primitive_indices",make_array(bvh_model.primitive_indices,num_primitives));
272
  //        }
273
  //        else
274
  //          bvh_model.primitive_indices = NULL;
275
  //      }
276
277
  bool with_bvs;
278

8
  ar >> make_nvp("with_bvs", with_bvs);
279
8
  if (with_bvs) {
280
    unsigned int num_bvs;
281

8
    ar >> make_nvp("num_bvs", num_bvs);
282
283
8
    if (num_bvs != bvh_model.num_bvs) {
284
4
      delete[] bvh_model.bvs;
285
4
      bvh_model.bvs = NULL;
286
4
      bvh_model.num_bvs = num_bvs;
287


17440
      if (num_bvs > 0) bvh_model.bvs = new BVNode<BV>[num_bvs];
288
    }
289
8
    if (num_bvs > 0) {
290

8
      ar >> make_nvp("bvs", make_array(reinterpret_cast<char *>(bvh_model.bvs),
291
8
                                       sizeof(Node) * (std::size_t)num_bvs));
292
    } else
293
      bvh_model.bvs = NULL;
294
  }
295
8
}
296
297
}  // namespace serialization
298
}  // namespace boost
299
300
namespace hpp {
301
namespace fcl {
302
303
namespace internal {
304
template <typename BV>
305
struct memory_footprint_evaluator< ::hpp::fcl::BVHModel<BV> > {
306
3
  static size_t run(const ::hpp::fcl::BVHModel<BV> &bvh_model) {
307
3
    return static_cast<size_t>(bvh_model.memUsage(false));
308
  }
309
};
310
}  // namespace internal
311
312
}  // namespace fcl
313
}  // namespace hpp
314
315
#endif  // ifndef HPP_FCL_SERIALIZATION_BVH_MODEL_H