16 #ifndef SURGSIM_MATH_AABB_H
17 #define SURGSIM_MATH_AABB_H
19 #include <Eigen/Geometry>
27 typedef Eigen::AlignedBox<float, 3>
Aabbf;
30 typedef Eigen::AlignedBox<double, 3>
Aabbd;
40 template <
class Scalar,
int Dim>
42 const Eigen::AlignedBox<Scalar, Dim>& aabb1,
45 typedef typename Eigen::AlignedBox<Scalar, Dim>::VectorType VectorType;
47 VectorType vector = (aabb1.center() - aabb0.center()).array().abs();
48 VectorType totalSizes = ((aabb0.sizes() + aabb1.sizes()) * 0.5).array() + tolerance;
50 return (vector.array() <= totalSizes.array()).all();
59 template <
class Scalar,
int Dim>
61 const Eigen::AlignedBox<Scalar, Dim>& b)
63 return !a.intersection(b).isEmpty();
71 template <
class Scalar,
int Dim,
int MType>
73 const Eigen::Matrix<Scalar, Dim, 1, MType>& vector0,
74 const Eigen::Matrix<Scalar, Dim, 1, MType>& vector1,
75 const Eigen::Matrix<Scalar, Dim, 1, MType>& vector2)
77 Eigen::AlignedBox<Scalar, Dim> result(vector0);
78 result.extend(vector1);
79 result.extend(vector2);
89 template <
class Scalar,
int Dim>
90 Eigen::AlignedBox<Scalar, Dim>
transformAabb(
const Eigen::Transform<Scalar, Dim, Eigen::Isometry>& transform,
91 const Eigen::AlignedBox<Scalar, Dim>& aabb)
93 static std::array<typename Eigen::AlignedBox<Scalar, Dim>::CornerType, 8> corners =
95 Eigen::AlignedBox<Scalar, Dim>::BottomLeftFloor, Eigen::AlignedBox<Scalar, Dim>::BottomRightFloor,
96 Eigen::AlignedBox<Scalar, Dim>::TopLeftFloor, Eigen::AlignedBox<Scalar, Dim>::TopRightFloor,
97 Eigen::AlignedBox<Scalar, Dim>::BottomLeftCeil, Eigen::AlignedBox<Scalar, Dim>::BottomRightCeil,
98 Eigen::AlignedBox<Scalar, Dim>::TopLeftCeil, Eigen::AlignedBox<Scalar, Dim>::TopRightCeil,
105 Eigen::AlignedBox<Scalar, Dim> result;
106 std::for_each(corners.cbegin(), corners.cend(),
107 [&result, &aabb, &transform](
typename Eigen::AlignedBox<Scalar, Dim>::CornerType c)
109 result.extend(transform * aabb.corner(c));