From ea07d17dbb8da0b52b097be9983a9f4e6f53f21e Mon Sep 17 00:00:00 2001
From: Orange <orange-cpp@yandex.ru>
Date: Sun, 12 Apr 2026 12:05:40 +0300
Subject: [PATCH] improved walkbot

---
 include/omath/pathfinding/walk_bot.hpp | 12 ++++++++--
 source/pathfinding/walk_bot.cpp        | 33 +++++++++++++++++++-------
 2 files changed, 34 insertions(+), 11 deletions(-)

diff --git a/include/omath/pathfinding/walk_bot.hpp b/include/omath/pathfinding/walk_bot.hpp
index 7d2906b..cc06473 100644
--- a/include/omath/pathfinding/walk_bot.hpp
+++ b/include/omath/pathfinding/walk_bot.hpp
@@ -5,16 +5,24 @@
 #include "navigation_mesh.hpp"
 #include "omath/linear_algebra/vector3.hpp"
 #include <functional>
+#include <memory>
 namespace omath::pathfinding
 {
     class WalkBot
     {
     public:
-        void update(const Vector3<float>& bot_position, const Vector3<float>& target_position, float min_node_distance) const;
+        WalkBot() = default;
+        explicit WalkBot(std::shared_ptr<NavigationMesh> mesh);
+
+        void set_nav_mesh(std::shared_ptr<NavigationMesh> mesh);
+
+        void update(const Vector3<float>& bot_position, const Vector3<float>& target_position, float min_node_distance);
 
         void on_path(std::function<void(const Vector3<float>&)> callback);
+
     private:
         std::weak_ptr<NavigationMesh> m_mav_mesh;
-        std::optional<std::function<void(const Vector3<float>&)>> m_on_next_path_node = nullptr;
+        std::optional<std::function<void(const Vector3<float>&)>> m_on_next_path_node;
+        std::optional<Vector3<float>> m_last_visited;
     };
 } // namespace omath::pathfinding
\ No newline at end of file
diff --git a/source/pathfinding/walk_bot.cpp b/source/pathfinding/walk_bot.cpp
index 171b91f..47f1ec7 100644
--- a/source/pathfinding/walk_bot.cpp
+++ b/source/pathfinding/walk_bot.cpp
@@ -7,26 +7,41 @@
 namespace omath::pathfinding
 {
 
-    void WalkBot::update(const Vector3<float>& bot_position, const Vector3<float>& target_position,
-                         const float min_node_distance) const
-    {
-        const auto nav_mesh = m_mav_mesh.lock();
+    WalkBot::WalkBot(std::shared_ptr<NavigationMesh> mesh) : m_mav_mesh(std::move(mesh)) {}
 
+    void WalkBot::set_nav_mesh(std::shared_ptr<NavigationMesh> mesh)
+    {
+        m_mav_mesh = std::move(mesh);
+    }
+
+    void WalkBot::update(const Vector3<float>& bot_position, const Vector3<float>& target_position,
+                         const float min_node_distance)
+    {
+        if (!m_on_next_path_node.has_value())
+            return;
+
+        const auto nav_mesh = m_mav_mesh.lock();
         if (!nav_mesh)
             return;
 
         const auto path = Astar::find_path(bot_position, target_position, *nav_mesh);
-
         if (path.empty())
             return;
 
-        if (!m_on_next_path_node.has_value())
-            return;
+        const auto& nearest = path.front();
 
-        if (path.size() > 1 && path.front().distance_to(bot_position) <= min_node_distance)
+        // Record the nearest node as visited once we are close enough to it.
+        if (nearest.distance_to(bot_position) <= 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()(path.front());
+            m_on_next_path_node->operator()(nearest);
     }
     void WalkBot::on_path(std::function<void(const Vector3<float>&)> callback)
     {