Added intersection/distance tests to AxisAlignedBoundingBox

cpp/open3d/geometry/BoundingVolume.cpp

@@ -29,6 +29,7 @@
 #include <Eigen/Eigenvalues>
 #include <numeric>
 
+#include "BoundingVolume.h"
 #include "open3d/geometry/PointCloud.h"
 #include "open3d/geometry/Qhull.h"
 #include "open3d/geometry/TriangleMesh.h"
@@ -270,12 +271,18 @@ AxisAlignedBoundingBox& AxisAlignedBoundingBox::operator+=(
         min_bound_ = other.min_bound_;
         max_bound_ = other.max_bound_;
     } else if (!other.IsEmpty()) {
-        min_bound_ = min_bound_.array().min(other.min_bound_.array()).matrix();
-        max_bound_ = max_bound_.array().max(other.max_bound_.array()).matrix();
+        Join(other);
     }
     return *this;
 }
 
+AxisAlignedBoundingBox& AxisAlignedBoundingBox::Join(
+        const AxisAlignedBoundingBox& other) {
+    min_bound_ = min_bound_.array().min(other.min_bound_.array()).matrix();
+    max_bound_ = max_bound_.array().max(other.max_bound_.array()).matrix();
+    return *this;
+}
+
 AxisAlignedBoundingBox AxisAlignedBoundingBox::CreateFromPoints(
         const std::vector<Eigen::Vector3d>& points) {
     AxisAlignedBoundingBox box;

cpp/open3d/geometry/BoundingVolume.h

@@ -199,8 +199,23 @@ class AxisAlignedBoundingBox : public Geometry3D {
     virtual AxisAlignedBoundingBox& Rotate(
             const Eigen::Matrix3d& R, const Eigen::Vector3d& center) override;
 
+    /// \brief Adds another AxisAlignedBoundingBox in place, such that the
+    /// current object becomes the union of both boxes. However, if this
+    /// AxisAlignedBoundingBox is empty (has a volume of 0), this object will
+    /// assume the bounds of the other box, regardless of any non-empty
+    /// dimensions. For a simpler union behavior that ignores the box volume use
+    /// the Join(...) method.
+    /// \param other an AxisAlignedBoundingBox to join with this one
+    /// \return a reference to this AxisAlignedBoundingBox
     AxisAlignedBoundingBox& operator+=(const AxisAlignedBoundingBox& other);
 
+    /// \brief Adds another AxisAlignedBoundingBox in place, such that the
+    /// current object becomes the union of both boxes. Does not consider the
+    /// box volume like the += operator does.
+    /// \param other an AxisAlignedBoundingBox to join with this one
+    /// \return a reference to this AxisAlignedBoundingBox
+    AxisAlignedBoundingBox& Join(const AxisAlignedBoundingBox& other);
+
     /// Get the extent/length of the bounding box in x, y, and z dimension.
     Eigen::Vector3d GetExtent() const { return (max_bound_ - min_bound_); }
 

cpp/open3d/geometry/IntersectionTest.cpp

@@ -50,6 +50,75 @@ bool IntersectionTest::AABBAABB(const Eigen::Vector3d& min0,
     return true;
 }
 
+bool IntersectionTest::AABBAABB(const AxisAlignedBoundingBox& box0,
+                                const AxisAlignedBoundingBox& box1) {
+    return AABBAABB(box0.min_bound_, box0.max_bound_, box1.min_bound_,
+                    box1.max_bound_);
+}
+
+bool IntersectionTest::PlaneAABB(const Eigen::Hyperplane<double, 3>& plane,
+                                 const AxisAlignedBoundingBox& box) {
+    // This test is based off of
+    // https://gdbooks.gitbooks.io/3dcollisions/content/Chapter2/static_aabb_plane.html
+    // and consists of projecting the extent onto the plane normal and checking
+    // the length
+
+    auto e = box.GetHalfExtent();
+    auto r = e.x() * std::abs(plane.normal().x()) +
+             e.y() * std::abs(plane.normal().y()) +
+             e.z() * std::abs(plane.normal().z());
+
+    auto s = plane.normal().dot(box.GetCenter()) - plane.offset();
+    return std::abs(s) <= r;
+}
+
+Eigen::Vector3d IntersectionTest::ClosestPointAABB(
+        const Eigen::Vector3d& point, const AxisAlignedBoundingBox& box) {
+    // The closest point within an axis aligned bounding box to another point in
+    // space can be found by clamping the test point to the AABB's bounds.
+    double x = std::min(std::max(point.x(), box.min_bound_.x()),
+                        box.max_bound_.x());
+    double y = std::min(std::max(point.y(), box.min_bound_.y()),
+                        box.max_bound_.y());
+    double z = std::min(std::max(point.z(), box.min_bound_.z()),
+                        box.max_bound_.z());
+    return {x, y, z};
+}
+
+Eigen::Vector3d IntersectionTest::FarthestPointAABB(
+        const Eigen::Vector3d& point, const AxisAlignedBoundingBox& box) {
+    // Each dimension of the farthest point will lie on either the maximum or
+    // minimum bound for that dimension.  The farthest point can be found by
+    // choosing the bound for each dimension which maximizes the distance to the
+    // test point dimension.
+    double x = std::abs(point.x() - box.min_bound_.x()) >
+                               std::abs(point.x() - box.max_bound_.x())
+                       ? box.min_bound_.x()
+                       : box.max_bound_.x();
+
+    double y = std::abs(point.y() - box.min_bound_.y()) >
+                               std::abs(point.y() - box.max_bound_.y())
+                       ? box.min_bound_.y()
+                       : box.max_bound_.y();
+
+    double z = std::abs(point.z() - box.min_bound_.z()) >
+                               std::abs(point.z() - box.max_bound_.z())
+                       ? box.min_bound_.z()
+                       : box.max_bound_.z();
+
+    return {x, y, z};
+}
+
+double IntersectionTest::ClosestDistanceAABB(
+        const Eigen::Vector3d& point, const AxisAlignedBoundingBox& box) {
+    return (ClosestPointAABB(point, box) - point).norm();
+}
+
+double IntersectionTest::FarthestDistanceAABB(
+        const Eigen::Vector3d& point, const AxisAlignedBoundingBox& box) {
+    return (FarthestPointAABB(point, box) - point).norm();
+}
+
 bool IntersectionTest::TriangleTriangle3d(const Eigen::Vector3d& p0,
                                           const Eigen::Vector3d& p1,
                                           const Eigen::Vector3d& p2,
@@ -216,6 +285,5 @@ double IntersectionTest::LineSegmentsMinimumDistance(
     double dist = (p - q).norm();
     return dist;
 }
-
 }  // namespace geometry
 }  // namespace open3d

cpp/open3d/geometry/IntersectionTest.h

@@ -36,11 +36,76 @@ namespace geometry {
 
 class IntersectionTest {
 public:
+    /// Computes whether or not there is an intersection between two axis
+    /// aligned bounding boxes, represented by four points specifying the
+    /// upper and lower bounds of each box. The arguments given must represent
+    /// valid AABBs where no minimum bound dimension is greater than its maximum
+    /// bound dimension, or the result will not be correct.
+    /// \param min0 the minimum bound of box 0
+    /// \param max0 the maximum bound of box 0
+    /// \param min1 the minimum bound of box 1
+    /// \param max1 the maximum bound of box 1
+    /// \return true if the boxes intersect, false if they do not
     static bool AABBAABB(const Eigen::Vector3d& min0,
                          const Eigen::Vector3d& max0,
                          const Eigen::Vector3d& min1,
                          const Eigen::Vector3d& max1);
 
+    /// Computes whether or not there is an intersection between two axis
+    /// aligned bounding boxes.
+    /// \param box0 The first box
+    /// \param box1 The second box
+    /// \return true if the boxes intersect, false if they do not
+    static bool AABBAABB(const AxisAlignedBoundingBox& box0,
+                         const AxisAlignedBoundingBox& box1);
+
+    /// Computes whether or not there is an intersection between a plane and an
+    /// axis aligned bounding box. Will test positive even if a single corner of
+    /// the box lies in the plane.
+    /// \param plane The test plane
+    /// \param box The axis aligned bounding box to check against the plane
+    /// \return true if the plane intersects the box, false if not
+    static bool PlaneAABB(const Eigen::Hyperplane<double, 3>& plane,
+                          const AxisAlignedBoundingBox& box);
+
+    /// Returns the closest point contained in an axis aligned bounding box to
+    /// a test point.
+    /// \param point The test point to find the closest point to
+    /// \param box The box which defines the region to search for the closest
+    /// point
+    /// \return A Vector3d containing the minimum distance point to the test
+    /// point contained by the box
+    static Eigen::Vector3d ClosestPointAABB(const Eigen::Vector3d& point,
+                                            const AxisAlignedBoundingBox& box);
+
+    /// Returns the farthest point contained in an axis aligned bounding box
+    /// from a test point.
+    /// \param point The test point to find the farthest point from
+    /// \param box The box which defines the region to search for the farthest
+    /// point
+    /// \return A Vector3d containing the maximum distance point to the test
+    /// point contained by the box
+    static Eigen::Vector3d FarthestPointAABB(const Eigen::Vector3d& point,
+                                             const AxisAlignedBoundingBox& box);
+
+    /// Returns the closest distance to an axis aligned bounding box from a test
+    /// point.
+    /// \param point The test point to find the closest distance to
+    /// \param box The box which defines the region to search for the closest
+    /// distance
+    /// \return the minimum linear distance from the test point to the AABB
+    static double ClosestDistanceAABB(const Eigen::Vector3d& point,
+                                      const AxisAlignedBoundingBox& box);
+
+    /// Returns the farthest distance to an axis aligned bounding box from a
+    /// test point.
+    /// \param point The test point to find the farthest distance to
+    /// \param box The box which defines the region to search for the farthest
+    /// distance
+    /// \return the maximum linear distance from the test point to the AABB
+    static double FarthestDistanceAABB(const Eigen::Vector3d& point,
+                                       const AxisAlignedBoundingBox& box);
+
     static bool TriangleTriangle3d(const Eigen::Vector3d& p0,
                                    const Eigen::Vector3d& p1,
                                    const Eigen::Vector3d& p2,

cpp/tests/geometry/IntersectionTest.cpp

@@ -28,6 +28,15 @@
 
 #include "tests/UnitTest.h"
 
+using namespace open3d::geometry;
+using namespace ::testing;
+using Eigen::Vector3d;
+
+using pnt_t = std::tuple<Vector3d, Vector3d>;
+using pln_t = std::tuple<Vector3d, Vector3d, bool>;
+#define ROOT2 1.0 / sqrt(2.0)
+#define ROOT3 1.0 / sqrt(3.0)
+
 namespace open3d {
 namespace tests {
 
@@ -86,5 +95,136 @@ TEST(IntersectionTest, LineSegmentsMinimumDistance) {
               1.);
 }
 
+TEST(IntersectionTest, ClosestDistanceAABBPoint) {
+    // Tests the closest distance from a point to an AABB. This single test is
+    // adequate under the assumption that the ClosestDistanceAABB uses the
+    // ClosestPointAABB method under the hood, and the parameterized tests
+    // in ClosestPointAABBTests pass.
+    AxisAlignedBoundingBox box{{0, 0, 0}, {1, 1, 1}};
+    auto value = IntersectionTest::ClosestDistanceAABB({4, 0.5, 0.5}, box);
+    EXPECT_DOUBLE_EQ(3.0, value);
+}
+
+// Closest point to AABB tests
+class ClosestPointAABBTests : public TestWithParam<pnt_t> {};
+
+TEST_P(ClosestPointAABBTests, CheckClosestPoints) {
+    AxisAlignedBoundingBox box{{0, 0, 0}, {1, 1, 1}};
+    auto test = std::get<0>(GetParam());
+    auto expected = std::get<1>(GetParam());
+
+    auto closest = IntersectionTest::ClosestPointAABB(test, box);
+    ExpectEQ(expected, closest);
+}
+
+INSTANTIATE_TEST_CASE_P(IntersectionTest,
+                        ClosestPointAABBTests,
+                        Values(  // Faces
+                                pnt_t({-1, 0.5, 0.5}, {0, 0.5, 0.5}),
+                                pnt_t({2, 0.5, 0.5}, {1, 0.5, 0.5}),
+                                pnt_t({0.5, -1, 0.5}, {0.5, 0, 0.5}),
+                                pnt_t({0.5, 2, 0.5}, {0.5, 1, 0.5}),
+                                pnt_t({0.5, 0.5, -1}, {0.5, 0.5, 0}),
+                                pnt_t({0.5, 0.5, 2}, {0.5, 0.5, 1}),
+
+                                // Edges
+                                pnt_t({0.5, -1, -1}, {0.5, 0, 0}),
+                                pnt_t({0.5, 2, 2}, {0.5, 1, 1}),
+                                pnt_t({-1, 0.5, -1}, {0, 0.5, 0}),
+                                pnt_t({2, 0.5, 2}, {1, 0.5, 1}),
+                                pnt_t({-1, -1, 0.5}, {0, 0, 0.5}),
+                                pnt_t({2, 2, 0.5}, {1, 1, 0.5}),
+
+                                // Corners
+                                pnt_t({-1, -1, -1}, {0, 0, 0}),
+                                pnt_t({2, 2, 2}, {1, 1, 1}),
+
+                                // Points already inside the AABB
+                                pnt_t({0, 1, 1}, {0, 1, 1}),
+                                pnt_t({0.5, 0.5, 0.5}, {0.5, 0.5, 0.5})));
+
+// Farthest point to AABB tests
+class FarthestPointAABBTests : public TestWithParam<pnt_t> {};
+
+TEST_P(FarthestPointAABBTests, CheckFarthestPoints) {
+    AxisAlignedBoundingBox box{{-1, -1, -1}, {1, 1, 1}};
+    auto test = std::get<0>(GetParam());
+    auto expected = std::get<1>(GetParam());
+
+    auto closest = IntersectionTest::FarthestPointAABB(test, box);
+    ExpectEQ(expected, closest);
+}
+
+INSTANTIATE_TEST_CASE_P(IntersectionTest,
+                        FarthestPointAABBTests,
+                        Values(  // Inside
+                                pnt_t({0.5, 0.5, 0.5}, {-1, -1, -1}),
+                                pnt_t({-0.5, 0.5, 0.5}, {1, -1, -1}),
+                                pnt_t({0.5, -0.5, 0.5}, {-1, 1, -1}),
+                                pnt_t({0.5, 0.5, -0.5}, {-1, -1, 1}),
+                                pnt_t({-0.5, -0.5, 0.5}, {1, 1, -1}),
+                                pnt_t({0.5, -0.5, -0.5}, {-1, 1, 1}),
+                                pnt_t({-0.5, 0.5, -0.5}, {1, -1, 1}),
+                                pnt_t({-0.5, -0.5, -0.5}, {1, 1, 1}),
+
+                                // Outside
+                                pnt_t({3, 3, 3}, {-1, -1, -1}),
+                                pnt_t({-3, 3, 3}, {1, -1, -1}),
+                                pnt_t({3, -3, 3}, {-1, 1, -1}),
+                                pnt_t({3, 3, -3}, {-1, -1, 1}),
+                                pnt_t({-3, -3, 3}, {1, 1, -1}),
+                                pnt_t({3, -3, -3}, {-1, 1, 1}),
+                                pnt_t({-3, 3, -3}, {1, -1, 1}),
+                                pnt_t({-3, -3, -3}, {1, 1, 1})));
+
+// Hyperplane to AABB Intersection Tests
+class PlaneAABBTests : public TestWithParam<pln_t> {};
+
+TEST_P(PlaneAABBTests, CheckFarthestPoints) {
+    AxisAlignedBoundingBox box{{-1, -1, -1}, {1, 1, 1}};
+    auto point = std::get<0>(GetParam());
+    auto normal = std::get<1>(GetParam());
+    auto expected = std::get<2>(GetParam());
+
+    Eigen::Hyperplane<double, 3> plane(normal, point);
+    auto intersects = IntersectionTest::PlaneAABB(plane, box);
+    EXPECT_EQ(expected, intersects);
+}
+
+INSTANTIATE_TEST_CASE_P(
+        IntersectionTest,
+        PlaneAABBTests,
+        Values(pln_t({2, 0, 0}, {1, 0, 0}, false),
+               pln_t({-2, 0, 0}, {1, 0, 0}, false),
+               pln_t({2, 0, 0}, {-1, 0, 0}, false),
+               pln_t({-2, 0, 0}, {-1, 0, 0}, false),
+               pln_t({0, 2, 0}, {0, 1, 0}, false),
+               pln_t({0, -2, 0}, {0, 1, 0}, false),
+               pln_t({0, 0, 2}, {0, 0, 1}, false),
+               pln_t({0, 0, -2}, {0, 0, 1}, false),
+               pln_t({2, 2, 2}, {ROOT3, ROOT3, ROOT3}, false),
+               pln_t({-2, 2, 2}, {-ROOT3, ROOT3, ROOT3}, false),
+               pln_t({2, -2, 2}, {ROOT3, -ROOT3, ROOT3}, false),
+               pln_t({2, 2, -2}, {ROOT3, ROOT3, -ROOT3}, false),
+               pln_t({-2, -2, -2}, {ROOT3, ROOT3, ROOT3}, false),
+
+               pln_t({0, 0, 0}, {1, 0, 0}, true),
+               pln_t({0, 0, 0}, {-1, 0, 0}, true),
+               pln_t({0, 0, 0}, {0, 1, 0}, true),
+               pln_t({0, 0, 0}, {0, -1, 0}, true),
+               pln_t({0, 0, 0}, {0, 0, 1}, true),
+               pln_t({0, 0, 0}, {0, 0, -1}, true),
+
+               pln_t({1, 0, 0}, {1, 0, 0}, true),
+               pln_t({0, 1, 0}, {0, 1, 0}, true),
+               pln_t({0, 0, 1}, {0, 0, 1}, true),
+
+               pln_t({2, 2, 2}, {-ROOT2, ROOT2, 0}, true),
+               pln_t({-1, -1, -1}, {-ROOT3, -ROOT3, -ROOT3}, true),
+               pln_t({1, -1, -1}, {ROOT3, -ROOT3, -ROOT3}, true),
+               pln_t({-1, 1, -1}, {-ROOT3, ROOT3, -ROOT3}, true),
+               pln_t({-1, -1, 1}, {-ROOT3, -ROOT3, ROOT3}, true),
+               pln_t({1, 1, 1}, {ROOT3, ROOT3, ROOT3}, true)));
+
 }  // namespace tests
 }  // namespace open3d