Added options to use the closest position to find a path when the end…

… can't be reached.
valantonini · Sep 2, 2021 · d218e7f · d218e7f
1 parent 186d565
commit d218e7f
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 2 deletions.
diff --git a/AStar/Options/PathFinderOptions.cs b/AStar/Options/PathFinderOptions.cs
@@ -12,6 +12,8 @@ public class PathFinderOptions
 
         public int SearchLimit { get; set; }
 
+        public bool ClosestNodeWhenCantReach { get; set; }
+
         public PathFinderOptions()
         {
             HeuristicFormula = HeuristicFormula.Manhattan;

diff --git a/AStar/PathFinder.cs b/AStar/PathFinder.cs
@@ -36,6 +36,8 @@ public Position[] FindPath(Position start, Position end)
         {
             var nodesVisited = 0;
             IModelAGraph<PathFinderNode> graph = new PathFinderGraph(_world.Height, _world.Width, _options.UseDiagonals);
+            var lowestNode = start;
+            var lowestNodeDist = int.MaxValue;
 
             var startNode = new PathFinderNode(position: start, g: 0, h: 2, parentNodePosition: start);
             graph.OpenNode(startNode);
@@ -62,6 +64,13 @@ public Position[] FindPath(Position start, Position end)
                     }
 
                     var newG = q.G + DistanceBetweenNodes;
+                    var distanceBetweenSuccessorAndEnd = this.CalculateDistance(q.Position, end);
+
+                    if (distanceBetweenSuccessorAndEnd < lowestNodeDist)
+                    {
+                        lowestNode = q.Position;
+                        lowestNodeDist = distanceBetweenSuccessorAndEnd;
+                    }
 
                     if (_options.PunishChangeDirection)
                     {
@@ -72,7 +81,7 @@ public Position[] FindPath(Position start, Position end)
                         {
                             if (qIsHorizontallyAdjacent)
                             {
-                                newG += Math.Abs(successor.Position.Row - end.Row) + Math.Abs(successor.Position.Column - end.Column);
+                                newG += this.CalculateDistance(successor.Position, end);
                             }
                         }
 
@@ -81,7 +90,7 @@ public Position[] FindPath(Position start, Position end)
                         {
                             if (!qIsHorizontallyAdjacent)
                             {
-                                newG += Math.Abs(successor.Position.Row - end.Row) + Math.Abs(successor.Position.Column - end.Column);
+                                newG += this.CalculateDistance(successor.Position, end);
                             }
                         }
                     }
@@ -101,9 +110,19 @@ public Position[] FindPath(Position start, Position end)
                 nodesVisited++;
             }
 
+            if (_options.ClosestNodeWhenCantReach && lowestNode != start)
+            {
+                return this.FindPath(start, lowestNode);
+            }
+
             return new Position[0];
         }
 
+        private int CalculateDistance(Position currentNode, Position endNode)
+        {             
+            return Math.Abs(currentNode.Row - endNode.Row) + Math.Abs(currentNode.Column - endNode.Column);
+        }
+
         private bool BetterPathToSuccessorFound(PathFinderNode updateSuccessor, PathFinderNode currentSuccessor)
         {
             return !currentSuccessor.HasBeenVisited ||