From dd421e329e3c240fb0859b7617bfa58863ad9d25 Mon Sep 17 00:00:00 2001 From: Orange Date: Wed, 8 Apr 2026 18:23:07 +0300 Subject: [PATCH] added files --- include/omath/collision/bvh_tree.hpp | 412 +++++++++++++ include/omath/omath.hpp | 1 + tests/general/unit_test_bvh_tree.cpp | 876 +++++++++++++++++++++++++++ 3 files changed, 1289 insertions(+) create mode 100644 include/omath/collision/bvh_tree.hpp create mode 100644 tests/general/unit_test_bvh_tree.cpp diff --git a/include/omath/collision/bvh_tree.hpp b/include/omath/collision/bvh_tree.hpp new file mode 100644 index 0000000..92f8611 --- /dev/null +++ b/include/omath/collision/bvh_tree.hpp @@ -0,0 +1,412 @@ +// +// Created by Orange on 04/08/2026. +// + +#pragma once +#include "omath/3d_primitives/aabb.hpp" +#include "omath/collision/line_tracer.hpp" +#include +#include +#include +#include +#include + +namespace omath::collision +{ + template + class BvhTree final + { + public: + using AabbType = primitives::Aabb; + + struct HitResult + { + std::size_t object_index; + Type distance_sqr; + }; + + BvhTree() = default; + + explicit BvhTree(std::span aabbs) + : m_aabbs(aabbs.begin(), aabbs.end()) + { + if (aabbs.empty()) + return; + + m_indices.resize(aabbs.size()); + std::iota(m_indices.begin(), m_indices.end(), std::size_t{0}); + + m_nodes.reserve(aabbs.size() * 2); + + build(m_aabbs, 0, aabbs.size()); + } + + [[nodiscard]] + std::vector query_overlaps(const AabbType& query_aabb) const + { + std::vector results; + + if (m_nodes.empty()) + return results; + + query_overlaps_impl(0, query_aabb, results); + return results; + } + + template> + [[nodiscard]] + std::vector query_ray(const RayType& ray) const + { + std::vector results; + + if (m_nodes.empty()) + return results; + + query_ray_impl(0, ray, results); + + std::ranges::sort(results, [](const HitResult& a, const HitResult& b) + { return a.distance_sqr < b.distance_sqr; }); + return results; + } + + [[nodiscard]] + std::size_t node_count() const noexcept + { + return m_nodes.size(); + } + + [[nodiscard]] + bool empty() const noexcept + { + return m_nodes.empty(); + } + + private: + static constexpr std::size_t k_sah_bucket_count = 12; + static constexpr std::size_t k_leaf_threshold = 1; + static constexpr std::size_t k_null_index = std::numeric_limits::max(); + + struct Node + { + AabbType bounds; + std::size_t left = k_null_index; + std::size_t right = k_null_index; + + // For leaf nodes: index range into m_indices + std::size_t first_index = 0; + std::size_t index_count = 0; + + [[nodiscard]] + bool is_leaf() const noexcept + { + return left == k_null_index; + } + }; + + struct SahBucket + { + AabbType bounds = { + {std::numeric_limits::max(), std::numeric_limits::max(), + std::numeric_limits::max()}, + {std::numeric_limits::lowest(), std::numeric_limits::lowest(), + std::numeric_limits::lowest()} + }; + std::size_t count = 0; + }; + + [[nodiscard]] + static constexpr Type surface_area(const AabbType& aabb) noexcept + { + const auto d = aabb.max - aabb.min; + return static_cast(2) * (d.x * d.y + d.y * d.z + d.z * d.x); + } + + [[nodiscard]] + static constexpr AabbType merge(const AabbType& a, const AabbType& b) noexcept + { + return { + {std::min(a.min.x, b.min.x), std::min(a.min.y, b.min.y), std::min(a.min.z, b.min.z)}, + {std::max(a.max.x, b.max.x), std::max(a.max.y, b.max.y), std::max(a.max.z, b.max.z)} + }; + } + + [[nodiscard]] + static constexpr bool overlaps(const AabbType& a, const AabbType& b) noexcept + { + return a.min.x <= b.max.x && a.max.x >= b.min.x + && a.min.y <= b.max.y && a.max.y >= b.min.y + && a.min.z <= b.max.z && a.max.z >= b.min.z; + } + + std::size_t build(std::span aabbs, std::size_t begin, std::size_t end) + { + const auto node_idx = m_nodes.size(); + m_nodes.emplace_back(); + + auto& node = m_nodes[node_idx]; + node.bounds = compute_bounds(aabbs, begin, end); + + const auto count = end - begin; + + if (count <= k_leaf_threshold) + { + node.first_index = begin; + node.index_count = count; + return node_idx; + } + + // Find best SAH split + const auto centroid_bounds = compute_centroid_bounds(aabbs, begin, end); + const auto split = find_best_split(aabbs, begin, end, node.bounds, centroid_bounds); + + // If SAH says don't split, make a leaf + if (!split.has_value()) + { + node.first_index = begin; + node.index_count = count; + return node_idx; + } + + const auto [axis, split_pos] = split.value(); + + // Partition indices around the split + const auto mid = partition_indices(aabbs, begin, end, axis, split_pos); + + // Degenerate partition fallback: split in the middle + const auto actual_mid = (mid == begin || mid == end) ? begin + count / 2 : mid; + + // Build children — careful: m_nodes may reallocate, so don't hold references across build calls + const auto left_idx = build(aabbs, begin, actual_mid); + const auto right_idx = build(aabbs, actual_mid, end); + + m_nodes[node_idx].left = left_idx; + m_nodes[node_idx].right = right_idx; + m_nodes[node_idx].index_count = 0; + + return node_idx; + } + + [[nodiscard]] + AabbType compute_bounds(std::span aabbs, std::size_t begin, std::size_t end) const + { + AabbType bounds = { + {std::numeric_limits::max(), std::numeric_limits::max(), + std::numeric_limits::max()}, + {std::numeric_limits::lowest(), std::numeric_limits::lowest(), + std::numeric_limits::lowest()} + }; + + for (auto i = begin; i < end; ++i) + bounds = merge(bounds, aabbs[m_indices[i]]); + + return bounds; + } + + [[nodiscard]] + AabbType compute_centroid_bounds(std::span aabbs, std::size_t begin, std::size_t end) const + { + AabbType bounds = { + {std::numeric_limits::max(), std::numeric_limits::max(), + std::numeric_limits::max()}, + {std::numeric_limits::lowest(), std::numeric_limits::lowest(), + std::numeric_limits::lowest()} + }; + + for (auto i = begin; i < end; ++i) + { + const auto c = aabbs[m_indices[i]].center(); + bounds.min.x = std::min(bounds.min.x, c.x); + bounds.min.y = std::min(bounds.min.y, c.y); + bounds.min.z = std::min(bounds.min.z, c.z); + bounds.max.x = std::max(bounds.max.x, c.x); + bounds.max.y = std::max(bounds.max.y, c.y); + bounds.max.z = std::max(bounds.max.z, c.z); + } + + return bounds; + } + + struct SplitResult + { + int axis; + Type position; + }; + + [[nodiscard]] + std::optional find_best_split(std::span aabbs, std::size_t begin, + std::size_t end, const AabbType& node_bounds, + const AabbType& centroid_bounds) const + { + const auto count = end - begin; + const auto leaf_cost = static_cast(count); + auto best_cost = leaf_cost; + std::optional best_split; + + for (int axis = 0; axis < 3; ++axis) + { + const auto axis_min = get_component(centroid_bounds.min, axis); + const auto axis_max = get_component(centroid_bounds.max, axis); + + if (axis_max - axis_min < std::numeric_limits::epsilon()) + continue; + + SahBucket buckets[k_sah_bucket_count] = {}; + + const auto inv_extent = static_cast(k_sah_bucket_count) / (axis_max - axis_min); + + // Fill buckets + for (auto i = begin; i < end; ++i) + { + const auto centroid = get_component(aabbs[m_indices[i]].center(), axis); + auto bucket_idx = static_cast((centroid - axis_min) * inv_extent); + bucket_idx = std::min(bucket_idx, k_sah_bucket_count - 1); + + buckets[bucket_idx].count++; + if (buckets[bucket_idx].count == 1) + buckets[bucket_idx].bounds = aabbs[m_indices[i]]; + else + buckets[bucket_idx].bounds = merge(buckets[bucket_idx].bounds, aabbs[m_indices[i]]); + } + + // Evaluate split costs using prefix/suffix sweeps + AabbType prefix_bounds[k_sah_bucket_count - 1]; + std::size_t prefix_count[k_sah_bucket_count - 1]; + + prefix_bounds[0] = buckets[0].bounds; + prefix_count[0] = buckets[0].count; + for (std::size_t i = 1; i < k_sah_bucket_count - 1; ++i) + { + prefix_bounds[i] = (buckets[i].count > 0) + ? merge(prefix_bounds[i - 1], buckets[i].bounds) + : prefix_bounds[i - 1]; + prefix_count[i] = prefix_count[i - 1] + buckets[i].count; + } + + AabbType suffix_bounds = buckets[k_sah_bucket_count - 1].bounds; + std::size_t suffix_count = buckets[k_sah_bucket_count - 1].count; + + const auto parent_area = surface_area(node_bounds); + const auto inv_parent_area = static_cast(1) / parent_area; + + for (auto i = static_cast(k_sah_bucket_count) - 2; i >= 0; --i) + { + const auto left_count = prefix_count[i]; + const auto right_count = suffix_count; + + if (left_count == 0 || right_count == 0) + { + if (i > 0) + { + suffix_bounds = (buckets[i].count > 0) + ? merge(suffix_bounds, buckets[i].bounds) + : suffix_bounds; + suffix_count += buckets[i].count; + } + continue; + } + + const auto cost = static_cast(1) + + (static_cast(left_count) * surface_area(prefix_bounds[i]) + + static_cast(right_count) * surface_area(suffix_bounds)) + * inv_parent_area; + + if (cost < best_cost) + { + best_cost = cost; + best_split = SplitResult{ + axis, + axis_min + static_cast(i + 1) * (axis_max - axis_min) + / static_cast(k_sah_bucket_count) + }; + } + + suffix_bounds = (buckets[i].count > 0) + ? merge(suffix_bounds, buckets[i].bounds) + : suffix_bounds; + suffix_count += buckets[i].count; + } + } + + return best_split; + } + + std::size_t partition_indices(std::span aabbs, std::size_t begin, std::size_t end, + int axis, Type split_pos) + { + auto it = std::partition(m_indices.begin() + static_cast(begin), + m_indices.begin() + static_cast(end), + [&](std::size_t idx) + { return get_component(aabbs[idx].center(), axis) < split_pos; }); + + return static_cast(std::distance(m_indices.begin(), it)); + } + + [[nodiscard]] + static constexpr Type get_component(const Vector3& v, int axis) noexcept + { + switch (axis) + { + case 0: + return v.x; + case 1: + return v.y; + default: + return v.z; + } + } + + void query_overlaps_impl(std::size_t node_idx, const AabbType& query_aabb, + std::vector& results) const + { + const auto& node = m_nodes[node_idx]; + + if (!overlaps(node.bounds, query_aabb)) + return; + + if (node.is_leaf()) + { + for (auto i = node.first_index; i < node.first_index + node.index_count; ++i) + if (overlaps(query_aabb, m_aabbs[m_indices[i]])) + results.push_back(m_indices[i]); + return; + } + + query_overlaps_impl(node.left, query_aabb, results); + query_overlaps_impl(node.right, query_aabb, results); + } + + template + void query_ray_impl(std::size_t node_idx, const RayType& ray, + std::vector& results) const + { + const auto& node = m_nodes[node_idx]; + + // Quick AABB-ray rejection using the slab method + const auto hit = LineTracer::get_ray_hit_point(ray, node.bounds); + if (hit == ray.end) + return; + + if (node.is_leaf()) + { + for (auto i = node.first_index; i < node.first_index + node.index_count; ++i) + { + const auto leaf_hit = LineTracer::get_ray_hit_point( + ray, m_aabbs[m_indices[i]]); + if (leaf_hit != ray.end) + { + const auto diff = leaf_hit - ray.start; + results.push_back({m_indices[i], diff.dot(diff)}); + } + } + return; + } + + query_ray_impl(node.left, ray, results); + query_ray_impl(node.right, ray, results); + } + + std::vector m_nodes; + std::vector m_indices; + std::vector m_aabbs; + }; +} // namespace omath::collision diff --git a/include/omath/omath.hpp b/include/omath/omath.hpp index bcf503a..9497db7 100644 --- a/include/omath/omath.hpp +++ b/include/omath/omath.hpp @@ -35,6 +35,7 @@ #include "omath/collision/line_tracer.hpp" #include "omath/collision/gjk_algorithm.hpp" #include "omath/collision/epa_algorithm.hpp" +#include "omath/collision/bvh_tree.hpp" // Pathfinding algorithms #include "omath/pathfinding/a_star.hpp" #include "omath/pathfinding/navigation_mesh.hpp" diff --git a/tests/general/unit_test_bvh_tree.cpp b/tests/general/unit_test_bvh_tree.cpp new file mode 100644 index 0000000..9b848ae --- /dev/null +++ b/tests/general/unit_test_bvh_tree.cpp @@ -0,0 +1,876 @@ +// +// Created by Orange on 04/08/2026. +// +#include +#include +#include +#include +#include + +using Aabb = omath::primitives::Aabb; +using BvhTree = omath::collision::BvhTree; +using Ray = omath::collision::Ray<>; +using HitResult = BvhTree::HitResult; + +using AabbD = omath::primitives::Aabb; +using BvhTreeD = omath::collision::BvhTree; + +// ============================================================================ +// Helper: brute-force overlap query for verification +// ============================================================================ +static std::set brute_force_overlaps(const std::vector& aabbs, const Aabb& query) +{ + std::set result; + for (std::size_t i = 0; i < aabbs.size(); ++i) + { + if (query.min.x <= aabbs[i].max.x && query.max.x >= aabbs[i].min.x + && query.min.y <= aabbs[i].max.y && query.max.y >= aabbs[i].min.y + && query.min.z <= aabbs[i].max.z && query.max.z >= aabbs[i].min.z) + result.insert(i); + } + return result; +} + +// ============================================================================ +// Construction tests +// ============================================================================ + +TEST(UnitTestBvhTree, EmptyTree) +{ + const BvhTree tree; + EXPECT_TRUE(tree.empty()); + EXPECT_EQ(tree.node_count(), 0); + EXPECT_TRUE(tree.query_overlaps({}).empty()); +} + +TEST(UnitTestBvhTree, EmptySpan) +{ + const std::vector empty; + const BvhTree tree(empty); + EXPECT_TRUE(tree.empty()); + EXPECT_EQ(tree.node_count(), 0); +} + +TEST(UnitTestBvhTree, SingleElement) +{ + const std::vector aabbs = { + {{0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}} + }; + + const BvhTree tree(aabbs); + EXPECT_FALSE(tree.empty()); + EXPECT_EQ(tree.node_count(), 1); + + const auto results = tree.query_overlaps({{0.5f, 0.5f, 0.5f}, {1.5f, 1.5f, 1.5f}}); + ASSERT_EQ(results.size(), 1); + EXPECT_EQ(results[0], 0); + + const auto miss = tree.query_overlaps({{5.f, 5.f, 5.f}, {6.f, 6.f, 6.f}}); + EXPECT_TRUE(miss.empty()); +} + +TEST(UnitTestBvhTree, TwoElements) +{ + const std::vector aabbs = { + {{0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}}, + {{5.f, 5.f, 5.f}, {6.f, 6.f, 6.f}}, + }; + + const BvhTree tree(aabbs); + EXPECT_FALSE(tree.empty()); + + // Hit first only + auto r = tree.query_overlaps({{-0.5f, -0.5f, -0.5f}, {0.5f, 0.5f, 0.5f}}); + ASSERT_EQ(r.size(), 1); + EXPECT_EQ(r[0], 0); + + // Hit second only + r = tree.query_overlaps({{5.5f, 5.5f, 5.5f}, {7.f, 7.f, 7.f}}); + ASSERT_EQ(r.size(), 1); + EXPECT_EQ(r[0], 1); + + // Hit both + r = tree.query_overlaps({{-1.f, -1.f, -1.f}, {10.f, 10.f, 10.f}}); + EXPECT_EQ(r.size(), 2); +} + +TEST(UnitTestBvhTree, ThreeElements) +{ + const std::vector aabbs = { + {{0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}}, + {{2.f, 2.f, 2.f}, {3.f, 3.f, 3.f}}, + {{10.f, 10.f, 10.f}, {11.f, 11.f, 11.f}}, + }; + + const BvhTree tree(aabbs); + + const auto results = tree.query_overlaps({{0.5f, 0.5f, 0.5f}, {2.5f, 2.5f, 2.5f}}); + EXPECT_EQ(results.size(), 2); + + const auto far = tree.query_overlaps({{9.5f, 9.5f, 9.5f}, {10.5f, 10.5f, 10.5f}}); + ASSERT_EQ(far.size(), 1); + EXPECT_EQ(far[0], 2); +} + +TEST(UnitTestBvhTree, NodeCountGrowsSublinearly) +{ + // For N objects, node count should be at most 2N-1 + std::vector aabbs; + for (int i = 0; i < 100; ++i) + { + const auto f = static_cast(i) * 3.f; + aabbs.push_back({{f, 0.f, 0.f}, {f + 1.f, 1.f, 1.f}}); + } + const BvhTree tree(aabbs); + EXPECT_LE(tree.node_count(), 2 * aabbs.size()); +} + +// ============================================================================ +// Overlap query tests +// ============================================================================ + +TEST(UnitTestBvhTree, OverlapExactTouch) +{ + // Two boxes share exactly one face + const std::vector aabbs = { + {{0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}}, + {{1.f, 0.f, 0.f}, {2.f, 1.f, 1.f}}, + }; + + const BvhTree tree(aabbs); + + // Query exactly at the shared face — should overlap both + const auto r = tree.query_overlaps({{0.5f, 0.f, 0.f}, {1.5f, 1.f, 1.f}}); + EXPECT_EQ(r.size(), 2); +} + +TEST(UnitTestBvhTree, OverlapEdgeTouch) +{ + // Query AABB edge-touches an object AABB + const std::vector aabbs = { + {{0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}}, + }; + + const BvhTree tree(aabbs); + + // Touching at corner point (1,1,1) + const auto r = tree.query_overlaps({{1.f, 1.f, 1.f}, {2.f, 2.f, 2.f}}); + EXPECT_EQ(r.size(), 1); +} + +TEST(UnitTestBvhTree, OverlapQueryInsideObject) +{ + // Query is fully inside an object + const std::vector aabbs = { + {{-10.f, -10.f, -10.f}, {10.f, 10.f, 10.f}}, + }; + + const BvhTree tree(aabbs); + const auto r = tree.query_overlaps({{-1.f, -1.f, -1.f}, {1.f, 1.f, 1.f}}); + ASSERT_EQ(r.size(), 1); + EXPECT_EQ(r[0], 0); +} + +TEST(UnitTestBvhTree, OverlapObjectInsideQuery) +{ + // Object is fully inside the query + const std::vector aabbs = { + {{4.f, 4.f, 4.f}, {5.f, 5.f, 5.f}}, + }; + + const BvhTree tree(aabbs); + const auto r = tree.query_overlaps({{0.f, 0.f, 0.f}, {10.f, 10.f, 10.f}}); + ASSERT_EQ(r.size(), 1); + EXPECT_EQ(r[0], 0); +} + +TEST(UnitTestBvhTree, OverlapMissOnSingleAxis) +{ + // Overlap on X and Y but not Z + const std::vector aabbs = { + {{0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}}, + }; + + const BvhTree tree(aabbs); + + const auto r = tree.query_overlaps({{0.f, 0.f, 5.f}, {1.f, 1.f, 6.f}}); + EXPECT_TRUE(r.empty()); +} + +TEST(UnitTestBvhTree, OverlapNegativeCoordinates) +{ + const std::vector aabbs = { + {{-5.f, -5.f, -5.f}, {-3.f, -3.f, -3.f}}, + {{-2.f, -2.f, -2.f}, {0.f, 0.f, 0.f}}, + {{1.f, 1.f, 1.f}, {3.f, 3.f, 3.f}}, + }; + + const BvhTree tree(aabbs); + + const auto r = tree.query_overlaps({{-6.f, -6.f, -6.f}, {-4.f, -4.f, -4.f}}); + ASSERT_EQ(r.size(), 1); + EXPECT_EQ(r[0], 0); +} + +TEST(UnitTestBvhTree, OverlapMixedNegativePositive) +{ + const std::vector aabbs = { + {{-1.f, -1.f, -1.f}, {1.f, 1.f, 1.f}}, + }; + + const BvhTree tree(aabbs); + + // Query spans negative and positive + const auto r = tree.query_overlaps({{-0.5f, -0.5f, -0.5f}, {0.5f, 0.5f, 0.5f}}); + ASSERT_EQ(r.size(), 1); +} + +TEST(UnitTestBvhTree, OverlapNoHitsAmongMany) +{ + std::vector aabbs; + for (int i = 0; i < 50; ++i) + { + const auto f = static_cast(i) * 5.f; + aabbs.push_back({{f, 0.f, 0.f}, {f + 1.f, 1.f, 1.f}}); + } + + const BvhTree tree(aabbs); + + // Query far from all objects + const auto r = tree.query_overlaps({{-100.f, -100.f, -100.f}, {-90.f, -90.f, -90.f}}); + EXPECT_TRUE(r.empty()); +} + +TEST(UnitTestBvhTree, OverlapAllObjects) +{ + std::vector aabbs; + for (int i = 0; i < 64; ++i) + { + const auto f = static_cast(i); + aabbs.push_back({{f, f, f}, {f + 0.5f, f + 0.5f, f + 0.5f}}); + } + + const BvhTree tree(aabbs); + + const auto r = tree.query_overlaps({{-1.f, -1.f, -1.f}, {100.f, 100.f, 100.f}}); + EXPECT_EQ(r.size(), 64); +} + +TEST(UnitTestBvhTree, OverlapReturnsCorrectIndices) +{ + // Specific index verification + const std::vector aabbs = { + {{0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}}, // 0 + {{10.f, 0.f, 0.f}, {11.f, 1.f, 1.f}}, // 1 + {{20.f, 0.f, 0.f}, {21.f, 1.f, 1.f}}, // 2 + {{30.f, 0.f, 0.f}, {31.f, 1.f, 1.f}}, // 3 + {{40.f, 0.f, 0.f}, {41.f, 1.f, 1.f}}, // 4 + }; + + const BvhTree tree(aabbs); + + // Hit only index 2 + auto r = tree.query_overlaps({{19.5f, -1.f, -1.f}, {20.5f, 2.f, 2.f}}); + ASSERT_EQ(r.size(), 1); + EXPECT_EQ(r[0], 2); + + // Hit only index 4 + r = tree.query_overlaps({{39.5f, -1.f, -1.f}, {40.5f, 2.f, 2.f}}); + ASSERT_EQ(r.size(), 1); + EXPECT_EQ(r[0], 4); +} + +// ============================================================================ +// Spatial distribution tests +// ============================================================================ + +TEST(UnitTestBvhTree, ObjectsAlongXAxis) +{ + // All objects on a line along X + std::vector aabbs; + for (int i = 0; i < 20; ++i) + { + const auto f = static_cast(i) * 4.f; + aabbs.push_back({{f, 0.f, 0.f}, {f + 1.f, 1.f, 1.f}}); + } + + const BvhTree tree(aabbs); + + const auto r = tree.query_overlaps({{0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}}); + EXPECT_EQ(r.size(), 1); + + const auto mid = tree.query_overlaps({{7.5f, -1.f, -1.f}, {8.5f, 2.f, 2.f}}); + EXPECT_EQ(mid.size(), 1); +} + +TEST(UnitTestBvhTree, ObjectsAlongYAxis) +{ + std::vector aabbs; + for (int i = 0; i < 20; ++i) + { + const auto f = static_cast(i) * 4.f; + aabbs.push_back({{0.f, f, 0.f}, {1.f, f + 1.f, 1.f}}); + } + + const BvhTree tree(aabbs); + + const auto r = tree.query_overlaps({{-1.f, 38.f, -1.f}, {2.f, 40.f, 2.f}}); + EXPECT_EQ(r.size(), 1); +} + +TEST(UnitTestBvhTree, ObjectsAlongZAxis) +{ + std::vector aabbs; + for (int i = 0; i < 20; ++i) + { + const auto f = static_cast(i) * 4.f; + aabbs.push_back({{0.f, 0.f, f}, {1.f, 1.f, f + 1.f}}); + } + + const BvhTree tree(aabbs); + + const auto r = tree.query_overlaps({{-1.f, -1.f, 38.f}, {2.f, 2.f, 40.f}}); + EXPECT_EQ(r.size(), 1); +} + +TEST(UnitTestBvhTree, ObjectsInPlaneXY) +{ + // Grid in the XY plane + std::vector aabbs; + for (int x = 0; x < 10; ++x) + for (int y = 0; y < 10; ++y) + { + const auto fx = static_cast(x) * 3.f; + const auto fy = static_cast(y) * 3.f; + aabbs.push_back({{fx, fy, 0.f}, {fx + 1.f, fy + 1.f, 1.f}}); + } + + const BvhTree tree(aabbs); + EXPECT_EQ(tree.query_overlaps({{-1.f, -1.f, -1.f}, {100.f, 100.f, 2.f}}).size(), 100); + + // Single cell query + const auto r = tree.query_overlaps({{0.f, 0.f, 0.f}, {0.5f, 0.5f, 0.5f}}); + EXPECT_EQ(r.size(), 1); +} + +TEST(UnitTestBvhTree, ClusteredObjects) +{ + // Two clusters far apart + std::vector aabbs; + for (int i = 0; i < 25; ++i) + { + const auto f = static_cast(i) * 0.5f; + aabbs.push_back({{f, f, f}, {f + 0.4f, f + 0.4f, f + 0.4f}}); + } + for (int i = 0; i < 25; ++i) + { + const auto f = 100.f + static_cast(i) * 0.5f; + aabbs.push_back({{f, f, f}, {f + 0.4f, f + 0.4f, f + 0.4f}}); + } + + const BvhTree tree(aabbs); + + // Query near first cluster + const auto r1 = tree.query_overlaps({{-1.f, -1.f, -1.f}, {15.f, 15.f, 15.f}}); + EXPECT_EQ(r1.size(), 25); + + // Query near second cluster + const auto r2 = tree.query_overlaps({{99.f, 99.f, 99.f}, {115.f, 115.f, 115.f}}); + EXPECT_EQ(r2.size(), 25); + + // Query between clusters — should find nothing + const auto gap = tree.query_overlaps({{50.f, 50.f, 50.f}, {60.f, 60.f, 60.f}}); + EXPECT_TRUE(gap.empty()); +} + +TEST(UnitTestBvhTree, OverlappingObjects) +{ + // Objects that overlap each other + const std::vector aabbs = { + {{0.f, 0.f, 0.f}, {2.f, 2.f, 2.f}}, + {{1.f, 1.f, 1.f}, {3.f, 3.f, 3.f}}, + {{1.5f, 1.5f, 1.5f}, {4.f, 4.f, 4.f}}, + }; + + const BvhTree tree(aabbs); + + // Query at the overlap region of all three + const auto r = tree.query_overlaps({{1.5f, 1.5f, 1.5f}, {2.f, 2.f, 2.f}}); + EXPECT_EQ(r.size(), 3); +} + +TEST(UnitTestBvhTree, IdenticalObjects) +{ + // All objects at the same position + std::vector aabbs; + for (int i = 0; i < 10; ++i) + aabbs.push_back({{0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}}); + + const BvhTree tree(aabbs); + + const auto r = tree.query_overlaps({{0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}}); + EXPECT_EQ(r.size(), 10); +} + +TEST(UnitTestBvhTree, DegenerateThickPlanes) +{ + // Very flat AABBs (thickness ~0 in one axis) + const std::vector aabbs = { + {{0.f, 0.f, 0.f}, {10.f, 10.f, 0.001f}}, + {{0.f, 0.f, 5.f}, {10.f, 10.f, 5.001f}}, + }; + + const BvhTree tree(aabbs); + + const auto r = tree.query_overlaps({{0.f, 0.f, -0.01f}, {10.f, 10.f, 0.01f}}); + ASSERT_EQ(r.size(), 1); +} + +TEST(UnitTestBvhTree, VaryingSizes) +{ + // Objects of wildly different sizes + const std::vector aabbs = { + {{0.f, 0.f, 0.f}, {0.01f, 0.01f, 0.01f}}, // tiny + {{-500.f, -500.f, -500.f}, {500.f, 500.f, 500.f}}, // huge + {{10.f, 10.f, 10.f}, {11.f, 11.f, 11.f}}, // normal + }; + + const BvhTree tree(aabbs); + + // The huge box should overlap almost any query + auto r = tree.query_overlaps({{200.f, 200.f, 200.f}, {201.f, 201.f, 201.f}}); + ASSERT_EQ(r.size(), 1); + EXPECT_EQ(r[0], 1); + + // Query at origin hits the tiny and the huge + r = tree.query_overlaps({{-0.1f, -0.1f, -0.1f}, {0.1f, 0.1f, 0.1f}}); + EXPECT_EQ(r.size(), 2); +} + +// ============================================================================ +// Ray query tests +// ============================================================================ + +TEST(UnitTestBvhTree, RayQueryBasic) +{ + const std::vector aabbs = { + {{0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}}, + {{5.f, 0.f, 0.f}, {6.f, 1.f, 1.f}}, + {{0.f, 5.f, 0.f}, {1.f, 6.f, 1.f}}, + }; + + const BvhTree tree(aabbs); + + Ray ray; + ray.start = {-1.f, 0.5f, 0.5f}; + ray.end = {10.f, 0.5f, 0.5f}; + ray.infinite_length = true; + + const auto hits = tree.query_ray(ray); + EXPECT_GE(hits.size(), 2); + + if (hits.size() >= 2) + EXPECT_LE(hits[0].distance_sqr, hits[1].distance_sqr); +} + +TEST(UnitTestBvhTree, RayQueryMissesAll) +{ + const std::vector aabbs = { + {{0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}}, + {{5.f, 0.f, 0.f}, {6.f, 1.f, 1.f}}, + }; + + const BvhTree tree(aabbs); + + // Ray above everything + Ray ray; + ray.start = {-1.f, 100.f, 0.5f}; + ray.end = {10.f, 100.f, 0.5f}; + ray.infinite_length = true; + + const auto hits = tree.query_ray(ray); + EXPECT_TRUE(hits.empty()); +} + +TEST(UnitTestBvhTree, RayQueryAlongY) +{ + const std::vector aabbs = { + {{0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}}, + {{0.f, 5.f, 0.f}, {1.f, 6.f, 1.f}}, + {{0.f, 10.f, 0.f}, {1.f, 11.f, 1.f}}, + }; + + const BvhTree tree(aabbs); + + Ray ray; + ray.start = {0.5f, -1.f, 0.5f}; + ray.end = {0.5f, 20.f, 0.5f}; + ray.infinite_length = true; + + const auto hits = tree.query_ray(ray); + EXPECT_EQ(hits.size(), 3); + + // Verify sorted by distance + for (std::size_t i = 1; i < hits.size(); ++i) + EXPECT_LE(hits[i - 1].distance_sqr, hits[i].distance_sqr); +} + +TEST(UnitTestBvhTree, RayQueryAlongZ) +{ + const std::vector aabbs = { + {{0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}}, + {{0.f, 0.f, 10.f}, {1.f, 1.f, 11.f}}, + }; + + const BvhTree tree(aabbs); + + Ray ray; + ray.start = {0.5f, 0.5f, -5.f}; + ray.end = {0.5f, 0.5f, 20.f}; + ray.infinite_length = true; + + const auto hits = tree.query_ray(ray); + EXPECT_EQ(hits.size(), 2); +} + +TEST(UnitTestBvhTree, RayQueryDiagonal) +{ + const std::vector aabbs = { + {{0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}}, + {{5.f, 5.f, 5.f}, {6.f, 6.f, 6.f}}, + {{10.f, 10.f, 10.f}, {11.f, 11.f, 11.f}}, + }; + + const BvhTree tree(aabbs); + + // Diagonal ray through all three + Ray ray; + ray.start = {-1.f, -1.f, -1.f}; + ray.end = {15.f, 15.f, 15.f}; + ray.infinite_length = true; + + const auto hits = tree.query_ray(ray); + EXPECT_EQ(hits.size(), 3); +} + +TEST(UnitTestBvhTree, RayQueryOnEmptyTree) +{ + const BvhTree tree; + + Ray ray; + ray.start = {0.f, 0.f, 0.f}; + ray.end = {10.f, 0.f, 0.f}; + ray.infinite_length = true; + + const auto hits = tree.query_ray(ray); + EXPECT_TRUE(hits.empty()); +} + +TEST(UnitTestBvhTree, RayQuerySortedByDistance) +{ + // Many boxes along a line + std::vector aabbs; + for (int i = 0; i < 20; ++i) + { + const auto f = static_cast(i) * 3.f; + aabbs.push_back({{f, 0.f, 0.f}, {f + 1.f, 1.f, 1.f}}); + } + + const BvhTree tree(aabbs); + + Ray ray; + ray.start = {-1.f, 0.5f, 0.5f}; + ray.end = {100.f, 0.5f, 0.5f}; + ray.infinite_length = true; + + const auto hits = tree.query_ray(ray); + EXPECT_EQ(hits.size(), 20); + + for (std::size_t i = 1; i < hits.size(); ++i) + EXPECT_LE(hits[i - 1].distance_sqr, hits[i].distance_sqr); +} + +// ============================================================================ +// Brute-force verification tests +// ============================================================================ + +TEST(UnitTestBvhTree, BruteForceVerificationGrid) +{ + std::vector aabbs; + for (int x = 0; x < 10; ++x) + for (int y = 0; y < 10; ++y) + for (int z = 0; z < 10; ++z) + { + const auto fx = static_cast(x) * 3.f; + const auto fy = static_cast(y) * 3.f; + const auto fz = static_cast(z) * 3.f; + aabbs.push_back({{fx, fy, fz}, {fx + 1.f, fy + 1.f, fz + 1.f}}); + } + + const BvhTree tree(aabbs); + + // Test several queries and compare to brute force + const std::vector queries = { + {{0.f, 0.f, 0.f}, {1.5f, 1.5f, 1.5f}}, + {{-1.f, -1.f, -1.f}, {100.f, 100.f, 100.f}}, + {{13.f, 13.f, 13.f}, {14.f, 14.f, 14.f}}, + {{-50.f, -50.f, -50.f}, {-40.f, -40.f, -40.f}}, + {{5.5f, 5.5f, 5.5f}, {7.5f, 7.5f, 7.5f}}, + }; + + for (const auto& q : queries) + { + const auto bvh_results = tree.query_overlaps(q); + const auto brute_results = brute_force_overlaps(aabbs, q); + + const std::set bvh_set(bvh_results.begin(), bvh_results.end()); + EXPECT_EQ(bvh_set, brute_results) + << "Mismatch for query [(" << q.min.x << "," << q.min.y << "," << q.min.z + << ") -> (" << q.max.x << "," << q.max.y << "," << q.max.z << ")]"; + } +} + +TEST(UnitTestBvhTree, BruteForceVerificationRandom) +{ + std::mt19937 rng(42); + std::uniform_real_distribution pos_dist(-50.f, 50.f); + std::uniform_real_distribution size_dist(0.5f, 3.f); + + std::vector aabbs; + for (int i = 0; i < 200; ++i) + { + const auto x = pos_dist(rng); + const auto y = pos_dist(rng); + const auto z = pos_dist(rng); + const auto sx = size_dist(rng); + const auto sy = size_dist(rng); + const auto sz = size_dist(rng); + aabbs.push_back({{x, y, z}, {x + sx, y + sy, z + sz}}); + } + + const BvhTree tree(aabbs); + + // Run 50 random queries + for (int i = 0; i < 50; ++i) + { + const auto qx = pos_dist(rng); + const auto qy = pos_dist(rng); + const auto qz = pos_dist(rng); + const auto qsx = size_dist(rng); + const auto qsy = size_dist(rng); + const auto qsz = size_dist(rng); + const Aabb query = {{qx, qy, qz}, {qx + qsx, qy + qsy, qz + qsz}}; + + const auto bvh_results = tree.query_overlaps(query); + const auto brute_results = brute_force_overlaps(aabbs, query); + + const std::set bvh_set(bvh_results.begin(), bvh_results.end()); + EXPECT_EQ(bvh_set, brute_results) << "Mismatch on random query iteration " << i; + } +} + +// ============================================================================ +// Large dataset tests +// ============================================================================ + +TEST(UnitTestBvhTree, LargeGridDataset) +{ + std::vector aabbs; + for (int x = 0; x < 10; ++x) + for (int y = 0; y < 10; ++y) + for (int z = 0; z < 10; ++z) + { + const auto fx = static_cast(x) * 3.f; + const auto fy = static_cast(y) * 3.f; + const auto fz = static_cast(z) * 3.f; + aabbs.push_back({{fx, fy, fz}, {fx + 1.f, fy + 1.f, fz + 1.f}}); + } + + const BvhTree tree(aabbs); + EXPECT_FALSE(tree.empty()); + + const auto results = tree.query_overlaps({{0.f, 0.f, 0.f}, {1.5f, 1.5f, 1.5f}}); + EXPECT_EQ(results.size(), 1); + + const auto all_results = tree.query_overlaps({{-1.f, -1.f, -1.f}, {100.f, 100.f, 100.f}}); + EXPECT_EQ(all_results.size(), 1000); +} + +TEST(UnitTestBvhTree, FiveThousandObjects) +{ + std::vector aabbs; + for (int i = 0; i < 5000; ++i) + { + const auto f = static_cast(i) * 2.f; + aabbs.push_back({{f, 0.f, 0.f}, {f + 1.f, 1.f, 1.f}}); + } + + const BvhTree tree(aabbs); + EXPECT_FALSE(tree.empty()); + + // Query that should hit exactly 1 + const auto r = tree.query_overlaps({{0.f, 0.f, 0.f}, {0.5f, 0.5f, 0.5f}}); + EXPECT_EQ(r.size(), 1); + + // Query that misses + const auto miss = tree.query_overlaps({{-100.f, -100.f, -100.f}, {-90.f, -90.f, -90.f}}); + EXPECT_TRUE(miss.empty()); +} + +// ============================================================================ +// Double precision tests +// ============================================================================ + +TEST(UnitTestBvhTree, DoublePrecision) +{ + const std::vector aabbs = { + {{0.0, 0.0, 0.0}, {1.0, 1.0, 1.0}}, + {{5.0, 5.0, 5.0}, {6.0, 6.0, 6.0}}, + {{10.0, 10.0, 10.0}, {11.0, 11.0, 11.0}}, + }; + + const BvhTreeD tree(aabbs); + EXPECT_FALSE(tree.empty()); + + const auto r = tree.query_overlaps({{0.5, 0.5, 0.5}, {1.5, 1.5, 1.5}}); + ASSERT_EQ(r.size(), 1); + EXPECT_EQ(r[0], 0); + + const auto r2 = tree.query_overlaps({{4.5, 4.5, 4.5}, {5.5, 5.5, 5.5}}); + ASSERT_EQ(r2.size(), 1); + EXPECT_EQ(r2[0], 1); +} + +TEST(UnitTestBvhTree, DoublePrecisionLargeCoordinates) +{ + const std::vector aabbs = { + {{1e10, 1e10, 1e10}, {1e10 + 1.0, 1e10 + 1.0, 1e10 + 1.0}}, + {{-1e10, -1e10, -1e10}, {-1e10 + 1.0, -1e10 + 1.0, -1e10 + 1.0}}, + }; + + const BvhTreeD tree(aabbs); + + const auto r = tree.query_overlaps({{1e10 - 0.5, 1e10 - 0.5, 1e10 - 0.5}, + {1e10 + 0.5, 1e10 + 0.5, 1e10 + 0.5}}); + ASSERT_EQ(r.size(), 1); + EXPECT_EQ(r[0], 0); +} + +// ============================================================================ +// Edge case tests +// ============================================================================ + +TEST(UnitTestBvhTree, ZeroSizeQuery) +{ + const std::vector aabbs = { + {{0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}}, + }; + + const BvhTree tree(aabbs); + + // Point query inside the box + const auto r = tree.query_overlaps({{0.5f, 0.5f, 0.5f}, {0.5f, 0.5f, 0.5f}}); + EXPECT_EQ(r.size(), 1); + + // Point query outside the box + const auto miss = tree.query_overlaps({{5.f, 5.f, 5.f}, {5.f, 5.f, 5.f}}); + EXPECT_TRUE(miss.empty()); +} + +TEST(UnitTestBvhTree, ZeroSizeObjects) +{ + // Point-like AABBs + const std::vector aabbs = { + {{1.f, 1.f, 1.f}, {1.f, 1.f, 1.f}}, + {{5.f, 5.f, 5.f}, {5.f, 5.f, 5.f}}, + }; + + const BvhTree tree(aabbs); + + const auto r = tree.query_overlaps({{0.f, 0.f, 0.f}, {2.f, 2.f, 2.f}}); + ASSERT_EQ(r.size(), 1); + EXPECT_EQ(r[0], 0); +} + +TEST(UnitTestBvhTree, NoDuplicateResults) +{ + std::vector aabbs; + for (int i = 0; i < 50; ++i) + { + const auto f = static_cast(i) * 2.f; + aabbs.push_back({{f, 0.f, 0.f}, {f + 1.f, 1.f, 1.f}}); + } + + const BvhTree tree(aabbs); + + const auto r = tree.query_overlaps({{-1.f, -1.f, -1.f}, {200.f, 2.f, 2.f}}); + + // Check for duplicates + const std::set unique_results(r.begin(), r.end()); + EXPECT_EQ(unique_results.size(), r.size()); + EXPECT_EQ(r.size(), 50); +} + +TEST(UnitTestBvhTree, LargeSpread) +{ + // Objects with huge gaps between them + const std::vector aabbs = { + {{0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}}, + {{1000.f, 0.f, 0.f}, {1001.f, 1.f, 1.f}}, + {{-1000.f, 0.f, 0.f}, {-999.f, 1.f, 1.f}}, + {{0.f, 1000.f, 0.f}, {1.f, 1001.f, 1.f}}, + {{0.f, -1000.f, 0.f}, {1.f, -999.f, 1.f}}, + }; + + const BvhTree tree(aabbs); + + auto r = tree.query_overlaps({{999.f, -1.f, -1.f}, {1002.f, 2.f, 2.f}}); + ASSERT_EQ(r.size(), 1); + EXPECT_EQ(r[0], 1); + + r = tree.query_overlaps({{-1001.f, -1.f, -1.f}, {-998.f, 2.f, 2.f}}); + ASSERT_EQ(r.size(), 1); + EXPECT_EQ(r[0], 2); +} + +TEST(UnitTestBvhTree, AllObjectsSameCenter) +{ + // All AABBs centered at origin but different sizes + std::vector aabbs; + for (int i = 1; i <= 10; ++i) + { + const auto s = static_cast(i); + aabbs.push_back({{-s, -s, -s}, {s, s, s}}); + } + + const BvhTree tree(aabbs); + + // Small query at origin should hit all + const auto r = tree.query_overlaps({{-0.1f, -0.1f, -0.1f}, {0.1f, 0.1f, 0.1f}}); + EXPECT_EQ(r.size(), 10); + + // Query touching only the largest box + const auto r2 = tree.query_overlaps({{9.5f, 9.5f, 9.5f}, {10.5f, 10.5f, 10.5f}}); + ASSERT_EQ(r2.size(), 1); + EXPECT_EQ(r2[0], 9); +} + +TEST(UnitTestBvhTree, MultipleQueriesSameTree) +{ + std::vector aabbs; + for (int i = 0; i < 100; ++i) + { + const auto f = static_cast(i) * 2.f; + aabbs.push_back({{f, 0.f, 0.f}, {f + 1.f, 1.f, 1.f}}); + } + + const BvhTree tree(aabbs); + + // Run many queries on the same tree + for (int i = 0; i < 100; ++i) + { + const auto f = static_cast(i) * 2.f; + const auto r = tree.query_overlaps({{f, 0.f, 0.f}, {f + 1.f, 1.f, 1.f}}); + ASSERT_GE(r.size(), 1) << "Query for object " << i << " should find at least itself"; + } +}