Set up basic deterministic lockstep

src/navigation/PathFinder.cpp

@@ -148,7 +148,8 @@ std::vector<Eigen::Vector3f> PathFinder::findPath(const Eigen::Vector3f& start,
 
         bool operator<(const QueueNode& other) const
         {
-            return priority > other.priority;
+            if (priority != other.priority) return priority > other.priority;
+            return index > other.index;
         }
     };
 
@@ -302,7 +303,8 @@ std::vector<Eigen::Vector3f> PathFinder::findPath(const Eigen::Vector3f& start,
 
         bool operator<(const QueueNode& other) const
         {
-            return priority > other.priority;
+            if (priority != other.priority) return priority > other.priority;
+            return index > other.index;
         }
     };
 
@@ -412,7 +414,10 @@ std::vector<Eigen::Vector3f> PathFinder::findNearestReachableImpl(
     struct QueueNode {
         int index = 0;
         float priority = 0.0f;
-        bool operator<(const QueueNode& other) const { return priority > other.priority; }
+        bool operator<(const QueueNode& other) const {
+            if (priority != other.priority) return priority > other.priority;
+            return index > other.index;
+        }
     };
 
     const int cellCount = gridWidth * gridDepth;