shape_mesh_consistency.cpp 74.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
#define BOOST_TEST_MODULE FCL_SHAPE_MESH_CONSISTENCY
39
#include <boost/test/included/unit_test.hpp>
40

41
42
43
44
#include <hpp/fcl/narrowphase/narrowphase.h>
#include <hpp/fcl/shape/geometric_shape_to_BVH_model.h>
#include <hpp/fcl/distance.h>
#include <hpp/fcl/collision.h>
45
#include "utility.h"
jpan's avatar
 
jpan committed
46
47


48
using namespace hpp::fcl;
jpan's avatar
 
jpan committed
49

50
51
#define BOOST_CHECK_FALSE(p) BOOST_CHECK(!(p))

jpan's avatar
 
jpan committed
52
53
FCL_REAL extents[6] = {0, 0, 0, 10, 10, 10};

54
BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere)
jpan's avatar
 
jpan committed
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
{
  Sphere s1(20);
  Sphere s2(20);
  BVHModel<RSS> s1_rss;
  BVHModel<RSS> s2_rss;

  generateBVHModel(s1_rss, s1, Transform3f(), 16, 16);
  generateBVHModel(s2_rss, s2, Transform3f(), 16, 16);

  DistanceRequest request;
  DistanceResult res, res1;

  Transform3f pose;

  pose.setTranslation(Vec3f(50, 0, 0));

  res.clear(); res1.clear();
72
73
  distance(&s1, Transform3f(), &s2, pose, request, res);
  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
74
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);  
jpan's avatar
 
jpan committed
75
76

  res1.clear();
77
  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
78
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
jpan's avatar
 
jpan committed
79
80
  
  res1.clear();
81
  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
82
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
jpan's avatar
 
jpan committed
83
84
85
86
87
88
89
90
91
92
93

  for(std::size_t i = 0; i < 10; ++i)
  {
    Transform3f t;
    generateRandomTransform(extents, t);
    
    Transform3f pose1(t);
    Transform3f pose2 = t * pose;

    res.clear(); res1.clear();
    distance(&s1, pose1, &s2, pose2, request, res);
94
    distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
95
    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
jpan's avatar
 
jpan committed
96
97

    res1.clear();
98
    distance(&s1, pose1, &s2_rss, pose2, request, res1);
99
    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
jpan's avatar
 
jpan committed
100
101

    res1.clear();
102
    distance(&s1_rss, pose1, &s2, pose2, request, res1);
103
    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
jpan's avatar
 
jpan committed
104
105
106
107
108
  }

  pose.setTranslation(Vec3f(40.1, 0, 0));

  res.clear(), res1.clear();
109
110
  distance(&s1, Transform3f(), &s2, pose, request, res);
  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
111
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
jpan's avatar
 
jpan committed
112
113

  res1.clear();
114
  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
115
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
jpan's avatar
 
jpan committed
116
117

  res1.clear();
118
  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
119
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
jpan's avatar
 
jpan committed
120
121
122
123
124
125
126
127
128
129
130


  for(std::size_t i = 0; i < 10; ++i)
  {
    Transform3f t;
    generateRandomTransform(extents, t);
    
    Transform3f pose1(t);
    Transform3f pose2 = t * pose;

    res.clear(); res1.clear();
131
132
    distance(&s1, pose1, &s2, pose2, request, res);
    distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
133
    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
jpan's avatar
 
jpan committed
134
135

    res1.clear();
136
    distance(&s1, pose1, &s2_rss, pose2, request, res1);
137
    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
jpan's avatar
 
jpan committed
138
139

    res1.clear();
140
    distance(&s1_rss, pose1, &s2, pose2, request, res1);
141
    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
jpan's avatar
 
jpan committed
142
143
144
  }
}

145
BOOST_AUTO_TEST_CASE(consistency_distance_boxbox)
jpan's avatar
 
jpan committed
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
{
  Box s1(20, 40, 50);
  Box s2(10, 10, 10);

  BVHModel<RSS> s1_rss;
  BVHModel<RSS> s2_rss;

  generateBVHModel(s1_rss, s1, Transform3f());
  generateBVHModel(s2_rss, s2, Transform3f());

  DistanceRequest request;
  DistanceResult res, res1;

  Transform3f pose;

  pose.setTranslation(Vec3f(50, 0, 0));

  res.clear(); res1.clear();
164
165
  distance(&s1, Transform3f(), &s2, pose, request, res);
  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
166
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
jpan's avatar
 
jpan committed
167
168
  
  res1.clear();
169
  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
170
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
jpan's avatar
 
jpan committed
171
172

  res1.clear();
173
  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
174
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
jpan's avatar
 
jpan committed
175
176
177
178
179
180
181
182
183
184

  for(std::size_t i = 0; i < 10; ++i)
  {
    Transform3f t;
    generateRandomTransform(extents, t);
    
    Transform3f pose1(t);
    Transform3f pose2 = t * pose;

    res.clear(); res1.clear();
185
186
    distance(&s1, pose1, &s2, pose2, request, res);
    distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
187
    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
jpan's avatar
 
jpan committed
188
189

    res1.clear();
190
    distance(&s1, pose1, &s2_rss, pose2, request, res1);
191
    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
jpan's avatar
 
jpan committed
192
193

    res1.clear();
194
    distance(&s1_rss, pose1, &s2, pose2, request, res1);
195
    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
jpan's avatar
 
jpan committed
196
197
198
199
200
  }

  pose.setTranslation(Vec3f(15.1, 0, 0));
  
  res.clear(); res1.clear();
201
202
  distance(&s1, Transform3f(), &s2, pose, request, res);
  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
203
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
jpan's avatar
 
jpan committed
204
205
  
  res1.clear();
206
  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
207
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
jpan's avatar
 
jpan committed
208
209

  res1.clear();
210
  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
211
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
jpan's avatar
 
jpan committed
212
213
214
215
216
217
218
219
220
221

  for(std::size_t i = 0; i < 10; ++i)
  {
    Transform3f t;
    generateRandomTransform(extents, t);
    
    Transform3f pose1(t);
    Transform3f pose2 = t * pose;

    res.clear(); res1.clear();
222
223
    distance(&s1, pose1, &s2, pose2, request, res);
    distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
224
    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
jpan's avatar
 
jpan committed
225
226

    res1.clear();
227
    distance(&s1, pose1, &s2_rss, pose2, request, res1);
228
    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
jpan's avatar
 
jpan committed
229
230

    res1.clear();
231
    distance(&s1_rss, pose1, &s2, pose2, request, res1);
232
    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
jpan's avatar
 
jpan committed
233
234
235
  }
}

236
BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder)
jpan's avatar
 
jpan committed
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
{
  Cylinder s1(5, 10);
  Cylinder s2(5, 10);

  BVHModel<RSS> s1_rss;
  BVHModel<RSS> s2_rss;

  generateBVHModel(s1_rss, s1, Transform3f(), 16, 16);
  generateBVHModel(s2_rss, s2, Transform3f(), 16, 16);

  DistanceRequest request;
  DistanceResult res, res1;

  Transform3f pose;

  pose.setTranslation(Vec3f(20, 0, 0));

  res.clear(); res1.clear();
255
256
  distance(&s1, Transform3f(), &s2, pose, request, res);
  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
257
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
jpan's avatar
 
jpan committed
258
259
  
  res1.clear();
260
  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
261
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
jpan's avatar
 
jpan committed
262
263
  
  res1.clear();
264
  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
265
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
jpan's avatar
 
jpan committed
266
267
268
269
270
271
272
273
274
275
  
  for(std::size_t i = 0; i < 10; ++i)
  {
    Transform3f t;
    generateRandomTransform(extents, t);
    
    Transform3f pose1(t);
    Transform3f pose2 = t * pose;

    res.clear(); res1.clear();
276
277
    distance(&s1, pose1, &s2, pose2, request, res);
    distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
278
    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
jpan's avatar
 
jpan committed
279
280

    res1.clear();
281
    distance(&s1, pose1, &s2_rss, pose2, request, res1);
282
    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
jpan's avatar
 
jpan committed
283
284

    res1.clear();
285
    distance(&s1_rss, pose1, &s2, pose2, request, res1);
286
    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
jpan's avatar
 
jpan committed
287
288
289
290
291
  }
  
  pose.setTranslation(Vec3f(15, 0, 0)); // libccd cannot use small value here :( 
  
  res.clear(); res1.clear();
292
293
  distance(&s1, Transform3f(), &s2, pose, request, res);
  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
294
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
jpan's avatar
 
jpan committed
295
296
  
  res1.clear();
297
  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
298
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
jpan's avatar
 
jpan committed
299
300

  res1.clear();
301
  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
302
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
jpan's avatar
 
jpan committed
303
304
305
306
307
308
309
310
311
312
  
  for(std::size_t i = 0; i < 10; ++i)
  {
    Transform3f t;
    generateRandomTransform(extents, t);
    
    Transform3f pose1(t);
    Transform3f pose2 = t * pose;

    res.clear(); res1.clear();
313
314
    distance(&s1, pose1, &s2, pose2, request, res);
    distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
315
    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
jpan's avatar
 
jpan committed
316
317

    res1.clear();
318
    distance(&s1, pose1, &s2_rss, pose2, request, res1);
319
    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
jpan's avatar
 
jpan committed
320
321

    res1.clear();
322
    distance(&s1_rss, pose1, &s2, pose2, request, res1);
323
    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
jpan's avatar
 
jpan committed
324
325
326
  }
}

327
BOOST_AUTO_TEST_CASE(consistency_distance_conecone)
jpan's avatar
 
jpan committed
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
{
  Cone s1(5, 10);
  Cone s2(5, 10);

  BVHModel<RSS> s1_rss;
  BVHModel<RSS> s2_rss;

  generateBVHModel(s1_rss, s1, Transform3f(), 16, 16);
  generateBVHModel(s2_rss, s2, Transform3f(), 16, 16);

  DistanceRequest request;
  DistanceResult res, res1;

  Transform3f pose;

  pose.setTranslation(Vec3f(20, 0, 0));
  
  res.clear(); res1.clear();
346
347
  distance(&s1, Transform3f(), &s2, pose, request, res);
  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
348
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
jpan's avatar
 
jpan committed
349
350
  
  res1.clear();
351
  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
352
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
jpan's avatar
 
jpan committed
353
  
354
  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
355
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
jpan's avatar
 
jpan committed
356
357
358
359
360
361
362
363
364
365
  
  for(std::size_t i = 0; i < 10; ++i)
  {
    Transform3f t;
    generateRandomTransform(extents, t);
    
    Transform3f pose1(t);
    Transform3f pose2 = t * pose;

    res.clear(); res1.clear();
366
367
    distance(&s1, pose1, &s2, pose2, request, res);
    distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
368
    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
jpan's avatar
 
jpan committed
369
370

    res1.clear();
371
    distance(&s1, pose1, &s2_rss, pose2, request, res1);
372
    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
jpan's avatar
 
jpan committed
373
374

    res1.clear();
375
    distance(&s1_rss, pose1, &s2, pose2, request, res1);
376
    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
jpan's avatar
 
jpan committed
377
378
379
380
381
  }
  
  pose.setTranslation(Vec3f(15, 0, 0)); // libccd cannot use small value here :( 
  
  res.clear(); res1.clear();
382
383
  distance(&s1, Transform3f(), &s2, pose, request, res);
  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
384
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
jpan's avatar
 
jpan committed
385
386
  
  res1.clear();
387
  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
388
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
jpan's avatar
 
jpan committed
389
390

  res1.clear();
391
  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
392
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
jpan's avatar
 
jpan committed
393
394
395
396
397
398
399
400
401
402
  
  for(std::size_t i = 0; i < 10; ++i)
  {
    Transform3f t;
    generateRandomTransform(extents, t);
    
    Transform3f pose1(t);
    Transform3f pose2 = t * pose;

    res.clear(); res1.clear();
403
404
    distance(&s1, pose1, &s2, pose2, request, res);
    distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
405
    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
jpan's avatar
 
jpan committed
406
407

    res1.clear();
408
    distance(&s1, pose1, &s2_rss, pose2, request, res1);
409
    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
jpan's avatar
 
jpan committed
410
411

    res1.clear();
412
    distance(&s1_rss, pose1, &s2, pose2, request, res1);
413
    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
jpan's avatar
 
jpan committed
414
415
416
417
  }
}


418
BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere_GJK)
jpan's avatar
 
jpan committed
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
{
  Sphere s1(20);
  Sphere s2(20);
  BVHModel<RSS> s1_rss;
  BVHModel<RSS> s2_rss;

  generateBVHModel(s1_rss, s1, Transform3f(), 16, 16);
  generateBVHModel(s2_rss, s2, Transform3f(), 16, 16);

  DistanceRequest request;
  DistanceResult res, res1;

  Transform3f pose;

  pose.setTranslation(Vec3f(50, 0, 0));

  res.clear(); res1.clear();
436
437
  distance(&s1, Transform3f(), &s2, pose, request, res);
  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
438
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);  
jpan's avatar
 
jpan committed
439
440

  res1.clear();
441
  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
442
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
jpan's avatar
 
jpan committed
443
444
  
  res1.clear();
445
  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
446
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
jpan's avatar
 
jpan committed
447
448
449
450
451
452
453
454
455
456
457
458


  for(std::size_t i = 0; i < 10; ++i)
  {
    Transform3f t;
    generateRandomTransform(extents, t);
    
    Transform3f pose1(t);
    Transform3f pose2 = t * pose;

    res.clear(); res1.clear();
    distance(&s1, pose1, &s2, pose2, request, res);
459
    distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
460
    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
jpan's avatar
 
jpan committed
461
462

    res1.clear();
463
    distance(&s1, pose1, &s2_rss, pose2, request, res1);
464
    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
jpan's avatar
 
jpan committed
465
466

    res1.clear();
467
    distance(&s1_rss, pose1, &s2, pose2, request, res1);
468
    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
jpan's avatar
 
jpan committed
469
470
471
472
473
  }

  pose.setTranslation(Vec3f(40.1, 0, 0));

  res.clear(); res1.clear();
474
475
  distance(&s1, Transform3f(), &s2, pose, request, res);
  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
476
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
jpan's avatar
 
jpan committed
477
478

  res1.clear();
479
  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
480
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
jpan's avatar
 
jpan committed
481
482

  res1.clear();
483
  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
484
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 4);
jpan's avatar
 
jpan committed
485
486
487
488
489
490
491
492
493
494
495


  for(std::size_t i = 0; i < 10; ++i)
  {
    Transform3f t;
    generateRandomTransform(extents, t);
    
    Transform3f pose1(t);
    Transform3f pose2 = t * pose;

    res.clear(); res1.clear();
496
497
    distance(&s1, pose1, &s2, pose2, request, res);
    distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
498
    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
jpan's avatar
 
jpan committed
499
500

    res1.clear();
501
    distance(&s1, pose1, &s2_rss, pose2, request, res1);
502
    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
jpan's avatar
 
jpan committed
503
504

    res1.clear();
505
    distance(&s1_rss, pose1, &s2, pose2, request, res1);
506
    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 4);
jpan's avatar
 
jpan committed
507
508
509
  }
}

510
BOOST_AUTO_TEST_CASE(consistency_distance_boxbox_GJK)
jpan's avatar
 
jpan committed
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
{
  Box s1(20, 40, 50);
  Box s2(10, 10, 10);

  BVHModel<RSS> s1_rss;
  BVHModel<RSS> s2_rss;

  generateBVHModel(s1_rss, s1, Transform3f());
  generateBVHModel(s2_rss, s2, Transform3f());

  DistanceRequest request;
  DistanceResult res, res1;

  Transform3f pose;

  pose.setTranslation(Vec3f(50, 0, 0));

  res.clear(); res1.clear();
529
530
  distance(&s1, Transform3f(), &s2, pose, request, res);
  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
531
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
jpan's avatar
 
jpan committed
532
533
  
  res1.clear();
534
  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
535
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
jpan's avatar
 
jpan committed
536
537

  res1.clear();
538
  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
539
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
jpan's avatar
 
jpan committed
540
541
542
543
544
545
546
547
548
549

  for(std::size_t i = 0; i < 10; ++i)
  {
    Transform3f t;
    generateRandomTransform(extents, t);
    
    Transform3f pose1(t);
    Transform3f pose2 = t * pose;

    res.clear(); res1.clear();
550
551
    distance(&s1, pose1, &s2, pose2, request, res);
    distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
552
    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
jpan's avatar
 
jpan committed
553
554

    res1.clear();
555
    distance(&s1, pose1, &s2_rss, pose2, request, res1);
556
    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
jpan's avatar
 
jpan committed
557
558

    res1.clear();
559
    distance(&s1_rss, pose1, &s2, pose2, request, res1);
560
    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
jpan's avatar
 
jpan committed
561
562
563
564
565
  }

  pose.setTranslation(Vec3f(15.1, 0, 0));
  
  res.clear(); res1.clear();
566
567
  distance(&s1, Transform3f(), &s2, pose, request, res);
  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
568
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
jpan's avatar
 
jpan committed
569
570
  
  res1.clear();
571
  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
572
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
jpan's avatar
 
jpan committed
573
574

  res1.clear();
575
  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
576
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
jpan's avatar
 
jpan committed
577
578
579
580
581
582
583
584
585
586

  for(std::size_t i = 0; i < 10; ++i)
  {
    Transform3f t;
    generateRandomTransform(extents, t);
    
    Transform3f pose1(t);
    Transform3f pose2 = t * pose;

    res.clear(); res1.clear();
587
588
    distance(&s1, pose1, &s2, pose2, request, res);
    distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
589
    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
jpan's avatar
 
jpan committed
590
591

    res1.clear();
592
    distance(&s1, pose1, &s2_rss, pose2, request, res1);
593
    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
jpan's avatar
 
jpan committed
594
595

    res1.clear();
596
    distance(&s1_rss, pose1, &s2, pose2, request, res1);
597
    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
jpan's avatar
 
jpan committed
598
599
600
  }
}

601
BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder_GJK)
jpan's avatar
 
jpan committed
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
{
  Cylinder s1(5, 10);
  Cylinder s2(5, 10);

  BVHModel<RSS> s1_rss;
  BVHModel<RSS> s2_rss;

  generateBVHModel(s1_rss, s1, Transform3f(), 16, 16);
  generateBVHModel(s2_rss, s2, Transform3f(), 16, 16);
  
  DistanceRequest request;
  DistanceResult res, res1;

  Transform3f pose;

  pose.setTranslation(Vec3f(20, 0, 0));
  
  res.clear(); res1.clear();
620
621
  distance(&s1, Transform3f(), &s2, pose, request, res);
  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
622
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
jpan's avatar
 
jpan committed
623
624
  
  res1.clear();
625
  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
626
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
jpan's avatar
 
jpan committed
627
628
  
  res1.clear();
629
  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
630
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
jpan's avatar
 
jpan committed
631
632
633
634
635
636
637
638
639
640
  
  for(std::size_t i = 0; i < 10; ++i)
  {
    Transform3f t;
    generateRandomTransform(extents, t);
    
    Transform3f pose1(t);
    Transform3f pose2 = t * pose;

    res.clear(); res1.clear();
641
642
    distance(&s1, pose1, &s2, pose2, request, res);
    distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
jpan's avatar
 
jpan committed
643
644
645
    if(fabs(res1.min_distance - res.min_distance) / res.min_distance > 0.05)
      std::cout << "low resolution: " << res1.min_distance << " " << res.min_distance << std::endl;
    else
646
      BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
jpan's avatar
 
jpan committed
647
648

    res1.clear();
649
    distance(&s1, pose1, &s2_rss, pose2, request, res1);
jpan's avatar
 
jpan committed
650
651
652
    if(fabs(res1.min_distance - res.min_distance) / res.min_distance > 0.05)
      std::cout << "low resolution: " << res1.min_distance << " " << res.min_distance << std::endl;
    else
653
      BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
jpan's avatar
 
jpan committed
654
655

    res1.clear();
656
    distance(&s1_rss, pose1, &s2, pose2, request, res1);
jpan's avatar
 
jpan committed
657
658
659
    if(fabs(res1.min_distance - res.min_distance) / res.min_distance > 0.05)
      std::cout << "low resolution: " << res1.min_distance << " " << res.min_distance << std::endl;
    else
660
      BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
jpan's avatar
 
jpan committed
661
662
663
664
665
  }
  
  pose.setTranslation(Vec3f(10.1, 0, 0));
  
  res.clear(); res1.clear();
666
667
  distance(&s1, Transform3f(), &s2, pose, request, res);
  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
668
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
jpan's avatar
 
jpan committed
669
670
  
  res1.clear();
671
  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
672
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
jpan's avatar
 
jpan committed
673
674

  res1.clear();
675
  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
676
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
jpan's avatar
 
jpan committed
677
678
679
680
681
682
683
684
685
686
  
  for(std::size_t i = 0; i < 10; ++i)
  {
    Transform3f t;
    generateRandomTransform(extents, t);
    
    Transform3f pose1(t);
    Transform3f pose2 = t * pose;

    res.clear(); res1.clear();
687
688
    distance(&s1, pose1, &s2, pose2, request, res);
    distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
689
    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
jpan's avatar
 
jpan committed
690
691

    res1.clear();
692
    distance(&s1, pose1, &s2_rss, pose2, request, res1);
693
    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
jpan's avatar
 
jpan committed
694
695

    res1.clear();
696
    distance(&s1_rss, pose1, &s2, pose2, request, res1);
697
    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
jpan's avatar
 
jpan committed
698
699
700
  }
}

701
BOOST_AUTO_TEST_CASE(consistency_distance_conecone_GJK)
jpan's avatar
 
jpan committed
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
{
  Cone s1(5, 10);
  Cone s2(5, 10);

  BVHModel<RSS> s1_rss;
  BVHModel<RSS> s2_rss;

  generateBVHModel(s1_rss, s1, Transform3f(), 16, 16);
  generateBVHModel(s2_rss, s2, Transform3f(), 16, 16);

  DistanceRequest request;
  DistanceResult res, res1;

  Transform3f pose;

  pose.setTranslation(Vec3f(20, 0, 0));

  res.clear(); res1.clear();
720
721
  distance(&s1, Transform3f(), &s2, pose, request, res);
  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
722
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
jpan's avatar
 
jpan committed
723
724
  
  res1.clear();
725
  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
726
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
jpan's avatar
 
jpan committed
727
728
  
  res1.clear();
729
  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
730
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
jpan's avatar
 
jpan committed
731
732
733
734
735
736
737
738
739
740
  
  for(std::size_t i = 0; i < 10; ++i)
  {
    Transform3f t;
    generateRandomTransform(extents, t);
    
    Transform3f pose1(t);
    Transform3f pose2 = t * pose;

    res.clear(); res1.clear();
741
742
    distance(&s1, pose1, &s2, pose2, request, res);
    distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
743
    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
jpan's avatar
 
jpan committed
744
745

    res1.clear();
746
    distance(&s1, pose1, &s2_rss, pose2, request, res1);
747
    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
jpan's avatar
 
jpan committed
748
749

    res1.clear();
750
    distance(&s1_rss, pose1, &s2, pose2, request, res1);
751
    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
jpan's avatar
 
jpan committed
752
753
754
755
756
  }
  
  pose.setTranslation(Vec3f(10.1, 0, 0));
  
  res.clear(); res1.clear();
757
758
  distance(&s1, Transform3f(), &s2, pose, request, res);
  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
759
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
jpan's avatar
 
jpan committed
760
761
  
  res1.clear();
762
  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
763
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
jpan's avatar
 
jpan committed
764
765

  res1.clear();
766
  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
767
  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
jpan's avatar
 
jpan committed
768
769
770
771
772
773
774
775
776
777
  
  for(std::size_t i = 0; i < 10; ++i)
  {
    Transform3f t;
    generateRandomTransform(extents, t);
    
    Transform3f pose1(t);
    Transform3f pose2 = t * pose;

    res.clear(); res1.clear();
778
779
    distance(&s1, pose1, &s2, pose2, request, res);
    distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
780
    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
jpan's avatar
 
jpan committed
781
782

    res1.clear();
783
    distance(&s1, pose1, &s2_rss, pose2, request, res1);
784
    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
jpan's avatar
 
jpan committed
785
786

    res1.clear();
787
    distance(&s1_rss, pose1, &s2, pose2, request, res1);
788
    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
jpan's avatar
 
jpan committed
789
790
791
792
793
  }
}



794
BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere)
jpan's avatar
 
jpan committed
795
796
797
798
799
800
801
802
803
804
805
806
807
{
  Sphere s1(20);
  Sphere s2(10);
  BVHModel<AABB> s1_aabb;
  BVHModel<AABB> s2_aabb;
  BVHModel<OBB> s1_obb;
  BVHModel<OBB> s2_obb;

  generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16);
  generateBVHModel(s2_aabb, s2, Transform3f(), 16, 16);
  generateBVHModel(s1_obb, s1, Transform3f(), 16, 16);
  generateBVHModel(s2_obb, s2, Transform3f(), 16, 16);

808
  CollisionRequest request (false, 1, false);
jpan's avatar
 
jpan committed
809
810
811
812
813
814
815
816
817
818
819
820
821
  CollisionResult result;

  bool res;

  Transform3f pose, pose_aabb, pose_obb;


  // s2 is within s1
  // both are shapes --> collision
  // s1 is shape, s2 is mesh --> in collision
  // s1 is mesh, s2 is shape --> collision free
  // all are reasonable
  result.clear();
822
  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
823
  BOOST_CHECK(res);
jpan's avatar
 
jpan committed
824
  result.clear();
825
  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
826
  BOOST_CHECK(res);
jpan's avatar
 
jpan committed
827
  result.clear();
828
  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
829
  BOOST_CHECK(res);
jpan's avatar
 
jpan committed
830
  result.clear();
831
  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
832
  BOOST_CHECK(res);
jpan's avatar
 
jpan committed
833
  result.clear();
834
  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
835
  BOOST_CHECK(res);
jpan's avatar
 
jpan committed
836
  result.clear();
837
  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
838
  BOOST_CHECK_FALSE(res);
jpan's avatar
 
jpan committed
839
  result.clear();
840
  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
841
  BOOST_CHECK_FALSE(res);
jpan's avatar
 
jpan committed
842
  result.clear();
843
  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
844
  BOOST_CHECK_FALSE(res);
jpan's avatar
 
jpan committed
845
  result.clear();
846
  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
847
  BOOST_CHECK_FALSE(res);
jpan's avatar
 
jpan committed
848
849
850
851
852
853

  pose.setTranslation(Vec3f(40, 0, 0));
  pose_aabb.setTranslation(Vec3f(40, 0, 0));
  pose_obb.setTranslation(Vec3f(40, 0, 0));

  result.clear();
854
  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
855
  BOOST_CHECK_FALSE(res);
jpan's avatar
 
jpan committed
856
  result.clear();
857
  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
858
  BOOST_CHECK_FALSE(res);
jpan's avatar
 
jpan committed
859
  result.clear();
860
  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
861
  BOOST_CHECK_FALSE(res);
jpan's avatar
 
jpan committed
862
  result.clear();
863
  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
864
  BOOST_CHECK_FALSE(res);
jpan's avatar
 
jpan committed
865
  result.clear();
866
  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
867
  BOOST_CHECK_FALSE(res);
jpan's avatar
 
jpan committed
868
  result.clear();
869
  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
870
  BOOST_CHECK_FALSE(res);
jpan's avatar
 
jpan committed
871
  result.clear();
872
  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
873
  BOOST_CHECK_FALSE(res);
jpan's avatar
 
jpan committed
874
  result.clear();
875
  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
876
  BOOST_CHECK_FALSE(res);
jpan's avatar
 
jpan committed
877
  result.clear();
878
  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
879
  BOOST_CHECK_FALSE(res);
jpan's avatar
 
jpan committed
880
881
882
883
884
885
  
  pose.setTranslation(Vec3f(30, 0, 0));
  pose_aabb.setTranslation(Vec3f(30, 0, 0));
  pose_obb.setTranslation(Vec3f(30, 0, 0));

  result.clear();
886
  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
887
  BOOST_CHECK(res);
jpan's avatar
 
jpan committed
888
  result.clear();
889
  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
890
  BOOST_CHECK_FALSE(res);
jpan's avatar
 
jpan committed
891
  result.clear();
892
  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
893
  BOOST_CHECK_FALSE(res);
jpan's avatar
 
jpan committed
894
  result.clear();
895
  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
896
  BOOST_CHECK_FALSE(res);
jpan's avatar
 
jpan committed
897
  result.clear();
898
  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
899
  BOOST_CHECK_FALSE(res);
jpan's avatar
 
jpan committed
900
  result.clear();
901
  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
902
  BOOST_CHECK_FALSE(res);
jpan's avatar
 
jpan committed
903
  result.clear();
904
  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
905
  BOOST_CHECK_FALSE(res);
jpan's avatar
 
jpan committed
906
  result.clear();
907
  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
908
  BOOST_CHECK_FALSE(res);
jpan's avatar
 
jpan committed
909
  result.clear();
910
  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
911
  BOOST_CHECK_FALSE(res);
jpan's avatar
 
jpan committed
912
913
914
915
916
917
  
  pose.setTranslation(Vec3f(29.9, 0, 0));
  pose_aabb.setTranslation(Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
  pose_obb.setTranslation(Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision

  result.clear();