diff --git a/cpp/open3d/geometry/BoundingVolume.cpp b/cpp/open3d/geometry/BoundingVolume.cpp index 30e3944ba6e..74fd14002e1 100644 --- a/cpp/open3d/geometry/BoundingVolume.cpp +++ b/cpp/open3d/geometry/BoundingVolume.cpp @@ -29,6 +29,7 @@ #include #include +#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& points) { AxisAlignedBoundingBox box; diff --git a/cpp/open3d/geometry/BoundingVolume.h b/cpp/open3d/geometry/BoundingVolume.h index 7ef2e570f82..30cdf2b1b0e 100644 --- a/cpp/open3d/geometry/BoundingVolume.h +++ b/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_); } diff --git a/cpp/open3d/geometry/IntersectionTest.cpp b/cpp/open3d/geometry/IntersectionTest.cpp index 1c5cd3477fc..22cf7847f7e 100644 --- a/cpp/open3d/geometry/IntersectionTest.cpp +++ b/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& 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 diff --git a/cpp/open3d/geometry/IntersectionTest.h b/cpp/open3d/geometry/IntersectionTest.h index bcc56a20a18..5d75d2066b3 100644 --- a/cpp/open3d/geometry/IntersectionTest.h +++ b/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& 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, diff --git a/cpp/tests/geometry/IntersectionTest.cpp b/cpp/tests/geometry/IntersectionTest.cpp index e2d71534eb9..53176248c6f 100644 --- a/cpp/tests/geometry/IntersectionTest.cpp +++ b/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; +using pln_t = std::tuple; +#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 {}; + +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 {}; + +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 {}; + +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 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