added files

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 <algorithm>
+#include <cstdint>
+#include <numeric>
+#include <span>
+#include <vector>
+
+namespace omath::collision
+{
+    template<class Type = float>
+    class BvhTree final
+    {
+    public:
+        using AabbType = primitives::Aabb<Type>;
+
+        struct HitResult
+        {
+            std::size_t object_index;
+            Type distance_sqr;
+        };
+
+        BvhTree() = default;
+
+        explicit BvhTree(std::span<const AabbType> 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<std::size_t> query_overlaps(const AabbType& query_aabb) const
+        {
+            std::vector<std::size_t> results;
+
+            if (m_nodes.empty())
+                return results;
+
+            query_overlaps_impl(0, query_aabb, results);
+            return results;
+        }
+
+        template<class RayType = Ray<>>
+        [[nodiscard]]
+        std::vector<HitResult> query_ray(const RayType& ray) const
+        {
+            std::vector<HitResult> 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<std::size_t>::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<Type>::max(), std::numeric_limits<Type>::max(),
+                 std::numeric_limits<Type>::max()},
+                {std::numeric_limits<Type>::lowest(), std::numeric_limits<Type>::lowest(),
+                 std::numeric_limits<Type>::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<Type>(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<const AabbType> 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<const AabbType> aabbs, std::size_t begin, std::size_t end) const
+        {
+            AabbType bounds = {
+                {std::numeric_limits<Type>::max(), std::numeric_limits<Type>::max(),
+                 std::numeric_limits<Type>::max()},
+                {std::numeric_limits<Type>::lowest(), std::numeric_limits<Type>::lowest(),
+                 std::numeric_limits<Type>::lowest()}
+            };
+
+            for (auto i = begin; i < end; ++i)
+                bounds = merge(bounds, aabbs[m_indices[i]]);
+
+            return bounds;
+        }
+
+        [[nodiscard]]
+        AabbType compute_centroid_bounds(std::span<const AabbType> aabbs, std::size_t begin, std::size_t end) const
+        {
+            AabbType bounds = {
+                {std::numeric_limits<Type>::max(), std::numeric_limits<Type>::max(),
+                 std::numeric_limits<Type>::max()},
+                {std::numeric_limits<Type>::lowest(), std::numeric_limits<Type>::lowest(),
+                 std::numeric_limits<Type>::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<SplitResult> find_best_split(std::span<const AabbType> 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<Type>(count);
+            auto best_cost = leaf_cost;
+            std::optional<SplitResult> 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<Type>::epsilon())
+                    continue;
+
+                SahBucket buckets[k_sah_bucket_count] = {};
+
+                const auto inv_extent = static_cast<Type>(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<std::size_t>((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<Type>(1) / parent_area;
+
+                for (auto i = static_cast<int>(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<Type>(1)
+                                      + (static_cast<Type>(left_count) * surface_area(prefix_bounds[i])
+                                         + static_cast<Type>(right_count) * surface_area(suffix_bounds))
+                                            * inv_parent_area;
+
+                    if (cost < best_cost)
+                    {
+                        best_cost = cost;
+                        best_split = SplitResult{
+                            axis,
+                            axis_min + static_cast<Type>(i + 1) * (axis_max - axis_min)
+                                           / static_cast<Type>(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<const AabbType> aabbs, std::size_t begin, std::size_t end,
+                                      int axis, Type split_pos)
+        {
+            auto it = std::partition(m_indices.begin() + static_cast<std::ptrdiff_t>(begin),
+                                     m_indices.begin() + static_cast<std::ptrdiff_t>(end),
+                                     [&](std::size_t idx)
+                                     { return get_component(aabbs[idx].center(), axis) < split_pos; });
+
+            return static_cast<std::size_t>(std::distance(m_indices.begin(), it));
+        }
+
+        [[nodiscard]]
+        static constexpr Type get_component(const Vector3<Type>& 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<std::size_t>& 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<class RayType>
+        void query_ray_impl(std::size_t node_idx, const RayType& ray,
+                            std::vector<HitResult>& results) const
+        {
+            const auto& node = m_nodes[node_idx];
+
+            // Quick AABB-ray rejection using the slab method
+            const auto hit = LineTracer<RayType>::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<RayType>::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<Node> m_nodes;
+        std::vector<std::size_t> m_indices;
+        std::vector<AabbType> m_aabbs;
+    };
+} // namespace omath::collision

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"

tests/general/unit_test_bvh_tree.cpp

@@ -0,0 +1,876 @@
+//
+// Created by Orange on 04/08/2026.
+//
+#include <gtest/gtest.h>
+#include <omath/collision/bvh_tree.hpp>
+#include <algorithm>
+#include <random>
+#include <set>
+
+using Aabb = omath::primitives::Aabb<float>;
+using BvhTree = omath::collision::BvhTree<float>;
+using Ray = omath::collision::Ray<>;
+using HitResult = BvhTree::HitResult;
+
+using AabbD = omath::primitives::Aabb<double>;
+using BvhTreeD = omath::collision::BvhTree<double>;
+
+// ============================================================================
+// Helper: brute-force overlap query for verification
+// ============================================================================
+static std::set<std::size_t> brute_force_overlaps(const std::vector<Aabb>& aabbs, const Aabb& query)
+{
+    std::set<std::size_t> 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<Aabb> empty;
+    const BvhTree tree(empty);
+    EXPECT_TRUE(tree.empty());
+    EXPECT_EQ(tree.node_count(), 0);
+}
+
+TEST(UnitTestBvhTree, SingleElement)
+{
+    const std::vector<Aabb> 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<Aabb> 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<Aabb> 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<Aabb> aabbs;
+    for (int i = 0; i < 100; ++i)
+    {
+        const auto f = static_cast<float>(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<Aabb> 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<Aabb> 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<Aabb> 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<Aabb> 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<Aabb> 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<Aabb> 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<Aabb> 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<Aabb> aabbs;
+    for (int i = 0; i < 50; ++i)
+    {
+        const auto f = static_cast<float>(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<Aabb> aabbs;
+    for (int i = 0; i < 64; ++i)
+    {
+        const auto f = static_cast<float>(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<Aabb> 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<Aabb> aabbs;
+    for (int i = 0; i < 20; ++i)
+    {
+        const auto f = static_cast<float>(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<Aabb> aabbs;
+    for (int i = 0; i < 20; ++i)
+    {
+        const auto f = static_cast<float>(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<Aabb> aabbs;
+    for (int i = 0; i < 20; ++i)
+    {
+        const auto f = static_cast<float>(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<Aabb> aabbs;
+    for (int x = 0; x < 10; ++x)
+        for (int y = 0; y < 10; ++y)
+        {
+            const auto fx = static_cast<float>(x) * 3.f;
+            const auto fy = static_cast<float>(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<Aabb> aabbs;
+    for (int i = 0; i < 25; ++i)
+    {
+        const auto f = static_cast<float>(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<float>(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<Aabb> 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<Aabb> 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<Aabb> 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<Aabb> 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<Aabb> 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<Aabb> 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<Aabb> 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<Aabb> 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<Aabb> 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<Aabb> aabbs;
+    for (int i = 0; i < 20; ++i)
+    {
+        const auto f = static_cast<float>(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<Aabb> 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<float>(x) * 3.f;
+                const auto fy = static_cast<float>(y) * 3.f;
+                const auto fz = static_cast<float>(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<Aabb> 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<std::size_t> 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<float> pos_dist(-50.f, 50.f);
+    std::uniform_real_distribution<float> size_dist(0.5f, 3.f);
+
+    std::vector<Aabb> 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<std::size_t> 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<Aabb> 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<float>(x) * 3.f;
+                const auto fy = static_cast<float>(y) * 3.f;
+                const auto fz = static_cast<float>(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<Aabb> aabbs;
+    for (int i = 0; i < 5000; ++i)
+    {
+        const auto f = static_cast<float>(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<AabbD> 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<AabbD> 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<Aabb> 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<Aabb> 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<Aabb> aabbs;
+    for (int i = 0; i < 50; ++i)
+    {
+        const auto f = static_cast<float>(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<std::size_t> 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<Aabb> 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<Aabb> aabbs;
+    for (int i = 1; i <= 10; ++i)
+    {
+        const auto s = static_cast<float>(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<Aabb> aabbs;
+    for (int i = 0; i < 100; ++i)
+    {
+        const auto f = static_cast<float>(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<float>(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";
+    }
+}