math/bv/utility-inl.h
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011-2014, Willow Garage, Inc.
5  * Copyright (c) 2014-2016, Open Source Robotics Foundation
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Open Source Robotics Foundation nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  */
35 
38 #ifndef FCL_MATH_BV_UTILITY_INL_H
39 #define FCL_MATH_BV_UTILITY_INL_H
40 
41 #include "fcl/math/bv/utility.h"
42 
43 #include "fcl/common/unused.h"
44 
45 #include "fcl/math/bv/AABB.h"
46 #include "fcl/math/bv/kDOP.h"
47 #include "fcl/math/bv/kIOS.h"
48 #include "fcl/math/bv/OBB.h"
49 #include "fcl/math/bv/OBBRSS.h"
50 #include "fcl/math/bv/RSS.h"
51 
53 namespace fcl {
54 
55 //==============================================================================
56 namespace detail {
57 //==============================================================================
58 
59 //==============================================================================
60 namespace OBB_fit_functions {
61 //==============================================================================
62 
63 //==============================================================================
64 template <typename S>
65 FCL_EXPORT
66 void fit1(const Vector3<S>* const ps, OBB<S>& bv)
67 {
68  bv.To = ps[0];
69  bv.axis.setIdentity();
70  bv.extent.setZero();
71 }
72 
73 //==============================================================================
74 template <typename S>
75 FCL_EXPORT
76 void fit2(const Vector3<S>* const ps, OBB<S>& bv)
77 {
78  const Vector3<S>& p1 = ps[0];
79  const Vector3<S>& p2 = ps[1];
80  const Vector3<S> p1p2 = p1 - p2;
81  const S len_p1p2 = p1p2.norm();
82 
83  bv.axis = generateCoordinateSystem(p1p2);
84 
85  bv.extent << len_p1p2 * 0.5, 0, 0;
86  bv.To.noalias() = 0.5 * (p1 + p2);
87 }
88 
89 //==============================================================================
90 template <typename S>
91 FCL_EXPORT
92 void fit3(const Vector3<S>* const ps, OBB<S>& bv)
93 {
94  const Vector3<S>& p1 = ps[0];
95  const Vector3<S>& p2 = ps[1];
96  const Vector3<S>& p3 = ps[2];
97  Vector3<S> e[3];
98  e[0] = p1 - p2;
99  e[1] = p2 - p3;
100  e[2] = p3 - p1;
101  S len[3];
102  len[0] = e[0].squaredNorm();
103  len[1] = e[1].squaredNorm();
104  len[2] = e[2].squaredNorm();
105 
106  int imax = 0;
107  if(len[1] > len[0]) imax = 1;
108  if(len[2] > len[imax]) imax = 2;
109 
110  bv.axis.col(2).noalias() = e[0].cross(e[1]);
111  bv.axis.col(2).normalize();
112  bv.axis.col(0) = e[imax];
113  bv.axis.col(0).normalize();
114  bv.axis.col(1).noalias() = bv.axis.col(2).cross(bv.axis.col(0));
115 
116  getExtentAndCenter<S>(ps, nullptr, nullptr, nullptr, 3, bv.axis, bv.To, bv.extent);
117 }
118 
119 //==============================================================================
120 template <typename S>
121 FCL_EXPORT
122 void fit6(const Vector3<S>* const ps, OBB<S>& bv)
123 {
124  OBB<S> bv1, bv2;
125  fit3(ps, bv1);
126  fit3(ps + 3, bv2);
127  bv = bv1 + bv2;
128 }
129 
130 //==============================================================================
131 template <typename S>
132 FCL_EXPORT
133 void fitn(const Vector3<S>* const ps, int n, OBB<S>& bv)
134 {
135  Matrix3<S> M;
136  Matrix3<S> E;
137  Vector3<S> s = Vector3<S>::Zero(); // three eigen values
138 
139  getCovariance<S>(ps, nullptr, nullptr, nullptr, n, M);
140  eigen_old(M, s, E);
141  axisFromEigen(E, s, bv.axis);
142 
143  // set obb centers and extensions
144  getExtentAndCenter<S>(ps, nullptr, nullptr, nullptr, n, bv.axis, bv.To, bv.extent);
145 }
146 
147 //==============================================================================
148 extern template
149 void fit1(const Vector3d* const ps, OBB<double>& bv);
150 
151 //==============================================================================
152 extern template
153 void fit2(const Vector3d* const ps, OBB<double>& bv);
154 
155 //==============================================================================
156 extern template
157 void fit3(const Vector3d* const ps, OBB<double>& bv);
158 
159 //==============================================================================
160 extern template
161 void fit6(const Vector3d* const ps, OBB<double>& bv);
162 
163 //==============================================================================
164 extern template
165 void fitn(const Vector3d* const ps, int n, OBB<double>& bv);
166 
167 //==============================================================================
168 } // namespace OBB_fit_functions
169 //==============================================================================
170 
171 //==============================================================================
172 namespace RSS_fit_functions {
173 //==============================================================================
174 
175 //==============================================================================
176 template <typename S>
177 FCL_EXPORT
178 void fit1(const Vector3<S>* const ps, RSS<S>& bv)
179 {
180  bv.To = ps[0];
181  bv.axis.setIdentity();
182  bv.l[0] = 0;
183  bv.l[1] = 0;
184  bv.r = 0;
185 }
186 
187 //==============================================================================
188 template <typename S>
189 FCL_EXPORT
190 void fit2(const Vector3<S>* const ps, RSS<S>& bv)
191 {
192  const Vector3<S>& p1 = ps[0];
193  const Vector3<S>& p2 = ps[1];
194  const Vector3<S> p1p2 = p1 - p2;
195  const S len_p1p2 = p1p2.norm();
196 
197  bv.axis = generateCoordinateSystem(p1p2);
198  bv.l[0] = len_p1p2;
199  bv.l[1] = 0;
200 
201  bv.To = p2;
202  bv.r = 0;
203 }
204 
205 //==============================================================================
206 template <typename S>
207 FCL_EXPORT
208 void fit3(const Vector3<S>* const ps, RSS<S>& bv)
209 {
210  const Vector3<S>& p1 = ps[0];
211  const Vector3<S>& p2 = ps[1];
212  const Vector3<S>& p3 = ps[2];
213  Vector3<S> e[3];
214  e[0] = p1 - p2;
215  e[1] = p2 - p3;
216  e[2] = p3 - p1;
217  S len[3];
218  len[0] = e[0].squaredNorm();
219  len[1] = e[1].squaredNorm();
220  len[2] = e[2].squaredNorm();
221 
222  int imax = 0;
223  if(len[1] > len[0]) imax = 1;
224  if(len[2] > len[imax]) imax = 2;
225 
226  bv.axis.col(2).noalias() = e[0].cross(e[1]).normalized();
227  bv.axis.col(0).noalias() = e[imax].normalized();
228  bv.axis.col(1).noalias() = bv.axis.col(2).cross(bv.axis.col(0));
229 
230  getRadiusAndOriginAndRectangleSize<S>(ps, nullptr, nullptr, nullptr, 3, bv.axis, bv.To, bv.l, bv.r);
231 }
232 
233 //==============================================================================
234 template <typename S>
235 FCL_EXPORT
236 void fit6(const Vector3<S>* const ps, RSS<S>& bv)
237 {
238  RSS<S> bv1, bv2;
239  fit3(ps, bv1);
240  fit3(ps + 3, bv2);
241  bv = bv1 + bv2;
242 }
243 
244 //==============================================================================
245 template <typename S>
246 FCL_EXPORT
247 void fitn(const Vector3<S>* const ps, int n, RSS<S>& bv)
248 {
249  Matrix3<S> M; // row first matrix
250  Matrix3<S> E; // row first eigen-vectors
252 
253  getCovariance<S>(ps, nullptr, nullptr, nullptr, n, M);
254  eigen_old(M, s, E);
255  axisFromEigen(E, s, bv.axis);
256 
257  // set rss origin, rectangle size and radius
258  getRadiusAndOriginAndRectangleSize<S>(ps, nullptr, nullptr, nullptr, n, bv.axis, bv.To, bv.l, bv.r);
259 }
260 
261 //==============================================================================
262 extern template
263 FCL_EXPORT
264 void fit1(const Vector3d* const ps, RSS<double>& bv);
265 
266 //==============================================================================
267 extern template
268 FCL_EXPORT
269 void fit2(const Vector3d* const ps, RSS<double>& bv);
270 
271 //==============================================================================
272 extern template
273 FCL_EXPORT
274 void fit3(const Vector3d* const ps, RSS<double>& bv);
275 
276 //==============================================================================
277 extern template
278 FCL_EXPORT
279 void fit6(const Vector3d* const ps, RSS<double>& bv);
280 
281 //==============================================================================
282 extern template
283 FCL_EXPORT
284 void fitn(const Vector3d* const ps, int n, RSS<double>& bv);
285 
286 //==============================================================================
287 } // namespace RSS_fit_functions
288 //==============================================================================
289 
290 //==============================================================================
291 namespace kIOS_fit_functions {
292 //==============================================================================
293 
294 //==============================================================================
295 template <typename S>
296 FCL_EXPORT
297 void fit1(const Vector3<S>* const ps, kIOS<S>& bv)
298 {
299  bv.num_spheres = 1;
300  bv.spheres[0].o = ps[0];
301  bv.spheres[0].r = 0;
302 
303  bv.obb.axis.setIdentity();
304  bv.obb.extent.setZero();
305  bv.obb.To = ps[0];
306 }
307 
308 //==============================================================================
309 template <typename S>
310 FCL_EXPORT
311 void fit2(const Vector3<S>* const ps, kIOS<S>& bv)
312 {
313  bv.num_spheres = 5;
314 
315  const Vector3<S>& p1 = ps[0];
316  const Vector3<S>& p2 = ps[1];
317  const Vector3<S> p1p2 = p1 - p2;
318  const S len_p1p2 = p1p2.norm();
319 
320  bv.obb.axis = generateCoordinateSystem(p1p2);
321 
322  S r0 = len_p1p2 * 0.5;
323  bv.obb.extent << r0, 0, 0;
324  bv.obb.To.noalias() = (p1 + p2) * 0.5;
325 
326  bv.spheres[0].o = bv.obb.To;
327  bv.spheres[0].r = r0;
328 
329  S r1 = r0 * kIOS<S>::invSinA();
330  S r1cosA = r1 * kIOS<S>::cosA();
331  bv.spheres[1].r = r1;
332  bv.spheres[2].r = r1;
333  Vector3<S> delta = bv.obb.axis.col(1) * r1cosA;
334  bv.spheres[1].o = bv.spheres[0].o - delta;
335  bv.spheres[2].o = bv.spheres[0].o + delta;
336 
337  bv.spheres[3].r = r1;
338  bv.spheres[4].r = r1;
339  delta.noalias() = bv.obb.axis.col(2) * r1cosA;
340  bv.spheres[3].o = bv.spheres[0].o - delta;
341  bv.spheres[4].o = bv.spheres[0].o + delta;
342 }
343 
344 //==============================================================================
345 template <typename S>
346 FCL_EXPORT
347 void fit3(const Vector3<S>* const ps, kIOS<S>& bv)
348 {
349  bv.num_spheres = 3;
350 
351  const Vector3<S>& p1 = ps[0];
352  const Vector3<S>& p2 = ps[1];
353  const Vector3<S>& p3 = ps[2];
354  Vector3<S> e[3];
355  e[0] = p1 - p2;
356  e[1] = p2 - p3;
357  e[2] = p3 - p1;
358  S len[3];
359  len[0] = e[0].squaredNorm();
360  len[1] = e[1].squaredNorm();
361  len[2] = e[2].squaredNorm();
362 
363  int imax = 0;
364  if(len[1] > len[0]) imax = 1;
365  if(len[2] > len[imax]) imax = 2;
366 
367  bv.obb.axis.col(2).noalias() = e[0].cross(e[1]).normalized();
368  bv.obb.axis.col(0) = e[imax].normalized();
369  bv.obb.axis.col(1).noalias() = bv.obb.axis.col(2).cross(bv.obb.axis.col(0));
370 
371  getExtentAndCenter<S>(ps, nullptr, nullptr, nullptr, 3, bv.obb.axis, bv.obb.To, bv.obb.extent);
372 
373  // compute radius and center
374  S r0;
375  Vector3<S> center;
376  circumCircleComputation(p1, p2, p3, center, r0);
377 
378  bv.spheres[0].o = center;
379  bv.spheres[0].r = r0;
380 
381  S r1 = r0 * kIOS<S>::invSinA();
382  Vector3<S> delta = bv.obb.axis.col(2) * (r1 * kIOS<S>::cosA());
383 
384  bv.spheres[1].r = r1;
385  bv.spheres[1].o = center - delta;
386  bv.spheres[2].r = r1;
387  bv.spheres[2].o = center + delta;
388 }
389 
390 //==============================================================================
391 template <typename S>
392 FCL_EXPORT
393 void fitn(const Vector3<S>* const ps, int n, kIOS<S>& bv)
394 {
395  Matrix3<S> M;
396  Matrix3<S> E;
397  Vector3<S> s = Vector3<S>::Zero(); // three eigen values;
398 
399  getCovariance<S>(ps, nullptr, nullptr, nullptr, n, M);
400  eigen_old(M, s, E);
401  axisFromEigen(E, s, bv.obb.axis);
402 
403  getExtentAndCenter<S>(ps, nullptr, nullptr, nullptr, n, bv.obb.axis, bv.obb.To, bv.obb.extent);
404 
405  // get center and extension
406  const Vector3<S>& center = bv.obb.To;
407  const Vector3<S>& extent = bv.obb.extent;
408  S r0 = maximumDistance<S>(ps, nullptr, nullptr, nullptr, n, center);
409 
410  // decide the k in kIOS<S>
411  if(extent[0] > kIOS<S>::ratio() * extent[2])
412  {
413  if(extent[0] > kIOS<S>::ratio() * extent[1]) bv.num_spheres = 5;
414  else bv.num_spheres = 3;
415  }
416  else bv.num_spheres = 1;
417 
418 
419  bv.spheres[0].o = center;
420  bv.spheres[0].r = r0;
421 
422  if(bv.num_spheres >= 3)
423  {
424  S r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * kIOS<S>::invSinA();
425  Vector3<S> delta = bv.obb.axis.col(2) * (r10 * kIOS<S>::cosA() - extent[2]);
426  bv.spheres[1].o = center - delta;
427  bv.spheres[2].o = center + delta;
428 
429  S r11 = 0, r12 = 0;
430  r11 = maximumDistance<S>(ps, nullptr, nullptr, nullptr, n, bv.spheres[1].o);
431  r12 = maximumDistance<S>(ps, nullptr, nullptr, nullptr, n, bv.spheres[2].o);
432  bv.spheres[1].o.noalias() += bv.obb.axis.col(2) * (-r10 + r11);
433  bv.spheres[2].o.noalias() += bv.obb.axis.col(2) * (r10 - r12);
434 
435  bv.spheres[1].r = r10;
436  bv.spheres[2].r = r10;
437  }
438 
439  if(bv.num_spheres >= 5)
440  {
441  S r10 = bv.spheres[1].r;
442  Vector3<S> delta = bv.obb.axis.col(1) * (sqrt(r10 * r10 - extent[0] * extent[0] - extent[2] * extent[2]) - extent[1]);
443  bv.spheres[3].o = bv.spheres[0].o - delta;
444  bv.spheres[4].o = bv.spheres[0].o + delta;
445 
446  S r21 = 0, r22 = 0;
447  r21 = maximumDistance<S>(ps, nullptr, nullptr, nullptr, n, bv.spheres[3].o);
448  r22 = maximumDistance<S>(ps, nullptr, nullptr, nullptr, n, bv.spheres[4].o);
449 
450  bv.spheres[3].o.noalias() += bv.obb.axis.col(1) * (-r10 + r21);
451  bv.spheres[4].o.noalias() += bv.obb.axis.col(1) * (r10 - r22);
452 
453  bv.spheres[3].r = r10;
454  bv.spheres[4].r = r10;
455  }
456 }
457 
458 //==============================================================================
459 extern template
460 FCL_EXPORT
461 void fit1(const Vector3d* const ps, kIOS<double>& bv);
462 
463 //==============================================================================
464 extern template
465 FCL_EXPORT
466 void fit2(const Vector3d* const ps, kIOS<double>& bv);
467 
468 //==============================================================================
469 extern template
470 FCL_EXPORT
471 void fit3(const Vector3d* const ps, kIOS<double>& bv);
472 
473 //==============================================================================
474 extern template
475 FCL_EXPORT
476 void fitn(const Vector3d* const ps, int n, kIOS<double>& bv);
477 
478 //==============================================================================
479 } // namespace kIOS_fit_functions
480 //==============================================================================
481 
482 //==============================================================================
483 namespace OBBRSS_fit_functions {
484 //==============================================================================
485 
486 //==============================================================================
487 template <typename S>
488 FCL_EXPORT
489 void fit1(const Vector3<S>* const ps, OBBRSS<S>& bv)
490 {
493 }
494 
495 //==============================================================================
496 template <typename S>
497 FCL_EXPORT
498 void fit2(const Vector3<S>* const ps, OBBRSS<S>& bv)
499 {
502 }
503 
504 //==============================================================================
505 template <typename S>
506 FCL_EXPORT
507 void fit3(const Vector3<S>* const ps, OBBRSS<S>& bv)
508 {
511 }
512 
513 //==============================================================================
514 template <typename S>
515 FCL_EXPORT
516 void fitn(const Vector3<S>* const ps, int n, OBBRSS<S>& bv)
517 {
518  OBB_fit_functions::fitn(ps, n, bv.obb);
519  RSS_fit_functions::fitn(ps, n, bv.rss);
520 }
521 
522 //==============================================================================
523 extern template
524 void fit1(const Vector3d* const ps, OBBRSS<double>& bv);
525 
526 //==============================================================================
527 extern template
528 void fit2(const Vector3d* const ps, OBBRSS<double>& bv);
529 
530 //==============================================================================
531 extern template
532 void fit3(const Vector3d* const ps, OBBRSS<double>& bv);
533 
534 //==============================================================================
535 extern template
536 void fitn(const Vector3d* const ps, int n, OBBRSS<double>& bv);
537 
538 //==============================================================================
539 } // namespace OBBRSS_fit_functions
540 //==============================================================================
541 
542 //==============================================================================
543 template <typename S, typename BV>
544 struct FCL_EXPORT Fitter
545 {
546  static void fit(const Vector3<S>* const ps, int n, BV& bv)
547  {
548  for(int i = 0; i < n; ++i)
549  bv += ps[i];
550  }
551 };
552 
553 //==============================================================================
554 template <typename S>
555 struct FCL_EXPORT Fitter<S, OBB<S>>
556 {
557  static void fit(const Vector3<S>* const ps, int n, OBB<S>& bv)
558  {
559  switch(n)
560  {
561  case 1:
562  OBB_fit_functions::fit1(ps, bv);
563  break;
564  case 2:
565  OBB_fit_functions::fit2(ps, bv);
566  break;
567  case 3:
568  OBB_fit_functions::fit3(ps, bv);
569  break;
570  case 6:
571  OBB_fit_functions::fit6(ps, bv);
572  break;
573  default:
574  OBB_fit_functions::fitn(ps, n, bv);
575  }
576  }
577 };
578 
579 //==============================================================================
580 template <typename S>
581 struct FCL_EXPORT Fitter<S, RSS<S>>
582 {
583  static void fit(const Vector3<S>* const ps, int n, RSS<S>& bv)
584  {
585  switch(n)
586  {
587  case 1:
588  RSS_fit_functions::fit1(ps, bv);
589  break;
590  case 2:
591  RSS_fit_functions::fit2(ps, bv);
592  break;
593  case 3:
594  RSS_fit_functions::fit3(ps, bv);
595  break;
596  default:
597  RSS_fit_functions::fitn(ps, n, bv);
598  }
599  }
600 };
601 
602 //==============================================================================
603 template <typename S>
604 struct FCL_EXPORT Fitter<S, kIOS<S>>
605 {
606  static void fit(const Vector3<S>* const ps, int n, kIOS<S>& bv)
607  {
608  switch(n)
609  {
610  case 1:
611  kIOS_fit_functions::fit1(ps, bv);
612  break;
613  case 2:
614  kIOS_fit_functions::fit2(ps, bv);
615  break;
616  case 3:
617  kIOS_fit_functions::fit3(ps, bv);
618  break;
619  default:
620  kIOS_fit_functions::fitn(ps, n, bv);
621  }
622  }
623 };
624 
625 //==============================================================================
626 template <typename S>
627 struct FCL_EXPORT Fitter<S, OBBRSS<S>>
628 {
629  static void fit(const Vector3<S>* const ps, int n, OBBRSS<S>& bv)
630  {
631  switch(n)
632  {
633  case 1:
635  break;
636  case 2:
638  break;
639  case 3:
641  break;
642  default:
643  OBBRSS_fit_functions::fitn(ps, n, bv);
644  }
645  }
646 };
647 
648 //==============================================================================
649 extern template
650 struct Fitter<double, OBB<double>>;
651 
652 //==============================================================================
653 extern template
654 struct Fitter<double, RSS<double>>;
655 
656 //==============================================================================
657 extern template
658 struct Fitter<double, kIOS<double>>;
659 
660 //==============================================================================
661 extern template
662 struct Fitter<double, OBBRSS<double>>;
663 
664 //==============================================================================
665 } // namespace detail
666 //==============================================================================
667 
668 //==============================================================================
669 template <typename BV>
670 FCL_EXPORT
671 void fit(const Vector3<typename BV::S>* const ps, int n, BV& bv)
672 {
674 }
675 
676 //==============================================================================
677 namespace detail {
678 //==============================================================================
679 
682 template <typename S, typename BV1, typename BV2>
683 class FCL_EXPORT ConvertBVImpl
684 {
685 private:
686  static void run(const BV1& bv1, const Transform3<S>& tf1, BV2& bv2)
687  {
688  FCL_UNUSED(bv1);
689  FCL_UNUSED(tf1);
690  FCL_UNUSED(bv2);
691 
692  // should only use the specialized version, so it is private.
693  }
694 };
695 
696 //==============================================================================
698 template <typename S>
699 class FCL_EXPORT ConvertBVImpl<S, AABB<S>, AABB<S>>
700 {
701 public:
702  static void run(const AABB<S>& bv1, const Transform3<S>& tf1, AABB<S>& bv2)
703  {
704  const Vector3<S> center = bv1.center();
705  S r = (bv1.max_ - bv1.min_).norm() * 0.5;
706  Vector3<S> center2 = tf1 * center;
707  Vector3<S> delta(r, r, r);
708  bv2.min_ = center2 - delta;
709  bv2.max_ = center2 + delta;
710  }
711 };
712 
713 //==============================================================================
714 template <typename S>
715 class FCL_EXPORT ConvertBVImpl<S, AABB<S>, OBB<S>>
716 {
717 public:
718  static void run(const AABB<S>& bv1, const Transform3<S>& tf1, OBB<S>& bv2)
719  {
720  /*
721  bv2.To = tf1 * bv1.center());
722 
724  S d[3] = {bv1.width(), bv1.height(), bv1.depth() };
725  std::size_t id[3] = {0, 1, 2};
726 
727  for(std::size_t i = 1; i < 3; ++i)
728  {
729  for(std::size_t j = i; j > 0; --j)
730  {
731  if(d[j] > d[j-1])
732  {
733  {
734  S tmp = d[j];
735  d[j] = d[j-1];
736  d[j-1] = tmp;
737  }
738  {
739  std::size_t tmp = id[j];
740  id[j] = id[j-1];
741  id[j-1] = tmp;
742  }
743  }
744  }
745  }
746 
747  Vector3<S> extent = (bv1.max_ - bv1.min_) * 0.5;
748  bv2.extent = Vector3<S>(extent[id[0]], extent[id[1]], extent[id[2]]);
749  const Matrix3<S>& R = tf1.linear();
750  bool left_hand = (id[0] == (id[1] + 1) % 3);
751  bv2.axis[0] = left_hand ? -R.col(id[0]) : R.col(id[0]);
752  bv2.axis[1] = R.col(id[1]);
753  bv2.axis[2] = R.col(id[2]);
754  */
755 
756  bv2.To.noalias() = tf1 * bv1.center();
757  bv2.extent.noalias() = (bv1.max_ - bv1.min_) * 0.5;
758  bv2.axis = tf1.linear();
759  }
760 };
761 
762 //==============================================================================
763 template <typename S>
764 class FCL_EXPORT ConvertBVImpl<S, OBB<S>, OBB<S>>
765 {
766 public:
767  static void run(const OBB<S>& bv1, const Transform3<S>& tf1, OBB<S>& bv2)
768  {
769  bv2.extent = bv1.extent;
770  bv2.To.noalias() = tf1 * bv1.To;
771  bv2.axis.noalias() = tf1.linear() * bv1.axis;
772  }
773 };
774 
775 //==============================================================================
776 template <typename S>
777 class FCL_EXPORT ConvertBVImpl<S, OBBRSS<S>, OBB<S>>
778 {
779 public:
780  static void run(const OBBRSS<S>& bv1, const Transform3<S>& tf1, OBB<S>& bv2)
781  {
782  ConvertBVImpl<S, OBB<S>, OBB<S>>::run(bv1.obb, tf1, bv2);
783  }
784 };
785 
786 //==============================================================================
787 template <typename S>
788 class FCL_EXPORT ConvertBVImpl<S, RSS<S>, OBB<S>>
789 {
790 public:
791  static void run(const RSS<S>& bv1, const Transform3<S>& tf1, OBB<S>& bv2)
792  {
793  bv2.extent << bv1.l[0] * 0.5 + bv1.r, bv1.l[1] * 0.5 + bv1.r, bv1.r;
794  bv2.To.noalias() = tf1 * bv1.center();
795  bv2.axis.noalias() = tf1.linear() * bv1.axis;
796  }
797 };
798 
799 //==============================================================================
800 template <typename S, typename BV1>
801 class FCL_EXPORT ConvertBVImpl<S, BV1, AABB<S>>
802 {
803 public:
804  static void run(const BV1& bv1, const Transform3<S>& tf1, AABB<S>& bv2)
805  {
806  const Vector3<S> center = bv1.center();
807  S r = Vector3<S>(bv1.width(), bv1.height(), bv1.depth()).norm() * 0.5;
808  Vector3<S> delta(r, r, r);
809  Vector3<S> center2 = tf1 * center;
810  bv2.min_ = center2 - delta;
811  bv2.max_ = center2 + delta;
812  }
813 };
814 
815 //==============================================================================
816 template <typename S, typename BV1>
817 class FCL_EXPORT ConvertBVImpl<S, BV1, OBB<S>>
818 {
819 public:
820  static void run(const BV1& bv1, const Transform3<S>& tf1, OBB<S>& bv2)
821  {
822  AABB<S> bv;
824  ConvertBVImpl<S, AABB<S>, OBB<S>>::run(bv, tf1, bv2);
825  }
826 };
827 
828 //==============================================================================
829 template <typename S>
830 class FCL_EXPORT ConvertBVImpl<S, OBB<S>, RSS<S>>
831 {
832 public:
833  static void run(const OBB<S>& bv1, const Transform3<S>& tf1, RSS<S>& bv2)
834  {
835  // OBB's rotation matrix in axis is required to be lined up with the
836  // x-axis along the longest edge, the y-axis on the next longest edge
837  // and the z-axis on the shortest edge. RSS requires the longest edge
838  // of the rectangle to be the x-axis and the next longest the y-axis.
839  // This maps perfectly from OBB to RSS so simply transform the rotation
840  // axis of the OBB into the RSS.
841  bv2.axis.noalias() = tf1.linear() * bv1.axis;
842 
843  // Set longest rectangle side for RSS to longest dimension of OBB.
844  bv2.l[0] = 2 * (bv1.extent[0]);
845  // Set shortest rectangle side for RSS to next-longest dimension of OBB.
846  bv2.l[1] = 2 * (bv1.extent[1]);
847  // Set radius for RSS to the smallest dimension of OBB.
848  bv2.r = bv1.extent[2];
849 
850  // OBB's To is at its center while RSS's To is at a corner.
851  bv2.setToFromCenter(tf1 * bv1.center());
852  }
853 };
854 
855 //==============================================================================
856 template <typename S>
857 class FCL_EXPORT ConvertBVImpl<S, RSS<S>, RSS<S>>
858 {
859 public:
860  static void run(const RSS<S>& bv1, const Transform3<S>& tf1, RSS<S>& bv2)
861  {
862  bv2.To.noalias() = tf1 * bv1.To;
863  bv2.axis.noalias() = tf1.linear() * bv1.axis;
864 
865  bv2.r = bv1.r;
866  bv2.l[0] = bv1.l[0];
867  bv2.l[1] = bv1.l[1];
868  }
869 };
870 
871 //==============================================================================
872 template <typename S>
873 class FCL_EXPORT ConvertBVImpl<S, OBBRSS<S>, RSS<S>>
874 {
875 public:
876  static void run(const OBBRSS<S>& bv1, const Transform3<S>& tf1, RSS<S>& bv2)
877  {
878  ConvertBVImpl<S, RSS<S>, RSS<S>>::run(bv1.rss, tf1, bv2);
879  }
880 };
881 
882 //==============================================================================
883 template <typename S>
884 class FCL_EXPORT ConvertBVImpl<S, AABB<S>, RSS<S>>
885 {
886 public:
887  static void run(const AABB<S>& bv1, const Transform3<S>& tf1, RSS<S>& bv2)
888  {
890  S d[3] = {bv1.width(), bv1.height(), bv1.depth() };
891  std::size_t id[3] = {0, 1, 2};
892 
893  for(std::size_t i = 1; i < 3; ++i)
894  {
895  for(std::size_t j = i; j > 0; --j)
896  {
897  if(d[j] > d[j-1])
898  {
899  {
900  S tmp = d[j];
901  d[j] = d[j-1];
902  d[j-1] = tmp;
903  }
904  {
905  std::size_t tmp = id[j];
906  id[j] = id[j-1];
907  id[j-1] = tmp;
908  }
909  }
910  }
911  }
912 
913  Vector3<S> extent = (bv1.max_ - bv1.min_) * 0.5;
914  bv2.r = extent[id[2]];
915  bv2.l[0] = (extent[id[0]]) * 2;
916  bv2.l[1] = (extent[id[1]]) * 2;
917 
918  const Matrix3<S>& R = tf1.linear();
919  bool left_hand = (id[0] == (id[1] + 1) % 3);
920  if (left_hand)
921  bv2.axis.col(0) = -R.col(id[0]);
922  else
923  bv2.axis.col(0) = R.col(id[0]);
924  bv2.axis.col(1) = R.col(id[1]);
925  bv2.axis.col(2) = R.col(id[2]);
926  bv2.setToFromCenter(tf1 * bv1.center());
927  }
928 };
929 
930 //==============================================================================
931 extern template
932 class FCL_EXPORT ConvertBVImpl<double, AABB<double>, AABB<double>>;
933 
934 //==============================================================================
935 extern template
936 class FCL_EXPORT ConvertBVImpl<double, AABB<double>, OBB<double>>;
937 
938 //==============================================================================
939 extern template
940 class FCL_EXPORT ConvertBVImpl<double, OBB<double>, OBB<double>>;
941 
942 //==============================================================================
943 extern template
944 class FCL_EXPORT ConvertBVImpl<double, OBBRSS<double>, OBB<double>>;
945 
946 //==============================================================================
947 extern template
948 class FCL_EXPORT ConvertBVImpl<double, RSS<double>, OBB<double>>;
949 
950 //==============================================================================
951 extern template
952 class FCL_EXPORT ConvertBVImpl<double, OBB<double>, RSS<double>>;
953 
954 //==============================================================================
955 extern template
956 class FCL_EXPORT ConvertBVImpl<double, RSS<double>, RSS<double>>;
957 
958 //==============================================================================
959 extern template
960 class FCL_EXPORT ConvertBVImpl<double, OBBRSS<double>, RSS<double>>;
961 
962 //==============================================================================
963 extern template
964 class FCL_EXPORT ConvertBVImpl<double, AABB<double>, RSS<double>>;
965 
966 //==============================================================================
967 } // namespace detail
968 //==============================================================================
969 
970 //==============================================================================
971 template <typename BV1, typename BV2>
972 FCL_EXPORT
974  const BV1& bv1, const Transform3<typename BV1::S>& tf1, BV2& bv2)
975 {
976  static_assert(std::is_same<typename BV1::S, typename BV2::S>::value,
977  "The scalar type of BV1 and BV2 should be the same");
978 
980 }
981 
982 } // namespace fcl
983 
984 #endif
fcl::OBB::axis
Matrix3< S > axis
Orientation of OBB. The axes of the rotation matrix are the principle directions of the box....
Definition: OBB.h:62
fcl::circumCircleComputation
template void circumCircleComputation(const Vector3d &a, const Vector3d &b, const Vector3d &c, Vector3d &center, double &radius)
fcl::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:91
fcl::detail::Fitter< S, OBBRSS< S > >::fit
static void fit(const Vector3< S > *const ps, int n, OBBRSS< S > &bv)
Definition: math/bv/utility-inl.h:629
fcl::axisFromEigen
template void axisFromEigen(const Matrix3d &eigenV, const Vector3d &eigenS, Matrix3d &axis)
fcl::detail::OBB_fit_functions::fit6
FCL_EXPORT void fit6(const Vector3< S > *const ps, OBB< S > &bv)
Definition: math/bv/utility-inl.h:122
fcl::kIOS::invSinA
static constexpr S invSinA()
Definition: kIOS.h:120
fcl::detail::Fitter< S, kIOS< S > >::fit
static void fit(const Vector3< S > *const ps, int n, kIOS< S > &bv)
Definition: math/bv/utility-inl.h:606
fcl::convertBV
FCL_EXPORT void convertBV(const BV1 &bv1, const Transform3< typename BV1::S > &tf1, BV2 &bv2)
Convert a bounding volume of type BV1 in configuration tf1 to bounding volume of type BV2 in identity...
Definition: math/bv/utility-inl.h:973
fcl::kIOS< S >
fcl::detail::ConvertBVImpl< S, BV1, AABB< S > >::run
static void run(const BV1 &bv1, const Transform3< S > &tf1, AABB< S > &bv2)
Definition: math/bv/utility-inl.h:804
fcl::detail::Fitter
Definition: math/bv/utility-inl.h:544
fcl::detail::ConvertBVImpl< S, BV1, OBB< S > >::run
static void run(const BV1 &bv1, const Transform3< S > &tf1, OBB< S > &bv2)
Definition: math/bv/utility-inl.h:820
fcl::detail::OBBRSS_fit_functions::fit1
FCL_EXPORT void fit1(const Vector3< S > *const ps, OBBRSS< S > &bv)
Definition: math/bv/utility-inl.h:489
fcl::detail::OBBRSS_fit_functions::fit2
template void fit2(const Vector3d *const ps, OBBRSS< double > &bv)
fcl::detail::Fitter< S, RSS< S > >::fit
static void fit(const Vector3< S > *const ps, int n, RSS< S > &bv)
Definition: math/bv/utility-inl.h:583
unused.h
fcl::AABB::center
Vector3< S > center() const
Center of the AABB.
Definition: AABB-inl.h:230
fcl::detail::RSS_fit_functions::fitn
FCL_EXPORT void fitn(const Vector3< S > *const ps, int n, RSS< S > &bv)
Definition: math/bv/utility-inl.h:247
fcl::detail::kIOS_fit_functions::fit2
FCL_EXPORT void fit2(const Vector3< S > *const ps, kIOS< S > &bv)
Definition: math/bv/utility-inl.h:311
fcl::RSS::To
Vector3< S > To
Origin of frame T in frame F.
Definition: RSS.h:76
fcl::detail::kIOS_fit_functions::fitn
FCL_EXPORT void fitn(const Vector3< S > *const ps, int n, kIOS< S > &bv)
Definition: math/bv/utility-inl.h:393
fcl::detail::ConvertBVImpl< S, RSS< S >, OBB< S > >::run
static void run(const RSS< S > &bv1, const Transform3< S > &tf1, OBB< S > &bv2)
Definition: math/bv/utility-inl.h:791
fcl::AABB
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Definition: AABB.h:49
fcl::detail::ConvertBVImpl< S, RSS< S >, RSS< S > >::run
static void run(const RSS< S > &bv1, const Transform3< S > &tf1, RSS< S > &bv2)
Definition: math/bv/utility-inl.h:860
fcl::RSS< S >
fcl::OBB::center
const Vector3< S > center() const
Center of the OBB.
Definition: OBB-inl.h:213
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
fcl::detail::RSS_fit_functions::fit3
FCL_EXPORT void fit3(const Vector3< S > *const ps, RSS< S > &bv)
Definition: math/bv/utility-inl.h:208
fcl::detail::OBBRSS_fit_functions::fit2
FCL_EXPORT void fit2(const Vector3< S > *const ps, OBBRSS< S > &bv)
Definition: math/bv/utility-inl.h:498
fcl::detail::OBB_fit_functions::fit2
FCL_EXPORT void fit2(const Vector3< S > *const ps, OBB< S > &bv)
Definition: math/bv/utility-inl.h:76
fcl::RSS::setToFromCenter
void setToFromCenter(const Vector3< S > &p_FoCenter_F)
Set To of the RSS from the center coordinates in frame F.
Definition: RSS-inl.h:424
fcl::Matrix3
Eigen::Matrix< S, 3, 3 > Matrix3
Definition: types.h:85
fcl::kIOS::cosA
static S cosA()
Definition: kIOS.h:121
fcl::detail::ConvertBVImpl< S, OBB< S >, OBB< S > >::run
static void run(const OBB< S > &bv1, const Transform3< S > &tf1, OBB< S > &bv2)
Definition: math/bv/utility-inl.h:767
fcl::detail::ConvertBVImpl< S, OBB< S >, RSS< S > >::run
static void run(const OBB< S > &bv1, const Transform3< S > &tf1, RSS< S > &bv2)
Definition: math/bv/utility-inl.h:833
fcl::OBB< S >
fcl::detail::RSS_fit_functions::fit6
template FCL_EXPORT void fit6(const Vector3d *const ps, RSS< double > &bv)
fcl::OBBRSS::obb
OBB< S > obb
OBB member, for rotation.
Definition: OBBRSS.h:57
fcl::AABB::max_
Vector3< S > max_
The max point in the AABB.
Definition: AABB.h:59
fcl::detail::Fitter::fit
static void fit(const Vector3< S > *const ps, int n, BV &bv)
Definition: math/bv/utility-inl.h:546
fcl::detail::Vector3d
Vector3< double > Vector3d
Definition: test_gjk_libccd-inl_extractClosestPoints.cpp:47
fcl::detail::ConvertBVImpl< S, OBBRSS< S >, RSS< S > >::run
static void run(const OBBRSS< S > &bv1, const Transform3< S > &tf1, RSS< S > &bv2)
Definition: math/bv/utility-inl.h:876
fcl::detail::OBBRSS_fit_functions::fit3
template void fit3(const Vector3d *const ps, OBBRSS< double > &bv)
fcl::kIOS::obb
OBB< S > obb
OBB related with kIOS.
Definition: kIOS.h:72
fcl::detail::ConvertBVImpl< S, AABB< S >, OBB< S > >::run
static void run(const AABB< S > &bv1, const Transform3< S > &tf1, OBB< S > &bv2)
Definition: math/bv/utility-inl.h:718
fcl::detail::OBB_fit_functions::fitn
FCL_EXPORT void fitn(const Vector3< S > *const ps, int n, OBB< S > &bv)
Definition: math/bv/utility-inl.h:133
fcl::RSS::l
S l[2]
Side lengths of rectangle in frame T.
Definition: RSS.h:79
fcl::AABB::min_
Vector3< S > min_
The min point in the AABB.
Definition: AABB.h:56
fcl::detail::OBBRSS_fit_functions::fitn
template void fitn(const Vector3d *const ps, int n, OBBRSS< double > &bv)
fcl::kIOS::num_spheres
unsigned int num_spheres
The number of spheres, no larger than 5.
Definition: kIOS.h:69
fcl::detail::OBBRSS_fit_functions::fit3
FCL_EXPORT void fit3(const Vector3< S > *const ps, OBBRSS< S > &bv)
Definition: math/bv/utility-inl.h:507
fcl::detail::RSS_fit_functions::fit2
FCL_EXPORT void fit2(const Vector3< S > *const ps, RSS< S > &bv)
Definition: math/bv/utility-inl.h:190
fcl::detail::OBB_fit_functions::fit3
FCL_EXPORT void fit3(const Vector3< S > *const ps, OBB< S > &bv)
Definition: math/bv/utility-inl.h:92
fcl::detail::kIOS_fit_functions::fit3
FCL_EXPORT void fit3(const Vector3< S > *const ps, kIOS< S > &bv)
Definition: math/bv/utility-inl.h:347
r
S r
Definition: test_sphere_box.cpp:171
fcl::detail::OBBRSS_fit_functions::fitn
FCL_EXPORT void fitn(const Vector3< S > *const ps, int n, OBBRSS< S > &bv)
Definition: math/bv/utility-inl.h:516
fcl::OBBRSS< S >
fcl::eigen_old
template void eigen_old(const Matrix3d &m, Vector3d &dout, Matrix3d &vout)
fcl::OBBRSS::rss
RSS< S > rss
RSS member, for distance.
Definition: OBBRSS.h:60
fcl::detail::ConvertBVImpl< S, AABB< S >, AABB< S > >::run
static void run(const AABB< S > &bv1, const Transform3< S > &tf1, AABB< S > &bv2)
Definition: math/bv/utility-inl.h:702
OBBRSS.h
fcl::AABB::depth
S depth() const
Depth of the AABB.
Definition: AABB-inl.h:202
fcl::detail::ConvertBVImpl::run
static void run(const BV1 &bv1, const Transform3< S > &tf1, BV2 &bv2)
Definition: math/bv/utility-inl.h:686
fcl::detail::Fitter< S, OBB< S > >::fit
static void fit(const Vector3< S > *const ps, int n, OBB< S > &bv)
Definition: math/bv/utility-inl.h:557
fcl::detail::RSS_fit_functions::fit1
FCL_EXPORT void fit1(const Vector3< S > *const ps, RSS< S > &bv)
Definition: math/bv/utility-inl.h:178
fcl::generateCoordinateSystem
template Matrix3d generateCoordinateSystem(const Vector3d &x_axis)
fcl::detail::ConvertBVImpl
Convert a bounding volume of type BV1 in configuration tf1 to a bounding volume of type BV2 in I conf...
Definition: math/bv/utility-inl.h:683
kDOP.h
FCL_UNUSED
#define FCL_UNUSED(x)
Definition: unused.h:39
fcl::RSS::r
S r
Radius of sphere summed with rectangle to form RSS.
Definition: RSS.h:82
utility.h
fcl::RSS::center
const Vector3< S > center() const
The RSS center C, measured and expressed in frame F: p_FoC_F.
Definition: RSS-inl.h:416
fcl::detail::OBB_fit_functions::fit1
FCL_EXPORT void fit1(const Vector3< S > *const ps, OBB< S > &bv)
Definition: math/bv/utility-inl.h:66
fcl::AABB::width
S width() const
Width of the AABB.
Definition: AABB-inl.h:188
OBB.h
fcl::detail::ConvertBVImpl< S, OBBRSS< S >, OBB< S > >::run
static void run(const OBBRSS< S > &bv1, const Transform3< S > &tf1, OBB< S > &bv2)
Definition: math/bv/utility-inl.h:780
AABB.h
RSS.h
fcl::detail::OBBRSS_fit_functions::fit1
template void fit1(const Vector3d *const ps, OBBRSS< double > &bv)
fcl::detail::ConvertBVImpl< S, AABB< S >, RSS< S > >::run
static void run(const AABB< S > &bv1, const Transform3< S > &tf1, RSS< S > &bv2)
Definition: math/bv/utility-inl.h:887
fcl::RSS::axis
Matrix3< S > axis
Frame T's orientation in frame F.
Definition: RSS.h:70
fcl::fit
FCL_EXPORT void fit(const Vector3< typename BV::S > *const ps, int n, BV &bv)
Compute a bounding volume that fits a set of n points.
Definition: math/bv/utility-inl.h:671
fcl::kIOS::spheres
kIOS_Sphere spheres[5]
The (at most) five spheres for intersection.
Definition: kIOS.h:66
fcl::detail::kIOS_fit_functions::fit1
FCL_EXPORT void fit1(const Vector3< S > *const ps, kIOS< S > &bv)
Definition: math/bv/utility-inl.h:297
fcl::AABB::height
S height() const
Height of the AABB.
Definition: AABB-inl.h:195
kIOS.h
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::OBB::extent
Vector3< S > extent
Half dimensions of OBB.
Definition: OBB.h:68
fcl::OBB::To
Vector3< S > To
Center of OBB.
Definition: OBB.h:65


fcl
Author(s):
autogenerated on Fri Jun 11 2021 02:38:04