38 #ifndef FCL_BV_OBB_INL_H
39 #define FCL_BV_OBB_INL_H
67 const Matrix3<double>& B,
68 const Vector3<double>& T,
69 const Vector3<double>& a,
70 const Vector3<double>& b);
75 const Transform3<double>& tf,
76 const Vector3<double>& a,
77 const Vector3<double>& b);
91 : axis(axis_), To(center_), extent(extent_)
105 axis.col(0).dot(t), axis.col(1).dot(t), axis.col(2).dot(t));
112 template <
typename S>
121 template <
typename S>
125 S proj = local_p.dot(axis.col(0));
126 if((proj > extent[0]) || (proj < -extent[0]))
129 proj = local_p.dot(axis.col(1));
130 if((proj > extent[1]) || (proj < -extent[1]))
133 proj = local_p.dot(axis.col(2));
134 if((proj > extent[2]) || (proj < -extent[2]))
141 template <
typename S>
151 template <
typename S>
154 *
this = *
this + other;
160 template <
typename S>
166 if(center_diff.norm() > 2 * (max_extent + max_extent2))
177 template <
typename S>
180 return 2 * extent[0];
184 template <
typename S>
187 return 2 * extent[1];
191 template <
typename S>
194 return 2 * extent[2];
198 template <
typename S>
201 return width() * height() * depth();
205 template <
typename S>
208 return extent.squaredNorm();
212 template <
typename S>
219 template <
typename S>
227 std::cerr <<
"OBB distance not implemented!\n";
232 template <
typename S>
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;
253 template <
typename S>
265 b.
axis.col(0).normalize();
268 for(
int i = 0; i < 16; ++i)
270 vertex_proj[i] = vertex[i];
271 vertex_proj[i].noalias() -= b.
axis.col(0) * vertex[i].dot(b.
axis.col(0));
274 getCovariance<S>(vertex_proj,
nullptr,
nullptr,
nullptr, 16, M);
294 else if (s[2] > s[
max])
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];
308 getExtentAndCenter<S>(
309 vertex,
nullptr,
nullptr,
nullptr, 16, b.
axis, b.
To, b.
extent);
315 template <
typename S>
319 b.
To = (b1.
To + b2.
To) * 0.5;
323 q1.coeffs() = -q1.coeffs();
327 b.
axis = q.toRotationMatrix();
332 Vector3<S> pmin(real_max, real_max, real_max);
333 Vector3<S> pmax(-real_max, -real_max, -real_max);
336 for(
int i = 0; i < 8; ++i)
338 diff = vertex[i] - b.
To;
339 for(
int j = 0; j < 3; ++j)
341 S dot = diff.dot(b.
axis.col(j));
344 else if(dot < pmin[j])
350 for(
int i = 0; i < 8; ++i)
352 diff = vertex[i] - b.
To;
353 for(
int j = 0; j < 3; ++j)
355 S dot = diff.dot(b.
axis.col(j));
358 else if(dot < pmin[j])
363 for(
int j = 0; j < 3; ++j)
365 b.
To += (b.
axis.col(j) * (0.5 * (pmax[j] + pmin[j])));
366 b.
extent[j] = 0.5 * (pmax[j] - pmin[j]);
373 template <
typename S,
typename Derived>
375 const OBB<S>& bv,
const Eigen::MatrixBase<Derived>& t)
383 template <
typename S,
typename DerivedA,
typename DerivedB>
384 bool overlap(
const Eigen::MatrixBase<DerivedA>& R0,
385 const Eigen::MatrixBase<DerivedB>& T0,
388 typename DerivedA::PlainObject R0b2 = R0 * b2.
axis;
389 typename DerivedA::PlainObject R = b1.
axis.transpose() * R0b2;
391 typename DerivedB::PlainObject Ttemp = R0 * b2.
To + T0 - b1.
To;
392 typename DerivedB::PlainObject T = Ttemp.transpose() * b1.
axis;
398 template <
typename S>
411 t = ((T[0] < 0.0) ? -T[0] : T[0]);
413 if(t > (a[0] + Bf.row(0).dot(b)))
418 t = ((s < 0.0) ? -s : s);
420 if(t > (b[0] + Bf.col(0).dot(a)))
424 t = ((T[1] < 0.0) ? -T[1] : T[1]);
426 if(t > (a[1] + Bf.row(1).dot(b)))
430 t =((T[2] < 0.0) ? -T[2] : T[2]);
432 if(t > (a[2] + Bf.row(2).dot(b)))
437 t = ((s < 0.0) ? -s : s);
439 if(t > (b[1] + Bf.col(1).dot(a)))
444 t = ((s < 0.0) ? -s : s);
446 if(t > (b[2] + Bf.col(2).dot(a)))
450 s = T[2] * B(1, 0) - T[1] * B(2, 0);
451 t = ((s < 0.0) ? -s : s);
453 if(t > (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) +
454 b[1] * Bf(0, 2) + b[2] * Bf(0, 1)))
458 s = T[2] * B(1, 1) - T[1] * B(2, 1);
459 t = ((s < 0.0) ? -s : s);
461 if(t > (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) +
462 b[0] * Bf(0, 2) + b[2] * Bf(0, 0)))
466 s = T[2] * B(1, 2) - T[1] * B(2, 2);
467 t = ((s < 0.0) ? -s : s);
469 if(t > (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) +
470 b[0] * Bf(0, 1) + b[1] * Bf(0, 0)))
474 s = T[0] * B(2, 0) - T[2] * B(0, 0);
475 t = ((s < 0.0) ? -s : s);
477 if(t > (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) +
478 b[1] * Bf(1, 2) + b[2] * Bf(1, 1)))
482 s = T[0] * B(2, 1) - T[2] * B(0, 1);
483 t = ((s < 0.0) ? -s : s);
485 if(t > (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) +
486 b[0] * Bf(1, 2) + b[2] * Bf(1, 0)))
490 s = T[0] * B(2, 2) - T[2] * B(0, 2);
491 t = ((s < 0.0) ? -s : s);
493 if(t > (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) +
494 b[0] * Bf(1, 1) + b[1] * Bf(1, 0)))
498 s = T[1] * B(0, 0) - T[0] * B(1, 0);
499 t = ((s < 0.0) ? -s : s);
501 if(t > (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) +
502 b[1] * Bf(2, 2) + b[2] * Bf(2, 1)))
506 s = T[1] * B(0, 1) - T[0] * B(1, 1);
507 t = ((s < 0.0) ? -s : s);
509 if(t > (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) +
510 b[0] * Bf(2, 2) + b[2] * Bf(2, 0)))
514 s = T[1] * B(0, 2) - T[0] * B(1, 2);
515 t = ((s < 0.0) ? -s : s);
517 if(t > (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) +
518 b[0] * Bf(2, 1) + b[1] * Bf(2, 0)))
526 template <
typename S>
541 t = ((tf.translation()[0] < 0.0) ? -tf.translation()[0] : tf.translation()[0]);
543 if(t > (a[0] + Bf.row(0).dot(b)))
547 s = tf.linear().col(0).dot(tf.translation());
548 t = ((s < 0.0) ? -s : s);
550 if(t > (b[0] + Bf.col(0).dot(a)))
554 t = ((tf.translation()[1] < 0.0) ? -tf.translation()[1] : tf.translation()[1]);
556 if(t > (a[1] + Bf.row(1).dot(b)))
560 t =((tf.translation()[2] < 0.0) ? -tf.translation()[2] : tf.translation()[2]);
562 if(t > (a[2] + Bf.row(2).dot(b)))
566 s = tf.linear().col(1).dot(tf.translation());
567 t = ((s < 0.0) ? -s : s);
569 if(t > (b[1] + Bf.col(1).dot(a)))
573 s = tf.linear().col(2).dot(tf.translation());
574 t = ((s < 0.0) ? -s : s);
576 if(t > (b[2] + Bf.col(2).dot(a)))
580 s = tf.translation()[2] * tf.linear()(1, 0) - tf.translation()[1] * tf.linear()(2, 0);
581 t = ((s < 0.0) ? -s : s);
583 if(t > (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) +
584 b[1] * Bf(0, 2) + b[2] * Bf(0, 1)))
588 s = tf.translation()[2] * tf.linear()(1, 1) - tf.translation()[1] * tf.linear()(2, 1);
589 t = ((s < 0.0) ? -s : s);
591 if(t > (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) +
592 b[0] * Bf(0, 2) + b[2] * Bf(0, 0)))
596 s = tf.translation()[2] * tf.linear()(1, 2) - tf.translation()[1] * tf.linear()(2, 2);
597 t = ((s < 0.0) ? -s : s);
599 if(t > (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) +
600 b[0] * Bf(0, 1) + b[1] * Bf(0, 0)))
604 s = tf.translation()[0] * tf.linear()(2, 0) - tf.translation()[2] * tf.linear()(0, 0);
605 t = ((s < 0.0) ? -s : s);
607 if(t > (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) +
608 b[1] * Bf(1, 2) + b[2] * Bf(1, 1)))
612 s = tf.translation()[0] * tf.linear()(2, 1) - tf.translation()[2] * tf.linear()(0, 1);
613 t = ((s < 0.0) ? -s : s);
615 if(t > (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) +
616 b[0] * Bf(1, 2) + b[2] * Bf(1, 0)))
620 s = tf.translation()[0] * tf.linear()(2, 2) - tf.translation()[2] * tf.linear()(0, 2);
621 t = ((s < 0.0) ? -s : s);
623 if(t > (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) +
624 b[0] * Bf(1, 1) + b[1] * Bf(1, 0)))
628 s = tf.translation()[1] * tf.linear()(0, 0) - tf.translation()[0] * tf.linear()(1, 0);
629 t = ((s < 0.0) ? -s : s);
631 if(t > (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) +
632 b[1] * Bf(2, 2) + b[2] * Bf(2, 1)))
636 s = tf.translation()[1] * tf.linear()(0, 1) - tf.translation()[0] * tf.linear()(1, 1);
637 t = ((s < 0.0) ? -s : s);
639 if(t > (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) +
640 b[0] * Bf(2, 2) + b[2] * Bf(2, 0)))
644 s = tf.translation()[1] * tf.linear()(0, 2) - tf.translation()[0] * tf.linear()(1, 2);
645 t = ((s < 0.0) ? -s : s);
647 if(t > (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) +
648 b[0] * Bf(2, 1) + b[1] * Bf(2, 0)))