diff --git a/octree.h b/octree.h index 51b5c36..bc03e7b 100644 --- a/octree.h +++ b/octree.h @@ -418,6 +418,26 @@ namespace OrthoTree return rMin < 0 ? rMax : rMin; } + + + // Plane intersection (Plane equation: dotProduct(planeNormal, pt) = distanceOfOrigo) + static constexpr bool does_plane_intersect(box_type const& box, geometry_type distanceOfOrigo, vector_type const& planeNormal, geometry_type tolerance) noexcept + { + autoc& minPoint = base::box_min_c(box); + autoc& maxPoint = base::box_max_c(box); + + autoc center = multiply(add(minPoint, maxPoint), 0.5); + autoc radius = subtract(maxPoint, center); + + double radiusProjected = 0.0; + for (dim_type iDimension = 0; iDimension < nDimension; ++iDimension) + radiusProjected += base::point_comp_c(radius, iDimension) * abs(base::point_comp_c(planeNormal, iDimension)); + + autoc centerProjected = dot(planeNormal, center); + + return abs(centerProjected - distanceOfOrigo) <= radiusProjected + tolerance; + } + }; @@ -2431,6 +2451,36 @@ namespace OrthoTree } + // Plane intersection (Plane equation: dotProduct(planeNormal, pt) = distanceOfOrigo) + vector PlaneIntersection(geometry_type const& distanceOfOrigo, vector_type const& planeNormal, geometry_type tolerance, span const& vBox) const noexcept + { + auto results = vector{}; + if constexpr (nDimension < 3) // under 3 dimension, every boxes will be intersected. + { + results.resize(vBox.size()); + iota(results.begin(), results.end(), 0); + return results; + } + + autoc selector = [&](morton_node_id_type id, Node const& node) -> bool + { + return AD::does_plane_intersect(node.box, distanceOfOrigo, planeNormal, tolerance); + }; + + autoc procedure = [&](morton_node_id_type id, Node const& node) + { + for (autoc id : node.vid) + if (AD::does_plane_intersect(vBox[id], distanceOfOrigo, planeNormal, tolerance)) + if (std::find(results.begin(), results.end(), id) == results.end()) + results.emplace_back(id); + }; + + this->VisitNodesInDFS(base::GetRootKey(), procedure, selector); + + return results; + } + + // Collision detection: Returns all overlapping boxes from the source trees. static vector> CollisionDetection(OrthoTreeBoundingBox const& treeL, span const& vBoxL, OrthoTreeBoundingBox const& treeR, span const& vBoxR) noexcept { diff --git a/octree_container.h b/octree_container.h index 8c1caa1..ea66e40 100644 --- a/octree_container.h +++ b/octree_container.h @@ -240,6 +240,11 @@ namespace OrthoTree return this->m_tree.template RangeSearch(range, this->m_vData); } + // Plane intersection (Plane equation: dotProduct(planeNormal, pt) = distanceOfOrigo) + inline vector PlaneIntersection(geometry_type distanceOfOrigo, vector_type const& planeNormal, geometry_type tolerance) const noexcept + { + return this->m_tree.PlaneIntersection(distanceOfOrigo, planeNormal, tolerance, this->m_vData); + } public: // Collision detection diff --git a/unittests/general.tests.cpp b/unittests/general.tests.cpp index 1e2d64d..bee686d 100644 --- a/unittests/general.tests.cpp +++ b/unittests/general.tests.cpp @@ -1976,6 +1976,112 @@ namespace Tree2DTest } + TEST_METHOD(PlaneIntersection_3D_XY_d0) + { + autoce boxes = array + { + BoundingBox3D{ { 0.0, 0.0, 0.0 }, { 1.0, 1.0, 0.0 } }, + BoundingBox3D{ { 1.0, 1.0, -1.0 }, { 2.0, 2.0, 0.0 } }, + BoundingBox3D{ { 2.0, 2.0, -1.0 }, { 3.0, 3.0, -0.2 } }, + BoundingBox3D{ { 3.0, 3.0, 1.0 }, { 4.0, 4.0, 0.2 } }, + BoundingBox3D{ { 1.2, 1.2, -1.0 }, { 2.8, 2.8, 1.0 } } + }; + + autoc octreebox = OctreeBox(boxes, 3, std::nullopt, 2); + autoc intersectedBoxes = octreebox.PlaneIntersection(0.0, Point3D{ 0.0, 0.0, 1.0 }, 0.01, boxes); + + Assert::IsTrue(std::ranges::is_permutation(vector{ 0, 1, 4 }, intersectedBoxes)); + } + + + TEST_METHOD(PlaneIntersection_3D_XY_dP1) + { + autoce boxes = array + { + BoundingBox3D{ { 0.0, 0.0, 0.0 }, { 1.0, 1.0, 0.0 } }, + BoundingBox3D{ { 1.0, 1.0, -1.0 }, { 2.0, 2.0, 0.0 } }, + BoundingBox3D{ { 2.0, 2.0, -1.0 }, { 3.0, 3.0, -0.2 } }, + BoundingBox3D{ { 3.0, 3.0, 1.0 }, { 4.0, 4.0, 0.2 } }, + BoundingBox3D{ { 1.2, 1.2, -1.0 }, { 2.8, 2.8, 1.1 } } + }; + + autoc octreebox = OctreeBox(boxes, 3, std::nullopt, 2); + autoc intersectedBoxes = octreebox.PlaneIntersection(1.0, Point3D{ 0.0, 0.0, 1.0 }, 0.01, boxes); + + Assert::IsTrue(std::ranges::is_permutation(vector{ 4 }, intersectedBoxes)); + } + + TEST_METHOD(PlaneIntersection_3D_XY_dN1) + { + autoce boxes = array + { + BoundingBox3D{ { 0.0, 0.0, 0.0 }, { 1.0, 1.0, 0.0 } }, + BoundingBox3D{ { 1.0, 1.0, -1.0 }, { 2.0, 2.0, 0.0 } }, + BoundingBox3D{ { 2.0, 2.0, -1.0 }, { 3.0, 3.0, -0.2 } }, + BoundingBox3D{ { 3.0, 3.0, 1.0 }, { 4.0, 4.0, 0.2 } }, + BoundingBox3D{ { 1.2, 1.2, -1.1 }, { 2.8, 2.8, 1.1 } } + }; + + autoc octreebox = OctreeBox(boxes, 3, std::nullopt, 2); + autoc intersectedBoxes = octreebox.PlaneIntersection(-1.0, Point3D{ 0.0, 0.0, 1.0 }, 0.01, boxes); + + Assert::IsTrue(std::ranges::is_permutation(vector{ 1, 2, 4 }, intersectedBoxes)); + } + + + TEST_METHOD(PlaneIntersection_3D_YZ_dP1) + { + autoce boxes = array + { + BoundingBox3D{ { 0.0, 0.0, 0.0 }, { 1.0, 1.0, 0.0 } }, + BoundingBox3D{ { 1.0, 1.0, -1.0 }, { 2.0, 2.0, 0.0 } }, + BoundingBox3D{ { 2.0, 2.0, -1.0 }, { 3.0, 3.0, -0.2 } }, + BoundingBox3D{ { 3.0, 3.0, 1.0 }, { 4.0, 4.0, 0.2 } }, + BoundingBox3D{ { 1.2, 1.2, -1.1 }, { 2.8, 2.8, 1.1 } } + }; + + autoc octreebox = OctreeBox(boxes, 3, std::nullopt, 2); + autoc intersectedBoxes = octreebox.PlaneIntersection(1.0, Point3D{ 1.0, 0.0, 0.0 }, 0.01, boxes); + + Assert::IsTrue(std::ranges::is_permutation(vector{ 0, 1 }, intersectedBoxes)); + } + + TEST_METHOD(PlaneIntersection_3D_YZ_dN1) + { + autoce boxes = array + { + BoundingBox3D{ { 0.0, 0.0, 0.0 }, { 1.0, 1.0, 0.0 } }, + BoundingBox3D{ { 1.0, 1.0, -1.0 }, { 2.0, 2.0, 0.0 } }, + BoundingBox3D{ { 2.0, 2.0, -1.0 }, { 3.0, 3.0, -0.2 } }, + BoundingBox3D{ { 3.0, 3.0, 1.0 }, { 4.0, 4.0, 0.2 } }, + BoundingBox3D{ { 1.2, 1.2, -1.1 }, { 2.8, 2.8, 1.1 } } + }; + + autoc octreebox = OctreeBox(boxes, 3, std::nullopt, 2); + autoc intersectedBoxes = octreebox.PlaneIntersection(-1.0, Point3D{ 1.0, 0.0, 0.0 }, 0.01, boxes); + + Assert::IsTrue(std::ranges::is_permutation(vector{}, intersectedBoxes)); + } + + TEST_METHOD(PlaneIntersection_3D_YZA_dPSQRT2) + { + autoce boxes = array + { + BoundingBox3D{ { 0.0, 0.0, 0.0 }, { 1.0, 1.0, 0.0 } }, + BoundingBox3D{ { 1.0, 1.0, -1.0 }, { 2.0, 2.0, 0.0 } }, + BoundingBox3D{ { 2.0, 2.0, -1.0 }, { 3.0, 3.0, -0.2 } }, + BoundingBox3D{ { 3.0, 3.0, 1.0 }, { 4.0, 4.0, 0.2 } }, + BoundingBox3D{ { 1.2, 1.2, -1.1 }, { 2.8, 2.8, 1.1 } } + }; + + autoc octreebox = OctreeBox(boxes, 3, std::nullopt, 2); + autoc sqrt2__2 = std::sqrt(2.0) * 0.5; + autoc intersectedBoxes = octreebox.PlaneIntersection(std::sqrt(2.0), Point3D{ 0.0, sqrt2__2, sqrt2__2 }, 0.01, boxes); + + Assert::IsTrue(std::ranges::is_permutation(vector{1, 2, 4}, intersectedBoxes)); + } + + TEST_METHOD(CollistionDetection__33_24_34) { autoce boxesL = array