OrthoTreeContainerBox::PlaneIntersection

attcs · Feb 13, 2024 · fbd4f9d · fbd4f9d
1 parent 8bf564a
commit fbd4f9d
Show file tree

Hide file tree

Showing 3 changed files with 161 additions and 0 deletions.
diff --git 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<entity_id_type> PlaneIntersection(geometry_type const& distanceOfOrigo, vector_type const& planeNormal, geometry_type tolerance, span<box_type const> const& vBox) const noexcept
+    {
+      auto results = vector<entity_id_type>{};
+      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<std::pair<entity_id_type, entity_id_type>> CollisionDetection(OrthoTreeBoundingBox const& treeL, span<box_type const> const& vBoxL, OrthoTreeBoundingBox const& treeR, span<box_type const> const& vBoxR) noexcept
     {

diff --git a/octree_container.h b/octree_container.h
@@ -240,6 +240,11 @@ namespace OrthoTree
       return this->m_tree.template RangeSearch<isFullyContained>(range, this->m_vData);
     }
 
+    // Plane intersection (Plane equation: dotProduct(planeNormal, pt) = distanceOfOrigo)
+    inline vector<entity_id_type> 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
@@ -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<size_t>{ 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<size_t>{ 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<size_t>{ 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<size_t>{ 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<size_t>{}, 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<size_t>{1, 2, 4}, intersectedBoxes));
+    }
+
+
     TEST_METHOD(CollistionDetection__33_24_34)
     {
       autoce boxesL = array