OBB-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_BV_OBB_INL_H
39 #define FCL_BV_OBB_INL_H
40 
41 #include "fcl/math/bv/OBB.h"
42 
43 #include "fcl/common/unused.h"
44 
45 namespace fcl
46 {
47 
48 //==============================================================================
49 extern template
50 class FCL_EXPORT OBB<double>;
51 
52 //==============================================================================
53 extern template
54 void computeVertices(const OBB<double>& b, Vector3<double> vertices[8]);
55 
56 //==============================================================================
57 extern template
59 
60 //==============================================================================
61 extern template
63 
64 //==============================================================================
65 extern template
66 bool obbDisjoint(
67  const Matrix3<double>& B,
68  const Vector3<double>& T,
69  const Vector3<double>& a,
70  const Vector3<double>& b);
71 
72 //==============================================================================
73 extern template
74 bool obbDisjoint(
75  const Transform3<double>& tf,
76  const Vector3<double>& a,
77  const Vector3<double>& b);
78 
79 //==============================================================================
80 template <typename S>
82 {
83  // Do nothing
84 }
85 
86 //==============================================================================
87 template <typename S>
88 OBB<S>::OBB(const Matrix3<S>& axis_,
89  const Vector3<S>& center_,
90  const Vector3<S>& extent_)
91  : axis(axis_), To(center_), extent(extent_)
92 {
93  // Do nothing
94 }
95 
96 //==============================================================================
97 template <typename S>
98 bool OBB<S>::overlap(const OBB<S>& other) const
99 {
102 
103  Vector3<S> t = other.To - To;
104  Vector3<S> T(
105  axis.col(0).dot(t), axis.col(1).dot(t), axis.col(2).dot(t));
106  Matrix3<S> R = axis.transpose() * other.axis;
107 
108  return !obbDisjoint(R, T, extent, other.extent);
109 }
110 
111 //==============================================================================
112 template <typename S>
113 bool OBB<S>::overlap(const OBB& other, OBB& overlap_part) const
114 {
115  FCL_UNUSED(overlap_part);
116 
117  return overlap(other);
118 }
119 
120 //==============================================================================
121 template <typename S>
122 bool OBB<S>::contain(const Vector3<S>& p) const
123 {
124  Vector3<S> local_p = p - To;
125  S proj = local_p.dot(axis.col(0));
126  if((proj > extent[0]) || (proj < -extent[0]))
127  return false;
128 
129  proj = local_p.dot(axis.col(1));
130  if((proj > extent[1]) || (proj < -extent[1]))
131  return false;
132 
133  proj = local_p.dot(axis.col(2));
134  if((proj > extent[2]) || (proj < -extent[2]))
135  return false;
136 
137  return true;
138 }
139 
140 //==============================================================================
141 template <typename S>
143 {
144  OBB<S> bvp(axis, p, Vector3<S>::Zero());
145  *this += bvp;
146 
147  return *this;
148 }
149 
150 //==============================================================================
151 template <typename S>
153 {
154  *this = *this + other;
155 
156  return *this;
157 }
158 
159 //==============================================================================
160 template <typename S>
161 OBB<S> OBB<S>::operator +(const OBB<S>& other) const
162 {
163  Vector3<S> center_diff = To - other.To;
164  S max_extent = std::max(std::max(extent[0], extent[1]), extent[2]);
165  S max_extent2 = std::max(std::max(other.extent[0], other.extent[1]), other.extent[2]);
166  if(center_diff.norm() > 2 * (max_extent + max_extent2))
167  {
168  return merge_largedist(*this, other);
169  }
170  else
171  {
172  return merge_smalldist(*this, other);
173  }
174 }
175 
176 //==============================================================================
177 template <typename S>
179 {
180  return 2 * extent[0];
181 }
182 
183 //==============================================================================
184 template <typename S>
186 {
187  return 2 * extent[1];
188 }
189 
190 //==============================================================================
191 template <typename S>
193 {
194  return 2 * extent[2];
195 }
196 
197 //==============================================================================
198 template <typename S>
200 {
201  return width() * height() * depth();
202 }
203 
204 //==============================================================================
205 template <typename S>
207 {
208  return extent.squaredNorm();
209 }
210 
211 //==============================================================================
212 template <typename S>
214 {
215  return To;
216 }
217 
218 //==============================================================================
219 template <typename S>
220 S OBB<S>::distance(const OBB& other, Vector3<S>* P,
221  Vector3<S>* Q) const
222 {
223  FCL_UNUSED(other);
224  FCL_UNUSED(P);
225  FCL_UNUSED(Q);
226 
227  std::cerr << "OBB distance not implemented!\n";
228  return 0.0;
229 }
230 
231 //==============================================================================
232 template <typename S>
233 void computeVertices(const OBB<S>& b, Vector3<S> vertices[8])
234 {
235  const Vector3<S>& extent = b.extent;
236  const Vector3<S>& To = b.To;
237 
238  Vector3<S> extAxis0 = b.axis.col(0) * extent[0];
239  Vector3<S> extAxis1 = b.axis.col(1) * extent[1];
240  Vector3<S> extAxis2 = b.axis.col(2) * extent[2];
241 
242  vertices[0] = To - extAxis0 - extAxis1 - extAxis2;
243  vertices[1] = To + extAxis0 - extAxis1 - extAxis2;
244  vertices[2] = To + extAxis0 + extAxis1 - extAxis2;
245  vertices[3] = To - extAxis0 + extAxis1 - extAxis2;
246  vertices[4] = To - extAxis0 - extAxis1 + extAxis2;
247  vertices[5] = To + extAxis0 - extAxis1 + extAxis2;
248  vertices[6] = To + extAxis0 + extAxis1 + extAxis2;
249  vertices[7] = To - extAxis0 + extAxis1 + extAxis2;
250 }
251 
252 //==============================================================================
253 template <typename S>
254 OBB<S> merge_largedist(const OBB<S>& b1, const OBB<S>& b2)
255 {
256  Vector3<S> vertex[16];
257  computeVertices(b1, vertex);
258  computeVertices(b2, vertex + 8);
259  Matrix3<S> M;
260  Matrix3<S> E;
261  Vector3<S> s(0, 0, 0);
262 
263  OBB<S> b;
264  b.axis.col(0) = b1.To - b2.To;
265  b.axis.col(0).normalize();
266 
267  Vector3<S> vertex_proj[16];
268  for(int i = 0; i < 16; ++i)
269  {
270  vertex_proj[i] = vertex[i];
271  vertex_proj[i].noalias() -= b.axis.col(0) * vertex[i].dot(b.axis.col(0));
272  }
273 
274  getCovariance<S>(vertex_proj, nullptr, nullptr, nullptr, 16, M);
275  eigen_old(M, s, E);
276 
277  int min, mid, max;
278  if (s[0] > s[1])
279  {
280  max = 0;
281  min = 1;
282  }
283  else
284  {
285  min = 0;
286  max = 1;
287  }
288 
289  if (s[2] < s[min])
290  {
291  mid = min;
292  min = 2;
293  }
294  else if (s[2] > s[max])
295  {
296  mid = max;
297  max = 2;
298  }
299  else
300  {
301  mid = 2;
302  }
303 
304  b.axis.col(1) << E.col(0)[max], E.col(1)[max], E.col(2)[max];
305  b.axis.col(2) << E.col(0)[mid], E.col(1)[mid], E.col(2)[mid];
306 
307  // set obb centers and extensions
308  getExtentAndCenter<S>(
309  vertex, nullptr, nullptr, nullptr, 16, b.axis, b.To, b.extent);
310 
311  return b;
312 }
313 
314 //==============================================================================
315 template <typename S>
316 OBB<S> merge_smalldist(const OBB<S>& b1, const OBB<S>& b2)
317 {
318  OBB<S> b;
319  b.To = (b1.To + b2.To) * 0.5;
320  Quaternion<S> q0(b1.axis);
321  Quaternion<S> q1(b2.axis);
322  if(q0.dot(q1) < 0)
323  q1.coeffs() = -q1.coeffs();
324 
325  Quaternion<S> q(q0.coeffs() + q1.coeffs());
326  q.normalize();
327  b.axis = q.toRotationMatrix();
328 
329 
330  Vector3<S> vertex[8], diff;
331  S real_max = std::numeric_limits<S>::max();
332  Vector3<S> pmin(real_max, real_max, real_max);
333  Vector3<S> pmax(-real_max, -real_max, -real_max);
334 
335  computeVertices(b1, vertex);
336  for(int i = 0; i < 8; ++i)
337  {
338  diff = vertex[i] - b.To;
339  for(int j = 0; j < 3; ++j)
340  {
341  S dot = diff.dot(b.axis.col(j));
342  if(dot > pmax[j])
343  pmax[j] = dot;
344  else if(dot < pmin[j])
345  pmin[j] = dot;
346  }
347  }
348 
349  computeVertices(b2, vertex);
350  for(int i = 0; i < 8; ++i)
351  {
352  diff = vertex[i] - b.To;
353  for(int j = 0; j < 3; ++j)
354  {
355  S dot = diff.dot(b.axis.col(j));
356  if(dot > pmax[j])
357  pmax[j] = dot;
358  else if(dot < pmin[j])
359  pmin[j] = dot;
360  }
361  }
362 
363  for(int j = 0; j < 3; ++j)
364  {
365  b.To += (b.axis.col(j) * (0.5 * (pmax[j] + pmin[j])));
366  b.extent[j] = 0.5 * (pmax[j] - pmin[j]);
367  }
368 
369  return b;
370 }
371 
372 //==============================================================================
373 template <typename S, typename Derived>
375  const OBB<S>& bv, const Eigen::MatrixBase<Derived>& t)
376 {
377  OBB<S> res(bv);
378  res.To += t;
379  return res;
380 }
381 
382 //==============================================================================
383 template <typename S, typename DerivedA, typename DerivedB>
384 bool overlap(const Eigen::MatrixBase<DerivedA>& R0,
385  const Eigen::MatrixBase<DerivedB>& T0,
386  const OBB<S>& b1, const OBB<S>& b2)
387 {
388  typename DerivedA::PlainObject R0b2 = R0 * b2.axis;
389  typename DerivedA::PlainObject R = b1.axis.transpose() * R0b2;
390 
391  typename DerivedB::PlainObject Ttemp = R0 * b2.To + T0 - b1.To;
392  typename DerivedB::PlainObject T = Ttemp.transpose() * b1.axis;
393 
394  return !obbDisjoint(R, T, b1.extent, b2.extent);
395 }
396 
397 //==============================================================================
398 template <typename S>
399 bool obbDisjoint(const Matrix3<S>& B, const Vector3<S>& T,
400  const Vector3<S>& a, const Vector3<S>& b)
401 {
402  S t, s;
403  const S reps = 1e-6;
404 
405  Matrix3<S> Bf = B.cwiseAbs();
406  Bf.array() += reps;
407 
408  // if any of these tests are one-sided, then the polyhedra are disjoint
409 
410  // A1 x A2 = A0
411  t = ((T[0] < 0.0) ? -T[0] : T[0]);
412 
413  if(t > (a[0] + Bf.row(0).dot(b)))
414  return true;
415 
416  // B1 x B2 = B0
417  s = B.col(0).dot(T);
418  t = ((s < 0.0) ? -s : s);
419 
420  if(t > (b[0] + Bf.col(0).dot(a)))
421  return true;
422 
423  // A2 x A0 = A1
424  t = ((T[1] < 0.0) ? -T[1] : T[1]);
425 
426  if(t > (a[1] + Bf.row(1).dot(b)))
427  return true;
428 
429  // A0 x A1 = A2
430  t =((T[2] < 0.0) ? -T[2] : T[2]);
431 
432  if(t > (a[2] + Bf.row(2).dot(b)))
433  return true;
434 
435  // B2 x B0 = B1
436  s = B.col(1).dot(T);
437  t = ((s < 0.0) ? -s : s);
438 
439  if(t > (b[1] + Bf.col(1).dot(a)))
440  return true;
441 
442  // B0 x B1 = B2
443  s = B.col(2).dot(T);
444  t = ((s < 0.0) ? -s : s);
445 
446  if(t > (b[2] + Bf.col(2).dot(a)))
447  return true;
448 
449  // A0 x B0
450  s = T[2] * B(1, 0) - T[1] * B(2, 0);
451  t = ((s < 0.0) ? -s : s);
452 
453  if(t > (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) +
454  b[1] * Bf(0, 2) + b[2] * Bf(0, 1)))
455  return true;
456 
457  // A0 x B1
458  s = T[2] * B(1, 1) - T[1] * B(2, 1);
459  t = ((s < 0.0) ? -s : s);
460 
461  if(t > (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) +
462  b[0] * Bf(0, 2) + b[2] * Bf(0, 0)))
463  return true;
464 
465  // A0 x B2
466  s = T[2] * B(1, 2) - T[1] * B(2, 2);
467  t = ((s < 0.0) ? -s : s);
468 
469  if(t > (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) +
470  b[0] * Bf(0, 1) + b[1] * Bf(0, 0)))
471  return true;
472 
473  // A1 x B0
474  s = T[0] * B(2, 0) - T[2] * B(0, 0);
475  t = ((s < 0.0) ? -s : s);
476 
477  if(t > (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) +
478  b[1] * Bf(1, 2) + b[2] * Bf(1, 1)))
479  return true;
480 
481  // A1 x B1
482  s = T[0] * B(2, 1) - T[2] * B(0, 1);
483  t = ((s < 0.0) ? -s : s);
484 
485  if(t > (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) +
486  b[0] * Bf(1, 2) + b[2] * Bf(1, 0)))
487  return true;
488 
489  // A1 x B2
490  s = T[0] * B(2, 2) - T[2] * B(0, 2);
491  t = ((s < 0.0) ? -s : s);
492 
493  if(t > (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) +
494  b[0] * Bf(1, 1) + b[1] * Bf(1, 0)))
495  return true;
496 
497  // A2 x B0
498  s = T[1] * B(0, 0) - T[0] * B(1, 0);
499  t = ((s < 0.0) ? -s : s);
500 
501  if(t > (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) +
502  b[1] * Bf(2, 2) + b[2] * Bf(2, 1)))
503  return true;
504 
505  // A2 x B1
506  s = T[1] * B(0, 1) - T[0] * B(1, 1);
507  t = ((s < 0.0) ? -s : s);
508 
509  if(t > (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) +
510  b[0] * Bf(2, 2) + b[2] * Bf(2, 0)))
511  return true;
512 
513  // A2 x B2
514  s = T[1] * B(0, 2) - T[0] * B(1, 2);
515  t = ((s < 0.0) ? -s : s);
516 
517  if(t > (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) +
518  b[0] * Bf(2, 1) + b[1] * Bf(2, 0)))
519  return true;
520 
521  return false;
522 
523 }
524 
525 //==============================================================================
526 template <typename S>
528  const Transform3<S>& tf,
529  const Vector3<S>& a,
530  const Vector3<S>& b)
531 {
532  S t, s;
533  const S reps = 1e-6;
534 
535  Matrix3<S> Bf = tf.linear().cwiseAbs();
536  Bf.array() += reps;
537 
538  // if any of these tests are one-sided, then the polyhedra are disjoint
539 
540  // A1 x A2 = A0
541  t = ((tf.translation()[0] < 0.0) ? -tf.translation()[0] : tf.translation()[0]);
542 
543  if(t > (a[0] + Bf.row(0).dot(b)))
544  return true;
545 
546  // B1 x B2 = B0
547  s = tf.linear().col(0).dot(tf.translation());
548  t = ((s < 0.0) ? -s : s);
549 
550  if(t > (b[0] + Bf.col(0).dot(a)))
551  return true;
552 
553  // A2 x A0 = A1
554  t = ((tf.translation()[1] < 0.0) ? -tf.translation()[1] : tf.translation()[1]);
555 
556  if(t > (a[1] + Bf.row(1).dot(b)))
557  return true;
558 
559  // A0 x A1 = A2
560  t =((tf.translation()[2] < 0.0) ? -tf.translation()[2] : tf.translation()[2]);
561 
562  if(t > (a[2] + Bf.row(2).dot(b)))
563  return true;
564 
565  // B2 x B0 = B1
566  s = tf.linear().col(1).dot(tf.translation());
567  t = ((s < 0.0) ? -s : s);
568 
569  if(t > (b[1] + Bf.col(1).dot(a)))
570  return true;
571 
572  // B0 x B1 = B2
573  s = tf.linear().col(2).dot(tf.translation());
574  t = ((s < 0.0) ? -s : s);
575 
576  if(t > (b[2] + Bf.col(2).dot(a)))
577  return true;
578 
579  // A0 x B0
580  s = tf.translation()[2] * tf.linear()(1, 0) - tf.translation()[1] * tf.linear()(2, 0);
581  t = ((s < 0.0) ? -s : s);
582 
583  if(t > (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) +
584  b[1] * Bf(0, 2) + b[2] * Bf(0, 1)))
585  return true;
586 
587  // A0 x B1
588  s = tf.translation()[2] * tf.linear()(1, 1) - tf.translation()[1] * tf.linear()(2, 1);
589  t = ((s < 0.0) ? -s : s);
590 
591  if(t > (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) +
592  b[0] * Bf(0, 2) + b[2] * Bf(0, 0)))
593  return true;
594 
595  // A0 x B2
596  s = tf.translation()[2] * tf.linear()(1, 2) - tf.translation()[1] * tf.linear()(2, 2);
597  t = ((s < 0.0) ? -s : s);
598 
599  if(t > (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) +
600  b[0] * Bf(0, 1) + b[1] * Bf(0, 0)))
601  return true;
602 
603  // A1 x B0
604  s = tf.translation()[0] * tf.linear()(2, 0) - tf.translation()[2] * tf.linear()(0, 0);
605  t = ((s < 0.0) ? -s : s);
606 
607  if(t > (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) +
608  b[1] * Bf(1, 2) + b[2] * Bf(1, 1)))
609  return true;
610 
611  // A1 x B1
612  s = tf.translation()[0] * tf.linear()(2, 1) - tf.translation()[2] * tf.linear()(0, 1);
613  t = ((s < 0.0) ? -s : s);
614 
615  if(t > (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) +
616  b[0] * Bf(1, 2) + b[2] * Bf(1, 0)))
617  return true;
618 
619  // A1 x B2
620  s = tf.translation()[0] * tf.linear()(2, 2) - tf.translation()[2] * tf.linear()(0, 2);
621  t = ((s < 0.0) ? -s : s);
622 
623  if(t > (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) +
624  b[0] * Bf(1, 1) + b[1] * Bf(1, 0)))
625  return true;
626 
627  // A2 x B0
628  s = tf.translation()[1] * tf.linear()(0, 0) - tf.translation()[0] * tf.linear()(1, 0);
629  t = ((s < 0.0) ? -s : s);
630 
631  if(t > (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) +
632  b[1] * Bf(2, 2) + b[2] * Bf(2, 1)))
633  return true;
634 
635  // A2 x B1
636  s = tf.translation()[1] * tf.linear()(0, 1) - tf.translation()[0] * tf.linear()(1, 1);
637  t = ((s < 0.0) ? -s : s);
638 
639  if(t > (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) +
640  b[0] * Bf(2, 2) + b[2] * Bf(2, 0)))
641  return true;
642 
643  // A2 x B2
644  s = tf.translation()[1] * tf.linear()(0, 2) - tf.translation()[0] * tf.linear()(1, 2);
645  t = ((s < 0.0) ? -s : s);
646 
647  if(t > (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) +
648  b[0] * Bf(2, 1) + b[1] * Bf(2, 0)))
649  return true;
650 
651  return false;
652 }
653 
654 } // namespace fcl
655 
656 #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::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:91
fcl::distance
S distance(const Eigen::MatrixBase< DerivedA > &R0, const Eigen::MatrixBase< DerivedB > &T0, const kIOS< S > &b1, const kIOS< S > &b2, Vector3< S > *P, Vector3< S > *Q)
Approximate distance between two kIOS bounding volumes.
Definition: kIOS-inl.h:266
fcl::Quaternion
Eigen::Quaternion< S > Quaternion
Definition: types.h:88
unused.h
max
static T max(T x, T y)
Definition: svm.cpp:52
fcl::computeVertices
void computeVertices(const OBB< S > &b, Vector3< S > vertices[8])
Compute the 8 vertices of a OBB.
Definition: OBB-inl.h:233
fcl::OBB< Shape::S >::S
Shape::S S
Definition: OBB.h:55
fcl::merge_largedist
OBB< S > merge_largedist(const OBB< S > &b1, const OBB< S > &b2)
OBB merge method when the centers of two smaller OBB are far away.
Definition: OBB-inl.h:254
fcl::translate
OBB< S > translate(const OBB< S > &bv, const Eigen::MatrixBase< Derived > &t)
Translate the OBB bv.
Definition: OBB-inl.h:374
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
fcl::Matrix3
Eigen::Matrix< S, 3, 3 > Matrix3
Definition: types.h:85
fcl::OBB
Oriented bounding box class.
Definition: OBB.h:51
fcl::merge_smalldist
OBB< S > merge_smalldist(const OBB< S > &b1, const OBB< S > &b2)
OBB merge method when the centers of two smaller OBB are close.
Definition: OBB-inl.h:316
fcl::obbDisjoint
bool obbDisjoint(const Transform3< S > &tf, const Vector3< S > &a, const Vector3< S > &b)
Check collision between two boxes: the first box is in configuration (R, T) and its half dimension is...
Definition: OBB-inl.h:527
fcl::eigen_old
template void eigen_old(const Matrix3d &m, Vector3d &dout, Matrix3d &vout)
fcl::overlap
bool overlap(const Eigen::MatrixBase< DerivedA > &R0, const Eigen::MatrixBase< DerivedB > &T0, const OBB< S > &b1, const OBB< S > &b2)
Check collision between two obbs, b1 is in configuration (R0, T0) and b2 is in identity.
Definition: OBB-inl.h:384
fcl::OBB< double >
template class FCL_EXPORT OBB< double >
fcl::merge_smalldist
template OBB< double > merge_smalldist(const OBB< double > &b1, const OBB< double > &b2)
FCL_UNUSED
#define FCL_UNUSED(x)
Definition: unused.h:39
fcl::computeVertices
template void computeVertices(const OBB< double > &b, Vector3< double > vertices[8])
fcl::merge_largedist
template OBB< double > merge_largedist(const OBB< double > &b1, const OBB< double > &b2)
min
static T min(T x, T y)
Definition: svm.cpp:49
OBB.h
fcl::OBB::OBB
OBB()
Constructor.
Definition: OBB-inl.h:81
fcl::obbDisjoint
template bool obbDisjoint(const Matrix3< double > &B, const Vector3< double > &T, const Vector3< double > &a, const Vector3< double > &b)
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::operator+
template TMatrix3< double > operator+(const Matrix3< double > &m1, const TMatrix3< double > &m2)


fcl
Author(s):
autogenerated on Tue Dec 5 2023 03:40:48