Attempt to optimize pathfind a bit.

awgil · Sep 8, 2024 · 380f206 · 380f206
1 parent 72b704d
commit 380f206
Show file tree

Hide file tree

Showing 2 changed files with 32 additions and 10 deletions.
diff --git a/BossMod/Pathfinding/MapVisualizer.cs b/BossMod/Pathfinding/MapVisualizer.cs
@@ -16,7 +16,6 @@ public class MapVisualizer
 
     private ThetaStar _pathfind;
     private float _lastExecTime;
-    private int _lastSteps;
 
     public MapVisualizer(Map map, WPos startPos, WPos goalPos, float goalRadius)
     {
@@ -177,7 +176,22 @@ public void Draw()
         if (ImGui.Button("Run pf"))
             RunPathfind();
         ImGui.SameLine();
-        ImGui.TextUnformatted($"Last op: {_lastExecTime:f3}s, last steps: {_lastSteps}");
+        if (ImGui.Button("Step back") && _pathfind.NumSteps > 0)
+        {
+            var s = _pathfind.NumSteps - 1;
+            ResetPathfind();
+            while (_pathfind.NumSteps < s && _pathfind.ExecuteStep())
+                ;
+        }
+        ImGui.SameLine();
+        if (ImGui.Button("Run until reopen"))
+        {
+            var startR = _pathfind.NumReopens;
+            while (_pathfind.ExecuteStep() && _pathfind.NumReopens == startR)
+                ;
+        }
+        ImGui.SameLine();
+        ImGui.TextUnformatted($"Last op: {_lastExecTime:f3}s, num steps: {_pathfind.NumSteps}, num reopens: {_pathfind.NumReopens}");
 
         var pfRes = _pathfind.BestIndex;
         if (pfRes >= 0)
@@ -192,12 +206,7 @@ public void Draw()
     }
 
     public void StepPathfind() => ExecTimed(() => _pathfind.ExecuteStep());
-    public void RunPathfind() => ExecTimed(() =>
-    {
-        _lastSteps = 0;
-        while (!_pathfind.IsVeryGood(_pathfind.BestIndex) && _pathfind.ExecuteStep())
-            ++_lastSteps;
-    });
+    public void RunPathfind() => ExecTimed(() => _pathfind.Execute());
     public void ResetPathfind() => ExecTimed(() => _pathfind = BuildPathfind());
 
     private void ExecTimed(Action action)

diff --git a/BossMod/Pathfinding/ThetaStar.cs b/BossMod/Pathfinding/ThetaStar.cs
@@ -25,6 +25,8 @@ public struct Node
     private float _startMaxG;
 
     public int BestIndex { get; private set; }
+    public int NumSteps { get; private set; }
+    public int NumReopens { get; private set; }
 
     private const float BorderCushion = 0.1f;
     private const float MaxNeighbourOffset = 0.5f - BorderCushion;
@@ -46,6 +48,7 @@ public void Start(Map map, WPos startPos, WPos goalPos, float goalRadius, float
             Array.Fill(_nodes, default, 0, numPixels);
         _openList.Clear();
         _deltaGSide = map.Resolution * gMultiplier;
+        NumSteps = NumReopens = 0;
 
         var startFrac = map.WorldToGridFrac(startPos);
         var start = map.ClampToGrid(map.FracToGrid(startFrac));
@@ -72,6 +75,7 @@ public bool ExecuteStep()
         if (_openList.Count == 0 /*|| _nodes[_openList[0]].HScore <= 0*/)
             return false;
 
+        ++NumSteps;
         int nextNodeIndex = PopMinOpen();
         var (nextNodeX, nextNodeY) = _map.IndexToGrid(nextNodeIndex);
         if (HeapLess(nextNodeIndex, BestIndex))
@@ -115,8 +119,10 @@ public bool IsLeftBetter(ref Node nodeL, ref Map.Pixel pixL, ref Node nodeR, ref
             return IsLeftCloserToTarget(ref nodeL, ref nodeR);
         }
 
-        var improveL = pixL.MaxG > _startMaxG && nodeL.PathMinG <= _startMaxG;
-        var improveR = pixR.MaxG > _startMaxG && nodeR.PathMinG <= _startMaxG;
+        var improveThreshold = _startMaxG + 0.5f;
+        var pathThreshold = _startMaxG - 0.5f;
+        var improveL = pixL.MaxG > improveThreshold && nodeL.PathMinG >= pathThreshold;
+        var improveR = pixR.MaxG > improveThreshold && nodeR.PathMinG >= pathThreshold;
         if (improveL != improveR)
             return improveL; // node that improves initial state (leaves some danger zone without entering even more dangerous ones) is better than a node that doesn't
 
@@ -248,6 +254,13 @@ private void VisitNeighbour(int parentX, int parentY, int parentIndex, int nodeX
 
         if (destNode.OpenHeapIndex == 0 || IsLeftBetter(ref altNode, ref destPix, ref destNode, ref destPix))
         {
+            if (destNode.OpenHeapIndex < 0)
+            {
+                // node was already visited before, ensure that new path is significantly better
+                if (altNode.PathLeeway < destNode.PathLeeway)
+                    return;
+                ++NumReopens;
+            }
             destNode = altNode;
             AddToOpen(nodeIndex);
         }