Working about pathfinding

src/navigation/PathFinder.h

@@ -1,6 +1,9 @@
 #pragma once
 
 #include <Eigen/Core>
+#include <cstdint>
+#include <memory>
+#include <mutex>
 #include <string>
 #include <vector>
 
@@ -79,6 +82,17 @@ private:
     std::vector<unsigned char> walkable;
     std::vector<NavigationArea> areas;
 
+    // Reused by synchronous authoritative path queries. Generation stamps avoid
+    // copying/clearing the complete map for every dynamic-obstacle search.
+    mutable std::vector<std::uint32_t> dynamicBlockedGeneration;
+    mutable std::uint32_t dynamicGeneration = 0;
+    mutable std::vector<int> searchCost;
+    mutable std::vector<int> searchCameFrom;
+    mutable std::vector<std::uint32_t> searchVisitedGeneration;
+    mutable std::uint32_t searchGeneration = 0;
+    mutable std::shared_ptr<std::mutex> queryMutex =
+        std::make_shared<std::mutex>();
+
     void loadConfig(const std::string& configPath, const std::string& zipPath);
     bool loadGrid(const std::string& path, const std::string& zipPath);
     void resetGridBounds();
@@ -103,11 +117,20 @@ private:
 	bool hasLineOfSight(const Cell& from, const Cell& to, const std::vector<unsigned char>& walkableGrid) const;
 	std::vector<Cell> smoothCells(const std::vector<Cell>& cells, const std::vector<unsigned char>& walkableGrid) const;
 
-	// A* that runs to exhaustion and returns a path to the reachable cell
-	// geometrically closest to end when end itself is unreachable.
-	std::vector<Eigen::Vector3f> findNearestReachableImpl(const Eigen::Vector3f& start,
-	                                                      const Eigen::Vector3f& end,
-	                                                      const std::vector<unsigned char>& walkableGrid) const;
+    std::uint32_t buildDynamicOverlay(
+        const std::vector<DynamicObstacle>& dynamicObstacles) const;
+    bool isCellWalkableOverlay(const Cell& cell, std::uint32_t generation) const;
+    bool findNearestWalkableCellOverlay(
+        const Eigen::Vector3f& point, Cell& out, std::uint32_t generation) const;
+    bool hasLineOfSightOverlay(
+        const Cell& from, const Cell& to, std::uint32_t generation) const;
+    std::vector<Cell> smoothCellsOverlay(
+        const std::vector<Cell>& cells, std::uint32_t generation) const;
+    std::vector<Eigen::Vector3f> runSearch(
+        const Eigen::Vector3f& start,
+        const Eigen::Vector3f& end,
+        std::uint32_t obstacleGeneration,
+        bool nearestReachable) const;
 };
 
 } // namespace FRG