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::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
hpp::fcl::Sphere
Center at zero point sphere.
Definition: shape/geometric_shapes.h:196
utility.h
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
hpp::fcl::Cylinder
Cylinder along Z axis. The cylinder is defined at its centroid.
Definition: shape/geometric_shapes.h:501
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere)
Definition: shape_mesh_consistency.cpp:53
hpp::fcl::DistanceResult::clear
void clear()
clear the result
Definition: collision_data.h:504
narrowphase.h
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,...
Definition: src/collision.cpp:63
res
res
distance.h
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
hpp::fcl::CollisionResult::clear
void clear()
clear the results obtained
Definition: collision_data.h:377
hpp::fcl::CollisionRequest
request to the collision algorithm
Definition: collision_data.h:235
t
tuple t
hpp::fcl::CollisionResult
collision result
Definition: collision_data.h:302
hpp::fcl::Cone
Cone The base of the cone is at and the top is at .
Definition: shape/geometric_shapes.h:414
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
Definition: broadphase_bruteforce.h:45
hpp::fcl::Transform3f
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
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::DistanceRequest
request to the distance computation
Definition: collision_data.h:392
hpp::fcl::DistanceResult
distance result
Definition: collision_data.h:420
geometric_shape_to_BVH_model.h
extents
FCL_REAL extents[6]
Definition: shape_mesh_consistency.cpp:51
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::Transform3f::setTranslation
void setTranslation(const Eigen::MatrixBase< Derived > &T_)
set transform from translation
Definition: transform.h:142
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
BOOST_CHECK_FALSE
#define BOOST_CHECK_FALSE(p)
Definition: shape_mesh_consistency.cpp:49
collision.h
hpp::fcl::Box
Center at zero point, axis aligned box.
Definition: shape/geometric_shapes.h:125

hpp-fcl
Author(s):
autogenerated on Fri Jan 26 2024 03:46:15