38 #include <gtest/gtest.h>
49 #include "fcl_resources/config.h"
63 template<
typename BV,
typename TraversalNode>
80 static std::vector<Contact<S>> static_global_pairs;
81 return static_global_pairs;
87 static std::vector<Contact<S>> static_global_pairs_now;
88 return static_global_pairs_now;
106 auto motion_a = fcl::make_aligned_shared<fcl::SplineMotion<S>>(
107 t[0], t[1], t[2], t[3],
108 r[0],
r[1],
r[2],
r[3]);
120 auto motion_b = fcl::make_aligned_shared<fcl::SplineMotion<S>>(
121 t[0], t[1], t[2], t[3],
122 r[0],
r[1],
r[2],
r[3]);
125 auto shape_a = std::make_shared<fcl::Sphere<S>>(1.0);
130 auto shape_b = std::make_shared<fcl::Sphere<S>>(1.0);
147 test_SplineMotion_rotated_spline_collide_test<double>();
150 template <
typename S>
153 S r_extents[] = {-1000, -1000, -1000, 1000, 1000, 1000};
162 convertBV(aabb1, rotate_transform[0], obb1);
165 constructBox(aabb1, rotate_transform[0], box1, box1_tf);
167 S
extents[] = {-1000, -1000, -1000, 1000, 1000, 1000};
168 std::size_t n = 1000;
173 for(std::size_t i = 0; i < transforms.size(); ++i)
188 bool overlap_obb = obb1.
overlap(obb2);
189 bool overlap_box = solver.
shapeIntersect(box1, box1_tf, box2, box2_tf,
nullptr);
195 template <
typename S>
198 S r_extents[] = {-1000, -1000, -1000, 1000, 1000, 1000};
207 convertBV(aabb1, rotate_transform[0], obb1);
210 constructBox(aabb1, rotate_transform[0], box1, box1_tf);
212 S
extents[] = {-1000, -1000, -1000, 1000, 1000, 1000};
213 std::size_t n = 1000;
218 for(std::size_t i = 0; i < transforms.size(); ++i)
220 S len = (aabb1.
max_[0] - aabb1.
min_[0]) * 0.5;
228 bool overlap_obb = obb1.
overlap(obb2);
229 bool overlap_sphere = solver.
shapeIntersect(box1, box1_tf, sphere, transforms[i],
nullptr);
235 computeBV(ellipsoid, transforms[i], obb2);
237 bool overlap_obb = obb1.
overlap(obb2);
238 bool overlap_ellipsoid = solver.
shapeIntersect(box1, box1_tf, ellipsoid, transforms[i],
nullptr);
246 bool overlap_obb = obb1.
overlap(obb2);
247 bool overlap_capsule = solver.
shapeIntersect(box1, box1_tf, capsule, transforms[i],
nullptr);
255 bool overlap_obb = obb1.
overlap(obb2);
256 bool overlap_cone = solver.
shapeIntersect(box1, box1_tf, cone, transforms[i],
nullptr);
262 computeBV(cylinder, transforms[i], obb2);
264 bool overlap_obb = obb1.
overlap(obb2);
265 bool overlap_cylinder = solver.
shapeIntersect(box1, box1_tf, cylinder, transforms[i],
nullptr);
271 template <
typename S>
274 S
extents[] = {-1000, -1000, -1000, 1000, 1000, 1000};
275 std::size_t n = 1000;
287 for(std::size_t i = 0; i < transforms.size(); ++i)
298 bool overlap_aabb = aabb1.
overlap(aabb2);
299 bool overlap_obb = obb1.
overlap(obb2);
300 if(overlap_aabb != overlap_obb)
302 std::cout << aabb1.
min_.transpose() <<
" " << aabb1.
max_.transpose() << std::endl;
303 std::cout << aabb2.
min_.transpose() <<
" " << aabb2.
max_.transpose() << std::endl;
304 std::cout << obb1.
To.transpose() <<
" " << obb1.
extent.transpose() <<
" " << obb1.
axis.col(0).transpose() <<
" " << obb1.
axis.col(1).transpose() <<
" " << obb1.
axis.col(2).transpose() << std::endl;
305 std::cout << obb2.
To.transpose() <<
" " << obb2.
extent.transpose() <<
" " << obb2.
axis.col(0).transpose() <<
" " << obb2.
axis.col(1).transpose() <<
" " << obb2.
axis.col(2).transpose() << std::endl;
310 std::cout << std::endl;
313 template <
typename S>
316 std::vector<Vector3<S>> p1, p2;
317 std::vector<Triangle> t1, t2;
323 S
extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
334 for(std::size_t i = 0; i < transforms.size(); ++i)
336 global_pairs<S>().clear();
337 global_pairs_now<S>().clear();
342 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
343 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
345 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
346 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
350 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
351 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
353 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
354 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
358 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
359 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
361 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
362 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
366 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
367 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
369 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
370 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
374 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
375 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
377 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
378 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
382 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
383 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
385 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
386 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
390 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
391 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
393 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
394 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
398 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
399 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
401 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
402 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
406 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
407 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
409 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
410 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
414 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
415 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
417 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
418 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
422 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
423 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
425 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
426 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
430 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
431 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
433 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
434 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
438 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
439 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
441 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
442 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
446 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
447 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
449 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
450 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
454 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
455 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
457 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
458 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
462 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
463 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
465 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
466 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
470 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
471 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
473 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
474 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
478 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
479 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
481 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
482 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
486 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
487 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
489 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
490 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
494 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
495 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
497 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
498 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
502 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
503 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
505 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
506 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
510 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
511 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
513 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
514 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
518 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
519 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
521 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
522 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
526 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
527 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
529 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
530 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
534 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
535 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
537 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
538 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
542 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
543 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
545 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
546 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
550 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
551 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
553 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
554 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
558 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
559 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
561 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
562 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
566 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
567 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
569 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
570 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
574 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
575 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
577 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
578 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
582 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
583 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
585 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
586 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
590 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
591 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
593 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
594 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
598 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
599 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
601 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
602 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
606 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
607 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
609 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
610 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
614 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
615 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
617 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
618 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
623 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
624 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
626 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
627 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
632 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
633 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
635 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
636 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
640 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
641 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
643 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
644 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
648 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
649 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
651 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
652 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
656 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
657 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
659 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
660 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
664 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
665 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
667 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
668 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
672 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
673 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
675 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
676 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
680 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
681 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
683 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
684 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
688 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
689 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
691 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
692 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
697 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
698 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
700 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
701 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
705 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
706 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
708 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
709 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
713 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
714 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
716 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
717 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
721 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
722 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
724 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
725 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
729 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
730 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
732 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
733 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
737 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
738 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
740 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
741 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
745 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
746 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
748 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
749 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
753 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
754 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
756 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
757 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
761 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
762 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
764 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
765 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
769 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
770 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
772 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
773 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
777 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
778 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
780 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
781 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
785 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
786 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
788 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
789 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
793 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
794 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
796 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
797 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
801 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
802 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
804 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
805 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
809 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
810 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
812 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
813 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
817 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
818 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
820 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
821 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
825 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
826 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
828 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
829 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
833 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
834 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
836 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
837 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
841 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
842 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
844 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
845 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
849 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
850 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
852 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
853 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
857 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
858 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
860 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
861 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
865 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
866 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
868 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
869 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
873 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
874 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
876 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
877 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
881 EXPECT_TRUE(global_pairs<S>().size() == global_pairs_now<S>().size());
882 for(std::size_t j = 0; j < global_pairs<S>().size(); ++j)
884 EXPECT_TRUE(global_pairs<S>()[j].b1 == global_pairs_now<S>()[j].b1);
885 EXPECT_TRUE(global_pairs<S>()[j].b2 == global_pairs_now<S>()[j].b2);
894 #if !defined(FCL_OS_MACOS) || !defined(NDEBUG)
895 test_OBB_Box_test<double>();
902 test_OBB_shape_test<double>();
908 test_OBB_AABB_test<double>();
914 test_mesh_mesh<double>();
917 template<
typename BV>
922 using S =
typename BV::S;
929 std::vector<Vector3<S>> vertices1_new(vertices1.size());
930 for(
unsigned int i = 0; i < vertices1_new.size(); ++i)
932 vertices1_new[i] = tf * vertices1[i];
950 if(!detail::initialize<BV>(node, m1, pose1, m2, pose2,
952 std::cout <<
"initialize error" << std::endl;
961 if(global_pairs<S>().size() == 0)
964 std::sort(global_pairs<S>().begin(), global_pairs<S>().end());
969 std::sort(global_pairs_now<S>().begin(), global_pairs_now<S>().end());
974 std::cout <<
"in collision " << local_result.
numContacts() <<
": " << std::endl;
980 if(
verbose) std::cout <<
"collision free " << std::endl;
986 template<
typename BV>
991 using S =
typename BV::S;
1012 if(!detail::initialize<BV>(node, m1, pose1, m2, pose2,
1014 std::cout <<
"initialize error" << std::endl;
1023 if(global_pairs<S>().size() == 0)
1026 std::sort(global_pairs<S>().begin(), global_pairs<S>().end());
1031 std::sort(global_pairs_now<S>().begin(), global_pairs_now<S>().end());
1035 std::cout <<
"in collision " << local_result.
numContacts() <<
": " << std::endl;
1041 if(
verbose) std::cout <<
"collision free " << std::endl;
1047 template<
typename BV,
typename TraversalNode>
1052 using S =
typename BV::S;
1074 std::cout <<
"initialize error" << std::endl;
1076 node.enable_statistics =
verbose;
1082 if(global_pairs<S>().size() == 0)
1085 std::sort(global_pairs<S>().begin(), global_pairs<S>().end());
1090 std::sort(global_pairs_now<S>().begin(), global_pairs_now<S>().end());
1094 std::cout <<
"in collision " << local_result.
numContacts() <<
": " << std::endl;
1095 if(
verbose) std::cout << node.num_bv_tests <<
" " << node.num_leaf_tests << std::endl;
1100 if(
verbose) std::cout <<
"collision free " << std::endl;
1101 if(
verbose) std::cout << node.num_bv_tests <<
" " << node.num_leaf_tests << std::endl;
1107 template<
typename BV>
1112 using S =
typename BV::S;
1130 std::vector<Contact<S>> contacts;
1134 int num_contacts =
collide(&m1, pose1, &m2, pose2, request, result);
1138 global_pairs_now<S>().resize(num_contacts);
1140 for(
int i = 0; i < num_contacts; ++i)
1142 global_pairs_now<S>()[i].b1 = contacts[i].b1;
1143 global_pairs_now<S>()[i].b2 = contacts[i].b2;
1146 std::sort(global_pairs_now<S>().begin(), global_pairs_now<S>().end());
1148 if(num_contacts > 0)
return true;
1155 ::testing::InitGoogleTest(&argc, argv);
1156 return RUN_ALL_TESTS();