FCL  0.6.0
Flexible Collision Library
utility-inl.h
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/math/bv/AABB.h"
44 #include "fcl/math/bv/kDOP.h"
45 #include "fcl/math/bv/kIOS.h"
46 #include "fcl/math/bv/OBB.h"
47 #include "fcl/math/bv/OBBRSS.h"
48 #include "fcl/math/bv/RSS.h"
49 
51 namespace fcl {
52 
53 //==============================================================================
54 namespace detail {
55 //==============================================================================
56 
57 //==============================================================================
58 namespace OBB_fit_functions {
59 //==============================================================================
60 
61 //==============================================================================
62 template <typename S>
63 void fit1(Vector3<S>* ps, OBB<S>& bv)
64 {
65  bv.To = ps[0];
66  bv.axis.setIdentity();
67  bv.extent.setZero();
68 }
69 
70 //==============================================================================
71 template <typename S>
72 void fit2(Vector3<S>* ps, OBB<S>& bv)
73 {
74  const Vector3<S>& p1 = ps[0];
75  const Vector3<S>& p2 = ps[1];
76  Vector3<S> p1p2 = p1 - p2;
77  S len_p1p2 = p1p2.norm();
78  p1p2.normalize();
79 
80  bv.axis.col(0) = p1p2;
81  generateCoordinateSystem(bv.axis);
82 
83  bv.extent << len_p1p2 * 0.5, 0, 0;
84  bv.To.noalias() = 0.5 * (p1 + p2);
85 }
86 
87 //==============================================================================
88 template <typename S>
89 void fit3(Vector3<S>* ps, OBB<S>& bv)
90 {
91  const Vector3<S>& p1 = ps[0];
92  const Vector3<S>& p2 = ps[1];
93  const Vector3<S>& p3 = ps[2];
94  Vector3<S> e[3];
95  e[0] = p1 - p2;
96  e[1] = p2 - p3;
97  e[2] = p3 - p1;
98  S len[3];
99  len[0] = e[0].squaredNorm();
100  len[1] = e[1].squaredNorm();
101  len[2] = e[2].squaredNorm();
102 
103  int imax = 0;
104  if(len[1] > len[0]) imax = 1;
105  if(len[2] > len[imax]) imax = 2;
106 
107  bv.axis.col(2).noalias() = e[0].cross(e[1]);
108  bv.axis.col(2).normalize();
109  bv.axis.col(0) = e[imax];
110  bv.axis.col(0).normalize();
111  bv.axis.col(1).noalias() = bv.axis.col(2).cross(bv.axis.col(0));
112 
113  getExtentAndCenter<S>(ps, nullptr, nullptr, nullptr, 3, bv.axis, bv.To, bv.extent);
114 }
115 
116 //==============================================================================
117 template <typename S>
118 void fit6(Vector3<S>* ps, OBB<S>& bv)
119 {
120  OBB<S> bv1, bv2;
121  fit3(ps, bv1);
122  fit3(ps + 3, bv2);
123  bv = bv1 + bv2;
124 }
125 
126 //==============================================================================
127 template <typename S>
128 void fitn(Vector3<S>* ps, int n, OBB<S>& bv)
129 {
130  Matrix3<S> M;
131  Matrix3<S> E;
132  Vector3<S> s = Vector3<S>::Zero(); // three eigen values
133 
134  getCovariance<S>(ps, nullptr, nullptr, nullptr, n, M);
135  eigen_old(M, s, E);
136  axisFromEigen(E, s, bv.axis);
137 
138  // set obb centers and extensions
139  getExtentAndCenter<S>(ps, nullptr, nullptr, nullptr, n, bv.axis, bv.To, bv.extent);
140 }
141 
142 //==============================================================================
143 extern template
144 void fit1(Vector3<double>* ps, OBB<double>& bv);
145 
146 //==============================================================================
147 extern template
148 void fit2(Vector3<double>* ps, OBB<double>& bv);
149 
150 //==============================================================================
151 extern template
152 void fit3(Vector3<double>* ps, OBB<double>& bv);
153 
154 //==============================================================================
155 extern template
156 void fit6(Vector3<double>* ps, OBB<double>& bv);
157 
158 //==============================================================================
159 extern template
160 void fitn(Vector3<double>* ps, int n, OBB<double>& bv);
161 
162 //==============================================================================
163 } // namespace OBB_fit_functions
164 //==============================================================================
165 
166 //==============================================================================
167 namespace RSS_fit_functions {
168 //==============================================================================
169 
170 //==============================================================================
171 template <typename S>
172 void fit1(Vector3<S>* ps, RSS<S>& bv)
173 {
174  bv.To = ps[0];
175  bv.axis.setIdentity();
176  bv.l[0] = 0;
177  bv.l[1] = 0;
178  bv.r = 0;
179 }
180 
181 //==============================================================================
182 template <typename S>
183 void fit2(Vector3<S>* ps, RSS<S>& bv)
184 {
185  const Vector3<S>& p1 = ps[0];
186  const Vector3<S>& p2 = ps[1];
187  Vector3<S> p1p2 = p1 - p2;
188  S len_p1p2 = p1p2.norm();
189  p1p2.normalize();
190 
191  bv.axis.col(0) = p1p2;
192  generateCoordinateSystem(bv.axis);
193  bv.l[0] = len_p1p2;
194  bv.l[1] = 0;
195 
196  bv.To = p2;
197  bv.r = 0;
198 }
199 
200 //==============================================================================
201 template <typename S>
202 void fit3(Vector3<S>* ps, RSS<S>& bv)
203 {
204  const Vector3<S>& p1 = ps[0];
205  const Vector3<S>& p2 = ps[1];
206  const Vector3<S>& p3 = ps[2];
207  Vector3<S> e[3];
208  e[0] = p1 - p2;
209  e[1] = p2 - p3;
210  e[2] = p3 - p1;
211  S len[3];
212  len[0] = e[0].squaredNorm();
213  len[1] = e[1].squaredNorm();
214  len[2] = e[2].squaredNorm();
215 
216  int imax = 0;
217  if(len[1] > len[0]) imax = 1;
218  if(len[2] > len[imax]) imax = 2;
219 
220  bv.axis.col(2).noalias() = e[0].cross(e[1]).normalized();
221  bv.axis.col(0).noalias() = e[imax].normalized();
222  bv.axis.col(1).noalias() = bv.axis.col(2).cross(bv.axis.col(0));
223 
224  getRadiusAndOriginAndRectangleSize<S>(ps, nullptr, nullptr, nullptr, 3, bv.axis, bv.To, bv.l, bv.r);
225 }
226 
227 //==============================================================================
228 template <typename S>
229 void fit6(Vector3<S>* ps, RSS<S>& bv)
230 {
231  RSS<S> bv1, bv2;
232  fit3(ps, bv1);
233  fit3(ps + 3, bv2);
234  bv = bv1 + bv2;
235 }
236 
237 //==============================================================================
238 template <typename S>
239 void fitn(Vector3<S>* ps, int n, RSS<S>& bv)
240 {
241  Matrix3<S> M; // row first matrix
242  Matrix3<S> E; // row first eigen-vectors
243  Vector3<S> s = Vector3<S>::Zero();
244 
245  getCovariance<S>(ps, nullptr, nullptr, nullptr, n, M);
246  eigen_old(M, s, E);
247  axisFromEigen(E, s, bv.axis);
248 
249  // set rss origin, rectangle size and radius
250  getRadiusAndOriginAndRectangleSize<S>(ps, nullptr, nullptr, nullptr, n, bv.axis, bv.To, bv.l, bv.r);
251 }
252 
253 //==============================================================================
254 extern template
255 void fit1(Vector3<double>* ps, RSS<double>& bv);
256 
257 //==============================================================================
258 extern template
259 void fit2(Vector3<double>* ps, RSS<double>& bv);
260 
261 //==============================================================================
262 extern template
263 void fit3(Vector3<double>* ps, RSS<double>& bv);
264 
265 //==============================================================================
266 extern template
267 void fit6(Vector3<double>* ps, RSS<double>& bv);
268 
269 //==============================================================================
270 extern template
271 void fitn(Vector3<double>* ps, int n, RSS<double>& bv);
272 
273 //==============================================================================
274 } // namespace RSS_fit_functions
275 //==============================================================================
276 
277 //==============================================================================
278 namespace kIOS_fit_functions {
279 //==============================================================================
280 
281 //==============================================================================
282 template <typename S>
283 void fit1(Vector3<S>* ps, kIOS<S>& bv)
284 {
285  bv.num_spheres = 1;
286  bv.spheres[0].o = ps[0];
287  bv.spheres[0].r = 0;
288 
289  bv.obb.axis.setIdentity();
290  bv.obb.extent.setZero();
291  bv.obb.To = ps[0];
292 }
293 
294 //==============================================================================
295 template <typename S>
296 void fit2(Vector3<S>* ps, kIOS<S>& bv)
297 {
298  bv.num_spheres = 5;
299 
300  const Vector3<S>& p1 = ps[0];
301  const Vector3<S>& p2 = ps[1];
302  Vector3<S> p1p2 = p1 - p2;
303  S len_p1p2 = p1p2.norm();
304  p1p2.normalize();
305 
306  bv.obb.axis.col(0) = p1p2;
307  generateCoordinateSystem(bv.obb.axis);
308 
309  S r0 = len_p1p2 * 0.5;
310  bv.obb.extent << r0, 0, 0;
311  bv.obb.To.noalias() = (p1 + p2) * 0.5;
312 
313  bv.spheres[0].o = bv.obb.To;
314  bv.spheres[0].r = r0;
315 
316  S r1 = r0 * kIOS<S>::invSinA();
317  S r1cosA = r1 * kIOS<S>::cosA();
318  bv.spheres[1].r = r1;
319  bv.spheres[2].r = r1;
320  Vector3<S> delta = bv.obb.axis.col(1) * r1cosA;
321  bv.spheres[1].o = bv.spheres[0].o - delta;
322  bv.spheres[2].o = bv.spheres[0].o + delta;
323 
324  bv.spheres[3].r = r1;
325  bv.spheres[4].r = r1;
326  delta.noalias() = bv.obb.axis.col(2) * r1cosA;
327  bv.spheres[3].o = bv.spheres[0].o - delta;
328  bv.spheres[4].o = bv.spheres[0].o + delta;
329 }
330 
331 //==============================================================================
332 template <typename S>
333 void fit3(Vector3<S>* ps, kIOS<S>& bv)
334 {
335  bv.num_spheres = 3;
336 
337  const Vector3<S>& p1 = ps[0];
338  const Vector3<S>& p2 = ps[1];
339  const Vector3<S>& p3 = ps[2];
340  Vector3<S> e[3];
341  e[0] = p1 - p2;
342  e[1] = p2 - p3;
343  e[2] = p3 - p1;
344  S len[3];
345  len[0] = e[0].squaredNorm();
346  len[1] = e[1].squaredNorm();
347  len[2] = e[2].squaredNorm();
348 
349  int imax = 0;
350  if(len[1] > len[0]) imax = 1;
351  if(len[2] > len[imax]) imax = 2;
352 
353  bv.obb.axis.col(2).noalias() = e[0].cross(e[1]).normalized();
354  bv.obb.axis.col(0) = e[imax].normalized();
355  bv.obb.axis.col(1).noalias() = bv.obb.axis.col(2).cross(bv.obb.axis.col(0));
356 
357  getExtentAndCenter<S>(ps, nullptr, nullptr, nullptr, 3, bv.obb.axis, bv.obb.To, bv.obb.extent);
358 
359  // compute radius and center
360  S r0;
361  Vector3<S> center;
362  circumCircleComputation(p1, p2, p3, center, r0);
363 
364  bv.spheres[0].o = center;
365  bv.spheres[0].r = r0;
366 
367  S r1 = r0 * kIOS<S>::invSinA();
368  Vector3<S> delta = bv.obb.axis.col(2) * (r1 * kIOS<S>::cosA());
369 
370  bv.spheres[1].r = r1;
371  bv.spheres[1].o = center - delta;
372  bv.spheres[2].r = r1;
373  bv.spheres[2].o = center + delta;
374 }
375 
376 //==============================================================================
377 template <typename S>
378 void fitn(Vector3<S>* ps, int n, kIOS<S>& bv)
379 {
380  Matrix3<S> M;
381  Matrix3<S> E;
382  Vector3<S> s = Vector3<S>::Zero(); // three eigen values;
383 
384  getCovariance<S>(ps, nullptr, nullptr, nullptr, n, M);
385  eigen_old(M, s, E);
386  axisFromEigen(E, s, bv.obb.axis);
387 
388  getExtentAndCenter<S>(ps, nullptr, nullptr, nullptr, n, bv.obb.axis, bv.obb.To, bv.obb.extent);
389 
390  // get center and extension
391  const Vector3<S>& center = bv.obb.To;
392  const Vector3<S>& extent = bv.obb.extent;
393  S r0 = maximumDistance<S>(ps, nullptr, nullptr, nullptr, n, center);
394 
395  // decide the k in kIOS<S>
396  if(extent[0] > kIOS<S>::ratio() * extent[2])
397  {
398  if(extent[0] > kIOS<S>::ratio() * extent[1]) bv.num_spheres = 5;
399  else bv.num_spheres = 3;
400  }
401  else bv.num_spheres = 1;
402 
403 
404  bv.spheres[0].o = center;
405  bv.spheres[0].r = r0;
406 
407  if(bv.num_spheres >= 3)
408  {
409  S r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * kIOS<S>::invSinA();
410  Vector3<S> delta = bv.obb.axis.col(2) * (r10 * kIOS<S>::cosA() - extent[2]);
411  bv.spheres[1].o = center - delta;
412  bv.spheres[2].o = center + delta;
413 
414  S r11 = 0, r12 = 0;
415  r11 = maximumDistance<S>(ps, nullptr, nullptr, nullptr, n, bv.spheres[1].o);
416  r12 = maximumDistance<S>(ps, nullptr, nullptr, nullptr, n, bv.spheres[2].o);
417  bv.spheres[1].o.noalias() += bv.obb.axis.col(2) * (-r10 + r11);
418  bv.spheres[2].o.noalias() += bv.obb.axis.col(2) * (r10 - r12);
419 
420  bv.spheres[1].r = r10;
421  bv.spheres[2].r = r10;
422  }
423 
424  if(bv.num_spheres >= 5)
425  {
426  S r10 = bv.spheres[1].r;
427  Vector3<S> delta = bv.obb.axis.col(1) * (sqrt(r10 * r10 - extent[0] * extent[0] - extent[2] * extent[2]) - extent[1]);
428  bv.spheres[3].o = bv.spheres[0].o - delta;
429  bv.spheres[4].o = bv.spheres[0].o + delta;
430 
431  S r21 = 0, r22 = 0;
432  r21 = maximumDistance<S>(ps, nullptr, nullptr, nullptr, n, bv.spheres[3].o);
433  r22 = maximumDistance<S>(ps, nullptr, nullptr, nullptr, n, bv.spheres[4].o);
434 
435  bv.spheres[3].o.noalias() += bv.obb.axis.col(1) * (-r10 + r21);
436  bv.spheres[4].o.noalias() += bv.obb.axis.col(1) * (r10 - r22);
437 
438  bv.spheres[3].r = r10;
439  bv.spheres[4].r = r10;
440  }
441 }
442 
443 //==============================================================================
444 extern template
445 void fit1(Vector3<double>* ps, kIOS<double>& bv);
446 
447 //==============================================================================
448 extern template
449 void fit2(Vector3<double>* ps, kIOS<double>& bv);
450 
451 //==============================================================================
452 extern template
453 void fit3(Vector3<double>* ps, kIOS<double>& bv);
454 
455 //==============================================================================
456 extern template
457 void fitn(Vector3<double>* ps, int n, kIOS<double>& bv);
458 
459 //==============================================================================
460 } // namespace kIOS_fit_functions
461 //==============================================================================
462 
463 //==============================================================================
464 namespace OBBRSS_fit_functions {
465 //==============================================================================
466 
467 //==============================================================================
468 template <typename S>
469 void fit1(Vector3<S>* ps, OBBRSS<S>& bv)
470 {
471  OBB_fit_functions::fit1(ps, bv.obb);
472  RSS_fit_functions::fit1(ps, bv.rss);
473 }
474 
475 //==============================================================================
476 template <typename S>
477 void fit2(Vector3<S>* ps, OBBRSS<S>& bv)
478 {
479  OBB_fit_functions::fit2(ps, bv.obb);
480  RSS_fit_functions::fit2(ps, bv.rss);
481 }
482 
483 //==============================================================================
484 template <typename S>
485 void fit3(Vector3<S>* ps, OBBRSS<S>& bv)
486 {
487  OBB_fit_functions::fit3(ps, bv.obb);
488  RSS_fit_functions::fit3(ps, bv.rss);
489 }
490 
491 //==============================================================================
492 template <typename S>
493 void fitn(Vector3<S>* ps, int n, OBBRSS<S>& bv)
494 {
495  OBB_fit_functions::fitn(ps, n, bv.obb);
496  RSS_fit_functions::fitn(ps, n, bv.rss);
497 }
498 
499 //==============================================================================
500 extern template
501 void fit1(Vector3<double>* ps, OBBRSS<double>& bv);
502 
503 //==============================================================================
504 extern template
505 void fit2(Vector3<double>* ps, OBBRSS<double>& bv);
506 
507 //==============================================================================
508 extern template
509 void fit3(Vector3<double>* ps, OBBRSS<double>& bv);
510 
511 //==============================================================================
512 extern template
513 void fitn(Vector3<double>* ps, int n, OBBRSS<double>& bv);
514 
515 //==============================================================================
516 } // namespace OBBRSS_fit_functions
517 //==============================================================================
518 
519 //==============================================================================
520 template <typename S, typename BV>
521 struct Fitter
522 {
523  static void fit(Vector3<S>* ps, int n, BV& bv)
524  {
525  for(int i = 0; i < n; ++i)
526  bv += ps[i];
527  }
528 };
529 
530 //==============================================================================
531 template <typename S>
532 struct Fitter<S, OBB<S>>
533 {
534  static void fit(Vector3<S>* ps, int n, OBB<S>& bv)
535  {
536  switch(n)
537  {
538  case 1:
539  OBB_fit_functions::fit1(ps, bv);
540  break;
541  case 2:
542  OBB_fit_functions::fit2(ps, bv);
543  break;
544  case 3:
545  OBB_fit_functions::fit3(ps, bv);
546  break;
547  case 6:
548  OBB_fit_functions::fit6(ps, bv);
549  break;
550  default:
551  OBB_fit_functions::fitn(ps, n, bv);
552  }
553  }
554 };
555 
556 //==============================================================================
557 template <typename S>
558 struct Fitter<S, RSS<S>>
559 {
560  static void fit(Vector3<S>* ps, int n, RSS<S>& bv)
561  {
562  switch(n)
563  {
564  case 1:
565  RSS_fit_functions::fit1(ps, bv);
566  break;
567  case 2:
568  RSS_fit_functions::fit2(ps, bv);
569  break;
570  case 3:
571  RSS_fit_functions::fit3(ps, bv);
572  break;
573  default:
574  RSS_fit_functions::fitn(ps, n, bv);
575  }
576  }
577 };
578 
579 //==============================================================================
580 template <typename S>
581 struct Fitter<S, kIOS<S>>
582 {
583  static void fit(Vector3<S>* ps, int n, kIOS<S>& bv)
584  {
585  switch(n)
586  {
587  case 1:
588  kIOS_fit_functions::fit1(ps, bv);
589  break;
590  case 2:
591  kIOS_fit_functions::fit2(ps, bv);
592  break;
593  case 3:
594  kIOS_fit_functions::fit3(ps, bv);
595  break;
596  default:
597  kIOS_fit_functions::fitn(ps, n, bv);
598  }
599  }
600 };
601 
602 //==============================================================================
603 template <typename S>
604 struct Fitter<S, OBBRSS<S>>
605 {
606  static void fit(Vector3<S>* ps, int n, OBBRSS<S>& bv)
607  {
608  switch(n)
609  {
610  case 1:
611  OBBRSS_fit_functions::fit1(ps, bv);
612  break;
613  case 2:
614  OBBRSS_fit_functions::fit2(ps, bv);
615  break;
616  case 3:
617  OBBRSS_fit_functions::fit3(ps, bv);
618  break;
619  default:
620  OBBRSS_fit_functions::fitn(ps, n, bv);
621  }
622  }
623 };
624 
625 //==============================================================================
626 extern template
627 struct Fitter<double, OBB<double>>;
628 
629 //==============================================================================
630 extern template
631 struct Fitter<double, RSS<double>>;
632 
633 //==============================================================================
634 extern template
635 struct Fitter<double, kIOS<double>>;
636 
637 //==============================================================================
638 extern template
639 struct Fitter<double, OBBRSS<double>>;
640 
641 //==============================================================================
642 } // namespace detail
643 //==============================================================================
644 
645 //==============================================================================
646 template <typename BV>
647 void fit(Vector3<typename BV::S>* ps, int n, BV& bv)
648 {
649  detail::Fitter<typename BV::S, BV>::fit(ps, n, bv);
650 }
651 
652 //==============================================================================
653 namespace detail {
654 //==============================================================================
655 
658 template <typename S, typename BV1, typename BV2>
659 class ConvertBVImpl
660 {
661 private:
662  static void run(const BV1& bv1, const Transform3<S>& tf1, BV2& bv2)
663  {
664  // should only use the specialized version, so it is private.
665  }
666 };
667 
668 //==============================================================================
670 template <typename S>
671 class ConvertBVImpl<S, AABB<S>, AABB<S>>
672 {
673 public:
674  static void run(const AABB<S>& bv1, const Transform3<S>& tf1, AABB<S>& bv2)
675  {
676  const Vector3<S> center = bv1.center();
677  S r = (bv1.max_ - bv1.min_).norm() * 0.5;
678  Vector3<S> center2 = tf1 * center;
679  Vector3<S> delta(r, r, r);
680  bv2.min_ = center2 - delta;
681  bv2.max_ = center2 + delta;
682  }
683 };
684 
685 //==============================================================================
686 template <typename S>
687 class ConvertBVImpl<S, AABB<S>, OBB<S>>
688 {
689 public:
690  static void run(const AABB<S>& bv1, const Transform3<S>& tf1, OBB<S>& bv2)
691  {
692  /*
693  bv2.To = tf1 * bv1.center());
694 
696  S d[3] = {bv1.width(), bv1.height(), bv1.depth() };
697  std::size_t id[3] = {0, 1, 2};
698 
699  for(std::size_t i = 1; i < 3; ++i)
700  {
701  for(std::size_t j = i; j > 0; --j)
702  {
703  if(d[j] > d[j-1])
704  {
705  {
706  S tmp = d[j];
707  d[j] = d[j-1];
708  d[j-1] = tmp;
709  }
710  {
711  std::size_t tmp = id[j];
712  id[j] = id[j-1];
713  id[j-1] = tmp;
714  }
715  }
716  }
717  }
718 
719  Vector3<S> extent = (bv1.max_ - bv1.min_) * 0.5;
720  bv2.extent = Vector3<S>(extent[id[0]], extent[id[1]], extent[id[2]]);
721  const Matrix3<S>& R = tf1.linear();
722  bool left_hand = (id[0] == (id[1] + 1) % 3);
723  bv2.axis[0] = left_hand ? -R.col(id[0]) : R.col(id[0]);
724  bv2.axis[1] = R.col(id[1]);
725  bv2.axis[2] = R.col(id[2]);
726  */
727 
728  bv2.To.noalias() = tf1 * bv1.center();
729  bv2.extent.noalias() = (bv1.max_ - bv1.min_) * 0.5;
730  bv2.axis = tf1.linear();
731  }
732 };
733 
734 //==============================================================================
735 template <typename S>
736 class ConvertBVImpl<S, OBB<S>, OBB<S>>
737 {
738 public:
739  static void run(const OBB<S>& bv1, const Transform3<S>& tf1, OBB<S>& bv2)
740  {
741  bv2.extent = bv1.extent;
742  bv2.To.noalias() = tf1 * bv1.To;
743  bv2.axis.noalias() = tf1.linear() * bv1.axis;
744  }
745 };
746 
747 //==============================================================================
748 template <typename S>
749 class ConvertBVImpl<S, OBBRSS<S>, OBB<S>>
750 {
751 public:
752  static void run(const OBBRSS<S>& bv1, const Transform3<S>& tf1, OBB<S>& bv2)
753  {
754  ConvertBVImpl<S, OBB<S>, OBB<S>>::run(bv1.obb, tf1, bv2);
755  }
756 };
757 
758 //==============================================================================
759 template <typename S>
760 class ConvertBVImpl<S, RSS<S>, OBB<S>>
761 {
762 public:
763  static void run(const RSS<S>& bv1, const Transform3<S>& tf1, OBB<S>& bv2)
764  {
765  bv2.extent << bv1.l[0] * 0.5 + bv1.r, bv1.l[1] * 0.5 + bv1.r, bv1.r;
766  bv2.To.noalias() = tf1 * bv1.To;
767  bv2.axis.noalias() = tf1.linear() * bv1.axis;
768  }
769 };
770 
771 //==============================================================================
772 template <typename S, typename BV1>
773 class ConvertBVImpl<S, BV1, AABB<S>>
774 {
775 public:
776  static void run(const BV1& bv1, const Transform3<S>& tf1, AABB<S>& bv2)
777  {
778  const Vector3<S> center = bv1.center();
779  S r = Vector3<S>(bv1.width(), bv1.height(), bv1.depth()).norm() * 0.5;
780  Vector3<S> delta(r, r, r);
781  Vector3<S> center2 = tf1 * center;
782  bv2.min_ = center2 - delta;
783  bv2.max_ = center2 + delta;
784  }
785 };
786 
787 //==============================================================================
788 template <typename S, typename BV1>
789 class ConvertBVImpl<S, BV1, OBB<S>>
790 {
791 public:
792  static void run(const BV1& bv1, const Transform3<S>& tf1, OBB<S>& bv2)
793  {
794  AABB<S> bv;
795  ConvertBVImpl<S, BV1, AABB<S>>::run(bv1, Transform3<S>::Identity(), bv);
796  ConvertBVImpl<S, AABB<S>, OBB<S>>::run(bv, tf1, bv2);
797  }
798 };
799 
800 //==============================================================================
801 template <typename S>
802 class ConvertBVImpl<S, OBB<S>, RSS<S>>
803 {
804 public:
805  static void run(const OBB<S>& bv1, const Transform3<S>& tf1, RSS<S>& bv2)
806  {
807  bv2.To.noalias() = tf1 * bv1.To;
808  bv2.axis.noalias() = tf1.linear() * bv1.axis;
809 
810  bv2.r = bv1.extent[2];
811  bv2.l[0] = 2 * (bv1.extent[0] - bv2.r);
812  bv2.l[1] = 2 * (bv1.extent[1] - bv2.r);
813  }
814 };
815 
816 //==============================================================================
817 template <typename S>
818 class ConvertBVImpl<S, RSS<S>, RSS<S>>
819 {
820 public:
821  static void run(const RSS<S>& bv1, const Transform3<S>& tf1, RSS<S>& bv2)
822  {
823  bv2.To.noalias() = tf1 * bv1.To;
824  bv2.axis.noalias() = tf1.linear() * bv1.axis;
825 
826  bv2.r = bv1.r;
827  bv2.l[0] = bv1.l[0];
828  bv2.l[1] = bv1.l[1];
829  }
830 };
831 
832 //==============================================================================
833 template <typename S>
834 class ConvertBVImpl<S, OBBRSS<S>, RSS<S>>
835 {
836 public:
837  static void run(const OBBRSS<S>& bv1, const Transform3<S>& tf1, RSS<S>& bv2)
838  {
839  ConvertBVImpl<S, RSS<S>, RSS<S>>::run(bv1.rss, tf1, bv2);
840  }
841 };
842 
843 //==============================================================================
844 template <typename S>
845 class ConvertBVImpl<S, AABB<S>, RSS<S>>
846 {
847 public:
848  static void run(const AABB<S>& bv1, const Transform3<S>& tf1, RSS<S>& bv2)
849  {
850  bv2.To.noalias() = tf1 * bv1.center();
851 
853  S d[3] = {bv1.width(), bv1.height(), bv1.depth() };
854  std::size_t id[3] = {0, 1, 2};
855 
856  for(std::size_t i = 1; i < 3; ++i)
857  {
858  for(std::size_t j = i; j > 0; --j)
859  {
860  if(d[j] > d[j-1])
861  {
862  {
863  S tmp = d[j];
864  d[j] = d[j-1];
865  d[j-1] = tmp;
866  }
867  {
868  std::size_t tmp = id[j];
869  id[j] = id[j-1];
870  id[j-1] = tmp;
871  }
872  }
873  }
874  }
875 
876  Vector3<S> extent = (bv1.max_ - bv1.min_) * 0.5;
877  bv2.r = extent[id[2]];
878  bv2.l[0] = (extent[id[0]] - bv2.r) * 2;
879  bv2.l[1] = (extent[id[1]] - bv2.r) * 2;
880 
881  const Matrix3<S>& R = tf1.linear();
882  bool left_hand = (id[0] == (id[1] + 1) % 3);
883  if (left_hand)
884  bv2.axis.col(0) = -R.col(id[0]);
885  else
886  bv2.axis.col(0) = R.col(id[0]);
887  bv2.axis.col(1) = R.col(id[1]);
888  bv2.axis.col(2) = R.col(id[2]);
889  }
890 };
891 
892 //==============================================================================
893 extern template
894 class ConvertBVImpl<double, AABB<double>, AABB<double>>;
895 
896 //==============================================================================
897 extern template
898 class ConvertBVImpl<double, AABB<double>, OBB<double>>;
899 
900 //==============================================================================
901 extern template
902 class ConvertBVImpl<double, OBB<double>, OBB<double>>;
903 
904 //==============================================================================
905 extern template
906 class ConvertBVImpl<double, OBBRSS<double>, OBB<double>>;
907 
908 //==============================================================================
909 extern template
910 class ConvertBVImpl<double, RSS<double>, OBB<double>>;
911 
912 //==============================================================================
913 extern template
914 class ConvertBVImpl<double, OBB<double>, RSS<double>>;
915 
916 //==============================================================================
917 extern template
918 class ConvertBVImpl<double, RSS<double>, RSS<double>>;
919 
920 //==============================================================================
921 extern template
922 class ConvertBVImpl<double, OBBRSS<double>, RSS<double>>;
923 
924 //==============================================================================
925 extern template
926 class ConvertBVImpl<double, AABB<double>, RSS<double>>;
927 
928 //==============================================================================
929 } // namespace detail
930 //==============================================================================
931 
932 //==============================================================================
933 template <typename BV1, typename BV2>
934 void convertBV(
935  const BV1& bv1, const Transform3<typename BV1::S>& tf1, BV2& bv2)
936 {
937  static_assert(std::is_same<typename BV1::S, typename BV2::S>::value,
938  "The scalar type of BV1 and BV2 should be the same");
939 
940  detail::ConvertBVImpl<typename BV1::S, BV1, BV2>::run(bv1, tf1, bv2);
941 }
942 
943 } // namespace fcl
944 
945 #endif
fcl::detail::ConvertBVImpl< S, AABB< S >, RSS< S > >::run
static void run(const AABB< S > &bv1, const Transform3< S > &tf1, RSS< S > &bv2)
Definition: utility-inl.h:848
fcl::convertBV
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: utility-inl.h:934
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::RSS::axis
Matrix3< S > axis
Orientation of RSS. The axes of the rotation matrix are the principle directions of the box...
Definition: RSS.h:60
fcl::RSS::To
Vector3< S > To
Origin of the rectangle in RSS.
Definition: RSS.h:63
fcl::RSS
A class for rectangle sphere-swept bounding volume.
Definition: RSS.h:49
fcl::AABB::height
S height() const
Height of the AABB.
Definition: AABB-inl.h:195
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::Fitter
Definition: utility-inl.h:521
fcl::AABB::max_
Vector3< S > max_
The max point in the AABB.
Definition: AABB.h:59
fcl::AABB::center
Vector3< S > center() const
Center of the AABB.
Definition: AABB-inl.h:230
fcl::fit
void fit(Vector3< typename BV::S > *ps, int n, BV &bv)
Compute a bounding volume that fits a set of n points.
Definition: utility-inl.h:647
fcl::OBBRSS::obb
OBB< S > obb
OBB member, for rotation.
Definition: OBBRSS.h:57
fcl::RSS::l
S l[2]
Side lengths of rectangle.
Definition: RSS.h:66
fcl::OBBRSS::rss
RSS< S > rss
RSS member, for distance.
Definition: OBBRSS.h:60
fcl::AABB::min_
Vector3< S > min_
The min point in the AABB.
Definition: AABB.h:56
fcl::detail::ConvertBVImpl
Convert a bounding volume of type BV1 in configuration tf1 to a bounding volume of type BV2 in I conf...
Definition: utility-inl.h:659
fcl::AABB::depth
S depth() const
Depth of the AABB.
Definition: AABB-inl.h:202
fcl::OBB::extent
Vector3< S > extent
Half dimensions of OBB.
Definition: OBB.h:68
fcl::RSS::r
S r
Radius of sphere summed with rectangle to form RSS.
Definition: RSS.h:69
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::OBBRSS
Class merging the OBB and RSS, can handle collision and distance simultaneously.
Definition: OBBRSS.h:50
fcl::OBB::To
Vector3< S > To
Center of OBB.
Definition: OBB.h:65
fcl::AABB::width
S width() const
Width of the AABB.
Definition: AABB-inl.h:188
fcl::kIOS
A class describing the kIOS collision structure, which is a set of spheres.
Definition: kIOS.h:48