frontlist.cpp 15.3 KB
Newer Older
jpan's avatar
 
jpan committed
1
2
3
/*
 * Software License Agreement (BSD License)
 *
4
5
 *  Copyright (c) 2011-2014, Willow Garage, Inc.
 *  Copyright (c) 2014-2015, Open Source Robotics Foundation
jpan's avatar
 
jpan committed
6
7
8
9
10
11
12
13
14
15
16
17
 *  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.
18
 *   * Neither the name of Open Source Robotics Foundation nor the names of its
jpan's avatar
 
jpan committed
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
 *     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.
 */

/** \author Jia Pan */

38

39
40
41
42
#define BOOST_TEST_MODULE FCL_FRONT_LIST
#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
43

44
45
#include <hpp/fcl/internal/traversal_node_bvhs.h>
#include <hpp/fcl/internal/traversal_node_setup.h>
46
#include <../src/collision_node.h>
Lucile Remigy's avatar
Lucile Remigy committed
47
#include <hpp/fcl/internal/BV_splitter.h>
48
#include "utility.h"
49
50
51

#include "fcl_resources/config.h"
#include <boost/filesystem.hpp>
jpan's avatar
 
jpan committed
52

53
using namespace hpp::fcl;
54
namespace utf = boost::unit_test::framework;
jpan's avatar
 
jpan committed
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75

template<typename BV>
bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2,
                             const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
                             const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2,
                             SplitMethodType split_method,
                             bool refit_bottomup, bool verbose);

template<typename BV, typename TraversalNode>
bool collide_front_list_Test_Oriented(const Transform3f& tf1, const Transform3f& tf2,
                                      const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
                                      const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2,
                                      SplitMethodType split_method, bool verbose);


template<typename BV>
bool collide_Test(const Transform3f& tf,
                  const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
                  const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method, bool verbose);

// TODO: randomly still have some runtime error
76
BOOST_AUTO_TEST_CASE(front_list)
jpan's avatar
 
jpan committed
77
78
79
{
  std::vector<Vec3f> p1, p2;
  std::vector<Triangle> t1, t2;
80
81
82
  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);
jpan's avatar
 
jpan committed
83
84
85
86
87

  std::vector<Transform3f> transforms; // t0
  std::vector<Transform3f> transforms2; // t1
  FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
  FCL_REAL delta_trans[] = {1, 1, 1};
88
89
#ifndef NDEBUG // if debug mode
  std::size_t n = 2;
90
#else
91
  std::size_t n = 20;
92
#endif
93
  n = getNbRun(utf::master_test_suite().argc, utf::master_test_suite().argv, n);
jpan's avatar
 
jpan committed
94
95
96
97
98
99
100
101
102
103
  bool verbose = false;

  generateRandomTransforms(extents, delta_trans, 0.005 * 2 * 3.1415, transforms, transforms2, n);

  bool res, res2;

  for(std::size_t i = 0; i < transforms.size(); ++i)
  {
    res = collide_Test<AABB>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
    res2 = collide_front_list_Test<AABB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose);
104
    BOOST_CHECK(res == res2);
jpan's avatar
 
jpan committed
105
106
    res = collide_Test<AABB>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
    res2 = collide_front_list_Test<AABB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose);
107
    BOOST_CHECK(res == res2);
jpan's avatar
 
jpan committed
108
109
    res = collide_Test<AABB>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
    res2 = collide_front_list_Test<AABB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose);
110
    BOOST_CHECK(res == res2);
jpan's avatar
 
jpan committed
111
112
113
114
115
116
  }

  for(std::size_t i = 0; i < transforms.size(); ++i)
  {
    res = collide_Test<OBB>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
    res2 = collide_front_list_Test<OBB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose);
117
    BOOST_CHECK(res == res2);
jpan's avatar
 
jpan committed
118
119
    res = collide_Test<OBB>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
    res2 = collide_front_list_Test<OBB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose);
120
    BOOST_CHECK(res == res2);
jpan's avatar
 
jpan committed
121
122
    res = collide_Test<OBB>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
    res2 = collide_front_list_Test<OBB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose);
123
    BOOST_CHECK(res == res2);
jpan's avatar
 
jpan committed
124
125
126
127
  }

  for(std::size_t i = 0; i < transforms.size(); ++i)
  {
128
129
130
131
    // Disabled broken test lines. Please see #25.
    // res = collide_Test<RSS>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
    // res2 = collide_front_list_Test<RSS>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose);
    // BOOST_CHECK(res == res2);
jpan's avatar
 
jpan committed
132
133
    res = collide_Test<RSS>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
    res2 = collide_front_list_Test<RSS>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose);
134
    BOOST_CHECK(res == res2);
jpan's avatar
 
jpan committed
135
136
    res = collide_Test<RSS>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
    res2 = collide_front_list_Test<RSS>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose);
137
    BOOST_CHECK(res == res2);
jpan's avatar
 
jpan committed
138
139
140
141
142
143
  }

  for(std::size_t i = 0; i < transforms.size(); ++i)
  {
    res = collide_Test<KDOP<16> >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
    res2 = collide_front_list_Test<KDOP<16> >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose);
144
    BOOST_CHECK(res == res2);
jpan's avatar
 
jpan committed
145
146
    res = collide_Test<KDOP<16> >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
    res2 = collide_front_list_Test<KDOP<16> >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose);
147
    BOOST_CHECK(res == res2);
jpan's avatar
 
jpan committed
148
149
    res = collide_Test<KDOP<16> >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
    res2 = collide_front_list_Test<KDOP<16> >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose);
150
    BOOST_CHECK(res == res2);
jpan's avatar
 
jpan committed
151
152
153
154
155
156
  }

  for(std::size_t i = 0; i < transforms.size(); ++i)
  {
    res = collide_Test<KDOP<18> >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
    res2 = collide_front_list_Test<KDOP<18> >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose);
157
    BOOST_CHECK(res == res2);
jpan's avatar
 
jpan committed
158
159
    res = collide_Test<KDOP<18> >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
    res2 = collide_front_list_Test<KDOP<18> >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose);
160
    BOOST_CHECK(res == res2);
jpan's avatar
 
jpan committed
161
162
    res = collide_Test<KDOP<18> >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
    res2 = collide_front_list_Test<KDOP<18> >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose);
163
    BOOST_CHECK(res == res2);
jpan's avatar
 
jpan committed
164
165
166
167
168
169
  }

  for(std::size_t i = 0; i < transforms.size(); ++i)
  {
    res = collide_Test<KDOP<24> >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
    res2 = collide_front_list_Test<KDOP<24> >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose);
170
    BOOST_CHECK(res == res2);
jpan's avatar
 
jpan committed
171
172
    res = collide_Test<KDOP<24> >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
    res2 = collide_front_list_Test<KDOP<24> >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose);
173
    BOOST_CHECK(res == res2);
jpan's avatar
 
jpan committed
174
175
    res = collide_Test<KDOP<24> >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
    res2 = collide_front_list_Test<KDOP<24> >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose);
176
    BOOST_CHECK(res == res2);
jpan's avatar
 
jpan committed
177
178
179
180
181
182
  }

  for(std::size_t i = 0; i < transforms.size(); ++i)
  {
    res = collide_Test<RSS>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
    res2 = collide_front_list_Test_Oriented<RSS, MeshCollisionTraversalNodeRSS>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
183
    BOOST_CHECK(res == res2);
jpan's avatar
 
jpan committed
184
185
    res = collide_Test<RSS>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
    res2 = collide_front_list_Test_Oriented<RSS, MeshCollisionTraversalNodeRSS>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
186
    BOOST_CHECK(res == res2);
jpan's avatar
 
jpan committed
187
188
    res = collide_Test<RSS>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
    res2 = collide_front_list_Test_Oriented<RSS, MeshCollisionTraversalNodeRSS>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
189
    BOOST_CHECK(res == res2);
jpan's avatar
 
jpan committed
190
191
192
193
194
195
  }

  for(std::size_t i = 0; i < transforms.size(); ++i)
  {
    res = collide_Test<OBB>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
    res2 = collide_front_list_Test_Oriented<OBB, MeshCollisionTraversalNodeOBB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
196
    BOOST_CHECK(res == res2);
jpan's avatar
 
jpan committed
197
198
    res = collide_Test<OBB>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
    res2 = collide_front_list_Test_Oriented<OBB, MeshCollisionTraversalNodeOBB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
199
    BOOST_CHECK(res == res2);
jpan's avatar
 
jpan committed
200
201
    res = collide_Test<OBB>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
    res2 = collide_front_list_Test_Oriented<OBB, MeshCollisionTraversalNodeOBB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
202
    BOOST_CHECK(res == res2);
jpan's avatar
 
jpan committed
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
  }

}

template<typename BV>
bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2,
                             const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
                             const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2,
                             SplitMethodType split_method,
                             bool refit_bottomup, bool verbose)
{
  BVHModel<BV> m1;
  BVHModel<BV> m2;
  m1.bv_splitter.reset(new BVSplitter<BV>(split_method));
  m2.bv_splitter.reset(new BVSplitter<BV>(split_method));

  BVHFrontList front_list;


  std::vector<Vec3f> vertices1_new(vertices1.size());
  for(std::size_t i = 0; i < vertices1_new.size(); ++i)
  {
    vertices1_new[i] = tf1.transform(vertices1[i]);
  }

  m1.beginModel();
  m1.addSubModel(vertices1_new, triangles1);
  m1.endModel();

  m2.beginModel();
  m2.addSubModel(vertices2, triangles2);
  m2.endModel();

  Transform3f pose1, pose2;

238
  CollisionResult local_result;
239
  CollisionRequest request (NO_REQUEST, std::numeric_limits<int>::max());
240
  MeshCollisionTraversalNode<BV> node (request);
jpan's avatar
 
jpan committed
241

242
  bool success = initialize <BV>(node, m1, pose1, m2, pose2, local_result);
243
  BOOST_REQUIRE (success);
jpan's avatar
 
jpan committed
244
245
246

  node.enable_statistics = verbose;

247
  collide(&node, request, local_result, &front_list);
jpan's avatar
 
jpan committed
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266

  if(verbose) std::cout << "front list size " << front_list.size() << std::endl;


  // update the mesh
  for(std::size_t i = 0; i < vertices1.size(); ++i)
  {
    vertices1_new[i] = tf2.transform(vertices1[i]);
  }

  m1.beginReplaceModel();
  m1.replaceSubModel(vertices1_new);
  m1.endReplaceModel(true, refit_bottomup);

  m2.beginReplaceModel();
  m2.replaceSubModel(vertices2);
  m2.endReplaceModel(true, refit_bottomup);

  local_result.clear();
267
  collide(&node, request, local_result, &front_list);
jpan's avatar
 
jpan committed
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301

  if(local_result.numContacts() > 0)
    return true;
  else
    return false;
}




template<typename BV, typename TraversalNode>
bool collide_front_list_Test_Oriented(const Transform3f& tf1, const Transform3f& tf2,
                                      const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
                                      const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2,
                                      SplitMethodType split_method, bool verbose)
{
  BVHModel<BV> m1;
  BVHModel<BV> m2;
  m1.bv_splitter.reset(new BVSplitter<BV>(split_method));
  m2.bv_splitter.reset(new BVSplitter<BV>(split_method));

  BVHFrontList front_list;

  m1.beginModel();
  m1.addSubModel(vertices1, triangles1);
  m1.endModel();

  m2.beginModel();
  m2.addSubModel(vertices2, triangles2);
  m2.endModel();

  Transform3f pose1(tf1), pose2;

  CollisionResult local_result;	
302
  CollisionRequest request (NO_REQUEST, std::numeric_limits<int>::max());
303
  TraversalNode node (request);
jpan's avatar
 
jpan committed
304

305
306
  bool success = initialize (node, (const BVHModel<BV>&)m1, pose1,
                             (const BVHModel<BV>&)m2, pose2, local_result);
307
  BOOST_REQUIRE (success);
jpan's avatar
 
jpan committed
308
309
310

  node.enable_statistics = verbose;

311
  collide(&node, request, local_result, &front_list);
jpan's avatar
 
jpan committed
312
313
314
315
316
317

  if(verbose) std::cout << "front list size " << front_list.size() << std::endl;


  // update the mesh
  pose1 = tf2;
318
319
  success = initialize (node, (const BVHModel<BV>&)m1, pose1,
                        (const BVHModel<BV>&)m2, pose2, local_result);
320
  BOOST_REQUIRE (success);
jpan's avatar
 
jpan committed
321
322

  local_result.clear();
323
  collide(&node, request, local_result, &front_list);
jpan's avatar
 
jpan committed
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351

  if(local_result.numContacts() > 0)
    return true;
  else
    return false;
}


template<typename BV>
bool collide_Test(const Transform3f& tf,
                  const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
                  const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method, bool verbose)
{
  BVHModel<BV> m1;
  BVHModel<BV> m2;
  m1.bv_splitter.reset(new BVSplitter<BV>(split_method));
  m2.bv_splitter.reset(new BVSplitter<BV>(split_method));

  m1.beginModel();
  m1.addSubModel(vertices1, triangles1);
  m1.endModel();

  m2.beginModel();
  m2.addSubModel(vertices2, triangles2);
  m2.endModel();

  Transform3f pose1(tf), pose2;

352
  CollisionResult local_result;
353
  CollisionRequest request (NO_REQUEST, std::numeric_limits<int>::max());
354
  MeshCollisionTraversalNode<BV> node (request);
jpan's avatar
 
jpan committed
355

356
  bool success = initialize <BV>(node, m1, pose1, m2, pose2, local_result);
357
  BOOST_REQUIRE (success);
jpan's avatar
 
jpan committed
358
359
360

  node.enable_statistics = verbose;

361
  collide(&node, request, local_result);
jpan's avatar
 
jpan committed
362
363
364
365
366
367
368

  if(local_result.numContacts() > 0)
    return true;
  else
    return false;
}