shape_mesh_consistency.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011-2014, Willow Garage, Inc.
5  * Copyright (c) 2014-2015, Open Source Robotics Foundation
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Open Source Robotics Foundation nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  */
35 
38 #define BOOST_TEST_MODULE FCL_SHAPE_MESH_CONSISTENCY
39 #include <boost/test/included/unit_test.hpp>
40 
41 #include <hpp/fcl/narrowphase/narrowphase.h>
42 #include <hpp/fcl/shape/geometric_shape_to_BVH_model.h>
43 #include <hpp/fcl/distance.h>
44 #include <hpp/fcl/collision.h>
45 #include "utility.h"
46 
47 using namespace hpp::fcl;
48 
49 #define BOOST_CHECK_FALSE(p) BOOST_CHECK(!(p))
50 
51 FCL_REAL extents[6] = {0, 0, 0, 10, 10, 10};
52 
53 BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere) {
54  Sphere s1(20);
55  Sphere s2(20);
56  BVHModel<RSS> s1_rss;
57  BVHModel<RSS> s2_rss;
58 
59  generateBVHModel(s1_rss, s1, Transform3f(), 16, 16);
60  generateBVHModel(s2_rss, s2, Transform3f(), 16, 16);
61 
62  DistanceRequest request;
63  DistanceResult res, res1;
64 
65  Transform3f pose;
66 
67  pose.setTranslation(Vec3f(50, 0, 0));
68 
69  res.clear();
70  res1.clear();
71  distance(&s1, Transform3f(), &s2, pose, request, res);
72  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
73  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
74  0.05);
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 <
79  0.05);
80 
81  res1.clear();
82  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
83  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
84  0.05);
85 
86  for (std::size_t i = 0; i < 10; ++i) {
87  Transform3f t;
88  generateRandomTransform(extents, t);
89 
90  Transform3f pose1(t);
91  Transform3f pose2 = t * pose;
92 
93  res.clear();
94  res1.clear();
95  distance(&s1, pose1, &s2, pose2, request, res);
96  distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
97  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
98  0.05);
99 
100  res1.clear();
101  distance(&s1, pose1, &s2_rss, pose2, request, res1);
102  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
103  0.05);
104 
105  res1.clear();
106  distance(&s1_rss, pose1, &s2, pose2, request, res1);
107  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
108  0.05);
109  }
110 
111  pose.setTranslation(Vec3f(40.1, 0, 0));
112 
113  res.clear(), res1.clear();
114  distance(&s1, Transform3f(), &s2, pose, request, res);
115  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
116  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
117  2);
118 
119  res1.clear();
120  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
121  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
122  2);
123 
124  res1.clear();
125  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
126  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
127  2);
128 
129  for (std::size_t i = 0; i < 10; ++i) {
130  Transform3f t;
131  generateRandomTransform(extents, t);
132 
133  Transform3f pose1(t);
134  Transform3f pose2 = t * pose;
135 
136  res.clear();
137  res1.clear();
138  distance(&s1, pose1, &s2, pose2, request, res);
139  distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
140  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
141  2);
142 
143  res1.clear();
144  distance(&s1, pose1, &s2_rss, pose2, request, res1);
145  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
146  2);
147 
148  res1.clear();
149  distance(&s1_rss, pose1, &s2, pose2, request, res1);
150  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
151  2);
152  }
153 }
154 
155 BOOST_AUTO_TEST_CASE(consistency_distance_boxbox) {
156  Box s1(20, 40, 50);
157  Box s2(10, 10, 10);
158 
159  BVHModel<RSS> s1_rss;
160  BVHModel<RSS> s2_rss;
161 
162  generateBVHModel(s1_rss, s1, Transform3f());
163  generateBVHModel(s2_rss, s2, Transform3f());
164 
165  DistanceRequest request;
166  DistanceResult res, res1;
167 
168  Transform3f pose;
169 
170  pose.setTranslation(Vec3f(50, 0, 0));
171 
172  res.clear();
173  res1.clear();
174  distance(&s1, Transform3f(), &s2, pose, request, res);
175  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
176  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
177  0.01);
178 
179  res1.clear();
180  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
181  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
182  0.01);
183 
184  res1.clear();
185  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
186  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
187  0.01);
188 
189  for (std::size_t i = 0; i < 10; ++i) {
190  Transform3f t;
191  generateRandomTransform(extents, t);
192 
193  Transform3f pose1(t);
194  Transform3f pose2 = t * pose;
195 
196  res.clear();
197  res1.clear();
198  distance(&s1, pose1, &s2, pose2, request, res);
199  distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
200  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
201  0.01);
202 
203  res1.clear();
204  distance(&s1, pose1, &s2_rss, pose2, request, res1);
205  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
206  0.01);
207 
208  res1.clear();
209  distance(&s1_rss, pose1, &s2, pose2, request, res1);
210  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
211  0.01);
212  }
213 
214  pose.setTranslation(Vec3f(15.1, 0, 0));
215 
216  res.clear();
217  res1.clear();
218  distance(&s1, Transform3f(), &s2, pose, request, res);
219  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
220  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
221  2);
222 
223  res1.clear();
224  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
225  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
226  2);
227 
228  res1.clear();
229  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
230  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
231  2);
232 
233  for (std::size_t i = 0; i < 10; ++i) {
234  Transform3f t;
235  generateRandomTransform(extents, t);
236 
237  Transform3f pose1(t);
238  Transform3f pose2 = t * pose;
239 
240  res.clear();
241  res1.clear();
242  distance(&s1, pose1, &s2, pose2, request, res);
243  distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
244  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
245  2);
246 
247  res1.clear();
248  distance(&s1, pose1, &s2_rss, pose2, request, res1);
249  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
250  2);
251 
252  res1.clear();
253  distance(&s1_rss, pose1, &s2, pose2, request, res1);
254  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
255  2);
256  }
257 }
258 
259 BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder) {
260  Cylinder s1(5, 10);
261  Cylinder s2(5, 10);
262 
263  BVHModel<RSS> s1_rss;
264  BVHModel<RSS> s2_rss;
265 
266  generateBVHModel(s1_rss, s1, Transform3f(), 16, 16);
267  generateBVHModel(s2_rss, s2, Transform3f(), 16, 16);
268 
269  DistanceRequest request;
270  DistanceResult res, res1;
271 
272  Transform3f pose;
273 
274  pose.setTranslation(Vec3f(20, 0, 0));
275 
276  res.clear();
277  res1.clear();
278  distance(&s1, Transform3f(), &s2, pose, request, res);
279  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
280  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
281  0.01);
282 
283  res1.clear();
284  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
285  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
286  0.01);
287 
288  res1.clear();
289  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
290  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
291  0.01);
292 
293  for (std::size_t i = 0; i < 10; ++i) {
294  Transform3f t;
295  generateRandomTransform(extents, t);
296 
297  Transform3f pose1(t);
298  Transform3f pose2 = t * pose;
299 
300  res.clear();
301  res1.clear();
302  distance(&s1, pose1, &s2, pose2, request, res);
303  distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
304  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
305  0.01);
306 
307  res1.clear();
308  distance(&s1, pose1, &s2_rss, pose2, request, res1);
309  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
310  0.01);
311 
312  res1.clear();
313  distance(&s1_rss, pose1, &s2, pose2, request, res1);
314  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
315  0.01);
316  }
317 
318  pose.setTranslation(
319  Vec3f(15, 0, 0)); // libccd cannot use small value here :(
320 
321  res.clear();
322  res1.clear();
323  distance(&s1, Transform3f(), &s2, pose, request, res);
324  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
325  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
326  2);
327 
328  res1.clear();
329  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
330  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
331  2);
332 
333  res1.clear();
334  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
335  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
336  2);
337 
338  for (std::size_t i = 0; i < 10; ++i) {
339  Transform3f t;
340  generateRandomTransform(extents, t);
341 
342  Transform3f pose1(t);
343  Transform3f pose2 = t * pose;
344 
345  res.clear();
346  res1.clear();
347  distance(&s1, pose1, &s2, pose2, request, res);
348  distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
349  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
350  2);
351 
352  res1.clear();
353  distance(&s1, pose1, &s2_rss, pose2, request, res1);
354  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
355  2);
356 
357  res1.clear();
358  distance(&s1_rss, pose1, &s2, pose2, request, res1);
359  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
360  2);
361  }
362 }
363 
364 BOOST_AUTO_TEST_CASE(consistency_distance_conecone) {
365  Cone s1(5, 10);
366  Cone s2(5, 10);
367 
368  BVHModel<RSS> s1_rss;
369  BVHModel<RSS> s2_rss;
370 
371  generateBVHModel(s1_rss, s1, Transform3f(), 16, 16);
372  generateBVHModel(s2_rss, s2, Transform3f(), 16, 16);
373 
374  DistanceRequest request;
375  DistanceResult res, res1;
376 
377  Transform3f pose;
378 
379  pose.setTranslation(Vec3f(20, 0, 0));
380 
381  res.clear();
382  res1.clear();
383  distance(&s1, Transform3f(), &s2, pose, request, res);
384  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
385  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
386  0.05);
387 
388  res1.clear();
389  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
390  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
391  0.05);
392 
393  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
394  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
395  0.05);
396 
397  for (std::size_t i = 0; i < 10; ++i) {
398  Transform3f t;
399  generateRandomTransform(extents, t);
400 
401  Transform3f pose1(t);
402  Transform3f pose2 = t * pose;
403 
404  res.clear();
405  res1.clear();
406  distance(&s1, pose1, &s2, pose2, request, res);
407  distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
408  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
409  0.05);
410 
411  res1.clear();
412  distance(&s1, pose1, &s2_rss, pose2, request, res1);
413  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
414  0.05);
415 
416  res1.clear();
417  distance(&s1_rss, pose1, &s2, pose2, request, res1);
418  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
419  0.05);
420  }
421 
422  pose.setTranslation(
423  Vec3f(15, 0, 0)); // libccd cannot use small value here :(
424 
425  res.clear();
426  res1.clear();
427  distance(&s1, Transform3f(), &s2, pose, request, res);
428  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
429  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
430  2);
431 
432  res1.clear();
433  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
434  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
435  2);
436 
437  res1.clear();
438  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
439  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
440  2);
441 
442  for (std::size_t i = 0; i < 10; ++i) {
443  Transform3f t;
444  generateRandomTransform(extents, t);
445 
446  Transform3f pose1(t);
447  Transform3f pose2 = t * pose;
448 
449  res.clear();
450  res1.clear();
451  distance(&s1, pose1, &s2, pose2, request, res);
452  distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
453  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
454  2);
455 
456  res1.clear();
457  distance(&s1, pose1, &s2_rss, pose2, request, res1);
458  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
459  2);
460 
461  res1.clear();
462  distance(&s1_rss, pose1, &s2, pose2, request, res1);
463  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
464  2);
465  }
466 }
467 
468 BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere_GJK) {
469  Sphere s1(20);
470  Sphere s2(20);
471  BVHModel<RSS> s1_rss;
472  BVHModel<RSS> s2_rss;
473 
474  generateBVHModel(s1_rss, s1, Transform3f(), 16, 16);
475  generateBVHModel(s2_rss, s2, Transform3f(), 16, 16);
476 
477  DistanceRequest request;
478  DistanceResult res, res1;
479 
480  Transform3f pose;
481 
482  pose.setTranslation(Vec3f(50, 0, 0));
483 
484  res.clear();
485  res1.clear();
486  distance(&s1, Transform3f(), &s2, pose, request, res);
487  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
488  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
489  0.05);
490 
491  res1.clear();
492  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
493  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
494  0.05);
495 
496  res1.clear();
497  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
498  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
499  0.05);
500 
501  for (std::size_t i = 0; i < 10; ++i) {
502  Transform3f t;
503  generateRandomTransform(extents, t);
504 
505  Transform3f pose1(t);
506  Transform3f pose2 = t * pose;
507 
508  res.clear();
509  res1.clear();
510  distance(&s1, pose1, &s2, pose2, request, res);
511  distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
512  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
513  0.05);
514 
515  res1.clear();
516  distance(&s1, pose1, &s2_rss, pose2, request, res1);
517  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
518  0.05);
519 
520  res1.clear();
521  distance(&s1_rss, pose1, &s2, pose2, request, res1);
522  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
523  0.05);
524  }
525 
526  pose.setTranslation(Vec3f(40.1, 0, 0));
527 
528  res.clear();
529  res1.clear();
530  distance(&s1, Transform3f(), &s2, pose, request, res);
531  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
532  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
533  2);
534 
535  res1.clear();
536  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
537  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
538  2);
539 
540  res1.clear();
541  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
542  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
543  4);
544 
545  for (std::size_t i = 0; i < 10; ++i) {
546  Transform3f t;
547  generateRandomTransform(extents, t);
548 
549  Transform3f pose1(t);
550  Transform3f pose2 = t * pose;
551 
552  res.clear();
553  res1.clear();
554  distance(&s1, pose1, &s2, pose2, request, res);
555  distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
556  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
557  2);
558 
559  res1.clear();
560  distance(&s1, pose1, &s2_rss, pose2, request, res1);
561  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
562  2);
563 
564  res1.clear();
565  distance(&s1_rss, pose1, &s2, pose2, request, res1);
566  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
567  4);
568  }
569 }
570 
571 BOOST_AUTO_TEST_CASE(consistency_distance_boxbox_GJK) {
572  Box s1(20, 40, 50);
573  Box s2(10, 10, 10);
574 
575  BVHModel<RSS> s1_rss;
576  BVHModel<RSS> s2_rss;
577 
578  generateBVHModel(s1_rss, s1, Transform3f());
579  generateBVHModel(s2_rss, s2, Transform3f());
580 
581  DistanceRequest request;
582  DistanceResult res, res1;
583 
584  Transform3f pose;
585 
586  pose.setTranslation(Vec3f(50, 0, 0));
587 
588  res.clear();
589  res1.clear();
590  distance(&s1, Transform3f(), &s2, pose, request, res);
591  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
592  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
593  0.01);
594 
595  res1.clear();
596  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
597  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
598  0.01);
599 
600  res1.clear();
601  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
602  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
603  0.01);
604 
605  for (std::size_t i = 0; i < 10; ++i) {
606  Transform3f t;
607  generateRandomTransform(extents, t);
608 
609  Transform3f pose1(t);
610  Transform3f pose2 = t * pose;
611 
612  res.clear();
613  res1.clear();
614  distance(&s1, pose1, &s2, pose2, request, res);
615  distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
616  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
617  0.01);
618 
619  res1.clear();
620  distance(&s1, pose1, &s2_rss, pose2, request, res1);
621  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
622  0.01);
623 
624  res1.clear();
625  distance(&s1_rss, pose1, &s2, pose2, request, res1);
626  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
627  0.01);
628  }
629 
630  pose.setTranslation(Vec3f(15.1, 0, 0));
631 
632  res.clear();
633  res1.clear();
634  distance(&s1, Transform3f(), &s2, pose, request, res);
635  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
636  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
637  2);
638 
639  res1.clear();
640  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
641  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
642  2);
643 
644  res1.clear();
645  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
646  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
647  2);
648 
649  for (std::size_t i = 0; i < 10; ++i) {
650  Transform3f t;
651  generateRandomTransform(extents, t);
652 
653  Transform3f pose1(t);
654  Transform3f pose2 = t * pose;
655 
656  res.clear();
657  res1.clear();
658  distance(&s1, pose1, &s2, pose2, request, res);
659  distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
660  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
661  2);
662 
663  res1.clear();
664  distance(&s1, pose1, &s2_rss, pose2, request, res1);
665  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
666  2);
667 
668  res1.clear();
669  distance(&s1_rss, pose1, &s2, pose2, request, res1);
670  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
671  2);
672  }
673 }
674 
675 BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder_GJK) {
676  Cylinder s1(5, 10);
677  Cylinder s2(5, 10);
678 
679  BVHModel<RSS> s1_rss;
680  BVHModel<RSS> s2_rss;
681 
682  generateBVHModel(s1_rss, s1, Transform3f(), 16, 16);
683  generateBVHModel(s2_rss, s2, Transform3f(), 16, 16);
684 
685  DistanceRequest request;
686  DistanceResult res, res1;
687 
688  Transform3f pose;
689 
690  pose.setTranslation(Vec3f(20, 0, 0));
691 
692  res.clear();
693  res1.clear();
694  distance(&s1, Transform3f(), &s2, pose, request, res);
695  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
696  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
697  0.05);
698 
699  res1.clear();
700  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
701  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
702  0.05);
703 
704  res1.clear();
705  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
706  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
707  0.05);
708 
709  for (std::size_t i = 0; i < 10; ++i) {
710  Transform3f t;
711  generateRandomTransform(extents, t);
712 
713  Transform3f pose1(t);
714  Transform3f pose2 = t * pose;
715 
716  res.clear();
717  res1.clear();
718  distance(&s1, pose1, &s2, pose2, request, res);
719  distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
720  if (fabs(res1.min_distance - res.min_distance) / res.min_distance > 0.05)
721  std::cout << "low resolution: " << res1.min_distance << " "
722  << res.min_distance << std::endl;
723  else
724  BOOST_CHECK(
725  fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
726 
727  res1.clear();
728  distance(&s1, pose1, &s2_rss, pose2, request, res1);
729  if (fabs(res1.min_distance - res.min_distance) / res.min_distance > 0.05)
730  std::cout << "low resolution: " << res1.min_distance << " "
731  << res.min_distance << std::endl;
732  else
733  BOOST_CHECK(
734  fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
735 
736  res1.clear();
737  distance(&s1_rss, pose1, &s2, pose2, request, res1);
738  if (fabs(res1.min_distance - res.min_distance) / res.min_distance > 0.05)
739  std::cout << "low resolution: " << res1.min_distance << " "
740  << res.min_distance << std::endl;
741  else
742  BOOST_CHECK(
743  fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
744  }
745 
746  pose.setTranslation(Vec3f(10.1, 0, 0));
747 
748  res.clear();
749  res1.clear();
750  distance(&s1, Transform3f(), &s2, pose, request, res);
751  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
752  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
753  2);
754 
755  res1.clear();
756  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
757  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
758  2);
759 
760  res1.clear();
761  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
762  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
763  2);
764 
765  for (std::size_t i = 0; i < 10; ++i) {
766  Transform3f t;
767  generateRandomTransform(extents, t);
768 
769  Transform3f pose1(t);
770  Transform3f pose2 = t * pose;
771 
772  res.clear();
773  res1.clear();
774  distance(&s1, pose1, &s2, pose2, request, res);
775  distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
776  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
777  2);
778 
779  res1.clear();
780  distance(&s1, pose1, &s2_rss, pose2, request, res1);
781  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
782  2);
783 
784  res1.clear();
785  distance(&s1_rss, pose1, &s2, pose2, request, res1);
786  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
787  2);
788  }
789 }
790 
791 BOOST_AUTO_TEST_CASE(consistency_distance_conecone_GJK) {
792  Cone s1(5, 10);
793  Cone s2(5, 10);
794 
795  BVHModel<RSS> s1_rss;
796  BVHModel<RSS> s2_rss;
797 
798  generateBVHModel(s1_rss, s1, Transform3f(), 16, 16);
799  generateBVHModel(s2_rss, s2, Transform3f(), 16, 16);
800 
801  DistanceRequest request;
802  DistanceResult res, res1;
803 
804  Transform3f pose;
805 
806  pose.setTranslation(Vec3f(20, 0, 0));
807 
808  res.clear();
809  res1.clear();
810  distance(&s1, Transform3f(), &s2, pose, request, res);
811  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
812  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
813  0.05);
814 
815  res1.clear();
816  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
817  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
818  0.05);
819 
820  res1.clear();
821  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
822  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
823  0.05);
824 
825  for (std::size_t i = 0; i < 10; ++i) {
826  Transform3f t;
827  generateRandomTransform(extents, t);
828 
829  Transform3f pose1(t);
830  Transform3f pose2 = t * pose;
831 
832  res.clear();
833  res1.clear();
834  distance(&s1, pose1, &s2, pose2, request, res);
835  distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
836  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
837  0.05);
838 
839  res1.clear();
840  distance(&s1, pose1, &s2_rss, pose2, request, res1);
841  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
842  0.05);
843 
844  res1.clear();
845  distance(&s1_rss, pose1, &s2, pose2, request, res1);
846  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
847  0.05);
848  }
849 
850  pose.setTranslation(Vec3f(10.1, 0, 0));
851 
852  res.clear();
853  res1.clear();
854  distance(&s1, Transform3f(), &s2, pose, request, res);
855  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
856  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
857  2);
858 
859  res1.clear();
860  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
861  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
862  2);
863 
864  res1.clear();
865  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
866  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
867  2);
868 
869  for (std::size_t i = 0; i < 10; ++i) {
870  Transform3f t;
871  generateRandomTransform(extents, t);
872 
873  Transform3f pose1(t);
874  Transform3f pose2 = t * pose;
875 
876  res.clear();
877  res1.clear();
878  distance(&s1, pose1, &s2, pose2, request, res);
879  distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
880  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
881  2);
882 
883  res1.clear();
884  distance(&s1, pose1, &s2_rss, pose2, request, res1);
885  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
886  2);
887 
888  res1.clear();
889  distance(&s1_rss, pose1, &s2, pose2, request, res1);
890  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance <
891  2);
892  }
893 }
894 
895 BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere) {
896  Sphere s1(20);
897  Sphere s2(10);
898  BVHModel<AABB> s1_aabb;
899  BVHModel<AABB> s2_aabb;
900  BVHModel<OBB> s1_obb;
901  BVHModel<OBB> s2_obb;
902 
903  generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16);
904  generateBVHModel(s2_aabb, s2, Transform3f(), 16, 16);
905  generateBVHModel(s1_obb, s1, Transform3f(), 16, 16);
906  generateBVHModel(s2_obb, s2, Transform3f(), 16, 16);
907 
908  CollisionRequest request(false, 1, false);
909  CollisionResult result;
910 
911  bool res;
912 
913  Transform3f pose, pose_aabb, pose_obb;
914 
915  // s2 is within s1
916  // both are shapes --> collision
917  // s1 is shape, s2 is mesh --> in collision
918  // s1 is mesh, s2 is shape --> collision free
919  // all are reasonable
920  result.clear();
921  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
922  BOOST_CHECK(res);
923  result.clear();
924  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
925  BOOST_CHECK(res);
926  result.clear();
927  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
928  BOOST_CHECK(res);
929  result.clear();
930  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
931  BOOST_CHECK(res);
932  result.clear();
933  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
934  BOOST_CHECK(res);
935  result.clear();
936  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
937  BOOST_CHECK_FALSE(res);
938  result.clear();
939  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
940  BOOST_CHECK_FALSE(res);
941  result.clear();
942  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
943  BOOST_CHECK_FALSE(res);
944  result.clear();
945  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
946  BOOST_CHECK_FALSE(res);
947 
948  pose.setTranslation(Vec3f(40, 0, 0));
949  pose_aabb.setTranslation(Vec3f(40, 0, 0));
950  pose_obb.setTranslation(Vec3f(40, 0, 0));
951 
952  result.clear();
953  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
954  BOOST_CHECK_FALSE(res);
955  result.clear();
956  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
957  BOOST_CHECK_FALSE(res);
958  result.clear();
959  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
960  BOOST_CHECK_FALSE(res);
961  result.clear();
962  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
963  BOOST_CHECK_FALSE(res);
964  result.clear();
965  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
966  BOOST_CHECK_FALSE(res);
967  result.clear();
968  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
969  BOOST_CHECK_FALSE(res);
970  result.clear();
971  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
972  BOOST_CHECK_FALSE(res);
973  result.clear();
974  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
975  BOOST_CHECK_FALSE(res);
976  result.clear();
977  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
978  BOOST_CHECK_FALSE(res);
979 
980  pose.setTranslation(Vec3f(30, 0, 0));
981  pose_aabb.setTranslation(Vec3f(30, 0, 0));
982  pose_obb.setTranslation(Vec3f(30, 0, 0));
983 
984  result.clear();
985  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
986  BOOST_CHECK(res);
987  result.clear();
988  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
989  BOOST_CHECK_FALSE(res);
990  result.clear();
991  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
992  BOOST_CHECK_FALSE(res);
993  result.clear();
994  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
995  BOOST_CHECK_FALSE(res);
996  result.clear();
997  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
998  BOOST_CHECK_FALSE(res);
999  result.clear();
1000  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
1001  BOOST_CHECK_FALSE(res);
1002  result.clear();
1003  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
1004  BOOST_CHECK_FALSE(res);
1005  result.clear();
1006  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1007  BOOST_CHECK_FALSE(res);
1008  result.clear();
1009  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1010  BOOST_CHECK_FALSE(res);
1011 
1012  pose.setTranslation(Vec3f(29.9, 0, 0));
1013  pose_aabb.setTranslation(
1014  Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
1015  pose_obb.setTranslation(
1016  Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
1017 
1018  result.clear();
1019  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
1020  BOOST_CHECK(res);
1021  result.clear();
1022  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
1023  BOOST_CHECK(res);
1024  result.clear();
1025  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
1026  BOOST_CHECK(res);
1027  result.clear();
1028  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
1029  BOOST_CHECK(res);
1030  result.clear();
1031  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
1032  BOOST_CHECK(res);
1033  result.clear();
1034  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
1035  BOOST_CHECK(res);
1036  result.clear();
1037  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
1038  BOOST_CHECK(res);
1039  result.clear();
1040  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1041  BOOST_CHECK(res);
1042  result.clear();
1043  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1044  BOOST_CHECK(res);
1045 
1046  pose.setTranslation(Vec3f(-29.9, 0, 0));
1047  pose_aabb.setTranslation(
1048  Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
1049  pose_obb.setTranslation(
1050  Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
1051 
1052  result.clear();
1053  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
1054  BOOST_CHECK(res);
1055  result.clear();
1056  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
1057  BOOST_CHECK(res);
1058  result.clear();
1059  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
1060  BOOST_CHECK(res);
1061  result.clear();
1062  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
1063  BOOST_CHECK(res);
1064  result.clear();
1065  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
1066  BOOST_CHECK(res);
1067  result.clear();
1068  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
1069  BOOST_CHECK(res);
1070  result.clear();
1071  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
1072  BOOST_CHECK(res);
1073  result.clear();
1074  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1075  BOOST_CHECK(res);
1076  result.clear();
1077  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1078  BOOST_CHECK(res);
1079 
1080  pose.setTranslation(Vec3f(-30, 0, 0));
1081  pose_aabb.setTranslation(Vec3f(-30, 0, 0));
1082  pose_obb.setTranslation(Vec3f(-30, 0, 0));
1083 
1084  result.clear();
1085  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
1086  BOOST_CHECK(res);
1087  result.clear();
1088  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
1089  BOOST_CHECK_FALSE(res);
1090  result.clear();
1091  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
1092  BOOST_CHECK_FALSE(res);
1093  result.clear();
1094  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
1095  BOOST_CHECK_FALSE(res);
1096  result.clear();
1097  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
1098  BOOST_CHECK_FALSE(res);
1099  result.clear();
1100  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
1101  BOOST_CHECK_FALSE(res);
1102  result.clear();
1103  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
1104  BOOST_CHECK_FALSE(res);
1105  result.clear();
1106  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1107  BOOST_CHECK_FALSE(res);
1108  result.clear();
1109  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1110  BOOST_CHECK_FALSE(res);
1111 }
1112 
1113 BOOST_AUTO_TEST_CASE(consistency_collision_boxbox) {
1114  Box s1(20, 40, 50);
1115  Box s2(10, 10, 10);
1116 
1117  BVHModel<AABB> s1_aabb;
1118  BVHModel<AABB> s2_aabb;
1119  BVHModel<OBB> s1_obb;
1120  BVHModel<OBB> s2_obb;
1121 
1122  generateBVHModel(s1_aabb, s1, Transform3f());
1123  generateBVHModel(s2_aabb, s2, Transform3f());
1124  generateBVHModel(s1_obb, s1, Transform3f());
1125  generateBVHModel(s2_obb, s2, Transform3f());
1126 
1127  CollisionRequest request(false, 1, false);
1128  CollisionResult result;
1129 
1130  bool res;
1131 
1132  Transform3f pose, pose_aabb, pose_obb;
1133 
1134  // s2 is within s1
1135  // both are shapes --> collision
1136  // s1 is shape, s2 is mesh --> in collision
1137  // s1 is mesh, s2 is shape --> collision free
1138  // all are reasonable
1139  result.clear();
1140  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
1141  BOOST_CHECK(res);
1142  result.clear();
1143  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
1144  BOOST_CHECK(res);
1145  result.clear();
1146  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
1147  BOOST_CHECK(res);
1148  result.clear();
1149  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
1150  BOOST_CHECK(res);
1151  result.clear();
1152  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
1153  BOOST_CHECK(res);
1154  result.clear();
1155  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
1156  BOOST_CHECK_FALSE(res);
1157  result.clear();
1158  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
1159  BOOST_CHECK_FALSE(res);
1160  result.clear();
1161  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1162  BOOST_CHECK_FALSE(res);
1163  result.clear();
1164  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1165  BOOST_CHECK_FALSE(res);
1166 
1167  pose.setTranslation(Vec3f(15.01, 0, 0));
1168  pose_aabb.setTranslation(Vec3f(15.01, 0, 0));
1169  pose_obb.setTranslation(Vec3f(15.01, 0, 0));
1170 
1171  result.clear();
1172  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
1173  BOOST_CHECK_FALSE(res);
1174  result.clear();
1175  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
1176  BOOST_CHECK_FALSE(res);
1177  result.clear();
1178  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
1179  BOOST_CHECK_FALSE(res);
1180  result.clear();
1181  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
1182  BOOST_CHECK_FALSE(res);
1183  result.clear();
1184  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
1185  BOOST_CHECK_FALSE(res);
1186  result.clear();
1187  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
1188  BOOST_CHECK_FALSE(res);
1189  result.clear();
1190  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
1191  BOOST_CHECK_FALSE(res);
1192  result.clear();
1193  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1194  BOOST_CHECK_FALSE(res);
1195  result.clear();
1196  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1197  BOOST_CHECK_FALSE(res);
1198 
1199  pose.setTranslation(Vec3f(14.99, 0, 0));
1200  pose_aabb.setTranslation(Vec3f(14.99, 0, 0));
1201  pose_obb.setTranslation(Vec3f(14.99, 0, 0));
1202 
1203  result.clear();
1204  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
1205  BOOST_CHECK(res);
1206  result.clear();
1207  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
1208  BOOST_CHECK(res);
1209  result.clear();
1210  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
1211  BOOST_CHECK(res);
1212  result.clear();
1213  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
1214  BOOST_CHECK(res);
1215  result.clear();
1216  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
1217  BOOST_CHECK(res);
1218  result.clear();
1219  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
1220  BOOST_CHECK(res);
1221  result.clear();
1222  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
1223  BOOST_CHECK(res);
1224  result.clear();
1225  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1226  BOOST_CHECK(res);
1227  result.clear();
1228  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1229  BOOST_CHECK(res);
1230 }
1231 
1232 BOOST_AUTO_TEST_CASE(consistency_collision_spherebox) {
1233  Sphere s1(20);
1234  Box s2(5, 5, 5);
1235 
1236  BVHModel<AABB> s1_aabb;
1237  BVHModel<AABB> s2_aabb;
1238  BVHModel<OBB> s1_obb;
1239  BVHModel<OBB> s2_obb;
1240 
1241  generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16);
1242  generateBVHModel(s2_aabb, s2, Transform3f());
1243  generateBVHModel(s1_obb, s1, Transform3f(), 16, 16);
1244  generateBVHModel(s2_obb, s2, Transform3f());
1245 
1246  CollisionRequest request(false, 1, false);
1247  CollisionResult result;
1248 
1249  bool res;
1250 
1251  Transform3f pose, pose_aabb, pose_obb;
1252 
1253  // s2 is within s1
1254  // both are shapes --> collision
1255  // s1 is shape, s2 is mesh --> in collision
1256  // s1 is mesh, s2 is shape --> collision free
1257  // all are reasonable
1258  result.clear();
1259  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
1260  BOOST_CHECK(res);
1261  result.clear();
1262  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
1263  BOOST_CHECK(res);
1264  result.clear();
1265  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
1266  BOOST_CHECK(res);
1267  result.clear();
1268  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
1269  BOOST_CHECK(res);
1270  result.clear();
1271  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
1272  BOOST_CHECK(res);
1273  result.clear();
1274  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
1275  BOOST_CHECK_FALSE(res);
1276  result.clear();
1277  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
1278  BOOST_CHECK_FALSE(res);
1279  result.clear();
1280  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1281  BOOST_CHECK_FALSE(res);
1282  result.clear();
1283  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1284  BOOST_CHECK_FALSE(res);
1285 
1286  pose.setTranslation(Vec3f(22.4, 0, 0));
1287  pose_aabb.setTranslation(Vec3f(22.4, 0, 0));
1288  pose_obb.setTranslation(Vec3f(22.4, 0, 0));
1289 
1290  result.clear();
1291  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
1292  BOOST_CHECK(res);
1293  result.clear();
1294  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
1295  BOOST_CHECK(res);
1296  result.clear();
1297  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
1298  BOOST_CHECK(res);
1299  result.clear();
1300  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
1301  BOOST_CHECK(res);
1302  result.clear();
1303  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
1304  BOOST_CHECK(res);
1305  result.clear();
1306  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
1307  BOOST_CHECK(res);
1308  result.clear();
1309  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
1310  BOOST_CHECK(res);
1311  result.clear();
1312  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1313  BOOST_CHECK(res);
1314  result.clear();
1315  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1316  BOOST_CHECK(res);
1317 
1318  pose.setTranslation(Vec3f(22.51, 0, 0));
1319  pose_aabb.setTranslation(Vec3f(22.51, 0, 0));
1320  pose_obb.setTranslation(Vec3f(22.51, 0, 0));
1321 
1322  result.clear();
1323  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
1324  BOOST_CHECK_FALSE(res);
1325  result.clear();
1326  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
1327  BOOST_CHECK_FALSE(res);
1328  result.clear();
1329  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
1330  BOOST_CHECK_FALSE(res);
1331  result.clear();
1332  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
1333  BOOST_CHECK_FALSE(res);
1334  result.clear();
1335  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
1336  BOOST_CHECK_FALSE(res);
1337  result.clear();
1338  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
1339  BOOST_CHECK_FALSE(res);
1340  result.clear();
1341  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
1342  BOOST_CHECK_FALSE(res);
1343  result.clear();
1344  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1345  BOOST_CHECK_FALSE(res);
1346  result.clear();
1347  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1348  BOOST_CHECK_FALSE(res);
1349 }
1350 
1351 BOOST_AUTO_TEST_CASE(consistency_collision_cylindercylinder) {
1352  Cylinder s1(5, 10);
1353  Cylinder s2(5, 10);
1354 
1355  BVHModel<AABB> s1_aabb;
1356  BVHModel<AABB> s2_aabb;
1357  BVHModel<OBB> s1_obb;
1358  BVHModel<OBB> s2_obb;
1359 
1360  generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16);
1361  generateBVHModel(s2_aabb, s2, Transform3f(), 16, 16);
1362  generateBVHModel(s1_obb, s1, Transform3f(), 16, 16);
1363  generateBVHModel(s2_obb, s2, Transform3f(), 16, 16);
1364 
1365  CollisionRequest request(false, 1, false);
1366  CollisionResult result;
1367 
1368  bool res;
1369 
1370  Transform3f pose, pose_aabb, pose_obb;
1371 
1372  pose.setTranslation(Vec3f(9.99, 0, 0));
1373  pose_aabb.setTranslation(Vec3f(9.99, 0, 0));
1374  pose_obb.setTranslation(Vec3f(9.99, 0, 0));
1375 
1376  result.clear();
1377  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
1378  BOOST_CHECK(res);
1379  result.clear();
1380  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
1381  BOOST_CHECK(res);
1382  result.clear();
1383  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
1384  BOOST_CHECK(res);
1385  result.clear();
1386  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
1387  BOOST_CHECK(res);
1388  result.clear();
1389  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
1390  BOOST_CHECK(res);
1391  result.clear();
1392  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
1393  BOOST_CHECK(res);
1394  result.clear();
1395  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
1396  BOOST_CHECK(res);
1397  result.clear();
1398  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1399  BOOST_CHECK(res);
1400  result.clear();
1401  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1402  BOOST_CHECK(res);
1403 
1404  pose.setTranslation(Vec3f(10.01, 0, 0));
1405  pose_aabb.setTranslation(Vec3f(10.01, 0, 0));
1406  pose_obb.setTranslation(Vec3f(10.01, 0, 0));
1407 
1408  result.clear();
1409  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
1410  BOOST_CHECK_FALSE(res);
1411  result.clear();
1412  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
1413  BOOST_CHECK_FALSE(res);
1414  result.clear();
1415  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
1416  BOOST_CHECK_FALSE(res);
1417  result.clear();
1418  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
1419  BOOST_CHECK_FALSE(res);
1420  result.clear();
1421  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
1422  BOOST_CHECK_FALSE(res);
1423  result.clear();
1424  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
1425  BOOST_CHECK_FALSE(res);
1426  result.clear();
1427  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
1428  BOOST_CHECK_FALSE(res);
1429  result.clear();
1430  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1431  BOOST_CHECK_FALSE(res);
1432  result.clear();
1433  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1434  BOOST_CHECK_FALSE(res);
1435 }
1436 
1437 BOOST_AUTO_TEST_CASE(consistency_collision_conecone) {
1438  Cone s1(5, 10);
1439  Cone s2(5, 10);
1440 
1441  BVHModel<AABB> s1_aabb;
1442  BVHModel<AABB> s2_aabb;
1443  BVHModel<OBB> s1_obb;
1444  BVHModel<OBB> s2_obb;
1445 
1446  generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16);
1447  generateBVHModel(s2_aabb, s2, Transform3f(), 16, 16);
1448  generateBVHModel(s1_obb, s1, Transform3f(), 16, 16);
1449  generateBVHModel(s2_obb, s2, Transform3f(), 16, 16);
1450 
1451  CollisionRequest request(false, 1, false);
1452  CollisionResult result;
1453 
1454  bool res;
1455 
1456  Transform3f pose, pose_aabb, pose_obb;
1457 
1458  pose.setTranslation(Vec3f(9.9, 0, 0));
1459  pose_aabb.setTranslation(Vec3f(9.9, 0, 0));
1460  pose_obb.setTranslation(Vec3f(9.9, 0, 0));
1461 
1462  result.clear();
1463  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
1464  BOOST_CHECK(res);
1465  result.clear();
1466  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
1467  BOOST_CHECK(res);
1468  result.clear();
1469  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
1470  BOOST_CHECK(res);
1471  result.clear();
1472  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
1473  BOOST_CHECK(res);
1474  result.clear();
1475  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
1476  BOOST_CHECK(res);
1477  result.clear();
1478  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
1479  BOOST_CHECK(res);
1480  result.clear();
1481  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
1482  BOOST_CHECK(res);
1483  result.clear();
1484  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1485  BOOST_CHECK(res);
1486  result.clear();
1487  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1488  BOOST_CHECK(res);
1489 
1490  pose.setTranslation(Vec3f(10.1, 0, 0));
1491  pose_aabb.setTranslation(Vec3f(10.1, 0, 0));
1492  pose_obb.setTranslation(Vec3f(10.1, 0, 0));
1493 
1494  result.clear();
1495  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
1496  BOOST_CHECK_FALSE(res);
1497  result.clear();
1498  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
1499  BOOST_CHECK_FALSE(res);
1500  result.clear();
1501  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
1502  BOOST_CHECK_FALSE(res);
1503  result.clear();
1504  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
1505  BOOST_CHECK_FALSE(res);
1506  result.clear();
1507  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
1508  BOOST_CHECK_FALSE(res);
1509  result.clear();
1510  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
1511  BOOST_CHECK_FALSE(res);
1512  result.clear();
1513  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
1514  BOOST_CHECK_FALSE(res);
1515  result.clear();
1516  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1517  BOOST_CHECK_FALSE(res);
1518  result.clear();
1519  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1520  BOOST_CHECK_FALSE(res);
1521 
1522  pose.setTranslation(Vec3f(0, 0, 9.9));
1523  pose_aabb.setTranslation(Vec3f(0, 0, 9.9));
1524  pose_obb.setTranslation(Vec3f(0, 0, 9.9));
1525 
1526  result.clear();
1527  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
1528  BOOST_CHECK(res);
1529  result.clear();
1530  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
1531  BOOST_CHECK(res);
1532  result.clear();
1533  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
1534  BOOST_CHECK(res);
1535  result.clear();
1536  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
1537  BOOST_CHECK(res);
1538  result.clear();
1539  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
1540  BOOST_CHECK(res);
1541  result.clear();
1542  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
1543  BOOST_CHECK(res);
1544  result.clear();
1545  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
1546  BOOST_CHECK(res);
1547  result.clear();
1548  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1549  BOOST_CHECK(res);
1550  result.clear();
1551  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1552  BOOST_CHECK(res);
1553 
1554  pose.setTranslation(Vec3f(0, 0, 10.1));
1555  pose_aabb.setTranslation(Vec3f(0, 0, 10.1));
1556  pose_obb.setTranslation(Vec3f(0, 0, 10.1));
1557 
1558  result.clear();
1559  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
1560  BOOST_CHECK_FALSE(res);
1561  result.clear();
1562  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
1563  BOOST_CHECK_FALSE(res);
1564  result.clear();
1565  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
1566  BOOST_CHECK_FALSE(res);
1567  result.clear();
1568  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
1569  BOOST_CHECK_FALSE(res);
1570  result.clear();
1571  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
1572  BOOST_CHECK_FALSE(res);
1573  result.clear();
1574  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
1575  BOOST_CHECK_FALSE(res);
1576  result.clear();
1577  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
1578  BOOST_CHECK_FALSE(res);
1579  result.clear();
1580  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1581  BOOST_CHECK_FALSE(res);
1582  result.clear();
1583  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1584  BOOST_CHECK_FALSE(res);
1585 }
1586 
1587 BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK) {
1588  Sphere s1(20);
1589  Sphere s2(10);
1590  BVHModel<AABB> s1_aabb;
1591  BVHModel<AABB> s2_aabb;
1592  BVHModel<OBB> s1_obb;
1593  BVHModel<OBB> s2_obb;
1594 
1595  generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16);
1596  generateBVHModel(s2_aabb, s2, Transform3f(), 16, 16);
1597  generateBVHModel(s1_obb, s1, Transform3f(), 16, 16);
1598  generateBVHModel(s2_obb, s2, Transform3f(), 16, 16);
1599 
1600  CollisionRequest request(false, 1, false);
1601 
1602  CollisionResult result;
1603 
1604  bool res;
1605 
1606  Transform3f pose, pose_aabb, pose_obb;
1607 
1608  // s2 is within s1
1609  // both are shapes --> collision
1610  // s1 is shape, s2 is mesh --> in collision
1611  // s1 is mesh, s2 is shape --> collision free
1612  // all are reasonable
1613  result.clear();
1614  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
1615  BOOST_CHECK(res);
1616  result.clear();
1617  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
1618  BOOST_CHECK(res);
1619  result.clear();
1620  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
1621  BOOST_CHECK(res);
1622  result.clear();
1623  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
1624  BOOST_CHECK(res);
1625  result.clear();
1626  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
1627  BOOST_CHECK(res);
1628  result.clear();
1629  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
1630  BOOST_CHECK_FALSE(res);
1631  result.clear();
1632  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
1633  BOOST_CHECK_FALSE(res);
1634  result.clear();
1635  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1636  BOOST_CHECK_FALSE(res);
1637  result.clear();
1638  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1639  BOOST_CHECK_FALSE(res);
1640 
1641  pose.setTranslation(Vec3f(40, 0, 0));
1642  pose_aabb.setTranslation(Vec3f(40, 0, 0));
1643  pose_obb.setTranslation(Vec3f(40, 0, 0));
1644 
1645  result.clear();
1646  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
1647  BOOST_CHECK_FALSE(res);
1648  result.clear();
1649  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
1650  BOOST_CHECK_FALSE(res);
1651  result.clear();
1652  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
1653  BOOST_CHECK_FALSE(res);
1654  result.clear();
1655  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
1656  BOOST_CHECK_FALSE(res);
1657  result.clear();
1658  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
1659  BOOST_CHECK_FALSE(res);
1660  result.clear();
1661  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
1662  BOOST_CHECK_FALSE(res);
1663  result.clear();
1664  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
1665  BOOST_CHECK_FALSE(res);
1666  result.clear();
1667  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1668  BOOST_CHECK_FALSE(res);
1669  result.clear();
1670  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1671  BOOST_CHECK_FALSE(res);
1672 
1673  pose.setTranslation(Vec3f(30, 0, 0));
1674  pose_aabb.setTranslation(Vec3f(30, 0, 0));
1675  pose_obb.setTranslation(Vec3f(30, 0, 0));
1676 
1677  result.clear();
1678  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
1679  BOOST_CHECK(res);
1680  result.clear();
1681  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
1682  BOOST_CHECK_FALSE(res);
1683  result.clear();
1684  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
1685  BOOST_CHECK_FALSE(res);
1686  result.clear();
1687  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
1688  BOOST_CHECK_FALSE(res);
1689  result.clear();
1690  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
1691  BOOST_CHECK_FALSE(res);
1692  result.clear();
1693  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
1694  BOOST_CHECK_FALSE(res);
1695  result.clear();
1696  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
1697  BOOST_CHECK_FALSE(res);
1698  result.clear();
1699  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1700  BOOST_CHECK_FALSE(res);
1701  result.clear();
1702  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1703  BOOST_CHECK_FALSE(res);
1704 
1705  pose.setTranslation(Vec3f(29.9, 0, 0));
1706  pose_aabb.setTranslation(
1707  Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
1708  pose_obb.setTranslation(
1709  Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
1710 
1711  result.clear();
1712  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
1713  BOOST_CHECK(res);
1714  result.clear();
1715  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
1716  BOOST_CHECK(res);
1717  result.clear();
1718  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
1719  BOOST_CHECK(res);
1720  result.clear();
1721  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
1722  BOOST_CHECK(res);
1723  result.clear();
1724  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
1725  BOOST_CHECK(res);
1726  result.clear();
1727  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
1728  BOOST_CHECK(res);
1729  result.clear();
1730  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
1731  BOOST_CHECK(res);
1732  result.clear();
1733  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1734  BOOST_CHECK(res);
1735  result.clear();
1736  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1737  BOOST_CHECK(res);
1738 
1739  pose.setTranslation(Vec3f(-29.9, 0, 0));
1740  pose_aabb.setTranslation(
1741  Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
1742  pose_obb.setTranslation(
1743  Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
1744 
1745  result.clear();
1746  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
1747  BOOST_CHECK(res);
1748  result.clear();
1749  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
1750  BOOST_CHECK(res);
1751  result.clear();
1752  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
1753  BOOST_CHECK(res);
1754  result.clear();
1755  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
1756  BOOST_CHECK(res);
1757  result.clear();
1758  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
1759  BOOST_CHECK(res);
1760  result.clear();
1761  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
1762  BOOST_CHECK(res);
1763  result.clear();
1764  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
1765  BOOST_CHECK(res);
1766  result.clear();
1767  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1768  BOOST_CHECK(res);
1769  result.clear();
1770  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1771  BOOST_CHECK(res);
1772 
1773  pose.setTranslation(Vec3f(-30, 0, 0));
1774  pose_aabb.setTranslation(Vec3f(-30, 0, 0));
1775  pose_obb.setTranslation(Vec3f(-30, 0, 0));
1776 
1777  result.clear();
1778  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
1779  BOOST_CHECK(res);
1780  result.clear();
1781  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
1782  BOOST_CHECK_FALSE(res);
1783  result.clear();
1784  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
1785  BOOST_CHECK_FALSE(res);
1786  result.clear();
1787  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
1788  BOOST_CHECK_FALSE(res);
1789  result.clear();
1790  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
1791  BOOST_CHECK_FALSE(res);
1792  result.clear();
1793  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
1794  BOOST_CHECK_FALSE(res);
1795  result.clear();
1796  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
1797  BOOST_CHECK_FALSE(res);
1798  result.clear();
1799  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1800  BOOST_CHECK_FALSE(res);
1801  result.clear();
1802  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1803  BOOST_CHECK_FALSE(res);
1804 }
1805 
1806 BOOST_AUTO_TEST_CASE(consistency_collision_boxbox_GJK) {
1807  Box s1(20, 40, 50);
1808  Box s2(10, 10, 10);
1809 
1810  BVHModel<AABB> s1_aabb;
1811  BVHModel<AABB> s2_aabb;
1812  BVHModel<OBB> s1_obb;
1813  BVHModel<OBB> s2_obb;
1814 
1815  generateBVHModel(s1_aabb, s1, Transform3f());
1816  generateBVHModel(s2_aabb, s2, Transform3f());
1817  generateBVHModel(s1_obb, s1, Transform3f());
1818  generateBVHModel(s2_obb, s2, Transform3f());
1819 
1820  CollisionRequest request(false, 1, false);
1821 
1822  CollisionResult result;
1823 
1824  bool res;
1825 
1826  Transform3f pose, pose_aabb, pose_obb;
1827 
1828  // s2 is within s1
1829  // both are shapes --> collision
1830  // s1 is shape, s2 is mesh --> in collision
1831  // s1 is mesh, s2 is shape --> collision free
1832  // all are reasonable
1833  result.clear();
1834  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
1835  BOOST_CHECK(res);
1836  result.clear();
1837  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
1838  BOOST_CHECK(res);
1839  result.clear();
1840  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
1841  BOOST_CHECK(res);
1842  result.clear();
1843  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
1844  BOOST_CHECK(res);
1845  result.clear();
1846  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
1847  BOOST_CHECK(res);
1848  result.clear();
1849  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
1850  BOOST_CHECK_FALSE(res);
1851  result.clear();
1852  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
1853  BOOST_CHECK_FALSE(res);
1854  result.clear();
1855  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1856  BOOST_CHECK_FALSE(res);
1857  result.clear();
1858  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1859  BOOST_CHECK_FALSE(res);
1860 
1861  pose.setTranslation(Vec3f(15.01, 0, 0));
1862  pose_aabb.setTranslation(Vec3f(15.01, 0, 0));
1863  pose_obb.setTranslation(Vec3f(15.01, 0, 0));
1864 
1865  result.clear();
1866  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
1867  BOOST_CHECK_FALSE(res);
1868  result.clear();
1869  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
1870  BOOST_CHECK_FALSE(res);
1871  result.clear();
1872  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
1873  BOOST_CHECK_FALSE(res);
1874  result.clear();
1875  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
1876  BOOST_CHECK_FALSE(res);
1877  result.clear();
1878  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
1879  BOOST_CHECK_FALSE(res);
1880  result.clear();
1881  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
1882  BOOST_CHECK_FALSE(res);
1883  result.clear();
1884  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
1885  BOOST_CHECK_FALSE(res);
1886  result.clear();
1887  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1888  BOOST_CHECK_FALSE(res);
1889  result.clear();
1890  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1891  BOOST_CHECK_FALSE(res);
1892 
1893  pose.setTranslation(Vec3f(14.99, 0, 0));
1894  pose_aabb.setTranslation(Vec3f(14.99, 0, 0));
1895  pose_obb.setTranslation(Vec3f(14.99, 0, 0));
1896 
1897  result.clear();
1898  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
1899  BOOST_CHECK(res);
1900  result.clear();
1901  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
1902  BOOST_CHECK(res);
1903  result.clear();
1904  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
1905  BOOST_CHECK(res);
1906  result.clear();
1907  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
1908  BOOST_CHECK(res);
1909  result.clear();
1910  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
1911  BOOST_CHECK(res);
1912  result.clear();
1913  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
1914  BOOST_CHECK(res);
1915  result.clear();
1916  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
1917  BOOST_CHECK(res);
1918  result.clear();
1919  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1920  BOOST_CHECK(res);
1921  result.clear();
1922  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1923  BOOST_CHECK(res);
1924 }
1925 
1926 BOOST_AUTO_TEST_CASE(consistency_collision_spherebox_GJK) {
1927  Sphere s1(20);
1928  Box s2(5, 5, 5);
1929 
1930  BVHModel<AABB> s1_aabb;
1931  BVHModel<AABB> s2_aabb;
1932  BVHModel<OBB> s1_obb;
1933  BVHModel<OBB> s2_obb;
1934 
1935  generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16);
1936  generateBVHModel(s2_aabb, s2, Transform3f());
1937  generateBVHModel(s1_obb, s1, Transform3f(), 16, 16);
1938  generateBVHModel(s2_obb, s2, Transform3f());
1939 
1940  CollisionRequest request(false, 1, false);
1941 
1942  CollisionResult result;
1943 
1944  bool res;
1945 
1946  Transform3f pose, pose_aabb, pose_obb;
1947 
1948  // s2 is within s1
1949  // both are shapes --> collision
1950  // s1 is shape, s2 is mesh --> in collision
1951  // s1 is mesh, s2 is shape --> collision free
1952  // all are reasonable
1953  result.clear();
1954  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
1955  BOOST_CHECK(res);
1956  result.clear();
1957  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
1958  BOOST_CHECK(res);
1959  result.clear();
1960  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
1961  BOOST_CHECK(res);
1962  result.clear();
1963  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
1964  BOOST_CHECK(res);
1965  result.clear();
1966  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
1967  BOOST_CHECK(res);
1968  result.clear();
1969  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
1970  BOOST_CHECK_FALSE(res);
1971  result.clear();
1972  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
1973  BOOST_CHECK_FALSE(res);
1974  result.clear();
1975  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1976  BOOST_CHECK_FALSE(res);
1977  result.clear();
1978  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
1979  BOOST_CHECK_FALSE(res);
1980 
1981  pose.setTranslation(Vec3f(22.4, 0, 0));
1982  pose_aabb.setTranslation(Vec3f(22.4, 0, 0));
1983  pose_obb.setTranslation(Vec3f(22.4, 0, 0));
1984 
1985  result.clear();
1986  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
1987  BOOST_CHECK(res);
1988  result.clear();
1989  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
1990  BOOST_CHECK(res);
1991  result.clear();
1992  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
1993  BOOST_CHECK(res);
1994  result.clear();
1995  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
1996  BOOST_CHECK(res);
1997  result.clear();
1998  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
1999  BOOST_CHECK(res);
2000  result.clear();
2001  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
2002  BOOST_CHECK(res);
2003  result.clear();
2004  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
2005  BOOST_CHECK(res);
2006  result.clear();
2007  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
2008  BOOST_CHECK(res);
2009  result.clear();
2010  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
2011  BOOST_CHECK(res);
2012 
2013  pose.setTranslation(Vec3f(22.51, 0, 0));
2014  pose_aabb.setTranslation(Vec3f(22.51, 0, 0));
2015  pose_obb.setTranslation(Vec3f(22.51, 0, 0));
2016 
2017  result.clear();
2018  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
2019  BOOST_CHECK_FALSE(res);
2020  result.clear();
2021  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
2022  BOOST_CHECK_FALSE(res);
2023  result.clear();
2024  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
2025  BOOST_CHECK_FALSE(res);
2026  result.clear();
2027  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
2028  BOOST_CHECK_FALSE(res);
2029  result.clear();
2030  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
2031  BOOST_CHECK_FALSE(res);
2032  result.clear();
2033  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
2034  BOOST_CHECK_FALSE(res);
2035  result.clear();
2036  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
2037  BOOST_CHECK_FALSE(res);
2038  result.clear();
2039  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
2040  BOOST_CHECK_FALSE(res);
2041  result.clear();
2042  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
2043  BOOST_CHECK_FALSE(res);
2044 }
2045 
2046 BOOST_AUTO_TEST_CASE(consistency_collision_cylindercylinder_GJK) {
2047  Cylinder s1(5, 10);
2048  Cylinder s2(5, 10);
2049 
2050  BVHModel<AABB> s1_aabb;
2051  BVHModel<AABB> s2_aabb;
2052  BVHModel<OBB> s1_obb;
2053  BVHModel<OBB> s2_obb;
2054 
2055  generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16);
2056  generateBVHModel(s2_aabb, s2, Transform3f(), 16, 16);
2057  generateBVHModel(s1_obb, s1, Transform3f(), 16, 16);
2058  generateBVHModel(s2_obb, s2, Transform3f(), 16, 16);
2059 
2060  CollisionRequest request(false, 1, false);
2061 
2062  CollisionResult result;
2063 
2064  bool res;
2065 
2066  Transform3f pose, pose_aabb, pose_obb;
2067 
2068  pose.setTranslation(Vec3f(9.99, 0, 0));
2069  pose_aabb.setTranslation(Vec3f(9.99, 0, 0));
2070  pose_obb.setTranslation(Vec3f(9.99, 0, 0));
2071 
2072  result.clear();
2073  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
2074  BOOST_CHECK(res);
2075  result.clear();
2076  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
2077  BOOST_CHECK(res);
2078  result.clear();
2079  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
2080  BOOST_CHECK(res);
2081  result.clear();
2082  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
2083  BOOST_CHECK(res);
2084  result.clear();
2085  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
2086  BOOST_CHECK(res);
2087  result.clear();
2088  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
2089  BOOST_CHECK(res);
2090  result.clear();
2091  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
2092  BOOST_CHECK(res);
2093  result.clear();
2094  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
2095  BOOST_CHECK(res);
2096  result.clear();
2097  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
2098  BOOST_CHECK(res);
2099 
2100  pose.setTranslation(Vec3f(10.01, 0, 0));
2101  pose_aabb.setTranslation(Vec3f(10.01, 0, 0));
2102  pose_obb.setTranslation(Vec3f(10.01, 0, 0));
2103 
2104  result.clear();
2105  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
2106  BOOST_CHECK_FALSE(res);
2107  result.clear();
2108  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
2109  BOOST_CHECK_FALSE(res);
2110  result.clear();
2111  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
2112  BOOST_CHECK_FALSE(res);
2113  result.clear();
2114  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
2115  BOOST_CHECK_FALSE(res);
2116  result.clear();
2117  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
2118  BOOST_CHECK_FALSE(res);
2119  result.clear();
2120  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
2121  BOOST_CHECK_FALSE(res);
2122  result.clear();
2123  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
2124  BOOST_CHECK_FALSE(res);
2125  result.clear();
2126  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
2127  BOOST_CHECK_FALSE(res);
2128  result.clear();
2129  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
2130  BOOST_CHECK_FALSE(res);
2131 }
2132 
2133 BOOST_AUTO_TEST_CASE(consistency_collision_conecone_GJK) {
2134  Cone s1(5, 10);
2135  Cone s2(5, 10);
2136 
2137  BVHModel<AABB> s1_aabb;
2138  BVHModel<AABB> s2_aabb;
2139  BVHModel<OBB> s1_obb;
2140  BVHModel<OBB> s2_obb;
2141 
2142  generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16);
2143  generateBVHModel(s2_aabb, s2, Transform3f(), 16, 16);
2144  generateBVHModel(s1_obb, s1, Transform3f(), 16, 16);
2145  generateBVHModel(s2_obb, s2, Transform3f(), 16, 16);
2146 
2147  CollisionRequest request(false, 1, false);
2148 
2149  CollisionResult result;
2150 
2151  bool res;
2152 
2153  Transform3f pose, pose_aabb, pose_obb;
2154 
2155  pose.setTranslation(Vec3f(9.9, 0, 0));
2156  pose_aabb.setTranslation(Vec3f(9.9, 0, 0));
2157  pose_obb.setTranslation(Vec3f(9.9, 0, 0));
2158 
2159  result.clear();
2160  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
2161  BOOST_CHECK(res);
2162  result.clear();
2163  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
2164  BOOST_CHECK(res);
2165  result.clear();
2166  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
2167  BOOST_CHECK(res);
2168  result.clear();
2169  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
2170  BOOST_CHECK(res);
2171  result.clear();
2172  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
2173  BOOST_CHECK(res);
2174  result.clear();
2175  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
2176  BOOST_CHECK(res);
2177  result.clear();
2178  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
2179  BOOST_CHECK(res);
2180  result.clear();
2181  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
2182  BOOST_CHECK(res);
2183  result.clear();
2184  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
2185  BOOST_CHECK(res);
2186 
2187  pose.setTranslation(Vec3f(10.1, 0, 0));
2188  pose_aabb.setTranslation(Vec3f(10.1, 0, 0));
2189  pose_obb.setTranslation(Vec3f(10.1, 0, 0));
2190 
2191  result.clear();
2192  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
2193  BOOST_CHECK_FALSE(res);
2194  result.clear();
2195  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
2196  BOOST_CHECK_FALSE(res);
2197  result.clear();
2198  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
2199  BOOST_CHECK_FALSE(res);
2200  result.clear();
2201  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
2202  BOOST_CHECK_FALSE(res);
2203  result.clear();
2204  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
2205  BOOST_CHECK_FALSE(res);
2206  result.clear();
2207  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
2208  BOOST_CHECK_FALSE(res);
2209  result.clear();
2210  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
2211  BOOST_CHECK_FALSE(res);
2212  result.clear();
2213  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
2214  BOOST_CHECK_FALSE(res);
2215  result.clear();
2216  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
2217  BOOST_CHECK_FALSE(res);
2218 
2219  pose.setTranslation(Vec3f(0, 0, 9.9));
2220  pose_aabb.setTranslation(Vec3f(0, 0, 9.9));
2221  pose_obb.setTranslation(Vec3f(0, 0, 9.9));
2222 
2223  result.clear();
2224  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
2225  BOOST_CHECK(res);
2226  result.clear();
2227  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
2228  BOOST_CHECK(res);
2229  result.clear();
2230  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
2231  BOOST_CHECK(res);
2232  result.clear();
2233  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
2234  BOOST_CHECK(res);
2235  result.clear();
2236  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
2237  BOOST_CHECK(res);
2238  result.clear();
2239  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
2240  BOOST_CHECK(res);
2241  result.clear();
2242  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
2243  BOOST_CHECK(res);
2244  result.clear();
2245  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
2246  BOOST_CHECK(res);
2247  result.clear();
2248  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
2249  BOOST_CHECK(res);
2250 
2251  pose.setTranslation(Vec3f(0, 0, 10.1));
2252  pose_aabb.setTranslation(Vec3f(0, 0, 10.1));
2253  pose_obb.setTranslation(Vec3f(0, 0, 10.1));
2254 
2255  result.clear();
2256  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
2257  BOOST_CHECK_FALSE(res);
2258  result.clear();
2259  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
2260  BOOST_CHECK_FALSE(res);
2261  result.clear();
2262  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
2263  BOOST_CHECK_FALSE(res);
2264  result.clear();
2265  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
2266  BOOST_CHECK_FALSE(res);
2267  result.clear();
2268  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
2269  BOOST_CHECK_FALSE(res);
2270  result.clear();
2271  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
2272  BOOST_CHECK_FALSE(res);
2273  result.clear();
2274  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
2275  BOOST_CHECK_FALSE(res);
2276  result.clear();
2277  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
2278  BOOST_CHECK_FALSE(res);
2279  result.clear();
2280  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
2281  BOOST_CHECK_FALSE(res);
2282 }
hpp::fcl::Transform3f
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
hpp::fcl::DistanceRequest
request to the distance computation
Definition: collision_data.h:392
hpp::fcl::Transform3f::setTranslation
void setTranslation(const Eigen::MatrixBase< Derived > &T_)
set transform from translation
Definition: transform.h:142
hpp::fcl::Cylinder
Cylinder along Z axis. The cylinder is defined at its centroid.
Definition: shape/geometric_shapes.h:501
t
t
hpp::fcl::DistanceResult::clear
void clear()
clear the result
Definition: collision_data.h:504
hpp::fcl::CollisionRequest
request to the collision algorithm
Definition: collision_data.h:235
hpp::fcl::CollisionResult::clear
void clear()
clear the results obtained
Definition: collision_data.h:377
hpp::fcl::distance
HPP_FCL_DLLAPI FCL_REAL distance(const Matrix3f &R0, const Vec3f &T0, const kIOS &b1, const kIOS &b2, Vec3f *P=NULL, Vec3f *Q=NULL)
Approximate distance between two kIOS bounding volumes.
Definition: kIOS.cpp:181
BOOST_CHECK_FALSE
#define BOOST_CHECK_FALSE(p)
Definition: shape_mesh_consistency.cpp:49
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere)
Definition: shape_mesh_consistency.cpp:53
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
hpp::fcl::DistanceResult::min_distance
FCL_REAL min_distance
minimum distance between two objects. if two objects are in collision, min_distance <= 0...
Definition: collision_data.h:424
hpp::fcl::Box
Center at zero point, axis aligned box.
Definition: shape/geometric_shapes.h:125
extents
FCL_REAL extents[6]
Definition: shape_mesh_consistency.cpp:51
hpp::fcl::BVHModel
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Definition: BVH/BVH_model.h:273
hpp::fcl::Cone
Cone The base of the cone is at and the top is at .
Definition: shape/geometric_shapes.h:414
geometric_shape_to_BVH_model.h
hpp::fcl::Sphere
Center at zero point sphere.
Definition: shape/geometric_shapes.h:196
res
res
hpp::fcl::collide
HPP_FCL_DLLAPI std::size_t collide(const CollisionObject *o1, const CollisionObject *o2, const CollisionRequest &request, CollisionResult &result)
Main collision interface: given two collision objects, and the requirements for contacts, including num of max contacts, whether perform exhaustive collision (i.e., returning returning all the contact points), whether return detailed contact information (i.e., normal, contact point, depth; otherwise only contact primitive id is returned), this function performs the collision between them. Return value is the number of contacts generated between the two objects.
Definition: src/collision.cpp:63
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
hpp::fcl::generateBVHModel
void generateBVHModel(BVHModel< BV > &model, const Box &shape, const Transform3f &pose)
Generate BVH model from box.
Definition: geometric_shape_to_BVH_model.h:50
hpp::fcl::generateRandomTransform
void generateRandomTransform(FCL_REAL extents[6], Transform3f &transform)
Generate one random transform whose translation is constrained by extents and rotation without constr...
Definition: utility.cpp:208

hpp-fcl
Author(s):
autogenerated on Fri Jun 2 2023 02:39:02