Merge pull request #181 from orange-cpp/feature/walk-bot

Feature/walk bot

include/omath/collision/bvh_tree.hpp

@@ -1,412 +0,0 @@
-//
-// 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,7 +35,6 @@
 #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"

include/omath/pathfinding/walk_bot.hpp

@@ -0,0 +1,46 @@
+//
+// Created by orange on 4/12/2026.
+//
+#pragma once
+#include "navigation_mesh.hpp"
+#include "omath/linear_algebra/vector3.hpp"
+#include <functional>
+#include <memory>
+namespace omath::pathfinding
+{
+    enum class WalkBotStatus
+    {
+        IDLE,
+        PATHING,
+        FINISHED
+    };
+    class WalkBot
+    {
+    public:
+        WalkBot() = default;
+        explicit WalkBot(const std::shared_ptr<NavigationMesh>& mesh, float min_node_distance = 1.f);
+
+        void set_nav_mesh(const std::shared_ptr<NavigationMesh>& mesh);
+        void set_min_node_distance(float distance);
+
+        void set_target(const Vector3<float>& target);
+
+        // Clear navigation state so the bot can be re-routed without stale
+        // visited-node memory.
+        void reset();
+
+        // Call every game tick with the current bot world position.
+        void update(const Vector3<float>& bot_position);
+
+        void on_path(const std::function<void(const Vector3<float>&)>& callback);
+        void on_status(const std::function<void(WalkBotStatus)>& callback);
+
+    private:
+        std::weak_ptr<NavigationMesh> m_nav_mesh;
+        std::optional<std::function<void(const Vector3<float>&)>> m_on_next_path_node;
+        std::optional<std::function<void(WalkBotStatus)>> m_on_status_update;
+        std::optional<Vector3<float>> m_last_visited;
+        std::optional<Vector3<float>> m_target;
+        float m_min_node_distance{1.f};
+    };
+} // namespace omath::pathfinding

source/pathfinding/a_star.cpp

@@ -87,11 +87,11 @@ namespace omath::pathfinding
 
             const auto current_node = current_node_it->second;
 
+            closed_list.emplace(current, current_node);
+
             if (current == end_vertex)
                 return reconstruct_final_path(closed_list, current);
 
-            closed_list.emplace(current, current_node);
-
             for (const auto& neighbor: nav_mesh.get_neighbors(current))
             {
                 if (closed_list.contains(neighbor))

source/pathfinding/walk_bot.cpp

@@ -0,0 +1,92 @@
+//
+// Created by orange on 4/12/2026.
+//
+#include "omath/pathfinding/walk_bot.hpp"
+#include "omath/pathfinding/a_star.hpp"
+
+namespace omath::pathfinding
+{
+
+    WalkBot::WalkBot(const std::shared_ptr<NavigationMesh>& mesh, const float min_node_distance)
+        : m_nav_mesh(mesh), m_min_node_distance(min_node_distance) {}
+
+    void WalkBot::set_nav_mesh(const std::shared_ptr<NavigationMesh>& mesh)
+    {
+        m_nav_mesh = mesh;
+    }
+
+    void WalkBot::set_min_node_distance(const float distance)
+    {
+        m_min_node_distance = distance;
+    }
+
+    void WalkBot::set_target(const Vector3<float>& target)
+    {
+        m_target = target;
+    }
+
+    void WalkBot::reset()
+    {
+        m_last_visited.reset();
+    }
+
+    void WalkBot::update(const Vector3<float>& bot_position)
+    {
+        if (!m_target.has_value())
+            return;
+
+        if (m_target->distance_to(bot_position) <= m_min_node_distance)
+        {
+            if (m_on_status_update.has_value())
+                m_on_status_update->operator()(WalkBotStatus::FINISHED);
+            return;
+        }
+
+        if (!m_on_next_path_node.has_value())
+            return;
+
+        const auto nav_mesh = m_nav_mesh.lock();
+        if (!nav_mesh)
+        {
+            if (m_on_status_update.has_value())
+                m_on_status_update->operator()(WalkBotStatus::IDLE);
+            return;
+        }
+
+        const auto path = Astar::find_path(bot_position, *m_target, *nav_mesh);
+        if (path.empty())
+        {
+            if (m_on_status_update.has_value())
+                m_on_status_update->operator()(WalkBotStatus::IDLE);
+            return;
+        }
+
+        const auto& nearest = path.front();
+
+        // Record the nearest node as visited once we are close enough to it.
+        if (nearest.distance_to(bot_position) <= m_min_node_distance)
+            m_last_visited = nearest;
+
+        // If the nearest node was already visited, advance to the next one so
+        // we never oscillate back to a node we just left.
+        // If the bot was displaced (blown back), nearest will be an unvisited
+        // node, so we route to it first before continuing forward.
+        if (m_last_visited.has_value() && *m_last_visited == nearest && path.size() > 1)
+            m_on_next_path_node->operator()(path[1]);
+        else
+            m_on_next_path_node->operator()(nearest);
+
+        if (m_on_status_update.has_value())
+            m_on_status_update->operator()(WalkBotStatus::PATHING);
+    }
+
+    void WalkBot::on_path(const std::function<void(const Vector3<float>&)>& callback)
+    {
+        m_on_next_path_node = callback;
+    }
+
+    void WalkBot::on_status(const std::function<void(WalkBotStatus)>& callback)
+    {
+        m_on_status_update = callback;
+    }
+} // namespace omath::pathfinding

tests/general/unit_test_a_star.cpp

@@ -40,8 +40,9 @@ TEST(AStarExtra, TrivialNeighbor)
     nav.m_vertex_map[v2] = {v1};
 
     const auto path = Astar::find_path(v1, v2, nav);
-    ASSERT_EQ(path.size(), 1u);
-    EXPECT_EQ(path.front(), v2);
+    ASSERT_EQ(path.size(), 2u);
+    EXPECT_EQ(path.front(), v1);
+    EXPECT_EQ(path.back(), v2);
 }
 
 TEST(AStarExtra, StartEqualsGoal)
@@ -101,7 +102,7 @@ TEST(AStarExtra, LongerPathAvoidsBlock)
     constexpr Vector3<float> goal = idx(2, 1);
     const auto path = Astar::find_path(start, goal, nav);
     ASSERT_FALSE(path.empty());
-    EXPECT_EQ(path.front(), goal);
+    EXPECT_EQ(path.back(), goal);
 }
 
 TEST(AstarTests, TrivialDirectNeighborPath)
@@ -114,8 +115,9 @@ TEST(AstarTests, TrivialDirectNeighborPath)
     nav.m_vertex_map.emplace(v2, std::vector<Vector3<float>>{v1});
 
     const auto path = Astar::find_path(v1, v2, nav);
-    ASSERT_EQ(path.size(), 1u);
-    EXPECT_EQ(path.front(), v2);
+    ASSERT_EQ(path.size(), 2u);
+    EXPECT_EQ(path.front(), v1);
+    EXPECT_EQ(path.back(), v2);
 }
 
 TEST(AstarTests, NoPathWhenDisconnected)

tests/general/unit_test_bvh_tree.cpp

@@ -1,876 +0,0 @@
-//
-// 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";
-    }
-}

tests/general/unit_test_walk_bot.cpp

@@ -0,0 +1,640 @@
+// Unit tests for omath::pathfinding::WalkBot
+// Covers all status transitions, callback behaviour, and a full walk simulation.
+#include <gtest/gtest.h>
+#include "omath/pathfinding/walk_bot.hpp"
+#include "omath/pathfinding/navigation_mesh.hpp"
+
+using namespace omath;
+using namespace omath::pathfinding;
+
+// ---------------------------------------------------------------------------
+// Helpers
+// ---------------------------------------------------------------------------
+
+// Build a simple bidirectional linear chain:
+//   (0,0,0) <-> (1,0,0) <-> (2,0,0) <-> ... <-> (n-1,0,0)
+static std::shared_ptr<NavigationMesh> make_linear_mesh(int n)
+{
+    auto mesh = std::make_shared<NavigationMesh>();
+    for (int i = 0; i < n; ++i)
+    {
+        const Vector3<float> v{static_cast<float>(i), 0.f, 0.f};
+        std::vector<Vector3<float>> neighbors;
+        if (i > 0)
+            neighbors.push_back(Vector3<float>{static_cast<float>(i - 1), 0.f, 0.f});
+        if (i + 1 < n)
+            neighbors.push_back(Vector3<float>{static_cast<float>(i + 1), 0.f, 0.f});
+        mesh->m_vertex_map[v] = neighbors;
+    }
+    return mesh;
+}
+
+// Collect every status update fired during one update() call.
+static auto make_status_recorder(std::vector<WalkBotStatus>& out)
+{
+    return [&out](WalkBotStatus s) { out.push_back(s); };
+}
+
+// Collect every "next node" hint fired during one update() call.
+static auto make_node_recorder(std::vector<Vector3<float>>& out)
+{
+    return [&out](const Vector3<float>& v) { out.push_back(v); };
+}
+
+// ---------------------------------------------------------------------------
+// Construction
+// ---------------------------------------------------------------------------
+
+TEST(WalkBotTests, DefaultConstructedBotIsInert)
+{
+    // A default-constructed bot with no mesh, no target, and no callbacks must
+    // not crash.
+    WalkBot bot;
+    EXPECT_NO_THROW(bot.update({0.f, 0.f, 0.f}));
+}
+
+TEST(WalkBotTests, ConstructWithMeshAndDistance)
+{
+    auto mesh = make_linear_mesh(3);
+    EXPECT_NO_THROW(WalkBot bot(mesh, 0.5f));
+}
+
+// ---------------------------------------------------------------------------
+// Status: FINISHED
+// ---------------------------------------------------------------------------
+
+TEST(WalkBotTests, FiresFinishedWhenBotIsAtTarget)
+{
+    auto mesh = make_linear_mesh(3);
+    WalkBot bot(mesh, 0.5f);
+    bot.set_target({2.f, 0.f, 0.f});
+
+    std::vector<WalkBotStatus> statuses;
+    bot.on_status(make_status_recorder(statuses));
+
+    // bot_position == target_position -> distance == 0, well within threshold
+    bot.update({2.f, 0.f, 0.f});
+
+    ASSERT_FALSE(statuses.empty());
+    EXPECT_EQ(statuses.front(), WalkBotStatus::FINISHED);
+}
+
+TEST(WalkBotTests, FiresFinishedWhenBotIsWithinMinDistance)
+{
+    auto mesh = make_linear_mesh(3);
+    WalkBot bot(mesh, 0.5f);
+    bot.set_target({0.4f, 0.f, 0.f});
+
+    std::vector<WalkBotStatus> statuses;
+    bot.on_status(make_status_recorder(statuses));
+
+    // distance between (0,0,0) and (0.4,0,0) is 0.4 < 0.5 threshold
+    bot.update({0.f, 0.f, 0.f});
+
+    ASSERT_FALSE(statuses.empty());
+    EXPECT_EQ(statuses.front(), WalkBotStatus::FINISHED);
+}
+
+TEST(WalkBotTests, NoFinishedWhenOutsideMinDistance)
+{
+    auto mesh = make_linear_mesh(5);
+    WalkBot bot(mesh, 0.5f);
+    bot.set_target({4.f, 0.f, 0.f});
+
+    std::vector<WalkBotStatus> statuses;
+    bot.on_status(make_status_recorder(statuses));
+
+    // Attach path callback so we get further status updates
+    std::vector<Vector3<float>> nodes;
+    bot.on_path(make_node_recorder(nodes));
+
+    bot.update({0.f, 0.f, 0.f});
+
+    // FINISHED must NOT appear in the status list
+    for (auto s : statuses)
+        EXPECT_NE(s, WalkBotStatus::FINISHED);
+}
+
+TEST(WalkBotTests, FinishedFiredEvenWithoutPathCallback)
+{
+    // FINISHED is emitted before the on_path guard, so it fires regardless.
+    auto mesh = make_linear_mesh(2);
+    WalkBot bot(mesh, 0.5f);
+    bot.set_target({1.f, 0.f, 0.f});
+
+    std::vector<WalkBotStatus> statuses;
+    bot.on_status(make_status_recorder(statuses));
+    // Intentionally do NOT register on_path callback.
+
+    bot.update({1.f, 0.f, 0.f});
+
+    ASSERT_EQ(statuses.size(), 1u);
+    EXPECT_EQ(statuses[0], WalkBotStatus::FINISHED);
+}
+
+TEST(WalkBotTests, FinishedDoesNotFallThroughToPathing)
+{
+    // update() must return after FINISHED — PATHING must not fire on the same tick.
+    auto mesh = make_linear_mesh(3);
+    WalkBot bot(mesh, 0.5f);
+    bot.set_target({1.f, 0.f, 0.f});
+
+    std::vector<WalkBotStatus> statuses;
+    bot.on_status(make_status_recorder(statuses));
+    std::vector<Vector3<float>> nodes;
+    bot.on_path(make_node_recorder(nodes));
+
+    bot.update({1.f, 0.f, 0.f}); // bot is at target
+
+    ASSERT_EQ(statuses.size(), 1u);
+    EXPECT_EQ(statuses[0], WalkBotStatus::FINISHED);
+    EXPECT_TRUE(nodes.empty());
+}
+
+// ---------------------------------------------------------------------------
+// No target set
+// ---------------------------------------------------------------------------
+
+TEST(WalkBotTests, NoUpdateWithoutTarget)
+{
+    auto mesh = make_linear_mesh(3);
+    WalkBot bot(mesh, 0.5f);
+    // Intentionally do NOT call set_target()
+
+    std::vector<WalkBotStatus> statuses;
+    bot.on_status(make_status_recorder(statuses));
+    std::vector<Vector3<float>> nodes;
+    bot.on_path(make_node_recorder(nodes));
+
+    bot.update({0.f, 0.f, 0.f});
+
+    EXPECT_TRUE(statuses.empty());
+    EXPECT_TRUE(nodes.empty());
+}
+
+// ---------------------------------------------------------------------------
+// Status: IDLE — no path callback registered
+// ---------------------------------------------------------------------------
+
+TEST(WalkBotTests, NoPathCallbackMeansNoPathingStatus)
+{
+    auto mesh = make_linear_mesh(4);
+    WalkBot bot(mesh, 0.5f);
+    bot.set_target({3.f, 0.f, 0.f});
+
+    std::vector<WalkBotStatus> statuses;
+    bot.on_status(make_status_recorder(statuses));
+    // No on_path registered -> update returns early after FINISHED check
+
+    bot.update({0.f, 0.f, 0.f});
+
+    EXPECT_TRUE(statuses.empty());
+}
+
+// ---------------------------------------------------------------------------
+// Status: IDLE — null/expired navigation mesh
+// ---------------------------------------------------------------------------
+
+TEST(WalkBotTests, FiresIdleWhenNavMeshIsNull)
+{
+    WalkBot bot; // no mesh assigned
+    bot.set_target({5.f, 0.f, 0.f});
+
+    std::vector<WalkBotStatus> statuses;
+    bot.on_status(make_status_recorder(statuses));
+
+    std::vector<Vector3<float>> nodes;
+    bot.on_path(make_node_recorder(nodes));
+
+    bot.update({0.f, 0.f, 0.f});
+
+    ASSERT_FALSE(statuses.empty());
+    EXPECT_EQ(statuses.back(), WalkBotStatus::IDLE);
+    EXPECT_TRUE(nodes.empty());
+}
+
+TEST(WalkBotTests, FiresIdleWhenNavMeshExpires)
+{
+    auto mesh = make_linear_mesh(4);
+    WalkBot bot(mesh, 0.5f);
+    bot.set_target({3.f, 0.f, 0.f});
+
+    std::vector<WalkBotStatus> statuses;
+    bot.on_status(make_status_recorder(statuses));
+
+    std::vector<Vector3<float>> nodes;
+    bot.on_path(make_node_recorder(nodes));
+
+    // Let the shared_ptr expire — WalkBot holds only a weak_ptr.
+    mesh.reset();
+
+    bot.update({0.f, 0.f, 0.f});
+
+    ASSERT_FALSE(statuses.empty());
+    EXPECT_EQ(statuses.back(), WalkBotStatus::IDLE);
+    EXPECT_TRUE(nodes.empty());
+}
+
+TEST(WalkBotTests, SetNavMeshRestoresPathing)
+{
+    WalkBot bot; // starts with no mesh
+    bot.set_target({3.f, 0.f, 0.f});
+
+    std::vector<WalkBotStatus> statuses;
+    bot.on_status(make_status_recorder(statuses));
+    std::vector<Vector3<float>> nodes;
+    bot.on_path(make_node_recorder(nodes));
+
+    // First call — no mesh -> IDLE
+    bot.update({0.f, 0.f, 0.f});
+    ASSERT_EQ(statuses.back(), WalkBotStatus::IDLE);
+
+    // Assign a mesh and call again. Keep the shared_ptr alive so the
+    // weak_ptr inside WalkBot does not expire before update() is called.
+    statuses.clear();
+    nodes.clear();
+    auto new_mesh = make_linear_mesh(4);
+    bot.set_nav_mesh(new_mesh);
+    bot.set_min_node_distance(0.5f);
+    bot.update({0.f, 0.f, 0.f});
+
+    ASSERT_FALSE(statuses.empty());
+    EXPECT_EQ(statuses.back(), WalkBotStatus::PATHING);
+    EXPECT_FALSE(nodes.empty());
+}
+
+// ---------------------------------------------------------------------------
+// Status: IDLE — A* finds no path
+// ---------------------------------------------------------------------------
+
+TEST(WalkBotTests, FiresIdleWhenNoPathExists)
+{
+    // Disconnected graph: two isolated vertices
+    auto mesh = std::make_shared<NavigationMesh>();
+    mesh->m_vertex_map[{0.f, 0.f, 0.f}] = {};
+    mesh->m_vertex_map[{10.f, 0.f, 0.f}] = {};
+
+    WalkBot bot(mesh, 0.5f);
+    bot.set_target({10.f, 0.f, 0.f});
+
+    std::vector<WalkBotStatus> statuses;
+    bot.on_status(make_status_recorder(statuses));
+    std::vector<Vector3<float>> nodes;
+    bot.on_path(make_node_recorder(nodes));
+
+    bot.update({0.f, 0.f, 0.f});
+
+    ASSERT_FALSE(statuses.empty());
+    EXPECT_EQ(statuses.back(), WalkBotStatus::IDLE);
+    EXPECT_TRUE(nodes.empty());
+}
+
+// ---------------------------------------------------------------------------
+// Status: PATHING — normal routing
+// ---------------------------------------------------------------------------
+
+TEST(WalkBotTests, FiresPathingAndProvidesNextNode)
+{
+    auto mesh = make_linear_mesh(4);
+    WalkBot bot(mesh, 0.5f);
+    bot.set_target({3.f, 0.f, 0.f});
+
+    std::vector<WalkBotStatus> statuses;
+    bot.on_status(make_status_recorder(statuses));
+    std::vector<Vector3<float>> nodes;
+    bot.on_path(make_node_recorder(nodes));
+
+    bot.update({0.f, 0.f, 0.f});
+
+    ASSERT_FALSE(statuses.empty());
+    EXPECT_EQ(statuses.back(), WalkBotStatus::PATHING);
+    ASSERT_FALSE(nodes.empty());
+}
+
+TEST(WalkBotTests, NextNodeIsOnThePath)
+{
+    auto mesh = make_linear_mesh(5);
+    WalkBot bot(mesh, 0.5f);
+    bot.set_target({4.f, 0.f, 0.f});
+
+    std::vector<Vector3<float>> nodes;
+    bot.on_path(make_node_recorder(nodes));
+
+    bot.update({0.f, 0.f, 0.f});
+
+    // The suggested node must be a mesh vertex (x in [0..4], y=0, z=0)
+    ASSERT_FALSE(nodes.empty());
+    const auto& hint = nodes.front();
+    EXPECT_GE(hint.x, 0.f);
+    EXPECT_LE(hint.x, 4.f);
+    EXPECT_FLOAT_EQ(hint.y, 0.f);
+    EXPECT_FLOAT_EQ(hint.z, 0.f);
+}
+
+// ---------------------------------------------------------------------------
+// set_min_node_distance
+// ---------------------------------------------------------------------------
+
+TEST(WalkBotTests, MinNodeDistanceAffectsFinishedThreshold)
+{
+    auto mesh = make_linear_mesh(3);
+    WalkBot bot(mesh, 0.1f); // very tight threshold
+    bot.set_target({1.f, 0.f, 0.f});
+
+    std::vector<WalkBotStatus> statuses;
+    bot.on_status(make_status_recorder(statuses));
+
+    // distance 0.4 — outside 0.1 threshold
+    bot.update({0.6f, 0.f, 0.f});
+    EXPECT_TRUE(statuses.empty() ||
+                std::find(statuses.begin(), statuses.end(), WalkBotStatus::FINISHED) == statuses.end());
+
+    statuses.clear();
+    bot.set_min_node_distance(0.5f); // widen threshold
+    bot.update({0.6f, 0.f, 0.f});   // now 0.4 < 0.5 -> FINISHED
+
+    ASSERT_FALSE(statuses.empty());
+    EXPECT_EQ(statuses.front(), WalkBotStatus::FINISHED);
+}
+
+// ---------------------------------------------------------------------------
+// reset()
+// ---------------------------------------------------------------------------
+
+TEST(WalkBotTests, ResetClearsLastVisited)
+{
+    auto mesh = make_linear_mesh(3);
+    WalkBot bot(mesh, 0.5f);
+    bot.set_target({2.f, 0.f, 0.f});
+
+    std::vector<Vector3<float>> nodes;
+    bot.on_path(make_node_recorder(nodes));
+
+    // Tick 1: mark node 0 visited -> hint is node 1
+    bot.update({0.05f, 0.f, 0.f});
+    ASSERT_FALSE(nodes.empty());
+    EXPECT_FLOAT_EQ(nodes.back().x, 1.f);
+
+    // Without reset, a second tick from the same position also gives node 1.
+    nodes.clear();
+    bot.reset();
+
+    // After reset, m_last_visited is cleared. The nearest node is 0 again,
+    // it is within threshold so it gets marked visited and we advance to 1.
+    // The hint should still be node 1 (same outcome, but state was cleanly reset).
+    bot.update({0.05f, 0.f, 0.f});
+    ASSERT_FALSE(nodes.empty());
+    // Confirm the bot still routes correctly after reset.
+    EXPECT_GE(nodes.back().x, 0.f);
+}
+
+// ---------------------------------------------------------------------------
+// Node advancement — visited node causes skip to next
+// ---------------------------------------------------------------------------
+
+TEST(WalkBotTests, AdvancesWhenNearestNodeAlreadyVisited)
+{
+    // Chain: (0,0,0) -> (1,0,0) -> (2,0,0)
+    auto mesh = make_linear_mesh(3);
+    WalkBot bot(mesh, 0.5f);
+    bot.set_target({2.f, 0.f, 0.f});
+
+    std::vector<Vector3<float>> nodes;
+    bot.on_path(make_node_recorder(nodes));
+
+    // Tick 1: bot is very close to node 0 -> node 0 marked visited -> hint is node 1.
+    bot.update({0.05f, 0.f, 0.f});
+    ASSERT_FALSE(nodes.empty());
+    EXPECT_FLOAT_EQ(nodes.back().x, 1.f);
+
+    nodes.clear();
+
+    // Tick 2: bot has moved to near node 1 -> node 1 marked visited -> hint advances to node 2.
+    bot.update({1.05f, 0.f, 0.f});
+    ASSERT_FALSE(nodes.empty());
+    EXPECT_GT(nodes.back().x, 1.f);
+}
+
+// ---------------------------------------------------------------------------
+// Displacement recovery — bot knocked back to unvisited node
+// ---------------------------------------------------------------------------
+
+TEST(WalkBotTests, RecoverAfterDisplacementToUnvisitedNode)
+{
+    // Chain: 0 -> 1 -> 2 -> 3 -> 4
+    auto mesh = make_linear_mesh(5);
+    WalkBot bot(mesh, 0.5f);
+    bot.set_target({4.f, 0.f, 0.f});
+
+    std::vector<Vector3<float>> nodes;
+    bot.on_path(make_node_recorder(nodes));
+
+    // Walk forward through nodes 0-3 to build visited state.
+    for (int i = 0; i <= 3; ++i)
+    {
+        nodes.clear();
+        bot.update(Vector3<float>{static_cast<float>(i) + 0.1f, 0.f, 0.f});
+    }
+
+    // Displace the bot back to near node 1. The bot should route toward node 1
+    // first rather than skipping ahead to node 4.
+    nodes.clear();
+    bot.update({1.1f, 0.f, 0.f});
+
+    ASSERT_FALSE(nodes.empty());
+    EXPECT_LE(nodes.back().x, 3.f);
+}
+
+// ---------------------------------------------------------------------------
+// Callback wiring
+// ---------------------------------------------------------------------------
+
+TEST(WalkBotTests, ReplacingPathCallbackTakesEffect)
+{
+    auto mesh = make_linear_mesh(4);
+    WalkBot bot(mesh, 0.5f);
+    bot.set_target({3.f, 0.f, 0.f});
+
+    int first_cb_calls = 0;
+    int second_cb_calls = 0;
+
+    bot.on_path([&](const Vector3<float>&) { ++first_cb_calls; });
+    bot.update({0.f, 0.f, 0.f});
+
+    bot.on_path([&](const Vector3<float>&) { ++second_cb_calls; });
+    bot.update({0.f, 0.f, 0.f});
+
+    EXPECT_EQ(first_cb_calls, 1);
+    EXPECT_EQ(second_cb_calls, 1);
+}
+
+TEST(WalkBotTests, ReplacingStatusCallbackTakesEffect)
+{
+    auto mesh = make_linear_mesh(4);
+    WalkBot bot(mesh, 0.5f);
+    bot.set_target({3.f, 0.f, 0.f});
+
+    std::vector<Vector3<float>> nodes;
+    bot.on_path(make_node_recorder(nodes));
+
+    int cb1 = 0, cb2 = 0;
+    bot.on_status([&](WalkBotStatus) { ++cb1; });
+    bot.update({0.f, 0.f, 0.f});
+
+    bot.on_status([&](WalkBotStatus) { ++cb2; });
+    bot.update({0.f, 0.f, 0.f});
+
+    EXPECT_EQ(cb1, 1);
+    EXPECT_EQ(cb2, 1);
+}
+
+// ---------------------------------------------------------------------------
+// Full walk simulation — bot traverses a linear mesh step by step
+// ---------------------------------------------------------------------------
+
+// Simulates one game-loop tick and immediately "teleports" the bot to the
+// suggested node so the next tick starts from there.
+struct WalkSimulator
+{
+    WalkBot bot;
+    Vector3<float> position;
+    std::vector<Vector3<float>> visited_nodes;
+    std::vector<WalkBotStatus> status_history;
+    bool finished{false};
+
+    WalkSimulator(const std::shared_ptr<NavigationMesh>& mesh,
+                  const Vector3<float>& start,
+                  const Vector3<float>& goal,
+                  float threshold)
+        : position(start)
+    {
+        bot = WalkBot(mesh, threshold);
+        bot.set_target(goal);
+        bot.on_path([this](const Vector3<float>& next) {
+            visited_nodes.push_back(next);
+            position = next; // teleport to the suggested node
+        });
+        bot.on_status([this](WalkBotStatus s) {
+            status_history.push_back(s);
+            if (s == WalkBotStatus::FINISHED)
+                finished = true;
+        });
+    }
+
+    void run(int max_ticks = 100)
+    {
+        for (int tick = 0; tick < max_ticks && !finished; ++tick)
+            bot.update(position);
+    }
+};
+
+TEST(WalkBotSimulation, BotReachesTargetOnLinearMesh)
+{
+    auto mesh = make_linear_mesh(5);
+    WalkSimulator sim(mesh, {0.f, 0.f, 0.f}, {4.f, 0.f, 0.f}, 0.5f);
+    sim.run(50);
+
+    EXPECT_TRUE(sim.finished);
+}
+
+TEST(WalkBotSimulation, StatusTransitionSequenceIsCorrect)
+{
+    // Expect: one or more PATHING updates, then exactly FINISHED at the end.
+    auto mesh = make_linear_mesh(4);
+    WalkSimulator sim(mesh, {0.f, 0.f, 0.f}, {3.f, 0.f, 0.f}, 0.5f);
+    sim.run(50);
+
+    ASSERT_TRUE(sim.finished);
+    ASSERT_FALSE(sim.status_history.empty());
+
+    // All entries before the last must be PATHING
+    for (std::size_t i = 0; i + 1 < sim.status_history.size(); ++i)
+        EXPECT_EQ(sim.status_history[i], WalkBotStatus::PATHING);
+
+    EXPECT_EQ(sim.status_history.back(), WalkBotStatus::FINISHED);
+}
+
+TEST(WalkBotSimulation, BotVisitsNodesInForwardOrder)
+{
+    auto mesh = make_linear_mesh(5);
+    WalkSimulator sim(mesh, {0.f, 0.f, 0.f}, {4.f, 0.f, 0.f}, 0.5f);
+    sim.run(50);
+
+    ASSERT_FALSE(sim.visited_nodes.empty());
+
+    // Verify that x-coordinates are non-decreasing (forward progress only).
+    for (std::size_t i = 1; i < sim.visited_nodes.size(); ++i)
+        EXPECT_GE(sim.visited_nodes[i].x, sim.visited_nodes[i - 1].x - 1e-3f);
+}
+
+TEST(WalkBotSimulation, TwoNodePathReachesGoal)
+{
+    auto mesh = make_linear_mesh(2); // (0,0,0) <-> (1,0,0)
+    WalkSimulator sim(mesh, {0.f, 0.f, 0.f}, {1.f, 0.f, 0.f}, 0.5f);
+    sim.run(10);
+
+    EXPECT_TRUE(sim.finished);
+}
+
+TEST(WalkBotSimulation, LongChainEventuallyFinishes)
+{
+    constexpr int kLength = 20;
+    auto mesh = make_linear_mesh(kLength);
+    WalkSimulator sim(mesh,
+                      {0.f, 0.f, 0.f},
+                      {static_cast<float>(kLength - 1), 0.f, 0.f},
+                      0.5f);
+    sim.run(200);
+
+    EXPECT_TRUE(sim.finished);
+}
+
+TEST(WalkBotSimulation, StartAlreadyAtTargetFinishesImmediately)
+{
+    auto mesh = make_linear_mesh(3);
+    WalkSimulator sim(mesh, {1.f, 0.f, 0.f}, {1.f, 0.f, 0.f}, 0.5f);
+    sim.run(5);
+
+    EXPECT_TRUE(sim.finished);
+    EXPECT_EQ(sim.status_history.front(), WalkBotStatus::FINISHED);
+    EXPECT_EQ(sim.status_history.size(), 1u);
+}
+
+TEST(WalkBotSimulation, NoIdleEmittedDuringSuccessfulWalk)
+{
+    auto mesh = make_linear_mesh(4);
+    WalkSimulator sim(mesh, {0.f, 0.f, 0.f}, {3.f, 0.f, 0.f}, 0.5f);
+    sim.run(50);
+
+    for (auto s : sim.status_history)
+        EXPECT_NE(s, WalkBotStatus::IDLE);
+}
+
+// ---------------------------------------------------------------------------
+// Walk simulation on a branching mesh
+// ---------------------------------------------------------------------------
+
+TEST(WalkBotSimulation, BotNavigatesBranchingMesh)
+{
+    // Diamond topology:
+    //        (1,1,0)
+    //       /       \
+    // (0,0,0)       (2,0,0)
+    //       \       /
+    //        (1,-1,0)
+    auto mesh = std::make_shared<NavigationMesh>();
+    const Vector3<float> start{0.f, 0.f, 0.f};
+    const Vector3<float> top{1.f, 1.f, 0.f};
+    const Vector3<float> bot_node{1.f, -1.f, 0.f};
+    const Vector3<float> goal{2.f, 0.f, 0.f};
+
+    mesh->m_vertex_map[start]    = {top, bot_node};
+    mesh->m_vertex_map[top]      = {start, goal};
+    mesh->m_vertex_map[bot_node] = {start, goal};
+    mesh->m_vertex_map[goal]     = {top, bot_node};
+
+    WalkSimulator sim(mesh, start, goal, 0.5f);
+    sim.run(20);
+
+    EXPECT_TRUE(sim.finished);
+}