obb.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2014-2016, CNRS-LAAS
5  * Copyright (c) 2023, Inria
6  * Author: Florent Lamiraux
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of CNRS nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  */
36 
37 #include <iostream>
38 #include <fstream>
39 #include <sstream>
40 
41 #include <chrono>
42 
43 #include <hpp/fcl/narrowphase/narrowphase.h>
44 
45 #include "../src/BV/OBB.h"
46 #include <hpp/fcl/internal/shape_shape_func.h>
47 #include "utility.h"
48 
49 using namespace hpp::fcl;
50 
51 void randomOBBs(Vec3f& a, Vec3f& b, FCL_REAL extentNorm) {
52  // Extent norm is between 0 and extentNorm on each axis
53  // a = (Vec3f::Ones()+Vec3f::Random()) * extentNorm / (2*sqrt(3));
54  // b = (Vec3f::Ones()+Vec3f::Random()) * extentNorm / (2*sqrt(3));
55 
56  a = extentNorm * Vec3f::Random().cwiseAbs().normalized();
57  b = extentNorm * Vec3f::Random().cwiseAbs().normalized();
58 }
59 
60 void randomTransform(Matrix3f& B, Vec3f& T, const Vec3f& a, const Vec3f& b,
61  const FCL_REAL extentNorm) {
62  // TODO Should we scale T to a and b norm ?
63  (void)a;
64  (void)b;
65  (void)extentNorm;
66 
67  FCL_REAL N = a.norm() + b.norm();
68  // A translation of norm N ensures there is no collision.
69  // Set translation to be between 0 and 2 * N;
70  T = (Vec3f::Random() / sqrt(3)) * 1.5 * N;
71  // T.setZero();
72 
73  Quaternion3f q;
74  q.coeffs().setRandom();
75  q.normalize();
76  B = q;
77 }
78 
79 #define NB_METHODS 7
80 #define MANUAL_PRODUCT 1
81 
82 #if MANUAL_PRODUCT
83 #define PRODUCT(M33, v3) \
84  (M33.col(0) * v3[0] + M33.col(1) * v3[1] + M33.col(2) * v3[2])
85 #else
86 #define PRODUCT(M33, v3) (M33 * v3)
87 #endif
88 
89 typedef std::chrono::high_resolution_clock clock_type;
90 typedef clock_type::duration duration_type;
91 
92 const char* sep = ",\t";
93 const FCL_REAL eps = 1.5e-7;
94 
95 const Eigen::IOFormat py_fmt(Eigen::FullPrecision, 0,
96  ", ", // Coeff separator
97  ",\n", // Row separator
98  "(", // row prefix
99  ",)", // row suffix
100  "( ", // mat prefix
101  ", )" // mat suffix
102 );
103 
104 namespace obbDisjoint_impls {
106 bool distance(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b,
107  FCL_REAL& distance) {
108  GJKSolver gjk;
109  Box ba(2 * a), bb(2 * b);
110  Transform3f tfa, tfb(B, T);
111 
112  Vec3f p1, p2, normal;
113  return gjk.shapeDistance(ba, tfa, bb, tfb, distance, p1, p2, normal);
114 }
115 
116 inline FCL_REAL _computeDistanceForCase1(const Vec3f& T, const Vec3f& a,
117  const Vec3f& b, const Matrix3f& Bf) {
118  Vec3f AABB_corner;
119  /* This seems to be slower
120  AABB_corner.noalias() = T.cwiseAbs () - a;
121  AABB_corner.noalias() -= PRODUCT(Bf,b);
122  /*/
123 #if MANUAL_PRODUCT
124  AABB_corner.noalias() = T.cwiseAbs() - a;
125  AABB_corner.noalias() -= Bf.col(0) * b[0];
126  AABB_corner.noalias() -= Bf.col(1) * b[1];
127  AABB_corner.noalias() -= Bf.col(2) * b[2];
128 #else
129  AABB_corner.noalias() = T.cwiseAbs() - Bf * b - a;
130 #endif
131  // */
132  return AABB_corner.array().max(FCL_REAL(0)).matrix().squaredNorm();
133 }
134 
135 inline FCL_REAL _computeDistanceForCase2(const Matrix3f& B, const Vec3f& T,
136  const Vec3f& a, const Vec3f& b,
137  const Matrix3f& Bf) {
138  /*
139  Vec3f AABB_corner(PRODUCT(B.transpose(), T).cwiseAbs() - b);
140  AABB_corner.noalias() -= PRODUCT(Bf.transpose(), a);
141  return AABB_corner.array().max(FCL_REAL(0)).matrix().squaredNorm ();
142  /*/
143 #if MANUAL_PRODUCT
144  FCL_REAL s, t = 0;
145  s = std::abs(B.col(0).dot(T)) - Bf.col(0).dot(a) - b[0];
146  if (s > 0) t += s * s;
147  s = std::abs(B.col(1).dot(T)) - Bf.col(1).dot(a) - b[1];
148  if (s > 0) t += s * s;
149  s = std::abs(B.col(2).dot(T)) - Bf.col(2).dot(a) - b[2];
150  if (s > 0) t += s * s;
151  return t;
152 #else
153  Vec3f AABB_corner((B.transpose() * T).cwiseAbs() - Bf.transpose() * a - b);
154  return AABB_corner.array().max(FCL_REAL(0)).matrix().squaredNorm();
155 #endif
156  // */
157 }
158 
159 int separatingAxisId(const Matrix3f& B, const Vec3f& T, const Vec3f& a,
160  const Vec3f& b, const FCL_REAL& breakDistance2,
161  FCL_REAL& squaredLowerBoundDistance) {
162  int id = 0;
163 
164  Matrix3f Bf(B.cwiseAbs());
165 
166  // Corner of b axis aligned bounding box the closest to the origin
167  squaredLowerBoundDistance = _computeDistanceForCase1(T, a, b, Bf);
168  if (squaredLowerBoundDistance > breakDistance2) return id;
169  ++id;
170 
171  // | B^T T| - b - Bf^T a
172  squaredLowerBoundDistance = _computeDistanceForCase2(B, T, a, b, Bf);
173  if (squaredLowerBoundDistance > breakDistance2) return id;
174  ++id;
175 
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);
180 
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));
183  // We need to divide by the norm || Aia x Bib ||
184  // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2
185  if (diff > 0) {
186  FCL_REAL sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib);
187  if (sinus2 > 1e-6) {
188  squaredLowerBoundDistance = diff * diff / sinus2;
189  if (squaredLowerBoundDistance > breakDistance2) {
190  return id;
191  }
192  }
193  /* // or
194  FCL_REAL sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib);
195  squaredLowerBoundDistance = diff * diff;
196  if (squaredLowerBoundDistance > breakDistance2 * sinus2) {
197  squaredLowerBoundDistance /= sinus2;
198  return true;
199  }
200  // */
201  }
202  ++id;
203 
204  jb = kb;
205  kb = ib;
206  }
207  ja = ka;
208  ka = ia;
209  }
210 
211  return id;
212 }
213 
214 // ------------------------ 0 --------------------------------------
215 bool withRuntimeLoop(const Matrix3f& B, const Vec3f& T, const Vec3f& a,
216  const Vec3f& b, const FCL_REAL& breakDistance2,
217  FCL_REAL& squaredLowerBoundDistance) {
218  Matrix3f Bf(B.cwiseAbs());
219 
220  // Corner of b axis aligned bounding box the closest to the origin
221  squaredLowerBoundDistance = _computeDistanceForCase1(T, a, b, Bf);
222  if (squaredLowerBoundDistance > breakDistance2) return true;
223 
224  // | B^T T| - b - Bf^T a
225  squaredLowerBoundDistance = _computeDistanceForCase2(B, T, a, b, Bf);
226  if (squaredLowerBoundDistance > breakDistance2) return true;
227 
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);
232 
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));
235  // We need to divide by the norm || Aia x Bib ||
236  // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2
237  if (diff > 0) {
238  FCL_REAL sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib);
239  if (sinus2 > 1e-6) {
240  squaredLowerBoundDistance = diff * diff / sinus2;
241  if (squaredLowerBoundDistance > breakDistance2) {
242  return true;
243  }
244  }
245  /* // or
246  FCL_REAL sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib);
247  squaredLowerBoundDistance = diff * diff;
248  if (squaredLowerBoundDistance > breakDistance2 * sinus2) {
249  squaredLowerBoundDistance /= sinus2;
250  return true;
251  }
252  // */
253  }
254 
255  jb = kb;
256  kb = ib;
257  }
258  ja = ka;
259  ka = ia;
260  }
261 
262  return false;
263 }
264 
265 // ------------------------ 1 --------------------------------------
266 bool withManualLoopUnrolling_1(const Matrix3f& B, const Vec3f& T,
267  const Vec3f& a, const Vec3f& b,
268  const FCL_REAL& breakDistance2,
269  FCL_REAL& squaredLowerBoundDistance) {
270  FCL_REAL t, s;
271  FCL_REAL diff;
272 
273  // Matrix3f Bf = abs(B);
274  // Bf += reps;
275  Matrix3f Bf(B.cwiseAbs());
276 
277  // Corner of b axis aligned bounding box the closest to the origin
278  Vec3f AABB_corner(T.cwiseAbs() - Bf * b);
279  Vec3f diff3(AABB_corner - a);
280  diff3 = diff3.cwiseMax(Vec3f::Zero());
281 
282  // for (Vec3f::Index i=0; i<3; ++i) diff3 [i] = std::max (0, diff3 [i]);
283  squaredLowerBoundDistance = diff3.squaredNorm();
284  if (squaredLowerBoundDistance > breakDistance2) return true;
285 
286  AABB_corner = (B.transpose() * T).cwiseAbs() - Bf.transpose() * a;
287  // diff3 = | B^T T| - b - Bf^T a
288  diff3 = AABB_corner - b;
289  diff3 = diff3.cwiseMax(Vec3f::Zero());
290  squaredLowerBoundDistance = diff3.squaredNorm();
291 
292  if (squaredLowerBoundDistance > breakDistance2) return true;
293 
294  // A0 x B0
295  s = T[2] * B(1, 0) - T[1] * B(2, 0);
296  t = ((s < 0.0) ? -s : s);
297  assert(t == fabs(s));
298 
299  FCL_REAL sinus2;
300  diff = t - (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + b[1] * Bf(0, 2) +
301  b[2] * Bf(0, 1));
302  // We need to divide by the norm || A0 x B0 ||
303  // As ||A0|| = ||B0|| = 1,
304  // 2 2
305  // || A0 x B0 || + (A0 | B0) = 1
306  if (diff > 0) {
307  sinus2 = 1 - Bf(0, 0) * Bf(0, 0);
308  if (sinus2 > 1e-6) {
309  squaredLowerBoundDistance = diff * diff / sinus2;
310  if (squaredLowerBoundDistance > breakDistance2) {
311  return true;
312  }
313  }
314  }
315 
316  // A0 x B1
317  s = T[2] * B(1, 1) - T[1] * B(2, 1);
318  t = ((s < 0.0) ? -s : s);
319  assert(t == fabs(s));
320 
321  diff = t - (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) + b[0] * Bf(0, 2) +
322  b[2] * Bf(0, 0));
323  if (diff > 0) {
324  sinus2 = 1 - Bf(0, 1) * Bf(0, 1);
325  if (sinus2 > 1e-6) {
326  squaredLowerBoundDistance = diff * diff / sinus2;
327  if (squaredLowerBoundDistance > breakDistance2) {
328  return true;
329  }
330  }
331  }
332 
333  // A0 x B2
334  s = T[2] * B(1, 2) - T[1] * B(2, 2);
335  t = ((s < 0.0) ? -s : s);
336  assert(t == fabs(s));
337 
338  diff = t - (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) + b[0] * Bf(0, 1) +
339  b[1] * Bf(0, 0));
340  if (diff > 0) {
341  sinus2 = 1 - Bf(0, 2) * Bf(0, 2);
342  if (sinus2 > 1e-6) {
343  squaredLowerBoundDistance = diff * diff / sinus2;
344  if (squaredLowerBoundDistance > breakDistance2) {
345  return true;
346  }
347  }
348  }
349 
350  // A1 x B0
351  s = T[0] * B(2, 0) - T[2] * B(0, 0);
352  t = ((s < 0.0) ? -s : s);
353  assert(t == fabs(s));
354 
355  diff = t - (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) + b[1] * Bf(1, 2) +
356  b[2] * Bf(1, 1));
357  if (diff > 0) {
358  sinus2 = 1 - Bf(1, 0) * Bf(1, 0);
359  if (sinus2 > 1e-6) {
360  squaredLowerBoundDistance = diff * diff / sinus2;
361  if (squaredLowerBoundDistance > breakDistance2) {
362  return true;
363  }
364  }
365  }
366 
367  // A1 x B1
368  s = T[0] * B(2, 1) - T[2] * B(0, 1);
369  t = ((s < 0.0) ? -s : s);
370  assert(t == fabs(s));
371 
372  diff = t - (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) + b[0] * Bf(1, 2) +
373  b[2] * Bf(1, 0));
374  if (diff > 0) {
375  sinus2 = 1 - Bf(1, 1) * Bf(1, 1);
376  if (sinus2 > 1e-6) {
377  squaredLowerBoundDistance = diff * diff / sinus2;
378  if (squaredLowerBoundDistance > breakDistance2) {
379  return true;
380  }
381  }
382  }
383 
384  // A1 x B2
385  s = T[0] * B(2, 2) - T[2] * B(0, 2);
386  t = ((s < 0.0) ? -s : s);
387  assert(t == fabs(s));
388 
389  diff = t - (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) + b[0] * Bf(1, 1) +
390  b[1] * Bf(1, 0));
391  if (diff > 0) {
392  sinus2 = 1 - Bf(1, 2) * Bf(1, 2);
393  if (sinus2 > 1e-6) {
394  squaredLowerBoundDistance = diff * diff / sinus2;
395  if (squaredLowerBoundDistance > breakDistance2) {
396  return true;
397  }
398  }
399  }
400 
401  // A2 x B0
402  s = T[1] * B(0, 0) - T[0] * B(1, 0);
403  t = ((s < 0.0) ? -s : s);
404  assert(t == fabs(s));
405 
406  diff = t - (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) + b[1] * Bf(2, 2) +
407  b[2] * Bf(2, 1));
408  if (diff > 0) {
409  sinus2 = 1 - Bf(2, 0) * Bf(2, 0);
410  if (sinus2 > 1e-6) {
411  squaredLowerBoundDistance = diff * diff / sinus2;
412  if (squaredLowerBoundDistance > breakDistance2) {
413  return true;
414  }
415  }
416  }
417 
418  // A2 x B1
419  s = T[1] * B(0, 1) - T[0] * B(1, 1);
420  t = ((s < 0.0) ? -s : s);
421  assert(t == fabs(s));
422 
423  diff = t - (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) + b[0] * Bf(2, 2) +
424  b[2] * Bf(2, 0));
425  if (diff > 0) {
426  sinus2 = 1 - Bf(2, 1) * Bf(2, 1);
427  if (sinus2 > 1e-6) {
428  squaredLowerBoundDistance = diff * diff / sinus2;
429  if (squaredLowerBoundDistance > breakDistance2) {
430  return true;
431  }
432  }
433  }
434 
435  // A2 x B2
436  s = T[1] * B(0, 2) - T[0] * B(1, 2);
437  t = ((s < 0.0) ? -s : s);
438  assert(t == fabs(s));
439 
440  diff = t - (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) + b[0] * Bf(2, 1) +
441  b[1] * Bf(2, 0));
442  if (diff > 0) {
443  sinus2 = 1 - Bf(2, 2) * Bf(2, 2);
444  if (sinus2 > 1e-6) {
445  squaredLowerBoundDistance = diff * diff / sinus2;
446  if (squaredLowerBoundDistance > breakDistance2) {
447  return true;
448  }
449  }
450  }
451 
452  return false;
453 }
454 
455 // ------------------------ 2 --------------------------------------
456 bool withManualLoopUnrolling_2(const Matrix3f& B, const Vec3f& T,
457  const Vec3f& a, const Vec3f& b,
458  const FCL_REAL& breakDistance2,
459  FCL_REAL& squaredLowerBoundDistance) {
460  Matrix3f Bf(B.cwiseAbs());
461 
462  // Corner of b axis aligned bounding box the closest to the origin
463  squaredLowerBoundDistance = _computeDistanceForCase1(T, a, b, Bf);
464  if (squaredLowerBoundDistance > breakDistance2) return true;
465 
466  squaredLowerBoundDistance = _computeDistanceForCase2(B, T, a, b, Bf);
467  if (squaredLowerBoundDistance > breakDistance2) return true;
468 
469  // A0 x B0
470  FCL_REAL t, s;
471  s = T[2] * B(1, 0) - T[1] * B(2, 0);
472  t = ((s < 0.0) ? -s : s);
473  assert(t == fabs(s));
474 
475  FCL_REAL sinus2;
476  FCL_REAL diff;
477  diff = t - (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + b[1] * Bf(0, 2) +
478  b[2] * Bf(0, 1));
479  // We need to divide by the norm || A0 x B0 ||
480  // As ||A0|| = ||B0|| = 1,
481  // 2 2
482  // || A0 x B0 || + (A0 | B0) = 1
483  if (diff > 0) {
484  sinus2 = 1 - Bf(0, 0) * Bf(0, 0);
485  if (sinus2 > 1e-6) {
486  squaredLowerBoundDistance = diff * diff / sinus2;
487  if (squaredLowerBoundDistance > breakDistance2) {
488  return true;
489  }
490  }
491  }
492 
493  // A0 x B1
494  s = T[2] * B(1, 1) - T[1] * B(2, 1);
495  t = ((s < 0.0) ? -s : s);
496  assert(t == fabs(s));
497 
498  diff = t - (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) + b[0] * Bf(0, 2) +
499  b[2] * Bf(0, 0));
500  if (diff > 0) {
501  sinus2 = 1 - Bf(0, 1) * Bf(0, 1);
502  if (sinus2 > 1e-6) {
503  squaredLowerBoundDistance = diff * diff / sinus2;
504  if (squaredLowerBoundDistance > breakDistance2) {
505  return true;
506  }
507  }
508  }
509 
510  // A0 x B2
511  s = T[2] * B(1, 2) - T[1] * B(2, 2);
512  t = ((s < 0.0) ? -s : s);
513  assert(t == fabs(s));
514 
515  diff = t - (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) + b[0] * Bf(0, 1) +
516  b[1] * Bf(0, 0));
517  if (diff > 0) {
518  sinus2 = 1 - Bf(0, 2) * Bf(0, 2);
519  if (sinus2 > 1e-6) {
520  squaredLowerBoundDistance = diff * diff / sinus2;
521  if (squaredLowerBoundDistance > breakDistance2) {
522  return true;
523  }
524  }
525  }
526 
527  // A1 x B0
528  s = T[0] * B(2, 0) - T[2] * B(0, 0);
529  t = ((s < 0.0) ? -s : s);
530  assert(t == fabs(s));
531 
532  diff = t - (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) + b[1] * Bf(1, 2) +
533  b[2] * Bf(1, 1));
534  if (diff > 0) {
535  sinus2 = 1 - Bf(1, 0) * Bf(1, 0);
536  if (sinus2 > 1e-6) {
537  squaredLowerBoundDistance = diff * diff / sinus2;
538  if (squaredLowerBoundDistance > breakDistance2) {
539  return true;
540  }
541  }
542  }
543 
544  // A1 x B1
545  s = T[0] * B(2, 1) - T[2] * B(0, 1);
546  t = ((s < 0.0) ? -s : s);
547  assert(t == fabs(s));
548 
549  diff = t - (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) + b[0] * Bf(1, 2) +
550  b[2] * Bf(1, 0));
551  if (diff > 0) {
552  sinus2 = 1 - Bf(1, 1) * Bf(1, 1);
553  if (sinus2 > 1e-6) {
554  squaredLowerBoundDistance = diff * diff / sinus2;
555  if (squaredLowerBoundDistance > breakDistance2) {
556  return true;
557  }
558  }
559  }
560 
561  // A1 x B2
562  s = T[0] * B(2, 2) - T[2] * B(0, 2);
563  t = ((s < 0.0) ? -s : s);
564  assert(t == fabs(s));
565 
566  diff = t - (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) + b[0] * Bf(1, 1) +
567  b[1] * Bf(1, 0));
568  if (diff > 0) {
569  sinus2 = 1 - Bf(1, 2) * Bf(1, 2);
570  if (sinus2 > 1e-6) {
571  squaredLowerBoundDistance = diff * diff / sinus2;
572  if (squaredLowerBoundDistance > breakDistance2) {
573  return true;
574  }
575  }
576  }
577 
578  // A2 x B0
579  s = T[1] * B(0, 0) - T[0] * B(1, 0);
580  t = ((s < 0.0) ? -s : s);
581  assert(t == fabs(s));
582 
583  diff = t - (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) + b[1] * Bf(2, 2) +
584  b[2] * Bf(2, 1));
585  if (diff > 0) {
586  sinus2 = 1 - Bf(2, 0) * Bf(2, 0);
587  if (sinus2 > 1e-6) {
588  squaredLowerBoundDistance = diff * diff / sinus2;
589  if (squaredLowerBoundDistance > breakDistance2) {
590  return true;
591  }
592  }
593  }
594 
595  // A2 x B1
596  s = T[1] * B(0, 1) - T[0] * B(1, 1);
597  t = ((s < 0.0) ? -s : s);
598  assert(t == fabs(s));
599 
600  diff = t - (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) + b[0] * Bf(2, 2) +
601  b[2] * Bf(2, 0));
602  if (diff > 0) {
603  sinus2 = 1 - Bf(2, 1) * Bf(2, 1);
604  if (sinus2 > 1e-6) {
605  squaredLowerBoundDistance = diff * diff / sinus2;
606  if (squaredLowerBoundDistance > breakDistance2) {
607  return true;
608  }
609  }
610  }
611 
612  // A2 x B2
613  s = T[1] * B(0, 2) - T[0] * B(1, 2);
614  t = ((s < 0.0) ? -s : s);
615  assert(t == fabs(s));
616 
617  diff = t - (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) + b[0] * Bf(2, 1) +
618  b[1] * Bf(2, 0));
619  if (diff > 0) {
620  sinus2 = 1 - Bf(2, 2) * Bf(2, 2);
621  if (sinus2 > 1e-6) {
622  squaredLowerBoundDistance = diff * diff / sinus2;
623  if (squaredLowerBoundDistance > breakDistance2) {
624  return true;
625  }
626  }
627  }
628 
629  return false;
630 }
631 
632 // ------------------------ 3 --------------------------------------
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>
635 struct loop_case_1 {
636  static inline bool run(const Matrix3f& B, const Vec3f& T, const Vec3f& a,
637  const Vec3f& b, const Matrix3f& Bf,
638  const FCL_REAL& breakDistance2,
639  FCL_REAL& squaredLowerBoundDistance) {
640  const FCL_REAL s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib);
641 
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));
644  // We need to divide by the norm || Aia x Bib ||
645  // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2
646  if (diff > 0) {
647  FCL_REAL sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib);
648  if (sinus2 > 1e-6) {
649  squaredLowerBoundDistance = diff * diff / sinus2;
650  if (squaredLowerBoundDistance > breakDistance2) {
651  return true;
652  }
653  }
654  /* // or
655  FCL_REAL sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib);
656  squaredLowerBoundDistance = diff * diff;
657  if (squaredLowerBoundDistance > breakDistance2 * sinus2) {
658  squaredLowerBoundDistance /= sinus2;
659  return true;
660  }
661  // */
662  }
663  return false;
664  }
665 };
666 
667 bool withTemplateLoopUnrolling_1(const Matrix3f& B, const Vec3f& T,
668  const Vec3f& a, const Vec3f& b,
669  const FCL_REAL& breakDistance2,
670  FCL_REAL& squaredLowerBoundDistance) {
671  Matrix3f Bf(B.cwiseAbs());
672 
673  // Corner of b axis aligned bounding box the closest to the origin
674  squaredLowerBoundDistance = _computeDistanceForCase1(T, a, b, Bf);
675  if (squaredLowerBoundDistance > breakDistance2) return true;
676 
677  squaredLowerBoundDistance = _computeDistanceForCase2(B, T, a, b, Bf);
678  if (squaredLowerBoundDistance > breakDistance2) return true;
679 
680  // Ai x Bj
681  if (loop_case_1<0, 0>::run(B, T, a, b, Bf, breakDistance2,
682  squaredLowerBoundDistance))
683  return true;
684  if (loop_case_1<0, 1>::run(B, T, a, b, Bf, breakDistance2,
685  squaredLowerBoundDistance))
686  return true;
687  if (loop_case_1<0, 2>::run(B, T, a, b, Bf, breakDistance2,
688  squaredLowerBoundDistance))
689  return true;
690  if (loop_case_1<1, 0>::run(B, T, a, b, Bf, breakDistance2,
691  squaredLowerBoundDistance))
692  return true;
693  if (loop_case_1<1, 1>::run(B, T, a, b, Bf, breakDistance2,
694  squaredLowerBoundDistance))
695  return true;
696  if (loop_case_1<1, 2>::run(B, T, a, b, Bf, breakDistance2,
697  squaredLowerBoundDistance))
698  return true;
699  if (loop_case_1<2, 0>::run(B, T, a, b, Bf, breakDistance2,
700  squaredLowerBoundDistance))
701  return true;
702  if (loop_case_1<2, 1>::run(B, T, a, b, Bf, breakDistance2,
703  squaredLowerBoundDistance))
704  return true;
705  if (loop_case_1<2, 2>::run(B, T, a, b, Bf, breakDistance2,
706  squaredLowerBoundDistance))
707  return true;
708 
709  return false;
710 }
711 
712 // ------------------------ 4 --------------------------------------
713 
714 template <int ib, int jb = (ib + 1) % 3, int kb = (ib + 2) % 3>
715 struct loop_case_2 {
716  static inline bool run(int ia, int ja, int ka, const Matrix3f& B,
717  const Vec3f& T, const Vec3f& a, const Vec3f& b,
718  const Matrix3f& Bf, const FCL_REAL& breakDistance2,
719  FCL_REAL& squaredLowerBoundDistance) {
720  const FCL_REAL s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib);
721 
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));
724  // We need to divide by the norm || Aia x Bib ||
725  // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2
726  if (diff > 0) {
727  FCL_REAL sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib);
728  if (sinus2 > 1e-6) {
729  squaredLowerBoundDistance = diff * diff / sinus2;
730  if (squaredLowerBoundDistance > breakDistance2) {
731  return true;
732  }
733  }
734  /* // or
735  FCL_REAL sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib);
736  squaredLowerBoundDistance = diff * diff;
737  if (squaredLowerBoundDistance > breakDistance2 * sinus2) {
738  squaredLowerBoundDistance /= sinus2;
739  return true;
740  }
741  // */
742  }
743  return false;
744  }
745 };
746 
747 bool withPartialTemplateLoopUnrolling_1(const Matrix3f& B, const Vec3f& T,
748  const Vec3f& a, const Vec3f& b,
749  const FCL_REAL& breakDistance2,
750  FCL_REAL& squaredLowerBoundDistance) {
751  Matrix3f Bf(B.cwiseAbs());
752 
753  // Corner of b axis aligned bounding box the closest to the origin
754  squaredLowerBoundDistance = _computeDistanceForCase1(T, a, b, Bf);
755  if (squaredLowerBoundDistance > breakDistance2) return true;
756 
757  squaredLowerBoundDistance = _computeDistanceForCase2(B, T, a, b, Bf);
758  if (squaredLowerBoundDistance > breakDistance2) return true;
759 
760  // Ai x Bj
761  int ja = 1, ka = 2;
762  for (int ia = 0; ia < 3; ++ia) {
763  if (loop_case_2<0>::run(ia, ja, ka, B, T, a, b, Bf, breakDistance2,
764  squaredLowerBoundDistance))
765  return true;
766  if (loop_case_2<1>::run(ia, ja, ka, B, T, a, b, Bf, breakDistance2,
767  squaredLowerBoundDistance))
768  return true;
769  if (loop_case_2<2>::run(ia, ja, ka, B, T, a, b, Bf, breakDistance2,
770  squaredLowerBoundDistance))
771  return true;
772  ja = ka;
773  ka = ia;
774  }
775 
776  return false;
777 }
778 
779 // ------------------------ 5 --------------------------------------
780 bool originalWithLowerBound(const Matrix3f& B, const Vec3f& T, const Vec3f& a,
781  const Vec3f& b, const FCL_REAL& breakDistance2,
782  FCL_REAL& squaredLowerBoundDistance) {
783  FCL_REAL t, s;
784  FCL_REAL diff;
785 
786  Matrix3f Bf(B.cwiseAbs());
787  squaredLowerBoundDistance = 0;
788 
789  // if any of these tests are one-sided, then the polyhedra are disjoint
790 
791  // A1 x A2 = A0
792  t = ((T[0] < 0.0) ? -T[0] : T[0]);
793 
794  diff = t - (a[0] + Bf.row(0).dot(b));
795  if (diff > 0) {
796  squaredLowerBoundDistance = diff * diff;
797  }
798 
799  // A2 x A0 = A1
800  t = ((T[1] < 0.0) ? -T[1] : T[1]);
801 
802  diff = t - (a[1] + Bf.row(1).dot(b));
803  if (diff > 0) {
804  squaredLowerBoundDistance += diff * diff;
805  }
806 
807  // A0 x A1 = A2
808  t = ((T[2] < 0.0) ? -T[2] : T[2]);
809 
810  diff = t - (a[2] + Bf.row(2).dot(b));
811  if (diff > 0) {
812  squaredLowerBoundDistance += diff * diff;
813  }
814 
815  if (squaredLowerBoundDistance > breakDistance2) return true;
816 
817  squaredLowerBoundDistance = 0;
818 
819  // B1 x B2 = B0
820  s = B.col(0).dot(T);
821  t = ((s < 0.0) ? -s : s);
822 
823  diff = t - (b[0] + Bf.col(0).dot(a));
824  if (diff > 0) {
825  squaredLowerBoundDistance += diff * diff;
826  }
827 
828  // B2 x B0 = B1
829  s = B.col(1).dot(T);
830  t = ((s < 0.0) ? -s : s);
831 
832  diff = t - (b[1] + Bf.col(1).dot(a));
833  if (diff > 0) {
834  squaredLowerBoundDistance += diff * diff;
835  }
836 
837  // B0 x B1 = B2
838  s = B.col(2).dot(T);
839  t = ((s < 0.0) ? -s : s);
840 
841  diff = t - (b[2] + Bf.col(2).dot(a));
842  if (diff > 0) {
843  squaredLowerBoundDistance += diff * diff;
844  }
845 
846  if (squaredLowerBoundDistance > breakDistance2) return true;
847 
848  // A0 x B0
849  s = T[2] * B(1, 0) - T[1] * B(2, 0);
850  t = ((s < 0.0) ? -s : s);
851 
852  FCL_REAL sinus2;
853  diff = t - (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + b[1] * Bf(0, 2) +
854  b[2] * Bf(0, 1));
855  // We need to divide by the norm || A0 x B0 ||
856  // As ||A0|| = ||B0|| = 1,
857  // 2 2
858  // || A0 x B0 || + (A0 | B0) = 1
859  if (diff > 0) {
860  sinus2 = 1 - Bf(0, 0) * Bf(0, 0);
861  if (sinus2 > 1e-6) {
862  squaredLowerBoundDistance = diff * diff / sinus2;
863  if (squaredLowerBoundDistance > breakDistance2) {
864  return true;
865  }
866  }
867  }
868 
869  // A0 x B1
870  s = T[2] * B(1, 1) - T[1] * B(2, 1);
871  t = ((s < 0.0) ? -s : s);
872 
873  diff = t - (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) + b[0] * Bf(0, 2) +
874  b[2] * Bf(0, 0));
875  if (diff > 0) {
876  sinus2 = 1 - Bf(0, 1) * Bf(0, 1);
877  if (sinus2 > 1e-6) {
878  squaredLowerBoundDistance = diff * diff / sinus2;
879  if (squaredLowerBoundDistance > breakDistance2) {
880  return true;
881  }
882  }
883  }
884 
885  // A0 x B2
886  s = T[2] * B(1, 2) - T[1] * B(2, 2);
887  t = ((s < 0.0) ? -s : s);
888 
889  diff = t - (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) + b[0] * Bf(0, 1) +
890  b[1] * Bf(0, 0));
891  if (diff > 0) {
892  sinus2 = 1 - Bf(0, 2) * Bf(0, 2);
893  if (sinus2 > 1e-6) {
894  squaredLowerBoundDistance = diff * diff / sinus2;
895  if (squaredLowerBoundDistance > breakDistance2) {
896  return true;
897  }
898  }
899  }
900 
901  // A1 x B0
902  s = T[0] * B(2, 0) - T[2] * B(0, 0);
903  t = ((s < 0.0) ? -s : s);
904 
905  diff = t - (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) + b[1] * Bf(1, 2) +
906  b[2] * Bf(1, 1));
907  if (diff > 0) {
908  sinus2 = 1 - Bf(1, 0) * Bf(1, 0);
909  if (sinus2 > 1e-6) {
910  squaredLowerBoundDistance = diff * diff / sinus2;
911  if (squaredLowerBoundDistance > breakDistance2) {
912  return true;
913  }
914  }
915  }
916 
917  // A1 x B1
918  s = T[0] * B(2, 1) - T[2] * B(0, 1);
919  t = ((s < 0.0) ? -s : s);
920 
921  diff = t - (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) + b[0] * Bf(1, 2) +
922  b[2] * Bf(1, 0));
923  if (diff > 0) {
924  sinus2 = 1 - Bf(1, 1) * Bf(1, 1);
925  if (sinus2 > 1e-6) {
926  squaredLowerBoundDistance = diff * diff / sinus2;
927  if (squaredLowerBoundDistance > breakDistance2) {
928  return true;
929  }
930  }
931  }
932 
933  // A1 x B2
934  s = T[0] * B(2, 2) - T[2] * B(0, 2);
935  t = ((s < 0.0) ? -s : s);
936 
937  diff = t - (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) + b[0] * Bf(1, 1) +
938  b[1] * Bf(1, 0));
939  if (diff > 0) {
940  sinus2 = 1 - Bf(1, 2) * Bf(1, 2);
941  if (sinus2 > 1e-6) {
942  squaredLowerBoundDistance = diff * diff / sinus2;
943  if (squaredLowerBoundDistance > breakDistance2) {
944  return true;
945  }
946  }
947  }
948 
949  // A2 x B0
950  s = T[1] * B(0, 0) - T[0] * B(1, 0);
951  t = ((s < 0.0) ? -s : s);
952 
953  diff = t - (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) + b[1] * Bf(2, 2) +
954  b[2] * Bf(2, 1));
955  if (diff > 0) {
956  sinus2 = 1 - Bf(2, 0) * Bf(2, 0);
957  if (sinus2 > 1e-6) {
958  squaredLowerBoundDistance = diff * diff / sinus2;
959  if (squaredLowerBoundDistance > breakDistance2) {
960  return true;
961  }
962  }
963  }
964 
965  // A2 x B1
966  s = T[1] * B(0, 1) - T[0] * B(1, 1);
967  t = ((s < 0.0) ? -s : s);
968 
969  diff = t - (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) + b[0] * Bf(2, 2) +
970  b[2] * Bf(2, 0));
971  if (diff > 0) {
972  sinus2 = 1 - Bf(2, 1) * Bf(2, 1);
973  if (sinus2 > 1e-6) {
974  squaredLowerBoundDistance = diff * diff / sinus2;
975  if (squaredLowerBoundDistance > breakDistance2) {
976  return true;
977  }
978  }
979  }
980 
981  // A2 x B2
982  s = T[1] * B(0, 2) - T[0] * B(1, 2);
983  t = ((s < 0.0) ? -s : s);
984 
985  diff = t - (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) + b[0] * Bf(2, 1) +
986  b[1] * Bf(2, 0));
987  if (diff > 0) {
988  sinus2 = 1 - Bf(2, 2) * Bf(2, 2);
989  if (sinus2 > 1e-6) {
990  squaredLowerBoundDistance = diff * diff / sinus2;
991  if (squaredLowerBoundDistance > breakDistance2) {
992  return true;
993  }
994  }
995  }
996 
997  return false;
998 }
999 
1000 // ------------------------ 6 --------------------------------------
1001 bool originalWithNoLowerBound(const Matrix3f& B, const Vec3f& T, const Vec3f& a,
1002  const Vec3f& b, const FCL_REAL&,
1003  FCL_REAL& squaredLowerBoundDistance) {
1004  FCL_REAL t, s;
1005  const FCL_REAL reps = 1e-6;
1006 
1007  squaredLowerBoundDistance = 0;
1008 
1009  Matrix3f Bf(B.array().abs() + reps);
1010  // Bf += reps;
1011 
1012  // if any of these tests are one-sided, then the polyhedra are disjoint
1013 
1014  // A1 x A2 = A0
1015  t = ((T[0] < 0.0) ? -T[0] : T[0]);
1016 
1017  // if(t > (a[0] + Bf.dotX(b)))
1018  if (t > (a[0] + Bf.row(0).dot(b))) return true;
1019 
1020  // B1 x B2 = B0
1021  // s = B.transposeDotX(T);
1022  s = B.col(0).dot(T);
1023  t = ((s < 0.0) ? -s : s);
1024 
1025  // if(t > (b[0] + Bf.transposeDotX(a)))
1026  if (t > (b[0] + Bf.col(0).dot(a))) return true;
1027 
1028  // A2 x A0 = A1
1029  t = ((T[1] < 0.0) ? -T[1] : T[1]);
1030 
1031  // if(t > (a[1] + Bf.dotY(b)))
1032  if (t > (a[1] + Bf.row(1).dot(b))) return true;
1033 
1034  // A0 x A1 = A2
1035  t = ((T[2] < 0.0) ? -T[2] : T[2]);
1036 
1037  // if(t > (a[2] + Bf.dotZ(b)))
1038  if (t > (a[2] + Bf.row(2).dot(b))) return true;
1039 
1040  // B2 x B0 = B1
1041  // s = B.transposeDotY(T);
1042  s = B.col(1).dot(T);
1043  t = ((s < 0.0) ? -s : s);
1044 
1045  // if(t > (b[1] + Bf.transposeDotY(a)))
1046  if (t > (b[1] + Bf.col(1).dot(a))) return true;
1047 
1048  // B0 x B1 = B2
1049  // s = B.transposeDotZ(T);
1050  s = B.col(2).dot(T);
1051  t = ((s < 0.0) ? -s : s);
1052 
1053  // if(t > (b[2] + Bf.transposeDotZ(a)))
1054  if (t > (b[2] + Bf.col(2).dot(a))) return true;
1055 
1056  // A0 x B0
1057  s = T[2] * B(1, 0) - T[1] * B(2, 0);
1058  t = ((s < 0.0) ? -s : s);
1059 
1060  if (t >
1061  (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + b[1] * Bf(0, 2) + b[2] * Bf(0, 1)))
1062  return true;
1063 
1064  // A0 x B1
1065  s = T[2] * B(1, 1) - T[1] * B(2, 1);
1066  t = ((s < 0.0) ? -s : s);
1067 
1068  if (t >
1069  (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) + b[0] * Bf(0, 2) + b[2] * Bf(0, 0)))
1070  return true;
1071 
1072  // A0 x B2
1073  s = T[2] * B(1, 2) - T[1] * B(2, 2);
1074  t = ((s < 0.0) ? -s : s);
1075 
1076  if (t >
1077  (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) + b[0] * Bf(0, 1) + b[1] * Bf(0, 0)))
1078  return true;
1079 
1080  // A1 x B0
1081  s = T[0] * B(2, 0) - T[2] * B(0, 0);
1082  t = ((s < 0.0) ? -s : s);
1083 
1084  if (t >
1085  (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) + b[1] * Bf(1, 2) + b[2] * Bf(1, 1)))
1086  return true;
1087 
1088  // A1 x B1
1089  s = T[0] * B(2, 1) - T[2] * B(0, 1);
1090  t = ((s < 0.0) ? -s : s);
1091 
1092  if (t >
1093  (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) + b[0] * Bf(1, 2) + b[2] * Bf(1, 0)))
1094  return true;
1095 
1096  // A1 x B2
1097  s = T[0] * B(2, 2) - T[2] * B(0, 2);
1098  t = ((s < 0.0) ? -s : s);
1099 
1100  if (t >
1101  (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) + b[0] * Bf(1, 1) + b[1] * Bf(1, 0)))
1102  return true;
1103 
1104  // A2 x B0
1105  s = T[1] * B(0, 0) - T[0] * B(1, 0);
1106  t = ((s < 0.0) ? -s : s);
1107 
1108  if (t >
1109  (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) + b[1] * Bf(2, 2) + b[2] * Bf(2, 1)))
1110  return true;
1111 
1112  // A2 x B1
1113  s = T[1] * B(0, 1) - T[0] * B(1, 1);
1114  t = ((s < 0.0) ? -s : s);
1115 
1116  if (t >
1117  (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) + b[0] * Bf(2, 2) + b[2] * Bf(2, 0)))
1118  return true;
1119 
1120  // A2 x B2
1121  s = T[1] * B(0, 2) - T[0] * B(1, 2);
1122  t = ((s < 0.0) ? -s : s);
1123 
1124  if (t >
1125  (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) + b[0] * Bf(2, 1) + b[1] * Bf(2, 0)))
1126  return true;
1127 
1128  return false;
1129 }
1130 } // namespace obbDisjoint_impls
1131 
1132 struct BenchmarkResult {
1136  int ifId;
1137  FCL_REAL distance;
1138  FCL_REAL squaredLowerBoundDistance;
1139  duration_type duration[NB_METHODS];
1140  bool failure;
1141 
1142  static std::ostream& headers(std::ostream& os) {
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;
1150  return os;
1151  }
1152 
1153  std::ostream& print(std::ostream& os) const {
1154  os << ifId << sep << std::sqrt(squaredLowerBoundDistance) << sep << distance
1155  << sep << failure;
1156  for (int i = 0; i < NB_METHODS; ++i)
1157  os << sep
1158  << static_cast<double>(
1159  std::chrono::duration_cast<std::chrono::nanoseconds>(
1160  duration[i])
1161  .count()) *
1162  1e-3;
1163  return os;
1164  }
1165 };
1166 
1167 std::ostream& operator<<(std::ostream& os, const BenchmarkResult& br) {
1168  return br.print(os);
1169 }
1170 
1171 BenchmarkResult benchmark_obb_case(const Matrix3f& B, const Vec3f& T,
1172  const Vec3f& a, const Vec3f& b,
1173  const CollisionRequest& request,
1174  std::size_t N) {
1175  const FCL_REAL breakDistance(request.break_distance +
1176  request.security_margin);
1177  const FCL_REAL breakDistance2 = breakDistance * breakDistance;
1178 
1179  BenchmarkResult result;
1180  // First determine which axis provide the answer
1181  result.ifId = obbDisjoint_impls::separatingAxisId(
1182  B, T, a, b, breakDistance2, result.squaredLowerBoundDistance);
1183  bool disjoint = obbDisjoint_impls::distance(B, T, a, b, result.distance);
1184  assert(0 <= result.ifId && result.ifId <= 11);
1185 
1186  // Sanity check
1187  result.failure = true;
1188  bool overlap = (result.ifId == 11);
1189  FCL_REAL dist_thr = request.break_distance + request.security_margin;
1190  if (!overlap && result.distance <= 0) {
1191  std::cerr << "Failure: negative distance for disjoint OBBs.";
1192  } else if (!overlap && result.squaredLowerBoundDistance < 0) {
1193  std::cerr << "Failure: negative distance lower bound.";
1194  } else if (!overlap && eps < std::sqrt(result.squaredLowerBoundDistance) -
1195  result.distance) {
1196  std::cerr << "Failure: distance is inferior to its lower bound (diff = "
1197  << std::sqrt(result.squaredLowerBoundDistance) - result.distance
1198  << ").";
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.";
1203  } else {
1204  result.failure = false;
1205  }
1206  if (result.failure) {
1207  std::cerr << "\nR = " << Quaternion3f(B).coeffs().transpose().format(py_fmt)
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'
1212  << std::endl;
1213  }
1214 
1215  // Compute time
1216  FCL_REAL tmp;
1217  clock_type::time_point start, end;
1218 
1219  // ------------------------ 0 --------------------------------------
1220  start = clock_type::now();
1221  for (std::size_t i = 0; i < N; ++i)
1222  obbDisjoint_impls::withRuntimeLoop(B, T, a, b, breakDistance2, tmp);
1223  end = clock_type::now();
1224  result.duration[0] = (end - start) / N;
1225 
1226  // ------------------------ 1 --------------------------------------
1227  start = clock_type::now();
1228  for (std::size_t i = 0; i < N; ++i)
1229  obbDisjoint_impls::withManualLoopUnrolling_1(B, T, a, b, breakDistance2,
1230  tmp);
1231  end = clock_type::now();
1232  result.duration[1] = (end - start) / N;
1233 
1234  // ------------------------ 2 --------------------------------------
1235  start = clock_type::now();
1236  for (std::size_t i = 0; i < N; ++i)
1237  obbDisjoint_impls::withManualLoopUnrolling_2(B, T, a, b, breakDistance2,
1238  tmp);
1239  end = clock_type::now();
1240  result.duration[2] = (end - start) / N;
1241 
1242  // ------------------------ 3 --------------------------------------
1243  start = clock_type::now();
1244  for (std::size_t i = 0; i < N; ++i)
1245  obbDisjoint_impls::withTemplateLoopUnrolling_1(B, T, a, b, breakDistance2,
1246  tmp);
1247  end = clock_type::now();
1248  result.duration[3] = (end - start) / N;
1249 
1250  // ------------------------ 4 --------------------------------------
1251  start = clock_type::now();
1252  for (std::size_t i = 0; i < N; ++i)
1253  obbDisjoint_impls::withPartialTemplateLoopUnrolling_1(B, T, a, b,
1254  breakDistance2, tmp);
1255  end = clock_type::now();
1256  result.duration[4] = (end - start) / N;
1257 
1258  // ------------------------ 5 --------------------------------------
1259  start = clock_type::now();
1260  for (std::size_t i = 0; i < N; ++i)
1261  obbDisjoint_impls::originalWithLowerBound(B, T, a, b, breakDistance2, tmp);
1262  end = clock_type::now();
1263  result.duration[5] = (end - start) / N;
1264 
1265  // ------------------------ 6 --------------------------------------
1266  start = clock_type::now();
1267  // The 2 last arguments are unused.
1268  for (std::size_t i = 0; i < N; ++i)
1269  obbDisjoint_impls::originalWithNoLowerBound(B, T, a, b, breakDistance2,
1270  tmp);
1271  end = clock_type::now();
1272  result.duration[6] = (end - start) / N;
1273 
1274  return result;
1275 }
1276 
1277 std::size_t obb_overlap_and_lower_bound_distance(std::ostream* output) {
1278  std::size_t nbFailure = 0;
1279 
1280  // Create two OBBs axis
1281  Vec3f a, b;
1282  Matrix3f B;
1283  Vec3f T;
1284  CollisionRequest request;
1285 
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;
1290 #else
1291  static const size_t nbRandomOBB = 100;
1292  static const size_t nbTransformPerOBB = 100;
1293  static const size_t nbRunForTimeMeas = 1000;
1294 #endif
1295  static const FCL_REAL extentNorm = 1.;
1296 
1297  if (output != NULL) *output << BenchmarkResult::headers << '\n';
1298 
1299  BenchmarkResult result;
1300  for (std::size_t iobb = 0; iobb < nbRandomOBB; ++iobb) {
1301  randomOBBs(a, b, extentNorm);
1302  for (std::size_t itf = 0; itf < nbTransformPerOBB; ++itf) {
1303  randomTransform(B, T, a, b, extentNorm);
1304  result = benchmark_obb_case(B, T, a, b, request, nbRunForTimeMeas);
1305  if (output != NULL) *output << result << '\n';
1306  if (result.failure) nbFailure++;
1307  }
1308  }
1309  return nbFailure;
1310 }
1311 
1312 int main(int argc, char** argv) {
1313  std::ostream* output = NULL;
1314  if (argc > 1 && strcmp(argv[1], "--generate-output") == 0) {
1315  output = &std::cout;
1316  }
1317 
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"
1322  "\nor OFF:"
1323  "\n\tsudo cpufreq-set --governor powersave"
1324  "\n";
1325 
1326  std::size_t nbFailure = obb_overlap_and_lower_bound_distance(output);
1327  if (nbFailure > INT_MAX) return INT_MAX;
1328  return (int)nbFailure;
1329 }
obbDisjoint_impls::loop_case_2::run
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)
Definition: obb.cpp:716
obbDisjoint_impls::loop_case_2
Definition: obb.cpp:715
obbDisjoint_impls::_computeDistanceForCase1
FCL_REAL _computeDistanceForCase1(const Vec3f &T, const Vec3f &a, const Vec3f &b, const Matrix3f &Bf)
Definition: obb.cpp:116
shape_shape_func.h
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
hpp::fcl::CollisionRequest::break_distance
FCL_REAL break_distance
Distance below which bounding volumes are broken down. See Collision.
Definition: collision_data.h:255
sep
const char * sep
Definition: obb.cpp:92
duration_type
clock_type::duration duration_type
Definition: obb.cpp:90
B
B
BenchmarkResult
Definition: obb.cpp:1132
utility.h
eps
const FCL_REAL eps
Definition: obb.cpp:93
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
BenchmarkResult::failure
bool failure
Definition: obb.cpp:1140
a
list a
narrowphase.h
obb_overlap_and_lower_bound_distance
std::size_t obb_overlap_and_lower_bound_distance(std::ostream *output)
Definition: obb.cpp:1277
BenchmarkResult::squaredLowerBoundDistance
FCL_REAL squaredLowerBoundDistance
Definition: obb.cpp:1138
obbDisjoint_impls::separatingAxisId
int separatingAxisId(const Matrix3f &B, const Vec3f &T, const Vec3f &a, const Vec3f &b, const FCL_REAL &breakDistance2, FCL_REAL &squaredLowerBoundDistance)
Definition: obb.cpp:159
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
BenchmarkResult::headers
static std::ostream & headers(std::ostream &os)
Definition: obb.cpp:1142
hpp::fcl::operator<<
static std::ostream & operator<<(std::ostream &o, const Quaternion3f &q)
Definition: transform.h:48
main
int main(int argc, char **argv)
Definition: obb.cpp:1312
hpp::fcl::GJKSolver
collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the ...
Definition: narrowphase.h:54
hpp::fcl::CollisionRequest::security_margin
FCL_REAL security_margin
Distance below which objects are considered in collision. See Collision.
Definition: collision_data.h:251
obbDisjoint_impls::withManualLoopUnrolling_2
bool withManualLoopUnrolling_2(const Matrix3f &B, const Vec3f &T, const Vec3f &a, const Vec3f &b, const FCL_REAL &breakDistance2, FCL_REAL &squaredLowerBoundDistance)
Definition: obb.cpp:456
BenchmarkResult::distance
FCL_REAL distance
Definition: obb.cpp:1137
benchmark_obb_case
BenchmarkResult benchmark_obb_case(const Matrix3f &B, const Vec3f &T, const Vec3f &a, const Vec3f &b, const CollisionRequest &request, std::size_t N)
Definition: obb.cpp:1171
hpp::fcl::CollisionRequest
request to the collision algorithm
Definition: collision_data.h:235
obbDisjoint_impls::originalWithNoLowerBound
bool originalWithNoLowerBound(const Matrix3f &B, const Vec3f &T, const Vec3f &a, const Vec3f &b, const FCL_REAL &, FCL_REAL &squaredLowerBoundDistance)
Definition: obb.cpp:1001
BenchmarkResult::duration
duration_type duration[NB_METHODS]
Definition: obb.cpp:1139
t
tuple t
q
q
hpp::fcl
Definition: broadphase_bruteforce.h:45
obbDisjoint_impls::distance
bool distance(const Matrix3f &B, const Vec3f &T, const Vec3f &a, const Vec3f &b, FCL_REAL &distance)
Definition: obb.cpp:106
octree.p1
tuple p1
Definition: octree.py:54
NB_METHODS
#define NB_METHODS
Definition: obb.cpp:79
hpp::fcl::Matrix3f
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:68
clock_type
std::chrono::high_resolution_clock clock_type
Definition: obb.cpp:89
obbDisjoint_impls::_computeDistanceForCase2
FCL_REAL _computeDistanceForCase2(const Matrix3f &B, const Vec3f &T, const Vec3f &a, const Vec3f &b, const Matrix3f &Bf)
Definition: obb.cpp:135
obbDisjoint_impls::withTemplateLoopUnrolling_1
bool withTemplateLoopUnrolling_1(const Matrix3f &B, const Vec3f &T, const Vec3f &a, const Vec3f &b, const FCL_REAL &breakDistance2, FCL_REAL &squaredLowerBoundDistance)
Definition: obb.cpp:667
generate_distance_plot.b
float b
Definition: generate_distance_plot.py:7
hpp::fcl::Transform3f
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
BenchmarkResult::print
std::ostream & print(std::ostream &os) const
Definition: obb.cpp:1153
hpp::fcl::overlap
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.
Definition: AABB.cpp:134
obbDisjoint_impls::loop_case_1::run
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)
Definition: obb.cpp:636
obbDisjoint_impls::withPartialTemplateLoopUnrolling_1
bool withPartialTemplateLoopUnrolling_1(const Matrix3f &B, const Vec3f &T, const Vec3f &a, const Vec3f &b, const FCL_REAL &breakDistance2, FCL_REAL &squaredLowerBoundDistance)
Definition: obb.cpp:747
obbDisjoint_impls::loop_case_1
Definition: obb.cpp:635
BenchmarkResult::ifId
int ifId
The test ID:
Definition: obb.cpp:1136
obbDisjoint_impls::withManualLoopUnrolling_1
bool withManualLoopUnrolling_1(const Matrix3f &B, const Vec3f &T, const Vec3f &a, const Vec3f &b, const FCL_REAL &breakDistance2, FCL_REAL &squaredLowerBoundDistance)
Definition: obb.cpp:266
obbDisjoint_impls::originalWithLowerBound
bool originalWithLowerBound(const Matrix3f &B, const Vec3f &T, const Vec3f &a, const Vec3f &b, const FCL_REAL &breakDistance2, FCL_REAL &squaredLowerBoundDistance)
Definition: obb.cpp:780
py_fmt
const Eigen::IOFormat py_fmt(Eigen::FullPrecision, 0, ", ", ",\n", "(", ",)", "( ", ", )")
randomOBBs
void randomOBBs(Vec3f &a, Vec3f &b, FCL_REAL extentNorm)
Definition: obb.cpp:51
obbDisjoint_impls
Definition: obb.cpp:104
obbDisjoint_impls::withRuntimeLoop
bool withRuntimeLoop(const Matrix3f &B, const Vec3f &T, const Vec3f &a, const Vec3f &b, const FCL_REAL &breakDistance2, FCL_REAL &squaredLowerBoundDistance)
Definition: obb.cpp:215
randomTransform
void randomTransform(Matrix3f &B, Vec3f &T, const Vec3f &a, const Vec3f &b, const FCL_REAL extentNorm)
Definition: obb.cpp:60
hpp::fcl::Quaternion3f
Eigen::Quaternion< FCL_REAL > Quaternion3f
Definition: transform.h:46
hpp::fcl::Box
Center at zero point, axis aligned box.
Definition: shape/geometric_shapes.h:125
gjk
Definition: doc/gjk.py:1

hpp-fcl
Author(s):
autogenerated on Fri Aug 2 2024 02:45:14