From ededa6b0909aaf22c4d74a74883382e007afa21a Mon Sep 17 00:00:00 2001 From: attcs Date: Mon, 23 Dec 2024 22:15:19 +0100 Subject: [PATCH] Internal Geometry module Abstract classes are enabled for TBox and TVector --- CHANGELOG.md | 26 + README.md | 1 - benchmarks/OrthoTreePointDynamicGeneral.h | 69 +- benchmarks/benchmarks.cpp | 4 +- octree.h | 1133 +++++++++++++-------- unittests/adaptor.tests.cpp | 279 ++++- unittests/compile_test.h | 12 +- unittests/general.tests.cpp | 37 +- 8 files changed, 1110 insertions(+), 451 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index a206806..e8d5aae 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,5 +1,31 @@ # Changelog +## 2024-12-26 +New features +* Support of abstract classes + +Refactorizations +* Internal geometry module + +Maintenance +* Support compilers w/o std::execution (e.g. Apple Clang) + +## 2024-03-26 +New features +* New adaptors: glm, boost, CGAL +* Built-in `Ray` and `Plane` types +* Rebalancing insert + +Refactorizations +* New, more capable adaptor concept +* Separated Adapator tests +* Support of `Eigen::Hyperplane` and `Eigen::ParametrizedLine` +* Support of Unreal Engine `FRay`, `FPlane` + +Maintenance +* GCC compile action +* Fix a bug in `Insert()` + ## 2024-03-02 New features * New adaptors: Eigen, Unreal Engine, XYZ diff --git a/README.md b/README.md index 750d56d..a89c9e0 100644 --- a/README.md +++ b/README.md @@ -26,7 +26,6 @@ What is the Octree and what is good for? https://en.wikipedia.org/wiki/Octree ## Limitations * Maximum number of dimensions is 63. * Maximum depth of octree solutions is 10. -* Abstract classes cannot be used for `TVector` and `TBox` ## Requirements * Language standard: C++20 or above diff --git a/benchmarks/OrthoTreePointDynamicGeneral.h b/benchmarks/OrthoTreePointDynamicGeneral.h index e42f2b6..7f2d614 100644 --- a/benchmarks/OrthoTreePointDynamicGeneral.h +++ b/benchmarks/OrthoTreePointDynamicGeneral.h @@ -16,8 +16,8 @@ template< typename TBox, typename TRay, typename TPlane, - typename TGeonetry = double, - typename adaptor_type = OrthoTree::AdaptorGeneral> + typename TGeometry = double, + typename adaptor_type = OrthoTree::AdaptorGeneral> class OrthoTreePointDynamicGeneral { static size_t constexpr _nChild = 1 << DIMENSION_NO; @@ -78,11 +78,39 @@ class OrthoTreePointDynamicGeneral } + static inline TBox BoxInvertedInit() noexcept + { + auto ext = TBox{}; + + for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) + { + AD::SetBoxMinC(ext, dimensionID, std::numeric_limits::max()); + AD::SetBoxMaxC(ext, dimensionID, std::numeric_limits::lowest()); + } + + return ext; + } + + static TBox GetBoxOfPoints(std::span const& points) noexcept + { + auto ext = BoxInvertedInit(); + for (autoc& point : points) + for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) + { + if (AD::GetBoxMinC(ext, dimensionID) > AD::GetPointC(point, dimensionID)) + AD::SetBoxMinC(ext, dimensionID, AD::GetPointC(point, dimensionID)); + + if (AD::GetBoxMaxC(ext, dimensionID) < AD::GetPointC(point, dimensionID)) + AD::SetBoxMaxC(ext, dimensionID, AD::GetPointC(point, dimensionID)); + } + + return ext; + } public: static OrthoTreePointDynamicGeneral Create(span const& vpt, size_t nDepthMax, std::optional const& obox, size_t nElementMax) { - autoc box = obox.has_value() ? *obox : AD::GetBoxOfPoints(vpt); + autoc box = obox.has_value() ? *obox : GetBoxOfPoints(vpt); autoc npt = vpt.size(); auto aid = vector(npt, IdEntityNode{}); @@ -110,8 +138,8 @@ template< typename TBox, typename TRay, typename TPlane, - typename TGeonetry = double, - typename adaptor_type = OrthoTree::AdaptorGeneral> + typename TGeometry = double, + typename adaptor_type = OrthoTree::AdaptorGeneral> class OrthoTreeBoxDynamicGeneral { static size_t constexpr _nChild = 1 << DIMENSION_NO; @@ -192,11 +220,40 @@ class OrthoTreeBoxDynamicGeneral } + static inline TBox BoxInvertedInit() noexcept + { + auto ext = TBox{}; + + for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) + { + AD::SetBoxMinC(ext, dimensionID, std::numeric_limits::max()); + AD::SetBoxMaxC(ext, dimensionID, std::numeric_limits::lowest()); + } + + return ext; + } + + static TBox GetBoxOfBoxes(std::span const& boxes) noexcept + { + auto ext = BoxInvertedInit(); + for (autoc& e : boxes) + { + for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) + { + if (AD::GetBoxMinC(ext, dimensionID) > AD::GetBoxMinC(e, dimensionID)) + AD::SetBoxMinC(ext, dimensionID, AD::GetBoxMinC(e, dimensionID)); + + if (AD::GetBoxMaxC(ext, dimensionID) < AD::GetBoxMaxC(e, dimensionID)) + AD::SetBoxMaxC(ext, dimensionID, AD::GetBoxMaxC(e, dimensionID)); + } + } + return ext; + } public: static OrthoTreeBoxDynamicGeneral Create(span const& vBox, size_t nDepthMax, std::optional const& obox = std::nullopt, size_t nElementMax = 20) { - autoc box = obox.has_value() ? *obox : AD::GetBoxOfBoxes(vBox); + autoc box = obox.has_value() ? *obox : GetBoxOfBoxes(vBox); autoc nEnt = vBox.size(); auto aid = vector(nEnt, IdEntityNode{}); diff --git a/benchmarks/benchmarks.cpp b/benchmarks/benchmarks.cpp index 1831f1c..3bf988f 100644 --- a/benchmarks/benchmarks.cpp +++ b/benchmarks/benchmarks.cpp @@ -772,7 +772,7 @@ int main() ofstream report; report.open("report.csv"); - autoce nDepth = depth_t{ 5 }; + autoce nDepth = depth_t{ 5 }; { autoc szName = string("Diagonally placed points"); autoc aPointDiag_100M = GenerateGeometry>>([&] { return CreatePoints_Diagonal(); }, szName, 100, report); @@ -840,7 +840,7 @@ int main() autoc aBox = GenerateGeometry>>([&] { return CreateBoxes_CylindricalSemiRandom(aSizeNonLog.back())>(); }, szName, aSizeNonLog.back(), report); autoc vTaskBruteForce = SelfConflictBruteForceBoxTasks("Box self conflict by brute force", aBox); autoc vTaskTreeCollisionDetection = CollisionDetectionBoxTasks(3, "Box self conflict by octree", aBox); - autoc vTaskTreeSearch = SearchTreeBoxTasks(3, "Box search by octree", aBox); + autoc vTaskTreeSearch = SearchTreeBoxTasks(5, "Box search by octree", aBox); RunTasks(vTaskBruteForce, report); RunTasks(vTaskTreeCollisionDetection, report); diff --git a/octree.h b/octree.h index 258697c..c1d10e2 100644 --- a/octree.h +++ b/octree.h @@ -22,6 +22,18 @@ OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ + +/* Settings +* Use the following define-s before the header include + +Node center is not stored within the nodes. It will be calculated ad-hoc everytime when it is required, e.g in search algorithm. +#define ORTHOTREE__DISABLED_NODECENTER + +Node size is not stored within the nodes. It will be calculated ad-hoc everytime when it is required, e.g in search algorithm. +#define ORTHOTREE__DISABLED_NODESIZE +*/ + + #ifndef ORTHOTREE_GUARD #define ORTHOTREE_GUARD @@ -77,7 +89,7 @@ SOFTWARE. #define EXEC_POL_TEMPLATE_PARAM typename TExecutionPolicy = std::execution::unsequenced_policy, #define EXEC_POL_TEMPLATE_ADD(func) template func #define EXEC_POL_TEMPLATE_ADDF(func) func -#define EXEC_POL_DEF(e) auto constexpr e = TExecutionPolicy{} +#define EXEC_POL_DEF(e) TExecutionPolicy constexpr e #define EXEC_POL_ADD(e) e, #else #define EXEC_POL_TEMPLATE_DECL @@ -301,15 +313,15 @@ namespace OrthoTree } && requires(TRay const& ray) { { TAdapter::GetRayOrigin(ray) - } -> std::convertible_to; + } -> std::convertible_to; } && requires(TRay const& ray) { { TAdapter::GetRayDirection(ray) - } -> std::convertible_to; + } -> std::convertible_to; } && requires(TPlane const& plane) { { TAdapter::GetPlaneNormal(plane) - } -> std::convertible_to; + } -> std::convertible_to; } && requires(TPlane const& plane) { { TAdapter::GetPlaneOrigoDistance(plane) @@ -334,10 +346,6 @@ namespace OrthoTree { TAdapter::GetPointPlaneRelation(box, distanceOfOrigo, planeNormal, tolerance) } -> std::convertible_to; - } && requires(TBox const& box, TGeometry distanceOfOrigo, TVector const& planeNormal, TGeometry tolerance) { - { - TAdapter::GetBoxPlaneRelation(box, distanceOfOrigo, planeNormal, tolerance) - } -> std::convertible_to; } && requires(TBox const& box, TVector const& rayBasePoint, TVector const& rayHeading, TGeometry tolerance) { { TAdapter::GetRayBoxDistance(box, rayBasePoint, rayHeading, tolerance) @@ -375,19 +383,6 @@ namespace OrthoTree using Base = TAdaptorBasics; static_assert(AdaptorBasicsConcept); - static constexpr TGeometry Size2(TVector const& point) noexcept - { - auto d2 = TGeometry{ 0 }; - for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) - { - autoc d = Base::GetPointC(point, dimensionID); - d2 += d * d; - } - return d2; - } - - static constexpr TGeometry Size(TVector const& point) noexcept { return std::sqrt(Size2(point)); } - static constexpr TVector Add(TVector const& ptL, TVector const& ptR) noexcept { auto point = TVector{}; @@ -397,24 +392,29 @@ namespace OrthoTree return point; } - static constexpr TVector Subtract(TVector const& ptL, TVector const& ptR) noexcept + static void MoveBox(TBox& box, TVector const& moveVector) noexcept { - auto point = TVector{}; + LOOPIVDEP for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) - Base::SetPointC(point, dimensionID, Base::GetPointC(ptL, dimensionID) - Base::GetPointC(ptR, dimensionID)); - - return point; + { + Base::SetBoxMinC(box, dimensionID, Base::GetBoxMinC(box, dimensionID) + Base::GetPointC(moveVector, dimensionID)); + Base::SetBoxMaxC(box, dimensionID, Base::GetBoxMaxC(box, dimensionID) + Base::GetPointC(moveVector, dimensionID)); + } } - static constexpr TVector Multiply(TVector const& ptL, double rScalarR) noexcept + static constexpr TGeometry Size2(TVector const& point) noexcept { - auto point = TVector{}; + auto d2 = TGeometry{ 0 }; for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) - Base::SetPointC(point, dimensionID, TGeometry(Base::GetPointC(ptL, dimensionID) * rScalarR)); - - return point; + { + autoc d = Base::GetPointC(point, dimensionID); + d2 += d * d; + } + return d2; } + static constexpr TGeometry Size(TVector const& point) noexcept { return std::sqrt(Size2(point)); } + static constexpr TGeometry Dot(TVector const& ptL, TVector const& ptR) noexcept { auto value = TGeometry{}; @@ -424,9 +424,19 @@ namespace OrthoTree return value; } - static constexpr TGeometry Distance(TVector const& ptL, TVector const& ptR) noexcept { return Size(Subtract(ptL, ptR)); } - static constexpr TGeometry Distance2(TVector const& ptL, TVector const& ptR) noexcept { return Size2(Subtract(ptL, ptR)); } + static constexpr TGeometry Distance2(TVector const& ptL, TVector const& ptR) noexcept + { + auto d2 = TGeometry{ 0 }; + for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) + { + autoc d = Base::GetPointC(ptL, dimensionID) - Base::GetPointC(ptR, dimensionID); + d2 += d * d; + } + return d2; + } + + static constexpr TGeometry Distance(TVector const& ptL, TVector const& ptR) noexcept { return std::sqrt(Distance2(ptL, ptR)); } static constexpr bool ArePointsEqual(TVector const& ptL, TVector const& ptR, TGeometry rAccuracy) noexcept { @@ -455,16 +465,6 @@ namespace OrthoTree return true; } - static constexpr bool DoesBoxContainPointStrict(TBox const& box, TVector const& point) noexcept - { - for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) - if (!(Base::GetBoxMinC(box, dimensionID) < Base::GetPointC(point, dimensionID) && - Base::GetPointC(point, dimensionID) < Base::GetBoxMaxC(box, dimensionID))) - return false; - - return true; - } - enum class EBoxRelation { Overlapped = -1, @@ -523,62 +523,6 @@ namespace OrthoTree } } - static inline TBox BoxInvertedInit() noexcept - { - auto ext = TBox{}; - - LOOPIVDEP - for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) - { - Base::SetBoxMinC(ext, dimensionID, std::numeric_limits::max()); - Base::SetBoxMaxC(ext, dimensionID, std::numeric_limits::lowest()); - } - - return ext; - } - - static TBox GetBoxOfPoints(std::span const& points) noexcept - { - auto ext = BoxInvertedInit(); - for (autoc& point : points) - for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) - { - if (Base::GetBoxMinC(ext, dimensionID) > Base::GetPointC(point, dimensionID)) - Base::SetBoxMinC(ext, dimensionID, Base::GetPointC(point, dimensionID)); - - if (Base::GetBoxMaxC(ext, dimensionID) < Base::GetPointC(point, dimensionID)) - Base::SetBoxMaxC(ext, dimensionID, Base::GetPointC(point, dimensionID)); - } - - return ext; - } - - static TBox GetBoxOfBoxes(std::span const& boxes) noexcept - { - auto ext = BoxInvertedInit(); - for (autoc& e : boxes) - { - for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) - { - if (Base::GetBoxMinC(ext, dimensionID) > Base::GetBoxMinC(e, dimensionID)) - Base::SetBoxMinC(ext, dimensionID, Base::GetBoxMinC(e, dimensionID)); - - if (Base::GetBoxMaxC(ext, dimensionID) < Base::GetBoxMaxC(e, dimensionID)) - Base::SetBoxMaxC(ext, dimensionID, Base::GetBoxMaxC(e, dimensionID)); - } - } - return ext; - } - - static void MoveBox(TBox& box, TVector const& moveVector) noexcept - { - LOOPIVDEP - for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) - { - Base::SetBoxMinC(box, dimensionID, Base::GetBoxMinC(box, dimensionID) + Base::GetPointC(moveVector, dimensionID)); - Base::SetBoxMaxC(box, dimensionID, Base::GetBoxMaxC(box, dimensionID) + Base::GetPointC(moveVector, dimensionID)); - } - } static constexpr std::optional GetRayBoxDistance(TBox const& box, TVector const& rayBasePoint, TVector const& rayHeading, TGeometry tolerance) noexcept { @@ -664,36 +608,6 @@ namespace OrthoTree return PlaneRelation::Hit; } - - // Get box-Hyperplane relation (Plane equation: dotProduct(planeNormal, point) = distanceOfOrigo) - static constexpr PlaneRelation GetBoxPlaneRelation(TBox const& box, TGeometry distanceOfOrigo, TVector const& planeNormal, TGeometry tolerance) noexcept - { - assert(IsNormalizedVector(planeNormal)); - - TVector center, radius; - for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) - { - autoc minComponent = Base::GetBoxMinC(box, dimensionID); - autoc maxComponent = Base::GetBoxMaxC(box, dimensionID); - autoc centerComponent = static_cast((minComponent + maxComponent) * 0.5); - Base::SetPointC(center, dimensionID, centerComponent); - Base::SetPointC(radius, dimensionID, centerComponent - minComponent); - } - - auto radiusProjected = double(tolerance); - for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) - radiusProjected += Base::GetPointC(radius, dimensionID) * std::abs(Base::GetPointC(planeNormal, dimensionID)); - - autoc centerProjected = Dot(planeNormal, center); - - if (centerProjected + radiusProjected < distanceOfOrigo) - return PlaneRelation::Negative; - - if (centerProjected - radiusProjected > distanceOfOrigo) - return PlaneRelation::Positive; - - return PlaneRelation::Hit; - } }; @@ -886,6 +800,338 @@ namespace OrthoTree } }; + namespace detail + { + // Internal geometry system which + // - can be instantiated + // - is float-based (and not suffer from integer aritmetic issues) + template + struct InternalGeometryModule + { + using Geometry = typename std::conditional, float, TGeometry>::type; + using Vector = std::array; + struct Box + { + Vector Min, Max; + }; + + static constexpr Geometry Size2(Vector const& vector) noexcept + { + auto d2 = Geometry{ 0 }; + LOOPIVDEP + for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) + d2 += vector[dimensionID] * vector[dimensionID]; + + return d2; + } + + static Geometry Size(Vector const& vector) noexcept { return std::sqrt(Size2(vector)); } + + static constexpr Vector GetBoxCenter(Box const& box) noexcept + { + Vector center; + LOOPIVDEP + for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) + center[dimensionID] = (box.Min[dimensionID] + box.Max[dimensionID]) * Geometry(0.5); + + return center; + } + + static constexpr Vector GetBoxCenterAD(TBox const& box) noexcept + { + Vector center; + LOOPIVDEP + for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) + center[dimensionID] = (AD::GetBoxMinC(box, dimensionID) + AD::GetBoxMaxC(box, dimensionID)) * Geometry(0.5); + + return center; + } + + static constexpr Vector GetBoxHalfSizeAD(TBox const& box) noexcept + { + Vector halfSize; + LOOPIVDEP + for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) + halfSize[dimensionID] = (AD::GetBoxMaxC(box, dimensionID) - AD::GetBoxMinC(box, dimensionID)) * Geometry(0.5); + + return halfSize; + } + + static bool AreBoxesOverlappingByCenter(Vector const& centerLhs, Vector const& centerRhs, Vector const& sizeLhs, Vector const& sizeRhs) noexcept + { + Vector distance; + LOOPIVDEP + for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) + distance[dimensionID] = centerLhs[dimensionID] - centerRhs[dimensionID]; + + Vector sizeLimit; + LOOPIVDEP + for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) + sizeLimit[dimensionID] = (sizeLhs[dimensionID] + sizeRhs[dimensionID]) * Geometry(0.5); + + for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) + if (sizeLimit[dimensionID] <= std::abs(distance[dimensionID])) + return false; + + return true; + } + + static constexpr void MoveAD(Vector& v, TVector const& moveVector) noexcept + { + LOOPIVDEP + for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) + v[dimensionID] += AD::GetPointC(moveVector, dimensionID); + } + + static constexpr void MoveAD(Box& box, TVector const& moveVector) noexcept + { + LOOPIVDEP + for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) + { + box.Min[dimensionID] += AD::GetPointC(moveVector, dimensionID); + box.Max[dimensionID] += AD::GetPointC(moveVector, dimensionID); + } + } + + static constexpr TGeometry DotAD(TVector const& ptL, Vector const& ptR) noexcept + { + auto value = TGeometry{}; + LOOPIVDEP + for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) + value += AD::GetPointC(ptL, dimensionID) * ptR[dimensionID]; + + return value; + } + + static constexpr bool DoesRangeContainBoxAD(TBox const& range, Box const& box) noexcept + { + for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) + { + if (AD::GetBoxMinC(range, dimensionID) > box.Min[dimensionID] || box.Min[dimensionID] > AD::GetBoxMaxC(range, dimensionID)) + return false; + + if (AD::GetBoxMinC(range, dimensionID) > box.Max[dimensionID] || box.Max[dimensionID] > AD::GetBoxMaxC(range, dimensionID)) + return false; + } + return true; + } + + + static constexpr bool DoesRangeContainBoxAD(Box const& range, TBox const& box) noexcept + { + for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) + { + if (range.Min[dimensionID] > AD::GetBoxMinC(box, dimensionID) || AD::GetBoxMinC(box, dimensionID) > range.Max[dimensionID]) + return false; + + if (range.Min[dimensionID] > AD::GetBoxMaxC(box, dimensionID) || AD::GetBoxMaxC(box, dimensionID) > range.Max[dimensionID]) + return false; + } + return true; + } + + static constexpr PlaneRelation GetBoxPlaneRelationAD( + Vector const& center, Vector const& halfSize, TGeometry distanceOfOrigo, TVector const& planeNormal, TGeometry tolerance) noexcept + { + assert(AD::IsNormalizedVector(planeNormal)); + + auto radiusProjected = double(tolerance); + for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) + radiusProjected += halfSize[dimensionID] * std::abs(AD::GetPointC(planeNormal, dimensionID)); + + autoc centerProjected = DotAD(planeNormal, center) - distanceOfOrigo; + + if (centerProjected + radiusProjected < 0.0) + return PlaneRelation::Negative; + + if (centerProjected - radiusProjected > 0.0) + return PlaneRelation::Positive; + + return PlaneRelation::Hit; + } + + static consteval Box BoxInvertedInit() noexcept + { + Box ext; + ext.Min.fill(+std::numeric_limits::max()); + ext.Max.fill(-std::numeric_limits::max()); + return ext; + } + + static constexpr Box GetBoxAD(TBox const& box) noexcept + { + Box boxIGM; + LOOPIVDEP + for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) + { + boxIGM.Min[dimensionID] = Geometry(AD::GetBoxMinC(box, dimensionID)); + boxIGM.Max[dimensionID] = Geometry(AD::GetBoxMaxC(box, dimensionID)); + } + + return boxIGM; + } + + template + static constexpr Box GetBoxOfPointsAD(TContainer const& points) noexcept + { + auto ext = BoxInvertedInit(); + for (autoc& e : points) + { + autoc& point = detail::getValuePart(e); + for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) + { + if (ext.Min[dimensionID] > AD::GetPointC(point, dimensionID)) + ext.Min[dimensionID] = Geometry(AD::GetPointC(point, dimensionID)); + + if (ext.Max[dimensionID] < AD::GetPointC(point, dimensionID)) + ext.Max[dimensionID] = Geometry(AD::GetPointC(point, dimensionID)); + } + } + return ext; + } + + template + static constexpr Box GetBoxOfBoxesAD(TContainer const& boxes) noexcept + { + auto ext = BoxInvertedInit(); + for (autoc& e : boxes) + { + autoc& box = detail::getValuePart(e); + for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) + { + if (ext.Min[dimensionID] > AD::GetBoxMinC(box, dimensionID)) + ext.Min[dimensionID] = Geometry(AD::GetBoxMinC(box, dimensionID)); + + if (ext.Max[dimensionID] < AD::GetBoxMaxC(box, dimensionID)) + ext.Max[dimensionID] = Geometry(AD::GetBoxMaxC(box, dimensionID)); + } + } + return ext; + } + + static constexpr bool DoesBoxContainPointAD(Box const& box, TVector const& point, TGeometry tolerance = 0) noexcept + { + if (tolerance != 0.0) + { + assert(tolerance > 0); + for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) + if (!(box.Min[dimensionID] - tolerance < AD::GetPointC(point, dimensionID) && + AD::GetPointC(point, dimensionID) < box.Max[dimensionID] + tolerance)) + return false; + } + else + { + for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) + if (!(box.Min[dimensionID] <= AD::GetPointC(point, dimensionID) && AD::GetPointC(point, dimensionID) <= box.Max[dimensionID])) + return false; + } + return true; + } + + + static constexpr bool DoesBoxContainPointAD(Vector const& center, Vector const& halfSizes, TVector const& point, TGeometry tolerance = 0) noexcept + { + if (tolerance != 0.0) + { + assert(tolerance > 0); + for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) + { + autoc pointDistance = std::abs(Geometry(AD::GetPointC(point, dimensionID)) - center[dimensionID]); + autoc halfSize = halfSizes[dimensionID] + tolerance; + if (pointDistance >= halfSize) + return false; + } + } + else + { + for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) + { + autoc pointDistance = std::abs(Geometry(AD::GetPointC(point, dimensionID)) - center[dimensionID]); + if (pointDistance > halfSizes[dimensionID]) + return false; + } + } + return true; + } + + static Vector GetBoxWallDistanceAD(TVector const& searchPoint, Vector const& centerPoint, Vector const& halfSize) noexcept + { + Vector distance; + for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) + { + autoc centerDistance = std::abs(centerPoint[dimensionID] - Geometry(AD::GetPointC(searchPoint, dimensionID))); + distance[dimensionID] = halfSize[dimensionID] - centerDistance; + } + + return distance; + } + + static constexpr std::optional GetRayBoxDistanceAD( + Vector const& center, Vector const& halfSizes, TVector const& rayBasePoint, TVector const& rayHeading, TGeometry tolerance) noexcept + { + if (DoesBoxContainPointAD(center, halfSizes, rayBasePoint, tolerance)) + return 0.0; + + autoce inf = std::numeric_limits::infinity(); + + auto minBoxDistances = Vector{}; + auto maxBoxDistances = Vector{}; + for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) + { + autoc dirComp = Geometry(AD::GetPointC(rayHeading, dimensionID)); + autoc minBox = center[dimensionID] - halfSizes[dimensionID] - Geometry(tolerance); + autoc maxBox = center[dimensionID] + halfSizes[dimensionID] + Geometry(tolerance); + if (dirComp == 0) + { + if (tolerance != 0.0) + { + // Box should be within tolerance (<, not <=) + + assert(tolerance > 0); + if (maxBox <= AD::GetPointC(rayBasePoint, dimensionID)) + return std::nullopt; + + if (minBox >= AD::GetPointC(rayBasePoint, dimensionID)) + return std::nullopt; + } + else + { + if (maxBox < AD::GetPointC(rayBasePoint, dimensionID)) + return std::nullopt; + + if (minBox > AD::GetPointC(rayBasePoint, dimensionID)) + return std::nullopt; + } + + minBoxDistances[dimensionID] = -inf; + maxBoxDistances[dimensionID] = +inf; + } + else + { + autoc pointComp = Geometry(AD::GetPointC(rayBasePoint, dimensionID)); + autoc dirCompRecip = Geometry(1.0) / dirComp; + if (dirComp < 0.0) + { + minBoxDistances[dimensionID] = (maxBox - pointComp) * dirCompRecip; + maxBoxDistances[dimensionID] = (minBox - pointComp) * dirCompRecip; + } + else + { + minBoxDistances[dimensionID] = (minBox - pointComp) * dirCompRecip; + maxBoxDistances[dimensionID] = (maxBox - pointComp) * dirCompRecip; + } + } + } + + autoc minBoxDistance = *std::ranges::max_element(minBoxDistances); + autoc maxBoxDistance = *std::ranges::min_element(maxBoxDistances); + if (minBoxDistance > maxBoxDistance || maxBoxDistance < 0.0) + return std::nullopt; + else + return minBoxDistance < 0 ? maxBoxDistance : minBoxDistance; + } + }; + } // namespace detail // OrthoTrees @@ -937,21 +1183,29 @@ namespace OrthoTree // Type system determined maximal depth. static autoce MAX_THEORETICAL_DEPTH = depth_t((CHAR_BIT * sizeof(MortonNodeID) - 1 /*sentinal bit*/) / DIMENSION_NO); + public: + template + using DimArray = std::array; + using IGM = typename detail::InternalGeometryModule; + using IGM_Geometry = typename IGM::Geometry; + public: class Node { private: std::vector m_children; - TBox m_box = {}; +#ifndef ORTHOTREE__DISABLED_NODECENTER + IGM::Vector m_center; +#endif public: // Public members std::vector Entities = {}; public: - constexpr void SetBox(TBox const& box) noexcept { m_box = box; } - constexpr void SetBox(TBox&& box) noexcept { m_box = std::move(box); } - constexpr TBox const& GetBoxInternal() const noexcept { return m_box; } - constexpr TBox const& GetBox() const noexcept { return m_box; } +#ifndef ORTHOTREE__DISABLED_NODECENTER + constexpr IGM::Vector const& GetCenter() const noexcept { return m_center; } + constexpr void SetCenter(IGM::Vector&& center) noexcept { m_center = std::move(center); } +#endif // !ORTHOTREE__DISABLED_NODECENTER constexpr void AddChild(MortonNodeIDCR childKey) noexcept { m_children.emplace_back(childKey); } @@ -1003,11 +1257,10 @@ namespace OrthoTree constexpr std::vector const& GetChildren() const noexcept { return m_children; } }; - protected: // Aid struct to partitioning and distance ordering struct ItemDistance { - TGeometry Distance; + IGM_Geometry Distance; auto operator<=>(ItemDistance const& rhs) const = default; }; @@ -1023,8 +1276,6 @@ namespace OrthoTree Node const& NodeReference; }; - template - using DimArray = std::array; template using LinearUnderlyingContainer = std::unordered_map; @@ -1037,16 +1288,78 @@ namespace OrthoTree protected: // Member variables UnderlyingContainer m_nodes; - TBox m_boxSpace = {}; + + std::size_t m_maxElementNo = 11; depth_t m_maxDepthNo = {}; GridID m_maxRasterResolution = {}; GridID m_maxRasterID = {}; - std::size_t m_maxElementNo = 11; - double m_volumeOfOverallSpace = {}; - DimArray m_rasterizerFactors; - DimArray m_sizeInDimensions; - DimArray m_minInDimensions; + IGM::Box m_boxSpace = {}; + IGM::Geometry m_volumeOfOverallSpace = {}; + IGM::Vector m_rasterizerFactors; + IGM::Vector m_sizeInDimensions; + std::vector m_nodeSizes; + + public: // Node helpers + // Calculate extent by box of the tree and the key of the node + constexpr IGM::Vector CalculateCenter(MortonNodeIDCR key) const noexcept + { + using IGM_Vector = typename IGM::Vector; + + autoc depthID = this->GetDepthID(key); + autoc halfGrid = IGM_Geometry(pow2(GetDepthMax() - depthID)) * IGM_Geometry(0.5); + + autoc gridID = MortonDecode(key, GetDepthMax()); + IGM_Vector center; + LOOPIVDEP + for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) + center[dimensionID] = + (IGM_Geometry(gridID[dimensionID]) + halfGrid) / this->m_rasterizerFactors[dimensionID] + this->m_boxSpace.Min[dimensionID]; + + return center; + } + +#ifdef ORTHOTREE__DISABLED_NODECENTER + constexpr IGM::Vector GetNodeCenter(MortonNodeIDCR key) const noexcept { return CalculateCenter(key); } +#define GetNodeCenterMacro(inst, key, node) inst->GetNodeCenter(key) +#else + inline IGM::Vector const& GetNodeCenter(MortonNodeIDCR key) const noexcept { return this->GetNode(key).GetCenter(); } +#define GetNodeCenterMacro(inst, key, node) node.GetCenter() +#endif // ORTHOTREE__DISABLED_NODECENTER + +#ifdef ORTHOTREE__DISABLED_NODESIZE + constexpr IGM::Vector GetNodeSize(depth_t depthID) const noexcept + { + autoc depthFactor = IGM_Geometry(1.0) / IGM_Geometry(pow2(depthID)); + typename IGM::Vector size; + LOOPIVDEP + for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) + size[dimensionID] = this->m_sizeInDimensions[dimensionID] * depthFactor; + + return size; + } +#else + constexpr IGM::Vector const& GetNodeSize(depth_t depthID) const noexcept { return this->m_nodeSizes[depthID]; } +#endif // ORTHOTREE__DISABLED_NODESIZE + + constexpr IGM::Vector GetNodeSizeByKey(MortonNodeIDCR key) const noexcept { return this->GetNodeSize(this->GetDepthID(key)); } + + constexpr IGM::Box GetNodeBox(depth_t depthID, IGM::Vector const& center) const noexcept + { + autoc& halfSize = this->GetNodeSize(depthID + 1); // +1: half size will be required + typename IGM::Box box{ .Min = center, .Max = center }; + + LOOPIVDEP + for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) + { + box.Min[dimensionID] -= halfSize[dimensionID]; + box.Max[dimensionID] += halfSize[dimensionID]; + } + + return box; + } + + constexpr IGM::Box GetNodeBox(MortonNodeIDCR key) const noexcept { return this->GetNodeBox(this->GetDepthID(key), this->GetNodeCenter(key)); } protected: // Aid functions static inline ChildID castMortonIdToChildId(NonLinearMortonGridID const& bs) noexcept @@ -1059,17 +1372,18 @@ namespace OrthoTree protected: // Grid functions struct RasterInfo { - DimArray rasterizerFactors; - DimArray sizeInDimensions; + IGM::Vector rasterizerFactors; + IGM::Vector sizeInDimensions; }; - static constexpr RasterInfo getGridRasterizer(TBox const& box, GridID subDivisionNo) noexcept + static constexpr RasterInfo getGridRasterizer(IGM::Box const& box, GridID subDivisionNo) noexcept { - auto ri = RasterInfo{}; - autoc subDivisionNoFactor = static_cast(subDivisionNo); + RasterInfo ri; + autoc subDivisionNoFactor = IGM_Geometry(subDivisionNo); for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) { - ri.sizeInDimensions[dimensionID] = static_cast(AD::GetBoxMaxC(box, dimensionID) - AD::GetBoxMinC(box, dimensionID)); - ri.rasterizerFactors[dimensionID] = ri.sizeInDimensions[dimensionID] == 0 ? 1.0 : (subDivisionNoFactor / ri.sizeInDimensions[dimensionID]); + ri.sizeInDimensions[dimensionID] = box.Max[dimensionID] - box.Min[dimensionID]; + autoc isFlat = ri.sizeInDimensions[dimensionID] == 0; + ri.rasterizerFactors[dimensionID] = isFlat ? IGM_Geometry(1.0) : (subDivisionNoFactor / ri.sizeInDimensions[dimensionID]); } return ri; @@ -1081,7 +1395,7 @@ namespace OrthoTree auto gridIDs = DimArray{}; for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) { - auto pointComponent = AD::GetPointC(point, dimensionID) - AD::GetBoxMinC(this->m_boxSpace, dimensionID); + auto pointComponent = IGM_Geometry(AD::GetPointC(point, dimensionID)) - this->m_boxSpace.Min[dimensionID]; if constexpr (ALLOW_OUT_OF_BOX_GEOMETRY) { if (pointComponent < 0.0) @@ -1104,10 +1418,8 @@ namespace OrthoTree auto aid = std::array, 2>{}; for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) { - autoc minComponentRasterID = - static_cast(AD::GetBoxMinC(box, dimensionID) - AD::GetBoxMinC(m_boxSpace, dimensionID)) * m_rasterizerFactors[dimensionID]; - autoc maxComponentRasterID = - static_cast(AD::GetBoxMaxC(box, dimensionID) - AD::GetBoxMinC(m_boxSpace, dimensionID)) * m_rasterizerFactors[dimensionID]; + autoc minComponentRasterID = (IGM_Geometry(AD::GetBoxMinC(box, dimensionID)) - m_boxSpace.Min[dimensionID]) * m_rasterizerFactors[dimensionID]; + autoc maxComponentRasterID = (IGM_Geometry(AD::GetBoxMaxC(box, dimensionID)) - m_boxSpace.Min[dimensionID]) * m_rasterizerFactors[dimensionID]; if constexpr (DO_POINT_LIKE_CLASSIFICATION) { @@ -1116,14 +1428,14 @@ namespace OrthoTree } else { - if (minComponentRasterID < 1.0) + if (minComponentRasterID < IGM_Geometry(1)) aid[0][dimensionID] = 0; else if (minComponentRasterID > m_maxRasterID) aid[0][dimensionID] = m_maxRasterID; else aid[0][dimensionID] = static_cast(minComponentRasterID); - if (maxComponentRasterID < 1.0) + if (maxComponentRasterID < IGM_Geometry(1)) aid[1][dimensionID] = 0; else if (maxComponentRasterID > m_maxRasterID) aid[1][dimensionID] = m_maxRasterID; @@ -1139,85 +1451,24 @@ namespace OrthoTree inline Node& createChild(Node& parentNode, ChildID childID, MortonNodeIDCR childKey) noexcept { assert(childID < this->CHILD_NO); - auto& nodeChild = m_nodes[childKey]; - if constexpr (std::is_integral_v) - { - DimArray minNodePoint = this->m_minInDimensions; - DimArray maxNodePoint; - - autoc nDepth = this->GetDepthID(childKey); - - auto scaleFactor = 1.0; - if constexpr (IS_LINEAR_TREE) - { - auto mask = MortonNodeID{ 1 } << (nDepth * DIMENSION_NO - 1); - for (depth_t iDepth = 0; iDepth < nDepth; ++iDepth) - { - scaleFactor *= 0.5; - for (dim_t dimensionID = DIMENSION_NO; dimensionID > 0; --dimensionID) - { - bool isGreater; - if constexpr (IS_LINEAR_TREE) - isGreater = (childKey & mask); - else - isGreater = (childKey & mask).any(); - minNodePoint[dimensionID - 1] += isGreater * this->m_sizeInDimensions[dimensionID - 1] * scaleFactor; - mask >>= 1; - } - } - } - else - { - auto bitID = (nDepth * DIMENSION_NO - 1); - for (depth_t iDepth = 0; iDepth < nDepth; ++iDepth) - { - scaleFactor *= 0.5; - for (dim_t dimensionID = DIMENSION_NO; dimensionID > 0; --dimensionID) - { - minNodePoint[dimensionID - 1] += childKey[bitID] * this->m_sizeInDimensions[dimensionID - 1] * scaleFactor; - --bitID; - } - } - } - - LOOPIVDEP - for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) - maxNodePoint[dimensionID] = minNodePoint[dimensionID] + this->m_sizeInDimensions[dimensionID] * scaleFactor; - TBox childBox; - LOOPIVDEP - for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) - { - AD::SetBoxMinC(childBox, dimensionID, static_cast(minNodePoint[dimensionID])); - AD::SetBoxMaxC(childBox, dimensionID, static_cast(maxNodePoint[dimensionID])); - } +#ifndef ORTHOTREE__DISABLED_NODECENTER + autoc depthID = this->GetDepthID(childKey); + autoc& parentCenter = parentNode.GetCenter(); + autoc& halfSizes = this->GetNodeSize(depthID + 1); - nodeChild.SetBox(std::move(childBox)); - } - else + typename IGM::Vector childCenter; + LOOPIVDEP + for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) { - TBox childBox; - LOOPIVDEP - for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) - { - autoc isGreater = ((ChildID{ 1 } << dimensionID) & childID) > 0; - AD::SetBoxMinC( - childBox, - dimensionID, - isGreater * (AD::GetBoxMaxC(parentNode.GetBoxInternal(), dimensionID) + AD::GetBoxMinC(parentNode.GetBoxInternal(), dimensionID)) * - TGeometry{ 0.5 } + - (!isGreater) * AD::GetBoxMinC(parentNode.GetBoxInternal(), dimensionID)); - - AD::SetBoxMaxC( - childBox, - dimensionID, - isGreater * AD::GetBoxMaxC(parentNode.GetBoxInternal(), dimensionID) + - (!isGreater) * ((AD::GetBoxMaxC(parentNode.GetBoxInternal(), dimensionID) + AD::GetBoxMinC(parentNode.GetBoxInternal(), dimensionID)) * - TGeometry{ 0.5 })); - } - nodeChild.SetBox(std::move(childBox)); + autoc isGreater = ((ChildID{ 1 } << dimensionID) & childID) > 0; + autoc sign = IGM_Geometry(isGreater * 2 - 1); + childCenter[dimensionID] = parentCenter[dimensionID] + sign * halfSizes[dimensionID]; } + + nodeChild.SetCenter(std::move(childCenter)); +#endif // ORTHOTREE__DISABLED_NODECENTER return nodeChild; } @@ -1326,7 +1577,6 @@ namespace OrthoTree } case ControlFlow::FullRebalancing: { - autoce isPointSolution = std::is_same_v; autoc parentFlag = parentNodeKey << DIMENSION_NO; if (!shouldInsertInParentNode) @@ -1362,7 +1612,7 @@ namespace OrthoTree childNode.Entities.emplace_back(entityID); } - if constexpr (!isPointSolution) + if constexpr (IS_BOX_TYPE) { --remainingEntityNo; parentNode.Entities[i] = parentNode.Entities[remainingEntityNo]; @@ -1370,7 +1620,7 @@ namespace OrthoTree } } - if constexpr (isPointSolution) + if constexpr (!IS_BOX_TYPE) parentNode.Entities = {}; // All reserved memory should be freed. else { @@ -1416,8 +1666,9 @@ namespace OrthoTree { auto& newNode = this->m_nodes[entityNodeKey]; newNode.Entities.emplace_back(entityID); - newNode.SetBox(this->CalculateExtent(entityNodeKey)); - +#ifndef ORTHOTREE__DISABLED_NODECENTER + newNode.SetCenter(this->CalculateCenter(entityNodeKey)); +#endif // Create all child between the new (entityNodeKey) and the smallest existing one (parentNodeKey) auto newParentNodeKey = entityNodeKey; do @@ -1427,7 +1678,10 @@ namespace OrthoTree assert(IsValidKey(parentNodeKey)); auto& newParentNode = this->m_nodes[newParentNodeKey]; newParentNode.AddChildInOrder(childNodeKey); - newParentNode.SetBox(this->CalculateExtent(newParentNodeKey)); +#ifndef ORTHOTREE__DISABLED_NODECENTER + newParentNode.SetCenter(this->CalculateCenter(newParentNodeKey)); +#endif + } while (newParentNodeKey != parentNodeKey); } else @@ -1671,7 +1925,7 @@ namespace OrthoTree public: // Main service functions // Alternative creation mode (instead of Create), Init then Insert items into leafs one by one. NOT RECOMMENDED. - constexpr void Init(TBox const& box, depth_t maxDepthNo, std::size_t maxElementNo = 11) noexcept + constexpr void init(IGM::Box const& box, depth_t maxDepthNo, std::size_t maxElementNo) noexcept { assert(this->m_nodes.empty()); // To build/setup/create the tree, use the Create() [recommended] or Init() function. If an already builded // tree is wanted to be reset, use the Reset() function before init. @@ -1687,20 +1941,42 @@ namespace OrthoTree this->m_maxRasterID = this->m_maxRasterResolution - 1; this->m_maxElementNo = maxElementNo; - auto& nodeRoot = this->m_nodes[GetRootKey()]; - nodeRoot.SetBox(box); - autoc ri = this->getGridRasterizer(this->m_boxSpace, this->m_maxRasterResolution); - this->m_rasterizerFactors = std::move(ri.rasterizerFactors); - this->m_sizeInDimensions = std::move(ri.sizeInDimensions); - this->m_volumeOfOverallSpace = 1.0; - for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) - this->m_volumeOfOverallSpace *= this->m_sizeInDimensions[dimensionID]; + [[maybe_unused]] auto& nodeRoot = this->m_nodes[GetRootKey()]; +#ifndef ORTHOTREE__DISABLED_NODECENTER + nodeRoot.SetCenter(IGM::GetBoxCenter(box)); +#endif // !ORTHOTREE__DISABLED_NODECENTER - LOOPIVDEP - for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) - this->m_minInDimensions[dimensionID] = static_cast(AD::GetBoxMinC(this->m_boxSpace, dimensionID)); + // Size and raster info + { + auto [rasterizerFactors, sizeInDimensions] = getGridRasterizer(this->m_boxSpace, this->m_maxRasterResolution); + this->m_rasterizerFactors = std::move(rasterizerFactors); + this->m_sizeInDimensions = std::move(sizeInDimensions); + + // the 0-based depth size of the tree is m_maxDepthNo+1, and a fictive childnode halfsize (+2) could be asked prematurely. + depth_t constexpr additionalDepth = 3; + autoc examinedDepthSize = this->m_maxDepthNo + additionalDepth; + this->m_nodeSizes.resize(examinedDepthSize, this->m_sizeInDimensions); + autoce multiplier = IGM_Geometry(0.5); + auto factor = multiplier; + for (depth_t depthID = 1; depthID < examinedDepthSize; ++depthID, factor *= multiplier) + for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) + this->m_nodeSizes[depthID][dimensionID] *= factor; + } + + // Volume calculation + { + this->m_volumeOfOverallSpace = 1; + for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) + this->m_volumeOfOverallSpace *= this->m_sizeInDimensions[dimensionID]; + } } + public: // Main service functions + // Alternative creation mode (instead of Create), Init then Insert items into leafs one by one. NOT RECOMMENDED. + constexpr void Init(TBox const& box, depth_t maxDepthNo, std::size_t maxElementNo = 11) noexcept + { + this->init(IGM::GetBoxAD(box), maxDepthNo, maxElementNo); + } using FProcedure = std::function; using FProcedureUnconditional = std::function; @@ -1826,48 +2102,6 @@ namespace OrthoTree } - // Calculate extent by box of the tree and the key of the node - TBox CalculateExtent(MortonNodeIDCR nodeKey) const noexcept - { - auto nodeBox = TBox(); - - LOOPIVDEP - for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) - AD::SetBoxMinC(nodeBox, dimensionID, AD::GetBoxMinC(m_boxSpace, dimensionID)); - - autoc depthIDOfNode = GetDepthID(nodeKey); - autoc rasterResolutionNoAtNodeDepth = pow2(depthIDOfNode); - autoc rasterFactorAtNodeDepth = 1.0 / static_cast(rasterResolutionNoAtNodeDepth); - - autoce one = MortonGridID{ 1 }; - auto examinationKey = nodeKey; - for (depth_t depthID = 0; depthID < depthIDOfNode; ++depthID) - { - autoc rasterFactorAtDepth = rasterFactorAtNodeDepth * (1 << depthID); - - LOOPIVDEP - for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) - { - autoc shouldApply = ((examinationKey >> dimensionID) & one) > MortonGridID{}; - AD::SetBoxMinC( - nodeBox, - dimensionID, - AD::GetBoxMinC(nodeBox, dimensionID) + static_cast((this->m_sizeInDimensions[dimensionID] * rasterFactorAtDepth)) * shouldApply); - } - examinationKey >>= DIMENSION_NO; - } - - LOOPIVDEP - for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) - AD::SetBoxMaxC( - nodeBox, - dimensionID, - AD::GetBoxMinC(nodeBox, dimensionID) + static_cast(this->m_sizeInDimensions[dimensionID] * rasterFactorAtNodeDepth)); - - return nodeBox; - } - - // Reset the tree void Reset() noexcept { @@ -1890,13 +2124,16 @@ namespace OrthoTree EXEC_POL_TEMPLATE_DECL void Move(TVector const& moveVector) noexcept { +#ifndef ORTHOTREE__DISABLED_NODECENTER EXEC_POL_DEF(ep); // GCC 11.3 std::for_each(EXEC_POL_ADD(ep) m_nodes.begin(), m_nodes.end(), [&moveVector](auto& pairKeyNode) { - auto box = pairKeyNode.second.GetBoxInternal(); - AD::MoveBox(box, moveVector); - pairKeyNode.second.SetBox(std::move(box)); + auto center = pairKeyNode.second.GetCenter(); + IGM::MoveAD(center, moveVector); + pairKeyNode.second.SetCenter(std::move(center)); }); - AD::MoveBox(this->m_boxSpace, moveVector); +#endif // !ORTHOTREE__DISABLED_NODECENTER + + IGM::MoveAD(this->m_boxSpace, moveVector); } std::tuple FindSmallestNodeKeyWithDepth(MortonNodeID searchKey) const noexcept @@ -1938,7 +2175,7 @@ namespace OrthoTree { if constexpr (!ALLOW_OUT_OF_BOX_GEOMETRY) { - if (!AD::DoesBoxContainPoint(this->m_boxSpace, searchPoint)) + if (!IGM::DoesBoxContainPointAD(this->m_boxSpace, searchPoint)) return MortonNodeID{}; } return this->FindSmallestNodeKey(this->GetNodeID(searchPoint)); @@ -1947,7 +2184,7 @@ namespace OrthoTree // Find smallest node which contains the box MortonNodeID FindSmallestNode(TBox const& box) const noexcept { - if (!AD::AreBoxesOverlapped(this->m_boxSpace, box)) + if (!IGM::DoesRangeContainBoxAD(this->m_boxSpace, box)) return MortonNodeID{}; return FindSmallestNodeKey(this->GetNodeID(box)); @@ -2013,30 +2250,61 @@ namespace OrthoTree template - void rangeSearch(TBox const& range, TContainer const& geometryCollection, Node const& currentNode, std::vector& foundEntities) const noexcept - { - rangeSearchCopy(range, geometryCollection, currentNode, foundEntities); + void rangeSearch( + TBox const& range, + TContainer const& geometryCollection, + depth_t depthID, + MortonNodeIDCR const& currentNodeKey, + std::vector& foundEntities) const noexcept + { + autoc& currentNode = this->GetNode(currentNodeKey); + if (!currentNode.IsAnyChildExist()) + { + rangeSearchCopy(range, geometryCollection, currentNode, foundEntities); + return; + } - for (MortonNodeIDCR keyChild : currentNode.GetChildren()) + autoc& center = GetNodeCenterMacro(this, currentNodeKey, currentNode); + auto locationMin = MortonNodeID{}; + auto locationMax = MortonNodeID{}; + auto flag = MortonNodeID{ 1 }; + for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID, flag <<= 1) { - autoc& childNode = this->GetNode(keyChild); + locationMin |= flag * (center[dimensionID] <= AD::GetBoxMinC(range, dimensionID)); + locationMax |= flag * (center[dimensionID] <= AD::GetBoxMaxC(range, dimensionID)); + } - auto isOverlapped = true; - for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO && isOverlapped; ++dimensionID) - { - autoc isUpperNodeInTheDimension = IsValidKey(keyChild & (MortonNodeID{ 1 } << dimensionID)); - if (isUpperNodeInTheDimension) - isOverlapped &= AD::GetBoxMinC(childNode.GetBoxInternal(), dimensionID) <= AD::GetBoxMaxC(range, dimensionID); - else - isOverlapped &= AD::GetBoxMaxC(childNode.GetBoxInternal(), dimensionID) >= AD::GetBoxMinC(range, dimensionID); - } + // Different min-max bit means: the dimension should be totally walked + // Same min-max bit means: only the min or max should be walked + + // The key will have signal bit also, dimensionMask is applied to calculate only the last, dimension part of the key + autoc dimensionMask = (MortonNodeID{ 1 } << DIMENSION_NO) - 1; + + // Sign the dimensions which should not be walked fully + autoc limitedDimensionsMask = (~(locationMin ^ locationMax)) & dimensionMask; + + if (limitedDimensionsMask == MortonNodeID{} && IGM::DoesRangeContainBoxAD(range, this->GetNodeBox(depthID, center))) + { + collectAllIdInDFS(currentNode, foundEntities); + return; + } + else + { + rangeSearchCopy(range, geometryCollection, currentNode, foundEntities); + } + + // Sign which element should be walked in the limited dimensions + autoc dimensionBoundaries = (locationMin & locationMax) & limitedDimensionsMask; + + ++depthID; + for (MortonNodeIDCR keyChild : currentNode.GetChildren()) + { + // keyChild should have the same elements in the limited dimensions + autoc isOverlapped = (keyChild & limitedDimensionsMask) == dimensionBoundaries; if (!isOverlapped) continue; - if (AD::AreBoxesOverlapped(range, childNode.GetBoxInternal())) - collectAllIdInDFS(childNode, foundEntities); - else - rangeSearch(range, geometryCollection, childNode, foundEntities); + rangeSearch(range, geometryCollection, depthID, keyChild, foundEntities); } } @@ -2044,7 +2312,7 @@ namespace OrthoTree bool rangeSearchRoot(TBox const& range, TContainer const& geometryCollection, std::vector& foundEntities) const noexcept { autoc entityNo = geometryCollection.size(); - if (AD::AreBoxesOverlapped(range, this->m_boxSpace)) + if (IGM::DoesRangeContainBoxAD(range, this->m_boxSpace)) { foundEntities.resize(entityNo); @@ -2084,8 +2352,7 @@ namespace OrthoTree this->m_volumeOfOverallSpace < 0.01 ? 10 : static_cast((rangeVolume * entityNo) / this->m_volumeOfOverallSpace); foundEntities.reserve(foundEntityNoEstimation); - autoc& node = this->GetNode(smallestNodeKey); - rangeSearch(range, geometryCollection, node, foundEntities); + rangeSearch(range, geometryCollection, this->GetDepthID(smallestNodeKey), smallestNodeKey, foundEntities); if constexpr (!DOES_LEAF_NODE_CONTAIN_ELEMENT_ONLY) { @@ -2098,8 +2365,8 @@ namespace OrthoTree static PlaneRelation getEntityPlaneRelation(TEntity const& entity, TGeometry distanceOfOrigo, TVector const& planeNormal, TGeometry tolerance) { - if constexpr (std::is_same::value) - return AD::GetBoxPlaneRelation(entity, distanceOfOrigo, planeNormal, tolerance); + if constexpr (IS_BOX_TYPE) + return IGM::GetBoxPlaneRelationAD(IGM::GetBoxCenterAD(entity), IGM::GetBoxHalfSizeAD(entity), distanceOfOrigo, planeNormal, tolerance); else return AD::GetPointPlaneRelation(entity, distanceOfOrigo, planeNormal, tolerance); } @@ -2109,8 +2376,9 @@ namespace OrthoTree assert(AD::IsNormalizedVector(planeNormal)); auto results = std::vector{}; - autoc selector = [&](MortonNodeIDCR, Node const& node) -> bool { - return AD::GetBoxPlaneRelation(node.GetBoxInternal(), distanceOfOrigo, planeNormal, tolerance) == PlaneRelation::Hit; + autoc selector = [&](MortonNodeIDCR key, Node const& node) -> bool { + autoc& halfSize = this->GetNodeSize(this->GetDepthID(key) + 1); + return IGM::GetBoxPlaneRelationAD(GetNodeCenterMacro(this, key, node), halfSize, distanceOfOrigo, planeNormal, tolerance) == PlaneRelation::Hit; }; autoc procedure = [&](MortonNodeIDCR, Node const& node) { @@ -2125,14 +2393,14 @@ namespace OrthoTree return results; } - std::vector planePositiveSegmentation( - TGeometry const& distanceOfOrigo, TVector const& planeNormal, TGeometry tolerance, TContainer const& data) const noexcept + std::vector planePositiveSegmentation(TGeometry distanceOfOrigo, TVector const& planeNormal, TGeometry tolerance, TContainer const& data) const noexcept { assert(AD::IsNormalizedVector(planeNormal)); auto results = std::vector{}; - autoc selector = [&](MortonNodeIDCR, Node const& node) -> bool { - autoc relation = AD::GetBoxPlaneRelation(node.GetBoxInternal(), distanceOfOrigo, planeNormal, tolerance); + autoc selector = [&](MortonNodeIDCR key, Node const& node) -> bool { + autoc& halfSize = this->GetNodeSize(this->GetDepthID(key) + 1); + autoc relation = IGM::GetBoxPlaneRelationAD(GetNodeCenterMacro(this, key, node), halfSize, distanceOfOrigo, planeNormal, tolerance); return relation != PlaneRelation::Negative; }; @@ -2160,12 +2428,17 @@ namespace OrthoTree if (boundaryPlanes.empty()) return results; - assert(std::all_of(boundaryPlanes.begin(), boundaryPlanes.end(), [](autoc& plane) { return AD::IsNormalizedVector(AD::GetPlaneNormal(plane)); })); + assert(std::all_of(boundaryPlanes.begin(), boundaryPlanes.end(), [](autoc& plane) -> bool { + return AD::IsNormalizedVector(AD::GetPlaneNormal(plane)); + })); + + autoc selector = [&](MortonNodeIDCR key, Node const& node) -> bool { + autoc& halfSize = this->GetNodeSize(this->GetDepthID(key) + 1); + autoc& center = GetNodeCenterMacro(this, key, node); - autoc selector = [&](MortonNodeIDCR, Node const& node) -> bool { for (autoc& plane : boundaryPlanes) { - autoc relation = AD::GetBoxPlaneRelation(node.GetBoxInternal(), AD::GetPlaneOrigoDistance(plane), AD::GetPlaneNormal(plane), tolerance); + autoc relation = IGM::GetBoxPlaneRelationAD(center, halfSize, AD::GetPlaneOrigoDistance(plane), AD::GetPlaneNormal(plane), tolerance); if (relation == PlaneRelation::Hit) return true; @@ -2217,6 +2490,8 @@ namespace OrthoTree using Base = OrthoTreeBase; using EntityDistance = typename Base::EntityDistance; using BoxDistance = typename Base::BoxDistance; + using IGM = typename Base::IGM; + using IGM_Geometry = typename IGM::Geometry; public: using AD = typename Base::AD; @@ -2323,11 +2598,11 @@ namespace OrthoTree std::optional const& boxSpaceOptional = std::nullopt, std::size_t maxElementNoInNode = DEFAULT_MAX_ELEMENT) noexcept { - autoc boxSpace = boxSpaceOptional.has_value() ? *boxSpaceOptional : getBoxOfPoints(points); + autoc boxSpace = boxSpaceOptional.has_value() ? IGM::GetBoxAD(*boxSpaceOptional) : IGM::GetBoxOfPointsAD(points); autoc pointNo = points.size(); autoc maxDepthNo = (!maxDepthNoIn || maxDepthNoIn == 0) ? Base::EstimateMaxDepth(pointNo, maxElementNoInNode) : *maxDepthNoIn; - tree.Init(boxSpace, maxDepthNo, maxElementNoInNode); + tree.init(boxSpace, maxDepthNo, maxElementNoInNode); Base::reserveContainer(tree.m_nodes, Base::EstimateNodeNumber(pointNo, maxDepthNo, maxElementNoInNode)); if (points.empty()) return; @@ -2337,7 +2612,7 @@ namespace OrthoTree std::transform(EXEC_POL_ADD(ept) points.begin(), points.end(), pointLocations.begin(), [&](autoc& point) { return Location{ detail::getKeyPart(points, point), tree.getLocationID(detail::getValuePart(point)) }; }); - + EXEC_POL_DEF(eps); // GCC 11.3 std::sort(EXEC_POL_ADD(eps) pointLocations.begin(), pointLocations.end(), [&](autoc& leftLocation, autoc& rightLocation) { return leftLocation.GridID < rightLocation.GridID; @@ -2353,7 +2628,7 @@ namespace OrthoTree public: // Edit functions bool InsertWithRebalancing(TEntityID newEntityID, TVector const& newPoint, TContainer const& points) noexcept { - if (!AD::DoesBoxContainPoint(this->m_boxSpace, newPoint)) + if (!IGM::DoesBoxContainPointAD(this->m_boxSpace, newPoint)) return false; autoc[entityDepth, entityLocation] = this->getDepthAndLocationID(newPoint); @@ -2368,7 +2643,7 @@ namespace OrthoTree // Insert item into a node. If doInsertToLeaf is true: The smallest node will be chosen by the max depth. If doInsertToLeaf is false: The smallest existing level on the branch will be chosen. bool Insert(TEntityID entityID, TVector const& newPoint, bool doInsertToLeaf = false) noexcept { - if (!AD::DoesBoxContainPoint(this->m_boxSpace, newPoint)) + if (!IGM::DoesBoxContainPointAD(this->m_boxSpace, newPoint)) return false; autoc entityNodeKey = this->GetNodeID(newPoint); @@ -2440,7 +2715,7 @@ namespace OrthoTree // Update id by the new point information bool Update(TEntityID entityID, TVector const& newPoint, bool doesInsertToLeaf = false) noexcept { - if (!AD::DoesBoxContainPoint(this->m_boxSpace, newPoint)) + if (!IGM::DoesBoxContainPointAD(this->m_boxSpace, newPoint)) return false; if (!this->EraseId(entityID)) @@ -2453,7 +2728,7 @@ namespace OrthoTree // Update id by the new point information and the erase part is aided by the old point geometry data bool Update(TEntityID entityID, TVector const& oldPoint, TVector const& newPoint, bool doesInsertToLeaf = false) noexcept { - if (!AD::DoesBoxContainPoint(this->m_boxSpace, newPoint)) + if (!IGM::DoesBoxContainPointAD(this->m_boxSpace, newPoint)) return false; if (!this->Erase(entityID, oldPoint)) @@ -2466,7 +2741,7 @@ namespace OrthoTree // Update id with rebalancing by the new point information bool Update(TEntityID entityID, TVector const& newPoint, TContainer const& points) noexcept { - if (!AD::DoesBoxContainPoint(this->m_boxSpace, newPoint)) + if (!IGM::DoesBoxContainPointAD(this->m_boxSpace, newPoint)) return false; if (!this->EraseId(entityID)) @@ -2479,7 +2754,7 @@ namespace OrthoTree // Update id with rebalacing by the new point information and the erase part is aided by the old point geometry data bool Update(TEntityID entityID, TVector const& oldPoint, TVector const& newPoint, TContainer const& points) noexcept { - if (!AD::DoesBoxContainPoint(this->m_boxSpace, newPoint)) + if (!IGM::DoesBoxContainPointAD(this->m_boxSpace, newPoint)) return false; if (!this->Erase(entityID, oldPoint)) @@ -2547,34 +2822,16 @@ namespace OrthoTree private: // K Nearest Neighbor helpers - static TGeometry getMinBoxWallDistance(TVector const& point, TBox const& box) noexcept - { - auto distances = std::vector(); - distances.reserve(DIMENSION_NO); - for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) - { - autoc actualDistance = distances.emplace_back(std::min( - std::abs(AD::GetPointC(point, dimensionID) - AD::GetBoxMinC(box, dimensionID)), - std::abs(AD::GetPointC(point, dimensionID) - AD::GetBoxMaxC(box, dimensionID)))); - - if (actualDistance == 0) - return 0.0; - } - - return *std::ranges::min_element(distances); - } - - static void createEntityDistance(Node const& node, TVector const& searchPoint, TContainer const& points, std::multiset& neighborEntities) noexcept { for (autoc id : node.Entities) neighborEntities.insert({ { AD::Distance(searchPoint, detail::at(points, id)) }, id }); } - static TGeometry getFarestDistance(std::multiset& neighborEntities, std::size_t neighborNo) noexcept + static IGM::Geometry getFarestDistance(std::multiset& neighborEntities, std::size_t neighborNo) noexcept { if (neighborEntities.size() < neighborNo) - return std::numeric_limits::max(); + return std::numeric_limits::max(); return std::next(neighborEntities.begin(), neighborNo - 1)->Distance; } @@ -2592,11 +2849,16 @@ namespace OrthoTree std::vector GetNearestNeighbors(TVector const& searchPoint, std::size_t neighborNo, TContainer const& points) const noexcept { auto neighborEntities = std::multiset(); - autoc smallestNodeKey = this->template FindSmallestNode(searchPoint); + autoc[smallestNodeKey, smallesDepthID] = this->FindSmallestNodeKeyWithDepth(this->template GetNodeID(searchPoint)); if (Base::IsValidKey(smallestNodeKey)) { autoc& smallestNode = this->GetNode(smallestNodeKey); - autoc wallDistance = getMinBoxWallDistance(searchPoint, smallestNode.GetBoxInternal()); + + autoc& halfSize = this->GetNodeSize(smallesDepthID + 1); + autoc& centerPoint = GetNodeCenterMacro(this, smallestNodeKey, smallestNode); + + autoc distance = IGM::GetBoxWallDistanceAD(searchPoint, centerPoint, halfSize); + autoc wallDistance = *std::min(distance.begin(), distance.end()); createEntityDistance(smallestNode, searchPoint, points, neighborEntities); if (!smallestNode.IsAnyChildExist()) if (getFarestDistance(neighborEntities, neighborNo) < wallDistance) @@ -2609,16 +2871,10 @@ namespace OrthoTree if (node.Entities.empty() || key == smallestNodeKey) return; - auto aDist = TVector{}; - for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) - { - autoc dMin = AD::GetBoxMinC(node.GetBoxInternal(), dimensionID) - AD::GetPointC(searchPoint, dimensionID); - autoc dMax = AD::GetBoxMaxC(node.GetBoxInternal(), dimensionID) - AD::GetPointC(searchPoint, dimensionID); - - // If point projection in dimensionID is within min and max the wall distance should be calculated. - AD::SetPointC(aDist, dimensionID, dMin * dMax < 0 ? 0 : std::min(std::abs(dMin), std::abs(dMax))); - } - nodeMinDistances.insert({ { AD::Size(aDist) }, key, node }); + autoc depthID = this->GetDepthID(key); + autoc& halfSize = this->GetNodeSize(depthID + 1); + autoc& centerPoint = GetNodeCenterMacro(this, key, node); + nodeMinDistances.insert({ { IGM::Size(IGM::GetBoxWallDistanceAD(searchPoint, centerPoint, halfSize)) }, key, node }); }); if (!nodeMinDistances.empty()) @@ -2661,6 +2917,8 @@ namespace OrthoTree using GridBoundary = typename Base::GridBoundary; template using DimArray = std::array; + using IGM = typename Base::IGM; + using IGM_Geometry = typename IGM::Geometry; public: using AD = typename Base::AD; @@ -2901,10 +3159,10 @@ namespace OrthoTree std::optional const& boxSpaceOptional = std::nullopt, std::size_t maxElementNoInNode = DEFAULT_MAX_ELEMENT) noexcept { - autoc boxSpace = boxSpaceOptional.has_value() ? *boxSpaceOptional : getBoxOfBoxes(boxes); + autoc boxSpace = boxSpaceOptional.has_value() ? IGM::GetBoxAD(*boxSpaceOptional) : IGM::GetBoxOfBoxesAD(boxes); autoc entityNo = boxes.size(); autoc maxDepthNo = (!maxDepthIn || maxDepthIn == 0) ? Base::EstimateMaxDepth(entityNo, maxElementNoInNode) : *maxDepthIn; - tree.Init(boxSpace, maxDepthNo, maxElementNoInNode); + tree.init(boxSpace, maxDepthNo, maxElementNoInNode); Base::reserveContainer(tree.m_nodes, Base::EstimateNodeNumber(entityNo, maxDepthNo, maxElementNoInNode)); if (entityNo == 0) @@ -2914,12 +3172,12 @@ namespace OrthoTree auto& nodeRoot = cont_at(tree.m_nodes, rootKey); autoce NON_SPLITTED = SPLIT_DEPTH_INCREASEMENT == 0; - #ifdef __cpp_lib_execution +#ifdef __cpp_lib_execution autoce NON_PARALLEL = std::is_same::value || std::is_same::value; - #else +#else autoce NON_PARALLEL = true; - #endif +#endif auto locations = LocationContainer(entityNo); if constexpr (NON_SPLITTED) @@ -2964,10 +3222,7 @@ namespace OrthoTree locations.resize(position); EXEC_POL_DEF(epf2); // GCC 11.3 std::for_each( - EXEC_POL_ADD(epf2) - additionalLocations.begin(), - additionalLocations.end(), - [&locations, &additionalLocationPositions](auto& additionalLocation) { + EXEC_POL_ADD(epf2) additionalLocations.begin(), additionalLocations.end(), [&locations, &additionalLocationPositions](auto& additionalLocation) { if (additionalLocation.second.empty()) return; @@ -2998,7 +3253,7 @@ namespace OrthoTree public: // Edit functions bool InsertWithRebalancing(TEntityID newEntityID, TBox const& newBox, TContainer const& boxes) noexcept { - if (!AD::AreBoxesOverlapped(this->m_boxSpace, newBox)) + if (!IGM::DoesRangeContainBoxAD(this->m_boxSpace, newBox)) return false; auto locations = std::vector(1); @@ -3021,7 +3276,7 @@ namespace OrthoTree // Insert item into a node. If doInsertToLeaf is true: The smallest node will be chosen by the max depth. If doInsertToLeaf is false: The smallest existing level on the branch will be chosen. bool Insert(TEntityID newEntityID, TBox const& newBox, bool doInsertToLeaf = false) noexcept { - if (!AD::AreBoxesOverlapped(this->m_boxSpace, newBox)) + if (!IGM::DoesRangeContainBoxAD(this->m_boxSpace, newBox)) return false; autoc smallestNodeKey = this->FindSmallestNode(newBox); @@ -3146,7 +3401,7 @@ namespace OrthoTree // Update id by the new bounding box information bool Update(TEntityID entityID, TBox const& boxNew, bool doInsertToLeaf = false) noexcept { - if (!AD::AreBoxesOverlapped(this->m_boxSpace, boxNew)) + if (!IGM::DoesRangeContainBoxAD(this->m_boxSpace, boxNew)) return false; if (!this->EraseId(entityID)) @@ -3159,7 +3414,7 @@ namespace OrthoTree // Update id by the new bounding box information and the erase part is aided by the old bounding box geometry data bool Update(TEntityID entityID, TBox const& oldBox, TBox const& newBox, bool doInsertToLeaf = false) noexcept { - if (!AD::AreBoxesOverlapped(this->m_boxSpace, newBox)) + if (!IGM::DoesRangeContainBoxAD(this->m_boxSpace, newBox)) return false; if constexpr (SPLIT_DEPTH_INCREASEMENT == 0) @@ -3176,7 +3431,7 @@ namespace OrthoTree // Update id with rebalancing by the new bounding box information bool Update(TEntityID entityID, TBox const& boxNew, TContainer const& boxes) noexcept { - if (!AD::AreBoxesOverlapped(this->m_boxSpace, boxNew)) + if (!IGM::DoesRangeContainBoxAD(this->m_boxSpace, boxNew)) return false; if (!this->EraseId(entityID)) @@ -3189,7 +3444,7 @@ namespace OrthoTree // Update id with rebalancing by the new bounding box information and the erase part is aided by the old bounding box geometry data bool Update(TEntityID entityID, TBox const& oldBox, TBox const& newBox, TContainer const& boxes) noexcept { - if (!AD::AreBoxesOverlapped(this->m_boxSpace, newBox)) + if (!IGM::DoesRangeContainBoxAD(this->m_boxSpace, newBox)) return false; if constexpr (SPLIT_DEPTH_INCREASEMENT == 0) @@ -3209,8 +3464,7 @@ namespace OrthoTree auto pointMinMaxGridID = std::array, 2>{}; for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID) { - autoc rasterID = static_cast(AD::GetPointC(point, dimensionID) - AD::GetBoxMinC(this->m_boxSpace, dimensionID)) * - this->m_rasterizerFactors[dimensionID]; + autoc rasterID = (IGM_Geometry(AD::GetPointC(point, dimensionID)) - this->m_boxSpace.Min[dimensionID]) * this->m_rasterizerFactors[dimensionID]; pointMinMaxGridID[0][dimensionID] = pointMinMaxGridID[1][dimensionID] = static_cast(rasterID); pointMinMaxGridID[0][dimensionID] -= (pointMinMaxGridID[0][dimensionID] > 0) && (floor(rasterID) == rasterID); } @@ -3218,19 +3472,45 @@ namespace OrthoTree } - void pickSearch(TVector const& pickPoint, TContainer const& boxes, Node const& parentNode, std::vector& foundEntitiyIDs) const noexcept + void pickSearch(TVector const& pickPoint, TContainer const& boxes, MortonNodeIDCR parentKey, std::vector& foundEntitiyIDs) const noexcept { + autoc& parentNode = this->GetNode(parentKey); std::ranges::copy_if(parentNode.Entities, back_inserter(foundEntitiyIDs), [&](autoc entityID) { return AD::DoesBoxContainPoint(detail::at(boxes, entityID), pickPoint); }); + autoc& centerPoint = GetNodeCenterMacro(this, parentKey, parentNode); + bool isPickPointInCenter = true; + bool isPickPointInCenterOngoing = true; for (MortonNodeIDCR keyChild : parentNode.GetChildren()) { - autoc& childNode = this->GetNode(keyChild); - if (!AD::DoesBoxContainPoint(childNode.GetBoxInternal(), pickPoint)) - continue; + if (!isPickPointInCenter || isPickPointInCenterOngoing) + { + auto mask = MortonNodeID{ 1 }; + for (dim_t dimensionID = 0; dimensionID < DIMENSION_NO; ++dimensionID, mask <<= 1) + { + autoc lower = (keyChild & mask) == MortonNodeID{}; + autoc center = TGeometry(centerPoint[dimensionID]); + if (lower) + { + if (center < AD::GetPointC(pickPoint, dimensionID)) + continue; + } + else + { + if (center > AD::GetPointC(pickPoint, dimensionID)) + continue; + } + + if (isPickPointInCenter) + isPickPointInCenter &= center == AD::GetPointC(pickPoint, dimensionID); + } + } + isPickPointInCenterOngoing = false; - pickSearch(pickPoint, boxes, childNode, foundEntitiyIDs); + pickSearch(pickPoint, boxes, keyChild, foundEntitiyIDs); + if (!isPickPointInCenter) + break; } } @@ -3240,7 +3520,7 @@ namespace OrthoTree std::vector PickSearch(TVector const& pickPoint, TContainer const& boxes) const noexcept { auto foundEntitiyIDs = std::vector(); - if (!AD::DoesBoxContainPoint(this->m_boxSpace, pickPoint)) + if (!IGM::DoesBoxContainPointAD(this->m_boxSpace, pickPoint)) return foundEntitiyIDs; foundEntitiyIDs.reserve(100); @@ -3261,7 +3541,7 @@ namespace OrthoTree nodeKey = this->FindSmallestNodeKey(rangeKey); autoc nodeIterator = this->m_nodes.find(nodeKey); if (nodeIterator != endIteratorOfNodes) - pickSearch(pickPoint, boxes, nodeIterator->second, foundEntitiyIDs); + pickSearch(pickPoint, boxes, nodeIterator->first, foundEntitiyIDs); nodeKey >>= DIMENSION_NO; } @@ -3360,6 +3640,8 @@ namespace OrthoTree autoc rootKey = Base::GetRootKey(); autoc trees = std::array{ &leftTree, &rightTree }; + autoc pLeftTree = &leftTree; + autoc pRightTree = &rightTree; auto nodePairToProceed = std::queue{}; nodePairToProceed.push({ NodeIteratorAndStatus{ leftTree.m_nodes.find(rootKey), false}, @@ -3401,11 +3683,16 @@ namespace OrthoTree if (!parentNodePair[sideID].Iterator->second.Entities.empty()) childNodes[sideID].push_back({ parentNodePair[sideID].Iterator, true }); - // Cartesian product of avitChildNode left and right + + // Cartesian product of childNodes left and right for (autoc& leftChildNode : childNodes[Left]) for (autoc& rightChildNode : childNodes[Right]) if (!(leftChildNode.Iterator == parentNodePair[Left].Iterator && rightChildNode.Iterator == parentNodePair[Right].Iterator)) - if (AD::AreBoxesOverlapped(leftChildNode.Iterator->second.GetBoxInternal(), rightChildNode.Iterator->second.GetBoxInternal(), false)) + if (IGM::AreBoxesOverlappingByCenter( + GetNodeCenterMacro(pLeftTree, leftChildNode.Iterator->first, leftChildNode.Iterator->second), + GetNodeCenterMacro(pRightTree, rightChildNode.Iterator->first, rightChildNode.Iterator->second), + leftTree.GetNodeSizeByKey(leftChildNode.Iterator->first), + rightTree.GetNodeSizeByKey(rightChildNode.Iterator->first))) nodePairToProceed.emplace(std::array{ leftChildNode, rightChildNode }); } @@ -3447,7 +3734,7 @@ namespace OrthoTree for (autoc entityID : node.Entities) entityIDNodeMap[entityID].emplace_back(nodeKey); }); - + EXEC_POL_DEF(ep); // GCC 11.3 std::for_each(EXEC_POL_ADD(ep) entityIDNodeMap.begin(), entityIDNodeMap.end(), [](auto& keys) { if constexpr (Base::IS_LINEAR_TREE) @@ -3465,7 +3752,7 @@ namespace OrthoTree std::set largeEntities; for (autoc entityID : nodeRoot.Entities) { - if (AD::AreBoxesOverlapped(detail::at(boxes, entityID), this->m_boxSpace)) + if (IGM::DoesRangeContainBoxAD(detail::at(boxes, entityID), this->m_boxSpace)) { largeEntities.insert(entityID); @@ -3663,9 +3950,8 @@ namespace OrthoTree EXEC_POL_TEMPLATE_DECL inline std::vector> CollisionDetection(TContainer const& boxes) const noexcept { - return EXEC_POL_TEMPLATE_ADDF(collisionDetection)(boxes, [](TEntityID id1, TBox const& e1, TEntityID id2, TBox const& e2) { - return AD::AreBoxesOverlappedStrict(e1, e2); - }); + return EXEC_POL_TEMPLATE_ADDF( + collisionDetection)(boxes, [](TEntityID id1, TBox const& e1, TEntityID id2, TBox const& e2) { return AD::AreBoxesOverlappedStrict(e1, e2); }); } @@ -3680,7 +3966,8 @@ namespace OrthoTree private: void getRayIntersectedAll( - Node const& node, + depth_t depthID, + MortonNodeIDCR parentKey, TContainer const& boxes, TVector const& rayBasePoint, TVector const& rayHeading, @@ -3688,7 +3975,10 @@ namespace OrthoTree TGeometry maxExaminationDistance, std::vector& foundEntities) const noexcept { - autoc isNodeHit = AD::GetRayBoxDistance(node.GetBoxInternal(), rayBasePoint, rayHeading, tolerance); + autoc& node = this->GetNode(parentKey); + + autoc isNodeHit = + IGM::GetRayBoxDistanceAD(GetNodeCenterMacro(this, parentKey, node), this->GetNodeSize(depthID + 1), rayBasePoint, rayHeading, tolerance); if (!isNodeHit) return; @@ -3696,52 +3986,57 @@ namespace OrthoTree { autoc entityDistance = AD::GetRayBoxDistance(detail::at(boxes, entityID), rayBasePoint, rayHeading, tolerance); if (entityDistance && (maxExaminationDistance == 0 || entityDistance.value() <= maxExaminationDistance)) - foundEntities.push_back({ { TGeometry(entityDistance.value()) }, entityID }); + foundEntities.push_back({ { IGM_Geometry(entityDistance.value()) }, entityID }); } + ++depthID; for (autoc childKey : node.GetChildren()) - getRayIntersectedAll(this->GetNode(childKey), boxes, rayBasePoint, rayHeading, tolerance, maxExaminationDistance, foundEntities); + getRayIntersectedAll(depthID, childKey, boxes, rayBasePoint, rayHeading, tolerance, maxExaminationDistance, foundEntities); } void getRayIntersectedFirst( - Node const& node, + depth_t depthID, + MortonNodeIDCR parentKey, TContainer const& boxes, TVector const& rayBasePoint, TVector const& rayHeading, TGeometry tolerance, std::multiset& foundEntities) const noexcept { - autoc maxExaminationDistance = - foundEntities.empty() ? std::numeric_limits::infinity() : static_cast(foundEntities.rbegin()->Distance); + autoc& node = this->GetNode(parentKey); + + autoc maxExaminationDistance = foundEntities.empty() ? std::numeric_limits::infinity() : foundEntities.rbegin()->Distance; for (autoc entityID : node.Entities) { autoc distance = AD::GetRayBoxDistance(detail::at(boxes, entityID), rayBasePoint, rayHeading, tolerance); if (!distance) continue; - if (*distance > maxExaminationDistance) + autoc distance_ = IGM_Geometry(*distance); + if (distance_ > maxExaminationDistance) continue; - foundEntities.insert({ { TGeometry(*distance) }, entityID }); + foundEntities.insert({ { distance_ }, entityID }); } - + ++depthID; auto nodeDistances = std::multiset(); + autoc halfSize = this->GetNodeSize(depthID + 1); for (autoc childKey : node.GetChildren()) { autoc& nodeChild = this->GetNode(childKey); - autoc distance = AD::GetRayBoxDistance(nodeChild.GetBoxInternal(), rayBasePoint, rayHeading, tolerance); + autoc distance = IGM::GetRayBoxDistanceAD(GetNodeCenterMacro(this, childKey, nodeChild), halfSize, rayBasePoint, rayHeading, tolerance); if (!distance) continue; if (*distance > maxExaminationDistance) continue; - nodeDistances.insert({ { static_cast(distance.value()) }, childKey, nodeChild }); + nodeDistances.insert({ { IGM_Geometry(distance.value()) }, childKey, nodeChild }); } for (autoc& nodeDistance : nodeDistances) - getRayIntersectedFirst(nodeDistance.NodeReference, boxes, rayBasePoint, rayHeading, tolerance, foundEntities); + getRayIntersectedFirst(depthID, nodeDistance.NodeKey, boxes, rayBasePoint, rayHeading, tolerance, foundEntities); } @@ -3752,7 +4047,7 @@ namespace OrthoTree { auto foundEntities = std::vector(); foundEntities.reserve(20); - getRayIntersectedAll(this->GetNode(this->GetRootKey()), boxes, rayBasePointPoint, rayHeading, tolerance, maxExaminationDistance, foundEntities); + getRayIntersectedAll(0, this->GetRootKey(), boxes, rayBasePointPoint, rayHeading, tolerance, maxExaminationDistance, foundEntities); autoc beginIteratorOfEntities = foundEntities.begin(); auto endIteratorOfEntities = foundEntities.end(); @@ -3772,13 +4067,13 @@ namespace OrthoTree // Get first box which is intersected by the ray std::optional RayIntersectedFirst(TVector const& rayBasePoint, TVector const& rayHeading, TContainer const& boxes, TGeometry tolerance) const noexcept { - autoc& node = this->GetNode(this->GetRootKey()); - autoc distance = AD::GetRayBoxDistance(node.GetBoxInternal(), rayBasePoint, rayHeading, tolerance); + autoc distance = IGM::GetRayBoxDistanceAD( + GetNodeCenterMacro(this, this->GetRootKey(), this->GetNode(this->GetRootKey())), this->GetNodeSize(1), rayBasePoint, rayHeading, tolerance); if (!distance) return std::nullopt; auto foundEntities = std::multiset(); - getRayIntersectedFirst(node, boxes, rayBasePoint, rayHeading, tolerance, foundEntities); + getRayIntersectedFirst(0, this->GetRootKey(), boxes, rayBasePoint, rayHeading, tolerance, foundEntities); if (foundEntities.empty()) return std::nullopt; diff --git a/unittests/adaptor.tests.cpp b/unittests/adaptor.tests.cpp index 00251e1..2d52ad9 100644 --- a/unittests/adaptor.tests.cpp +++ b/unittests/adaptor.tests.cpp @@ -21,6 +21,8 @@ #endif // !UNREAL_DUMMY_TYPES #include "../adaptor.unreal.h" +#include + using namespace OrthoTree; using std::array; @@ -134,7 +136,14 @@ namespace AdaptorTest autoc& nodes = tree.GetNodes(); Assert::IsTrue(nodes.size() == 1); Assert::IsTrue(nodes.at(1).Entities.empty()); - Assert::IsTrue(AreEqualAlmost(tree.GetBox(), MyBox2D{})); + + autoc& box = tree.GetBox(); + Assert::IsTrue(AreEqualAlmost( + MyBox2D{ + MyPoint2D{box.Min[0], box.Min[1]}, + MyPoint2D{box.Max[0], box.Min[1]} + }, + MyBox2D{})); } TEST_METHOD(Insert_NonLeaf_Successful) @@ -424,6 +433,274 @@ namespace AdaptorTest }; } // namespace XYZAdaptorTest + namespace AbstractAdaptorTest + { + // User-defined geometrical objects + + struct MyPoint2DBase + { + float x; + float y; + + MyPoint2DBase(float x, float y): x(x), y(y){} + virtual ~MyPoint2DBase() {} + + virtual float GetDistance() const = 0; + }; + + struct MyPoint2DConcrete1 : MyPoint2DBase + { + using MyPoint2DBase::MyPoint2DBase; + + virtual float GetDistance() const override { return x + y; }; + }; + + struct MyPoint2DConcrete2 : MyPoint2DBase + { + using MyPoint2DBase::MyPoint2DBase; + + virtual float GetDistance() const override { return std::sqrt(x*x + y*y); }; + }; + + struct MyBox2DBase + { + std::unique_ptr Min, Max; + + MyBox2DBase() = default; + MyBox2DBase(std::unique_ptr&& minPoint, std::unique_ptr&& maxPoint) + : Min(std::move(minPoint)) + , Max(std::move(maxPoint)) + { + } + + virtual ~MyBox2DBase() {} + virtual float GetSize() const = 0; + }; + + struct MyBox2DConcrete1 : MyBox2DBase + { + using MyBox2DBase::MyBox2DBase; + virtual float GetSize() const override { return Max->x - Min->x; } + }; + + struct MyBox2DConcrete2 : MyBox2DBase + { + using MyBox2DBase::MyBox2DBase; + virtual float GetSize() const override { return Max->y - Min->y; } + }; + + struct MyRay2DBase + { + std::unique_ptr Origin, Direction; + }; + struct MyPlane2DBase + { + float OrigoDistance; + std::unique_ptr Normal; + + MyPlane2DBase(float origoDistance, std::unique_ptr normal) + : OrigoDistance(origoDistance) + , Normal(std::move(normal)) + {} + }; + + + // Adaptor + + struct AdaptorBasicsCustom + { + static float GetPointC(MyPoint2DBase const* pt, OrthoTree::dim_t i) + { + switch (i) + { + case 0: return pt->x; + case 1: return pt->y; + default: assert(false); return pt->x; + } + } + + static void SetPointC(MyPoint2DBase* pt, OrthoTree::dim_t i, float v) + { + switch (i) + { + case 0: pt->x = v; break; + case 1: pt->y = v; break; + default: assert(false); + } + } + + static void SetBoxMinC(MyBox2DBase* box, dim_t i, float v) { SetPointC(box->Min.get(), i, v); } + static void SetBoxMaxC(MyBox2DBase* box, dim_t i, float v) { SetPointC(box->Max.get(), i, v); } + static float GetBoxMinC(MyBox2DBase const* box, dim_t i) { return GetPointC(box->Min.get(), i); } + static float GetBoxMaxC(MyBox2DBase const* box, dim_t i) { return GetPointC(box->Max.get(), i); } + + static MyPoint2DBase* GetRayDirection(MyRay2DBase const& ray) { return ray.Direction.get(); } + static MyPoint2DBase* GetRayOrigin(MyRay2DBase const& ray) { return ray.Origin.get(); } + + static MyPoint2DBase* GetPlaneNormal(MyPlane2DBase const& plane) { return plane.Normal.get(); } + static float GetPlaneOrigoDistance(MyPlane2DBase const& plane) { return plane.OrigoDistance; } + }; + + using MyPoint2DPtr = MyPoint2DBase*; + using MyBox2DPtr = MyBox2DBase*; + using AdaptorCustom = OrthoTree::AdaptorGeneralBase<2, MyPoint2DBase*, MyBox2DBase*, MyRay2DBase, MyPlane2DBase, float, AdaptorBasicsCustom>; + + + // Tailored Quadtree objects + + using QuadtreePointCustom = OrthoTree::OrthoTreePoint<2, MyPoint2DBase*, MyBox2DBase*, MyRay2DBase, MyPlane2DBase, float, AdaptorCustom>; + + using QuadtreeBoxCustom = OrthoTree::OrthoTreeBoundingBox<2, MyPoint2DBase*, MyBox2DBase*, MyRay2DBase, MyPlane2DBase, float, 2, AdaptorCustom>; + using QuadtreeBoxCustomC = OrthoTreeContainerBox; + + TEST_CLASS(AbstractAdaptorTest) + { + public: + TEST_METHOD(PointGeneral2D) + { + + auto vptOwner = std::vector>{}; + vptOwner.emplace_back(std::make_unique(0.0f, 0.0f)); + vptOwner.emplace_back(std::make_unique(1.0f, 0.0f)); + vptOwner.emplace_back(std::make_unique(1.0f, 1.0f)); + vptOwner.emplace_back(std::make_unique(2.0f, 2.0f)); + vptOwner.emplace_back(std::make_unique(3.0f, 3.0f)); + vptOwner.emplace_back(std::make_unique(4.0f, 4.0f)); + vptOwner.emplace_back(std::make_unique(0.0f, 1.0f)); + vptOwner.emplace_back(std::make_unique(0.0f, 4.0f)); + vptOwner.emplace_back(std::make_unique(4.0f, 0.0f)); + vptOwner.emplace_back(std::make_unique(1.5f, 1.5f)); + vptOwner.emplace_back(std::make_unique(1.0f, 1.5f)); + + auto vptView = std::vector{}; + std::ranges::transform(vptOwner, std::back_inserter(vptView), [](auto& ppt) { return ppt.get(); }); + + auto tree = QuadtreePointCustom(vptView, 3, std::nullopt, 2); + + auto entityIDsInBFS = tree.CollectAllIdInBFS(tree.GetRootKey()); + auto entityIDsInDFS = tree.CollectAllIdInDFS(tree.GetRootKey()); + + auto searchBox = std::make_unique(); + searchBox->Min = std::make_unique(0.0f, 0.0f); + searchBox->Max = std::make_unique(2.0f, 2.0f); + auto pointsInSearchBox = tree.RangeSearch(searchBox.get(), vptView); + + auto sqrt2Reciproc = float(1.0 / sqrt(2.0)); + auto planeNormal = std::make_unique(sqrt2Reciproc, sqrt2Reciproc); + auto pointsInPlane = tree.PlaneSearch(2.6f, planeNormal.get(), 0.3f, vptView); + + auto planesForFustrum = std::vector{}; + planesForFustrum.push_back({ -2.0f, std::make_unique(-1.0f, 0.0f) }); + planesForFustrum.push_back({ -2.0f, std::make_unique(0.0f, -1.0f) }); + planesForFustrum.push_back({ -2.0f, std::make_unique(+1.0f, 0.0f) }); + auto pointsInFrustum = tree.FrustumCulling(planesForFustrum, 0.01f, vptView); + + autoc n = vptView.size(); + vptView.emplace_back(vptOwner.emplace_back(std::make_unique(1.0f, 1.1f)).get()); + tree.Insert(n, vptOwner.back().get()); + tree.Erase(0, vptOwner.front().get()); + auto entityIDsInDFS_AfterErase = tree.CollectAllIdInDFS(); + + auto searchPoint = std::make_unique(1.0f, 1.0f); + auto entityIDsKNN = tree.GetNearestNeighbors(searchPoint.get(), 3, vptView); + + Assert::IsTrue(std::ranges::is_permutation(std::vector{ 0, 1, 2, 3, 6, 9, 10 }, pointsInSearchBox)); + Assert::IsTrue(std::ranges::is_permutation(std::vector{ 3, 7, 8 }, pointsInPlane)); + Assert::IsTrue(std::ranges::is_permutation(std::vector{ 2, 11, 10 }, entityIDsKNN)); + Assert::IsTrue(std::ranges::is_permutation(std::vector{ 0, 1, 2, 3, 6, 9, 10 }, pointsInFrustum)); + + Assert::IsTrue(std::vector{ 8, 7, 0, 1, 6, 3, 2, 10, 9, 4, 5 } == entityIDsInBFS); + Assert::IsTrue(std::vector{ 0, 1, 6, 2, 10, 9, 8, 7, 3, 4, 5 } == entityIDsInDFS); + Assert::IsTrue(std::vector{ 1, 6, 2, 11, 10, 9, 8, 7, 3, 4, 5 } == entityIDsInDFS_AfterErase); + } + + TEST_METHOD(BoxGeneral2DC_Example2) + { + auto boxes = std::vector>{}; + boxes.emplace_back( + std::make_unique(std::make_unique(0.0f, 0.0f), std::make_unique(1.0f, 1.0f))); + boxes.emplace_back( + std::make_unique(std::make_unique(1.0f, 1.0f), std::make_unique(2.0f, 2.0f))); + boxes.emplace_back( + std::make_unique(std::make_unique(2.0f, 2.0f), std::make_unique(3.0f, 3.0f))); + boxes.emplace_back( + std::make_unique(std::make_unique(3.0f, 3.0f), std::make_unique(4.0f, 4.0f))); + boxes.emplace_back( + std::make_unique(std::make_unique(1.2f, 1.2f), std::make_unique(2.8f, 2.8f))); + + auto boxesView = std::vector{}; + std::ranges::transform(boxes, std::back_inserter(boxesView), [](auto& ppt) { return ppt.get(); }); + + auto quadtree = QuadtreeBoxCustomC( + boxesView, + 3, // max depth + std::nullopt, // user-provided bounding Box for all + 2, // max element in a node + false // parallel calculation option + ); + + auto collidingIDPairs = quadtree.CollisionDetection(); //: { {1,4}, {2,4} } + + auto searchBox = MyBox2DConcrete1( + std::make_unique(1.0f, 1.0f), + std::make_unique(3.1f, 3.1f) + ); + + // Boxes within the range + auto insideBoxIDs = quadtree.RangeSearch(&searchBox); //: { 1, 2, 4 } + + // Overlapping Boxes with the range + constexpr bool shouldFullyContain = false; // overlap is enough + auto overlappingBoxIDs = quadtree.RangeSearch(&searchBox); //: { 1, 2, 3, 4 } + + // Picked boxes + auto pickPoint = MyPoint2DConcrete1( 2.5f, 2.5f ); + auto pickedIDs = quadtree.PickSearch(&pickPoint); //: { 2, 4 } + + // Ray intersections + auto rayBasePoint = std::make_unique( 1.5f, 2.5f ); + auto rayHeading = MyPoint2DConcrete1( 1.5f, 0.5f ); + auto firstIntersectedBox = quadtree.RayIntersectedFirst(rayBasePoint.get(), &rayHeading, 0.01f); //: 4 + auto intersectedPoints = quadtree.RayIntersectedAll(rayBasePoint.get(), &rayHeading, 0.01f); //: { 4, 2, 3 } in distance order! + + // Plane + auto sqrt2 = sqrtf(2.0); + auto sqrt2Reciproc = 1.0f / sqrt2; + + // Cross-point of the planes: 2;2;2 + auto frustumBoundaryPlanes = std::vector(); + frustumBoundaryPlanes.emplace_back(2 * sqrt2, std::make_unique(sqrt2Reciproc, sqrt2Reciproc)); + frustumBoundaryPlanes.emplace_back(2.0f, std::make_unique(0.0f, -1.0f)); + + auto boxesInFrustum = quadtree.FrustumCulling( + frustumBoundaryPlanes, + 0.01f); + + // Collect all IDs in breadth/depth first order + auto entityIDsInDFS = quadtree.CollectAllIdInBFS(); + auto entityIDsInBFS = quadtree.CollectAllIdInDFS(); + + Assert::IsTrue(std::ranges::is_permutation( + std::vector>{ + {1, 4}, + {2, 4} + }, + collidingIDPairs)); + Assert::IsTrue(std::ranges::is_permutation(std::vector{ 1, 2, 4 }, insideBoxIDs)); + Assert::IsTrue(std::ranges::is_permutation(std::vector{ 1, 2, 3, 4 }, overlappingBoxIDs)); + Assert::IsTrue(std::ranges::is_permutation(std::vector{ 2, 4 }, pickedIDs)); + Assert::IsTrue(firstIntersectedBox.has_value()); + Assert::AreEqual(std::size_t(4), *firstIntersectedBox); + + Assert::IsTrue(std::ranges::is_permutation(std::vector{ 1, 2, 4 }, boxesInFrustum)); + + Assert::IsTrue(std::vector{ 4, 2, 3 } == intersectedPoints); + Assert::IsTrue(std::vector{ 4, 0, 1, 2, 3 } == entityIDsInDFS); + Assert::IsTrue(std::vector{ 4, 0, 1, 2, 3 } == entityIDsInBFS); + } + }; + } // namespace XYZAdaptorTest + namespace UnrealAdaptorTest { diff --git a/unittests/compile_test.h b/unittests/compile_test.h index 3792568..1135033 100644 --- a/unittests/compile_test.h +++ b/unittests/compile_test.h @@ -56,7 +56,8 @@ void testCompilePoint() // const member functions { autoc keyRoot = tree.GetRootKey(); - autoc extent = tree.CalculateExtent(keyRoot); + autoc center = tree.GetNodeCenter(keyRoot); + autoc size = tree.GetNodeSize(0); autoc allidBFS = tree.CollectAllIdInBFS(keyRoot); autoc allidDFS = tree.CollectAllIdInDFS(keyRoot); @@ -152,7 +153,8 @@ void testCompilePointMap() // const member functions { autoc keyRoot = tree.GetRootKey(); - autoc extent = tree.CalculateExtent(keyRoot); + autoc center = tree.GetNodeCenter(keyRoot); + autoc size = tree.GetNodeSize(0); autoc allidBFS = tree.CollectAllIdInBFS(keyRoot); autoc allidDFS = tree.CollectAllIdInDFS(keyRoot); @@ -252,7 +254,8 @@ void testCompileBox() // const member functions { autoc keyRoot = tree.GetRootKey(); - autoc extent = tree.CalculateExtent(keyRoot); + autoc center = tree.GetNodeCenter(keyRoot); + autoc size = tree.GetNodeSize(0); autoc allidBFS = tree.CollectAllIdInBFS(keyRoot); autoc allidDFS = tree.CollectAllIdInDFS(keyRoot); @@ -345,7 +348,8 @@ void testCompileBoxMap() // const member functions { autoc keyRoot = tree.GetRootKey(); - autoc extent = tree.CalculateExtent(keyRoot); + autoc center = tree.GetNodeCenter(keyRoot); + autoc size = tree.GetNodeSize(0); autoc allidBFS = tree.CollectAllIdInBFS(keyRoot); autoc allidDFS = tree.CollectAllIdInDFS(keyRoot); diff --git a/unittests/general.tests.cpp b/unittests/general.tests.cpp index 58d62d1..d74ba3d 100644 --- a/unittests/general.tests.cpp +++ b/unittests/general.tests.cpp @@ -372,7 +372,9 @@ namespace GeneralTest auto tree = DualtreePoint{}; autoc bb = BoundingBox1D{ -1, +1 }; tree.Init(bb, 3, 10); - Assert::IsTrue(AreEqualAlmost(bb, tree.GetBox())); + autoc& box = tree.GetBox(); + + Assert::IsTrue(AreEqualAlmost(bb, BoundingBox1D{ .Min = box.Min, .Max = box.Max })); autoc& nodes = tree.GetNodes(); Assert::AreEqual(1, nodes.size()); @@ -759,6 +761,8 @@ namespace GeneralTest using AD = AdaptorGeneral, BoundingBoxND, RayND, PlaneND>; autoce rAcc = std::numeric_limits::min(); + autoc ptPre = &tPre; + autoc ptAfter = &tAfter; autoc nodesPre = tPre.GetNodes(); autoc nodesAfter = tAfter.GetNodes(); @@ -771,22 +775,13 @@ namespace GeneralTest if (pairPre.first != pairAfter.first) return false; - autoc& nodePre = pairPre.second; - autoc& nodeAfter = pairAfter.second; - - auto vMoveActualMin = PointND{}; - auto vMoveActualMax = PointND{}; + auto vMoveActual = PointND{}; + autoc& centerPre = GetNodeCenterMacro(ptPre, pairPre.first, pairPre.second); + autoc& centerAfter = GetNodeCenterMacro(ptAfter, pairAfter.first, pairAfter.second); for (dim_t dimensionID = 0; dimensionID < N; ++dimensionID) - { - AD::SetPointC( - vMoveActualMin, dimensionID, AD::GetBoxMinC(nodeAfter.GetBox(), dimensionID) - AD::GetBoxMinC(nodePre.GetBox(), dimensionID)); - AD::SetPointC( - vMoveActualMax, dimensionID, AD::GetBoxMaxC(nodeAfter.GetBox(), dimensionID) - AD::GetBoxMaxC(nodePre.GetBox(), dimensionID)); - } + AD::SetPointC(vMoveActual, dimensionID, centerAfter[dimensionID] - centerPre[dimensionID]); - autoc bMin = AD::ArePointsEqual(vMoveActualMin, vMoveExpected, rAcc); - autoc bMax = AD::ArePointsEqual(vMoveActualMax, vMoveExpected, rAcc); - return bMin && bMax; + return AD::ArePointsEqual(vMoveActual, vMoveExpected, rAcc); }); return std::ranges::all_of(vMatch, [](autoc bMatch) { return bMatch; }); @@ -1158,7 +1153,9 @@ namespace Tree1DTest autoc& nodes = tree.GetNodes(); Assert::IsTrue(nodes.size() == 1); Assert::IsTrue(nodes.at(1).Entities.empty()); - Assert::IsTrue(AreEqualAlmost(tree.GetBox(), BB1_INV)); + + autoc& box = tree.GetBox(); + Assert::IsTrue(AreEqualAlmost(BoundingBox1D{ .Min = box.Min, .Max = box.Max }, BB1_INV)); } // ext //! @@ -1168,7 +1165,9 @@ namespace Tree1DTest autoc& nodes = tree.GetNodes(); Assert::IsTrue(nodes.size() == 1); Assert::IsFalse(nodes.at(1).Entities.empty()); - Assert::IsTrue(AreEqualAlmost(tree.GetBox(), BoundingBox1D{ Point1D{1.0}, Point1D{1.0} })); + + autoc& box = tree.GetBox(); + Assert::IsTrue(AreEqualAlmost(BoundingBox1D{ .Min = box.Min, .Max = box.Max }, BoundingBox1D{ Point1D{ 1.0 }, Point1D{ 1.0 } })); } // ext //! @@ -1186,7 +1185,9 @@ namespace Tree1DTest autoc tree = DualtreePoint(vpt, 2, std::nullopt, 2); Assert::AreEqual(tree.GetNodes().size(), 7); - Assert::IsTrue(AreEqualAlmost(tree.GetBox(), BoundingBox1D{ Point1D{0.0}, Point1D{3.0} })); + + autoc& box = tree.GetBox(); + Assert::IsTrue(AreEqualAlmost(BoundingBox1D{ .Min = box.Min, .Max = box.Max }, BoundingBox1D{ Point1D{ 0.0 }, Point1D{ 3.0 } })); } //!