45 #include "../src/BV/OBB.h" 56 a = extentNorm * Vec3f::Random().cwiseAbs().normalized();
57 b = extentNorm * Vec3f::Random().cwiseAbs().normalized();
70 T = (Vec3f::Random() / sqrt(3)) * 1.5 * N;
74 q.coeffs().setRandom();
80 #define MANUAL_PRODUCT 1 83 #define PRODUCT(M33, v3) \ 84 (M33.col(0) * v3[0] + M33.col(1) * v3[1] + M33.col(2) * v3[2]) 86 #define PRODUCT(M33, v3) (M33 * v3) 92 const char*
sep =
",\t";
95 const Eigen::IOFormat
py_fmt(Eigen::FullPrecision, 0,
109 Box ba(2 * a), bb(2 * b);
113 return gjk.
shapeDistance(ba, tfa, bb, tfb, distance, p1, p2, normal);
132 return AABB_corner.array().max(0).matrix().squaredNorm();
161 FCL_REAL& squaredLowerBoundDistance) {
168 if (squaredLowerBoundDistance > breakDistance2)
return id;
173 if (squaredLowerBoundDistance > breakDistance2)
return id;
176 int ja = 1, ka = 2, jb = 1, kb = 2;
177 for (
int ia = 0; ia < 3; ++ia) {
178 for (
int ib = 0; ib < 3; ++ib) {
179 const FCL_REAL s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib);
181 const FCL_REAL diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) +
182 b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb));
186 FCL_REAL sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib);
188 squaredLowerBoundDistance = diff * diff / sinus2;
189 if (squaredLowerBoundDistance > breakDistance2) {
217 FCL_REAL& squaredLowerBoundDistance) {
222 if (squaredLowerBoundDistance > breakDistance2)
return true;
226 if (squaredLowerBoundDistance > breakDistance2)
return true;
228 int ja = 1, ka = 2, jb = 1, kb = 2;
229 for (
int ia = 0; ia < 3; ++ia) {
230 for (
int ib = 0; ib < 3; ++ib) {
231 const FCL_REAL s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib);
233 const FCL_REAL diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) +
234 b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb));
238 FCL_REAL sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib);
240 squaredLowerBoundDistance = diff * diff / sinus2;
241 if (squaredLowerBoundDistance > breakDistance2) {
269 FCL_REAL& squaredLowerBoundDistance) {
278 Vec3f AABB_corner(T.cwiseAbs() - Bf *
b);
279 Vec3f diff3(AABB_corner - a);
280 diff3 = diff3.cwiseMax(Vec3f::Zero());
283 squaredLowerBoundDistance = diff3.squaredNorm();
284 if (squaredLowerBoundDistance > breakDistance2)
return true;
286 AABB_corner = (B.transpose() * T).cwiseAbs() - Bf.transpose() * a;
288 diff3 = AABB_corner -
b;
289 diff3 = diff3.cwiseMax(Vec3f::Zero());
290 squaredLowerBoundDistance = diff3.squaredNorm();
292 if (squaredLowerBoundDistance > breakDistance2)
return true;
295 s = T[2] * B(1, 0) - T[1] * B(2, 0);
296 t = ((s < 0.0) ? -s : s);
297 assert(t == fabs(s));
300 diff = t - (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + b[1] * Bf(0, 2) +
307 sinus2 = 1 - Bf(0, 0) * Bf(0, 0);
309 squaredLowerBoundDistance = diff * diff / sinus2;
310 if (squaredLowerBoundDistance > breakDistance2) {
317 s = T[2] * B(1, 1) - T[1] * B(2, 1);
318 t = ((s < 0.0) ? -s : s);
319 assert(t == fabs(s));
321 diff = t - (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) + b[0] * Bf(0, 2) +
324 sinus2 = 1 - Bf(0, 1) * Bf(0, 1);
326 squaredLowerBoundDistance = diff * diff / sinus2;
327 if (squaredLowerBoundDistance > breakDistance2) {
334 s = T[2] * B(1, 2) - T[1] * B(2, 2);
335 t = ((s < 0.0) ? -s : s);
336 assert(t == fabs(s));
338 diff = t - (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) + b[0] * Bf(0, 1) +
341 sinus2 = 1 - Bf(0, 2) * Bf(0, 2);
343 squaredLowerBoundDistance = diff * diff / sinus2;
344 if (squaredLowerBoundDistance > breakDistance2) {
351 s = T[0] * B(2, 0) - T[2] * B(0, 0);
352 t = ((s < 0.0) ? -s : s);
353 assert(t == fabs(s));
355 diff = t - (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) + b[1] * Bf(1, 2) +
358 sinus2 = 1 - Bf(1, 0) * Bf(1, 0);
360 squaredLowerBoundDistance = diff * diff / sinus2;
361 if (squaredLowerBoundDistance > breakDistance2) {
368 s = T[0] * B(2, 1) - T[2] * B(0, 1);
369 t = ((s < 0.0) ? -s : s);
370 assert(t == fabs(s));
372 diff = t - (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) + b[0] * Bf(1, 2) +
375 sinus2 = 1 - Bf(1, 1) * Bf(1, 1);
377 squaredLowerBoundDistance = diff * diff / sinus2;
378 if (squaredLowerBoundDistance > breakDistance2) {
385 s = T[0] * B(2, 2) - T[2] * B(0, 2);
386 t = ((s < 0.0) ? -s : s);
387 assert(t == fabs(s));
389 diff = t - (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) + b[0] * Bf(1, 1) +
392 sinus2 = 1 - Bf(1, 2) * Bf(1, 2);
394 squaredLowerBoundDistance = diff * diff / sinus2;
395 if (squaredLowerBoundDistance > breakDistance2) {
402 s = T[1] * B(0, 0) - T[0] * B(1, 0);
403 t = ((s < 0.0) ? -s : s);
404 assert(t == fabs(s));
406 diff = t - (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) + b[1] * Bf(2, 2) +
409 sinus2 = 1 - Bf(2, 0) * Bf(2, 0);
411 squaredLowerBoundDistance = diff * diff / sinus2;
412 if (squaredLowerBoundDistance > breakDistance2) {
419 s = T[1] * B(0, 1) - T[0] * B(1, 1);
420 t = ((s < 0.0) ? -s : s);
421 assert(t == fabs(s));
423 diff = t - (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) + b[0] * Bf(2, 2) +
426 sinus2 = 1 - Bf(2, 1) * Bf(2, 1);
428 squaredLowerBoundDistance = diff * diff / sinus2;
429 if (squaredLowerBoundDistance > breakDistance2) {
436 s = T[1] * B(0, 2) - T[0] * B(1, 2);
437 t = ((s < 0.0) ? -s : s);
438 assert(t == fabs(s));
440 diff = t - (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) + b[0] * Bf(2, 1) +
443 sinus2 = 1 - Bf(2, 2) * Bf(2, 2);
445 squaredLowerBoundDistance = diff * diff / sinus2;
446 if (squaredLowerBoundDistance > breakDistance2) {
459 FCL_REAL& squaredLowerBoundDistance) {
464 if (squaredLowerBoundDistance > breakDistance2)
return true;
467 if (squaredLowerBoundDistance > breakDistance2)
return true;
471 s = T[2] * B(1, 0) - T[1] * B(2, 0);
472 t = ((s < 0.0) ? -s : s);
473 assert(t == fabs(s));
477 diff = t - (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + b[1] * Bf(0, 2) +
484 sinus2 = 1 - Bf(0, 0) * Bf(0, 0);
486 squaredLowerBoundDistance = diff * diff / sinus2;
487 if (squaredLowerBoundDistance > breakDistance2) {
494 s = T[2] * B(1, 1) - T[1] * B(2, 1);
495 t = ((s < 0.0) ? -s : s);
496 assert(t == fabs(s));
498 diff = t - (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) + b[0] * Bf(0, 2) +
501 sinus2 = 1 - Bf(0, 1) * Bf(0, 1);
503 squaredLowerBoundDistance = diff * diff / sinus2;
504 if (squaredLowerBoundDistance > breakDistance2) {
511 s = T[2] * B(1, 2) - T[1] * B(2, 2);
512 t = ((s < 0.0) ? -s : s);
513 assert(t == fabs(s));
515 diff = t - (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) + b[0] * Bf(0, 1) +
518 sinus2 = 1 - Bf(0, 2) * Bf(0, 2);
520 squaredLowerBoundDistance = diff * diff / sinus2;
521 if (squaredLowerBoundDistance > breakDistance2) {
528 s = T[0] * B(2, 0) - T[2] * B(0, 0);
529 t = ((s < 0.0) ? -s : s);
530 assert(t == fabs(s));
532 diff = t - (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) + b[1] * Bf(1, 2) +
535 sinus2 = 1 - Bf(1, 0) * Bf(1, 0);
537 squaredLowerBoundDistance = diff * diff / sinus2;
538 if (squaredLowerBoundDistance > breakDistance2) {
545 s = T[0] * B(2, 1) - T[2] * B(0, 1);
546 t = ((s < 0.0) ? -s : s);
547 assert(t == fabs(s));
549 diff = t - (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) + b[0] * Bf(1, 2) +
552 sinus2 = 1 - Bf(1, 1) * Bf(1, 1);
554 squaredLowerBoundDistance = diff * diff / sinus2;
555 if (squaredLowerBoundDistance > breakDistance2) {
562 s = T[0] * B(2, 2) - T[2] * B(0, 2);
563 t = ((s < 0.0) ? -s : s);
564 assert(t == fabs(s));
566 diff = t - (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) + b[0] * Bf(1, 1) +
569 sinus2 = 1 - Bf(1, 2) * Bf(1, 2);
571 squaredLowerBoundDistance = diff * diff / sinus2;
572 if (squaredLowerBoundDistance > breakDistance2) {
579 s = T[1] * B(0, 0) - T[0] * B(1, 0);
580 t = ((s < 0.0) ? -s : s);
581 assert(t == fabs(s));
583 diff = t - (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) + b[1] * Bf(2, 2) +
586 sinus2 = 1 - Bf(2, 0) * Bf(2, 0);
588 squaredLowerBoundDistance = diff * diff / sinus2;
589 if (squaredLowerBoundDistance > breakDistance2) {
596 s = T[1] * B(0, 1) - T[0] * B(1, 1);
597 t = ((s < 0.0) ? -s : s);
598 assert(t == fabs(s));
600 diff = t - (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) + b[0] * Bf(2, 2) +
603 sinus2 = 1 - Bf(2, 1) * Bf(2, 1);
605 squaredLowerBoundDistance = diff * diff / sinus2;
606 if (squaredLowerBoundDistance > breakDistance2) {
613 s = T[1] * B(0, 2) - T[0] * B(1, 2);
614 t = ((s < 0.0) ? -s : s);
615 assert(t == fabs(s));
617 diff = t - (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) + b[0] * Bf(2, 1) +
620 sinus2 = 1 - Bf(2, 2) * Bf(2, 2);
622 squaredLowerBoundDistance = diff * diff / sinus2;
623 if (squaredLowerBoundDistance > breakDistance2) {
633 template <
int ia,
int ib,
int ja = (ia + 1) % 3,
int ka = (ia + 2) % 3,
634 int jb = (ib + 1) % 3,
int kb = (ib + 2) % 3>
639 FCL_REAL& squaredLowerBoundDistance) {
640 const FCL_REAL s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib);
642 const FCL_REAL diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) +
643 b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb));
647 FCL_REAL sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib);
649 squaredLowerBoundDistance = diff * diff / sinus2;
650 if (squaredLowerBoundDistance > breakDistance2) {
670 FCL_REAL& squaredLowerBoundDistance) {
675 if (squaredLowerBoundDistance > breakDistance2)
return true;
678 if (squaredLowerBoundDistance > breakDistance2)
return true;
682 squaredLowerBoundDistance))
685 squaredLowerBoundDistance))
688 squaredLowerBoundDistance))
691 squaredLowerBoundDistance))
694 squaredLowerBoundDistance))
697 squaredLowerBoundDistance))
700 squaredLowerBoundDistance))
703 squaredLowerBoundDistance))
706 squaredLowerBoundDistance))
714 template <
int ib,
int jb = (ib + 1) % 3,
int kb = (ib + 2) % 3>
719 FCL_REAL& squaredLowerBoundDistance) {
720 const FCL_REAL s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib);
722 const FCL_REAL diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) +
723 b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb));
727 FCL_REAL sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib);
729 squaredLowerBoundDistance = diff * diff / sinus2;
730 if (squaredLowerBoundDistance > breakDistance2) {
750 FCL_REAL& squaredLowerBoundDistance) {
755 if (squaredLowerBoundDistance > breakDistance2)
return true;
758 if (squaredLowerBoundDistance > breakDistance2)
return true;
762 for (
int ia = 0; ia < 3; ++ia) {
764 squaredLowerBoundDistance))
767 squaredLowerBoundDistance))
770 squaredLowerBoundDistance))
782 FCL_REAL& squaredLowerBoundDistance) {
787 squaredLowerBoundDistance = 0;
792 t = ((T[0] < 0.0) ? -T[0] : T[0]);
794 diff = t - (a[0] + Bf.row(0).dot(b));
796 squaredLowerBoundDistance = diff * diff;
800 t = ((T[1] < 0.0) ? -T[1] : T[1]);
802 diff = t - (a[1] + Bf.row(1).dot(b));
804 squaredLowerBoundDistance += diff * diff;
808 t = ((T[2] < 0.0) ? -T[2] : T[2]);
810 diff = t - (a[2] + Bf.row(2).dot(b));
812 squaredLowerBoundDistance += diff * diff;
815 if (squaredLowerBoundDistance > breakDistance2)
return true;
817 squaredLowerBoundDistance = 0;
821 t = ((s < 0.0) ? -s : s);
823 diff = t - (b[0] + Bf.col(0).dot(a));
825 squaredLowerBoundDistance += diff * diff;
830 t = ((s < 0.0) ? -s : s);
832 diff = t - (b[1] + Bf.col(1).dot(a));
834 squaredLowerBoundDistance += diff * diff;
839 t = ((s < 0.0) ? -s : s);
841 diff = t - (b[2] + Bf.col(2).dot(a));
843 squaredLowerBoundDistance += diff * diff;
846 if (squaredLowerBoundDistance > breakDistance2)
return true;
849 s = T[2] * B(1, 0) - T[1] * B(2, 0);
850 t = ((s < 0.0) ? -s : s);
853 diff = t - (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + b[1] * Bf(0, 2) +
860 sinus2 = 1 - Bf(0, 0) * Bf(0, 0);
862 squaredLowerBoundDistance = diff * diff / sinus2;
863 if (squaredLowerBoundDistance > breakDistance2) {
870 s = T[2] * B(1, 1) - T[1] * B(2, 1);
871 t = ((s < 0.0) ? -s : s);
873 diff = t - (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) + b[0] * Bf(0, 2) +
876 sinus2 = 1 - Bf(0, 1) * Bf(0, 1);
878 squaredLowerBoundDistance = diff * diff / sinus2;
879 if (squaredLowerBoundDistance > breakDistance2) {
886 s = T[2] * B(1, 2) - T[1] * B(2, 2);
887 t = ((s < 0.0) ? -s : s);
889 diff = t - (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) + b[0] * Bf(0, 1) +
892 sinus2 = 1 - Bf(0, 2) * Bf(0, 2);
894 squaredLowerBoundDistance = diff * diff / sinus2;
895 if (squaredLowerBoundDistance > breakDistance2) {
902 s = T[0] * B(2, 0) - T[2] * B(0, 0);
903 t = ((s < 0.0) ? -s : s);
905 diff = t - (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) + b[1] * Bf(1, 2) +
908 sinus2 = 1 - Bf(1, 0) * Bf(1, 0);
910 squaredLowerBoundDistance = diff * diff / sinus2;
911 if (squaredLowerBoundDistance > breakDistance2) {
918 s = T[0] * B(2, 1) - T[2] * B(0, 1);
919 t = ((s < 0.0) ? -s : s);
921 diff = t - (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) + b[0] * Bf(1, 2) +
924 sinus2 = 1 - Bf(1, 1) * Bf(1, 1);
926 squaredLowerBoundDistance = diff * diff / sinus2;
927 if (squaredLowerBoundDistance > breakDistance2) {
934 s = T[0] * B(2, 2) - T[2] * B(0, 2);
935 t = ((s < 0.0) ? -s : s);
937 diff = t - (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) + b[0] * Bf(1, 1) +
940 sinus2 = 1 - Bf(1, 2) * Bf(1, 2);
942 squaredLowerBoundDistance = diff * diff / sinus2;
943 if (squaredLowerBoundDistance > breakDistance2) {
950 s = T[1] * B(0, 0) - T[0] * B(1, 0);
951 t = ((s < 0.0) ? -s : s);
953 diff = t - (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) + b[1] * Bf(2, 2) +
956 sinus2 = 1 - Bf(2, 0) * Bf(2, 0);
958 squaredLowerBoundDistance = diff * diff / sinus2;
959 if (squaredLowerBoundDistance > breakDistance2) {
966 s = T[1] * B(0, 1) - T[0] * B(1, 1);
967 t = ((s < 0.0) ? -s : s);
969 diff = t - (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) + b[0] * Bf(2, 2) +
972 sinus2 = 1 - Bf(2, 1) * Bf(2, 1);
974 squaredLowerBoundDistance = diff * diff / sinus2;
975 if (squaredLowerBoundDistance > breakDistance2) {
982 s = T[1] * B(0, 2) - T[0] * B(1, 2);
983 t = ((s < 0.0) ? -s : s);
985 diff = t - (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) + b[0] * Bf(2, 1) +
988 sinus2 = 1 - Bf(2, 2) * Bf(2, 2);
990 squaredLowerBoundDistance = diff * diff / sinus2;
991 if (squaredLowerBoundDistance > breakDistance2) {
1003 FCL_REAL& squaredLowerBoundDistance) {
1007 squaredLowerBoundDistance = 0;
1009 Matrix3f Bf(B.array().abs() + reps);
1015 t = ((T[0] < 0.0) ? -T[0] : T[0]);
1018 if (t > (a[0] + Bf.row(0).dot(b)))
return true;
1022 s = B.col(0).dot(T);
1023 t = ((s < 0.0) ? -s : s);
1026 if (t > (b[0] + Bf.col(0).dot(a)))
return true;
1029 t = ((T[1] < 0.0) ? -T[1] : T[1]);
1032 if (t > (a[1] + Bf.row(1).dot(b)))
return true;
1035 t = ((T[2] < 0.0) ? -T[2] : T[2]);
1038 if (t > (a[2] + Bf.row(2).dot(b)))
return true;
1042 s = B.col(1).dot(T);
1043 t = ((s < 0.0) ? -s : s);
1046 if (t > (b[1] + Bf.col(1).dot(a)))
return true;
1050 s = B.col(2).dot(T);
1051 t = ((s < 0.0) ? -s : s);
1054 if (t > (b[2] + Bf.col(2).dot(a)))
return true;
1057 s = T[2] * B(1, 0) - T[1] * B(2, 0);
1058 t = ((s < 0.0) ? -s : s);
1061 (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + b[1] * Bf(0, 2) + b[2] * Bf(0, 1)))
1065 s = T[2] * B(1, 1) - T[1] * B(2, 1);
1066 t = ((s < 0.0) ? -s : s);
1069 (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) + b[0] * Bf(0, 2) + b[2] * Bf(0, 0)))
1073 s = T[2] * B(1, 2) - T[1] * B(2, 2);
1074 t = ((s < 0.0) ? -s : s);
1077 (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) + b[0] * Bf(0, 1) + b[1] * Bf(0, 0)))
1081 s = T[0] * B(2, 0) - T[2] * B(0, 0);
1082 t = ((s < 0.0) ? -s : s);
1085 (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) + b[1] * Bf(1, 2) + b[2] * Bf(1, 1)))
1089 s = T[0] * B(2, 1) - T[2] * B(0, 1);
1090 t = ((s < 0.0) ? -s : s);
1093 (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) + b[0] * Bf(1, 2) + b[2] * Bf(1, 0)))
1097 s = T[0] * B(2, 2) - T[2] * B(0, 2);
1098 t = ((s < 0.0) ? -s : s);
1101 (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) + b[0] * Bf(1, 1) + b[1] * Bf(1, 0)))
1105 s = T[1] * B(0, 0) - T[0] * B(1, 0);
1106 t = ((s < 0.0) ? -s : s);
1109 (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) + b[1] * Bf(2, 2) + b[2] * Bf(2, 1)))
1113 s = T[1] * B(0, 1) - T[0] * B(1, 1);
1114 t = ((s < 0.0) ? -s : s);
1117 (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) + b[0] * Bf(2, 2) + b[2] * Bf(2, 0)))
1121 s = T[1] * B(0, 2) - T[0] * B(1, 2);
1122 t = ((s < 0.0) ? -s : s);
1125 (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) + b[0] * Bf(2, 1) + b[1] * Bf(2, 0)))
1143 const std::string unit =
" (us)";
1144 os <<
"separating axis" <<
sep <<
"distance lower bound" <<
sep 1145 <<
"distance" <<
sep <<
"failure" <<
sep <<
"Runtime Loop" << unit <<
sep 1146 <<
"Manual Loop Unrolling 1" << unit <<
sep <<
"Manual Loop Unrolling 2" 1147 << unit <<
sep <<
"Template Unrolling" << unit <<
sep 1148 <<
"Partial Template Unrolling" << unit <<
sep <<
"Original (LowerBound)" 1149 << unit <<
sep <<
"Original (NoLowerBound)" << unit;
1153 std::ostream&
print(std::ostream& os)
const {
1154 os << ifId <<
sep << std::sqrt(squaredLowerBoundDistance) <<
sep << distance
1158 << static_cast<double>(
1159 std::chrono::duration_cast<std::chrono::nanoseconds>(
1168 return br.
print(os);
1177 const FCL_REAL breakDistance2 = breakDistance * breakDistance;
1184 assert(0 <= result.
ifId && result.
ifId <= 11);
1190 if (!overlap && result.
distance <= 0) {
1191 std::cerr <<
"Failure: negative distance for disjoint OBBs.";
1193 std::cerr <<
"Failure: negative distance lower bound.";
1196 std::cerr <<
"Failure: distance is inferior to its lower bound (diff = " 1199 }
else if (overlap != !disjoint && result.
distance >= dist_thr -
eps) {
1200 std::cerr <<
"Failure: overlapping test and distance query mismatch.";
1201 }
else if (overlap && result.
distance >= dist_thr -
eps) {
1202 std::cerr <<
"Failure: positive distance for overlapping OBBs.";
1208 <<
"\nT = " << T.transpose().format(
py_fmt)
1209 <<
"\na = " << a.transpose().format(
py_fmt)
1210 <<
"\nb = " << b.transpose().format(
py_fmt)
1211 <<
"\nresult = " << result <<
'\n' 1217 clock_type::time_point start, end;
1220 start = clock_type::now();
1221 for (std::size_t i = 0; i < N; ++i)
1223 end = clock_type::now();
1224 result.
duration[0] = (end - start) / N;
1227 start = clock_type::now();
1228 for (std::size_t i = 0; i < N; ++i)
1231 end = clock_type::now();
1232 result.
duration[1] = (end - start) / N;
1235 start = clock_type::now();
1236 for (std::size_t i = 0; i < N; ++i)
1239 end = clock_type::now();
1240 result.
duration[2] = (end - start) / N;
1243 start = clock_type::now();
1244 for (std::size_t i = 0; i < N; ++i)
1247 end = clock_type::now();
1248 result.
duration[3] = (end - start) / N;
1251 start = clock_type::now();
1252 for (std::size_t i = 0; i < N; ++i)
1254 breakDistance2, tmp);
1255 end = clock_type::now();
1256 result.
duration[4] = (end - start) / N;
1259 start = clock_type::now();
1260 for (std::size_t i = 0; i < N; ++i)
1262 end = clock_type::now();
1263 result.
duration[5] = (end - start) / N;
1266 start = clock_type::now();
1268 for (std::size_t i = 0; i < N; ++i)
1271 end = clock_type::now();
1272 result.
duration[6] = (end - start) / N;
1278 std::size_t nbFailure = 0;
1286 #ifndef NDEBUG // if debug mode 1287 static const size_t nbRandomOBB = 10;
1288 static const size_t nbTransformPerOBB = 10;
1289 static const size_t nbRunForTimeMeas = 10;
1291 static const size_t nbRandomOBB = 100;
1292 static const size_t nbTransformPerOBB = 100;
1293 static const size_t nbRunForTimeMeas = 1000;
1295 static const FCL_REAL extentNorm = 1.;
1300 for (std::size_t iobb = 0; iobb < nbRandomOBB; ++iobb) {
1302 for (std::size_t itf = 0; itf < nbTransformPerOBB; ++itf) {
1305 if (output != NULL) *output << result <<
'\n';
1306 if (result.failure) nbFailure++;
1313 std::ostream* output = NULL;
1314 if (argc > 1 && strcmp(argv[1],
"--generate-output") == 0) {
1315 output = &std::cout;
1318 std::cout <<
"The benchmark real time measurements may be noisy and " 1319 "will incur extra overhead." 1320 "\nUse the following commands to turn ON:" 1321 "\n\tsudo cpufreq-set --governor performance" 1323 "\n\tsudo cpufreq-set --governor powersave" 1327 if (nbFailure > INT_MAX)
return INT_MAX;
1328 return (
int)nbFailure;
void randomTransform(Matrix3f &B, Vec3f &T, const Vec3f &a, const Vec3f &b, const FCL_REAL extentNorm)
clock_type::duration duration_type
static bool run(int ia, int ja, int ka, const Matrix3f &B, const Vec3f &T, const Vec3f &a, const Vec3f &b, const Matrix3f &Bf, const FCL_REAL &breakDistance2, FCL_REAL &squaredLowerBoundDistance)
const Eigen::IOFormat py_fmt(Eigen::FullPrecision, 0, ", ", ",\, "(", ",)", "(", ",)")
bool originalWithLowerBound(const Matrix3f &B, const Vec3f &T, const Vec3f &a, const Vec3f &b, const FCL_REAL &breakDistance2, FCL_REAL &squaredLowerBoundDistance)
bool withRuntimeLoop(const Matrix3f &B, const Vec3f &T, const Vec3f &a, const Vec3f &b, const FCL_REAL &breakDistance2, FCL_REAL &squaredLowerBoundDistance)
std::size_t obb_overlap_and_lower_bound_distance(std::ostream *output)
BenchmarkResult benchmark_obb_case(const Matrix3f &B, const Vec3f &T, const Vec3f &a, const Vec3f &b, const CollisionRequest &request, std::size_t N)
int main(int argc, char **argv)
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
void randomOBBs(Vec3f &a, Vec3f &b, FCL_REAL extentNorm)
bool distance(const Matrix3f &B, const Vec3f &T, const Vec3f &a, const Vec3f &b, FCL_REAL &distance)
request to the collision algorithm
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.
bool withManualLoopUnrolling_2(const Matrix3f &B, const Vec3f &T, const Vec3f &a, const Vec3f &b, const FCL_REAL &breakDistance2, FCL_REAL &squaredLowerBoundDistance)
FCL_REAL squaredLowerBoundDistance
FCL_REAL _computeDistanceForCase1(const Vec3f &T, const Vec3f &a, const Vec3f &b, const Matrix3f &Bf)
Center at zero point, axis aligned box.
static std::ostream & operator<<(std::ostream &o, const Quaternion3f &q)
bool originalWithNoLowerBound(const Matrix3f &B, const Vec3f &T, const Vec3f &a, const Vec3f &b, const FCL_REAL &, FCL_REAL &squaredLowerBoundDistance)
static std::ostream & headers(std::ostream &os)
FCL_REAL break_distance
Distance below which bounding volumes are broken down. See Collision.
std::chrono::high_resolution_clock clock_type
FCL_REAL _computeDistanceForCase2(const Matrix3f &B, const Vec3f &T, const Vec3f &a, const Vec3f &b, const Matrix3f &Bf)
bool shapeDistance(const S1 &s1, const Transform3f &tf1, const S2 &s2, const Transform3f &tf2, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal) const
distance computation between two shapes
Eigen::Quaternion< FCL_REAL > Quaternion3f
int separatingAxisId(const Matrix3f &B, const Vec3f &T, const Vec3f &a, const Vec3f &b, const FCL_REAL &breakDistance2, FCL_REAL &squaredLowerBoundDistance)
bool withManualLoopUnrolling_1(const Matrix3f &B, const Vec3f &T, const Vec3f &a, const Vec3f &b, const FCL_REAL &breakDistance2, FCL_REAL &squaredLowerBoundDistance)
HPP_FCL_DLLAPI bool overlap(const Matrix3f &R0, const Vec3f &T0, const AABB &b1, const AABB &b2)
Check collision between two aabbs, b1 is in configuration (R0, T0) and b2 is in identity.
duration_type duration[NB_METHODS]
static bool run(const Matrix3f &B, const Vec3f &T, const Vec3f &a, const Vec3f &b, const Matrix3f &Bf, const FCL_REAL &breakDistance2, FCL_REAL &squaredLowerBoundDistance)
bool withTemplateLoopUnrolling_1(const Matrix3f &B, const Vec3f &T, const Vec3f &a, const Vec3f &b, const FCL_REAL &breakDistance2, FCL_REAL &squaredLowerBoundDistance)
collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the ...
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
std::ostream & print(std::ostream &os) const
bool withPartialTemplateLoopUnrolling_1(const Matrix3f &B, const Vec3f &T, const Vec3f &a, const Vec3f &b, const FCL_REAL &breakDistance2, FCL_REAL &squaredLowerBoundDistance)
FCL_REAL security_margin
Distance below which objects are considered in collision. See Collision.