serialization.cpp 8.21 KB
Newer Older
1
2
3
/*
 *  Software License Agreement (BSD License)
 *
4
 *  Copyright (c) 2021 INRIA.
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */


#define BOOST_TEST_MODULE FCL_SERIALIZATION
#include <fstream>
#include <boost/test/included/unit_test.hpp>

#include <hpp/fcl/collision.h>
#include <hpp/fcl/distance.h>
42
43
#include <hpp/fcl/BV/OBBRSS.h>
#include <hpp/fcl/BVH/BVH_model.h>
44
45

#include <hpp/fcl/serialization/collision_data.h>
46
#include <hpp/fcl/serialization/AABB.h>
47
#include <hpp/fcl/serialization/BVH_model.h>
48
#include <hpp/fcl/serialization/geometric_shapes.h>
49
#include <hpp/fcl/serialization/memory.h>
50
51
52
53
54
55
56

#include "utility.h"
#include "fcl_resources/config.h"

#include <boost/archive/tmpdir.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>
57
58
59
60
#include <boost/archive/xml_iarchive.hpp>
#include <boost/archive/xml_oarchive.hpp>
#include <boost/archive/binary_iarchive.hpp>
#include <boost/archive/binary_oarchive.hpp>
61
#include <boost/filesystem.hpp>
62

63
64
#include <boost/asio/streambuf.hpp>

65
66
67
68
namespace utf = boost::unit_test::framework;

using namespace hpp::fcl;

69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
template<typename T>
void saveToBinary(const T & object,
                  boost::asio::streambuf & buffer)
{
  boost::archive::binary_oarchive oa(buffer);
  oa & object;
}

template<typename T>
inline void loadFromBinary(T & object,
                           boost::asio::streambuf & buffer)
{
  boost::archive::binary_iarchive ia(buffer);
  ia >> object;
}

85
86
87
88
89
90
template<typename T>
bool check(const T & value, const T & other)
{
  return value == other;
}

91
92
93
94
95
96
97
98
enum SerializationMode
{
  TXT = 1,
  XML = 2,
  BIN = 4,
  STREAM = 8
};

99
template<typename T>
100
101
void test_serialization(const T & value, T & other_value,
                        const int mode = TXT | XML | BIN | STREAM)
102
{
103
104
105
  const std::string tmp_dir(boost::archive::tmpdir());
  const std::string txt_filename = tmp_dir + "file.txt";
  const std::string bin_filename = tmp_dir + "file.bin";
106

107
  // TXT
108
  if(mode & 0x1)
109
  {
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
    {
      std::ofstream ofs(txt_filename.c_str());
      
      boost::archive::text_oarchive oa(ofs);
      oa << value;
    }
    BOOST_CHECK(check(value,value));
    
    {
      std::ifstream ifs(txt_filename.c_str());
      boost::archive::text_iarchive ia(ifs);
      
      ia >> other_value;
    }
    BOOST_CHECK(check(value,other_value));
125
  }
126
127
  
  // BIN
128
  if(mode & 0x4)
129
  {
130
131
132
133
134
135
136
137
138
139
140
141
142
143
    {
      std::ofstream ofs(bin_filename.c_str(), std::ios::binary);
      boost::archive::binary_oarchive oa(ofs);
      oa << value;
    }
    BOOST_CHECK(check(value,value));
    
    {
      std::ifstream ifs(bin_filename.c_str(), std::ios::binary);
      boost::archive::binary_iarchive ia(ifs);
      
      ia >> other_value;
    }
    BOOST_CHECK(check(value,other_value));
144
145
  }
  
146
147
  // Stream Buffer
  if(mode & 0x8)
148
  {
149
150
151
152
153
154
    boost::asio::streambuf buffer;
    saveToBinary(value,buffer);
    BOOST_CHECK(check(value,value));
    
    loadFromBinary(other_value,buffer);
    BOOST_CHECK(check(value,other_value));
155
  }
156
157
158
}

template<typename T>
159
160
void test_serialization(const T & value,
                        const int mode = TXT | XML | BIN | STREAM)
161
162
{
  T other_value;
163
  test_serialization(value,other_value,mode);
164
165
}

166
BOOST_AUTO_TEST_CASE(test_aabb)
167
168
169
170
171
{
  AABB aabb(-Vec3f::Ones(),Vec3f::Ones());
  test_serialization(aabb);
}

172
BOOST_AUTO_TEST_CASE(test_collision_data)
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
{
  Contact contact(NULL, NULL, 1, 2, Vec3f::Ones(), Vec3f::Zero(), -10.);
  test_serialization(contact);
  
  CollisionRequest collision_request(CONTACT,10);
  test_serialization(collision_request);
  
  CollisionResult collision_result;
  collision_result.addContact(contact);
  collision_result.addContact(contact);
  collision_result.distance_lower_bound = 0.1;
  test_serialization(collision_result);
  
  DistanceRequest distance_request(true,1.,2.);
  test_serialization(distance_request);
  
  DistanceResult distance_result;
  distance_result.normal.setOnes();
  distance_result.nearest_points[0].setRandom();
  distance_result.nearest_points[1].setRandom();
  test_serialization(distance_result);
}
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224

BOOST_AUTO_TEST_CASE(test_BVHModel)
{
  std::vector<Vec3f> p1, p2;
  std::vector<Triangle> t1, t2;
  boost::filesystem::path path(TEST_RESOURCES_DIR);
  
  loadOBJFile((path / "env.obj").string().c_str(), p1, t1);
  loadOBJFile((path / "rob.obj").string().c_str(), p2, t2);

  BVHModel<OBBRSS> m1,m2;

  m1.beginModel();
  m1.addSubModel(p1, t1);
  m1.endModel();
  BOOST_CHECK(m1 == m1);

  m2.beginModel();
  m2.addSubModel(p2, t2);
  m2.endModel();
  BOOST_CHECK(m2 == m2);
  BOOST_CHECK(m1 != m2);
  
  // Test CollisionGeometry
  {
    CollisionGeometry & m1_cg = static_cast<CollisionGeometry &>(m1);
    BVHModel<OBBRSS> m1_copy;
    CollisionGeometry & m1_copy_cg = static_cast<CollisionGeometry &>(m1);
    test_serialization(m1_cg,m1_copy_cg);
  }
225

226
227
228
229
230
231
232
  // Test BVHModelBase
  {
    BVHModelBase & m1_base = static_cast<BVHModelBase &>(m1);
    BVHModel<OBBRSS> m1_copy;
    BVHModelBase & m1_copy_base = static_cast<BVHModelBase &>(m1);
    test_serialization(m1_base,m1_copy_base);
  }
233

234
235
236
237
238
  // Test BVHModel
  {
    BVHModel<OBBRSS> m1_copy;
    test_serialization(m1,m1_copy);
  }
239
240
241
242
  {
    BVHModel<OBBRSS> m1_copy;
    test_serialization(m1,m1_copy,STREAM);
  }
243
}
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290

BOOST_AUTO_TEST_CASE(test_shapes)
{
  
  {
    TriangleP triangle(Vec3f::UnitX(),
                       Vec3f::UnitY(),
                       Vec3f::UnitZ());
    TriangleP triangle_copy(Vec3f::Random(),Vec3f::Random(),Vec3f::Random());
    test_serialization(triangle,triangle_copy);
  }
  
  {
    Box box(Vec3f::UnitX()), box_copy(Vec3f::Random());
    test_serialization(box,box_copy);
  }
  
  {
    Sphere sphere(1.), sphere_copy(2.);
    test_serialization(sphere,sphere_copy);
  }
  
  {
    Capsule capsule(1.,2.), capsule_copy(10.,10.);
    test_serialization(capsule,capsule_copy);
  }
  
  {
    Cone cone(1.,2.), cone_copy(10.,10.);
    test_serialization(cone,cone_copy);
  }
  
  {
    Cylinder cylinder(1.,2.), cylinder_copy(10.,10.);
    test_serialization(cylinder,cylinder_copy);
  }
  
  {
    Halfspace hs(Vec3f::Random(),1.), hs_copy(Vec3f::Zero(),0.);
    test_serialization(hs,hs_copy);
  }
  
  {
    Plane plane(Vec3f::Random(),1.), plane_copy(Vec3f::Zero(),0.);
    test_serialization(plane,plane_copy);
  }
}
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312

BOOST_AUTO_TEST_CASE(test_memory_footprint)
{
  Sphere sphere(1.);
  BOOST_CHECK(sizeof(Sphere) == computeMemoryFootprint(sphere));
  
  std::vector<Vec3f> p1;
  std::vector<Triangle> t1;
  boost::filesystem::path path(TEST_RESOURCES_DIR);
  
  loadOBJFile((path / "env.obj").string().c_str(), p1, t1);

  BVHModel<OBBRSS> m1;

  m1.beginModel();
  m1.addSubModel(p1, t1);
  m1.endModel();
  
  std::cout << "computeMemoryFootprint(m1): " << computeMemoryFootprint(m1) << std::endl;
  BOOST_CHECK(sizeof(BVHModel<OBBRSS>) < computeMemoryFootprint(m1));
  BOOST_CHECK(m1.memUsage(false) == computeMemoryFootprint(m1));
}