From edb9b76f9d51045f572f941bfb59c0e99a2c4a57 Mon Sep 17 00:00:00 2001
From: Ellpeck <megamaximal@gmail.com>
Date: Tue, 6 Aug 2019 15:33:45 +0200
Subject: [PATCH] added pathfinding

---
 MLEM.Extended/MLEM.Extended.csproj |  10 ++-
 MLEM/MLEM.csproj                   |   6 +-
 MLEM/Pathfinding/AStar.cs          | 114 +++++++++++++++++++++++++++++
 3 files changed, 125 insertions(+), 5 deletions(-)
 create mode 100644 MLEM/Pathfinding/AStar.cs

diff --git a/MLEM.Extended/MLEM.Extended.csproj b/MLEM.Extended/MLEM.Extended.csproj
index d5e742d..9bee67f 100644
--- a/MLEM.Extended/MLEM.Extended.csproj
+++ b/MLEM.Extended/MLEM.Extended.csproj
@@ -10,11 +10,15 @@
         <PackageProjectUrl>https://github.com/Ellpeck/MLEM</PackageProjectUrl>
         <RepositoryUrl>https://github.com/Ellpeck/MLEM</RepositoryUrl>
         <PackageLicenseUrl>https://github.com/Ellpeck/MLEM/blob/master/LICENSE</PackageLicenseUrl>
-        <Version>1.0.1</Version>
+        <Version>1.0.2</Version>
     </PropertyGroup>
 
     <ItemGroup>
-        <PackageReference Include="MonoGame.Extended" Version="3.6.0-beta0001" PrivateAssets="All" />
-        <PackageReference Include="MonoGame.Framework.Portable" Version="3.7.0.1708" PrivateAssets="All" />
+        <PackageReference Include="MonoGame.Extended" Version="3.6.0-beta0001">
+            <PrivateAssets>all</PrivateAssets>
+        </PackageReference>
+        <PackageReference Include="MonoGame.Framework.Portable" Version="3.7.0.1708">
+            <PrivateAssets>all</PrivateAssets>
+        </PackageReference>
     </ItemGroup>
 </Project>
\ No newline at end of file
diff --git a/MLEM/MLEM.csproj b/MLEM/MLEM.csproj
index 30ca666..85e0002 100644
--- a/MLEM/MLEM.csproj
+++ b/MLEM/MLEM.csproj
@@ -10,10 +10,12 @@
         <PackageProjectUrl>https://github.com/Ellpeck/MLEM</PackageProjectUrl>
         <RepositoryUrl>https://github.com/Ellpeck/MLEM</RepositoryUrl>
         <PackageLicenseUrl>https://github.com/Ellpeck/MLEM/blob/master/LICENSE</PackageLicenseUrl>
-        <Version>1.0.4</Version>
+        <Version>1.0.5</Version>
     </PropertyGroup>
 
     <ItemGroup>
-      <PackageReference Include="MonoGame.Framework.Portable" Version="3.7.0.1708" PrivateAssets="All" />
+        <PackageReference Include="MonoGame.Framework.Portable" Version="3.7.0.1708">
+            <PrivateAssets>all</PrivateAssets>
+        </PackageReference>
     </ItemGroup>
 </Project>
\ No newline at end of file
diff --git a/MLEM/Pathfinding/AStar.cs b/MLEM/Pathfinding/AStar.cs
new file mode 100644
index 0000000..f3923d9
--- /dev/null
+++ b/MLEM/Pathfinding/AStar.cs
@@ -0,0 +1,114 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using Microsoft.Xna.Framework;
+
+namespace MLEM.Pathfinding {
+    public static class AStar {
+
+        private static readonly Point[] AdjacentDirections = {
+            new Point(1, 0),
+            new Point(-1, 0),
+            new Point(0, 1),
+            new Point(0, -1)
+        };
+
+        private static readonly Point[] AllDirections = AdjacentDirections.Concat(new[] {
+            new Point(1, 1),
+            new Point(-1, 1),
+            new Point(1, -1),
+            new Point(-1, -1)
+        }).ToArray();
+
+        public static Stack<Point> FindPath(Point start, Point goal, int defaultCost, GetCost getCost, int maxTries = 10000, bool allowDiagonals = false) {
+            var open = new List<PathPoint>();
+            var closed = new List<PathPoint>();
+            open.Add(new PathPoint(start, goal, null, 0, defaultCost));
+
+            var count = 0;
+            while (open.Count > 0) {
+                PathPoint current = null;
+                var lowestF = int.MaxValue;
+                foreach (var point in open)
+                    if (point.F < lowestF) {
+                        current = point;
+                        lowestF = point.F;
+                    }
+                if (current == null)
+                    break;
+
+                open.Remove(current);
+                closed.Add(current);
+
+                if (current.Pos.Equals(goal))
+                    return CompilePath(current);
+
+                var dirsUsed = allowDiagonals ? AllDirections : AdjacentDirections;
+                foreach (var dir in dirsUsed) {
+                    var neighborPos = current.Pos + dir;
+                    var cost = getCost(neighborPos);
+                    if (cost < int.MaxValue) {
+                        var neighbor = new PathPoint(neighborPos, goal, current, cost, defaultCost);
+                        if (!closed.Contains(neighbor)) {
+                            var alreadyIndex = open.IndexOf(neighbor);
+                            if (alreadyIndex < 0) {
+                                open.Add(neighbor);
+                            } else {
+                                var alreadyNeighbor = open[alreadyIndex];
+                                if (neighbor.G < alreadyNeighbor.G) {
+                                    open.Remove(alreadyNeighbor);
+                                    open.Add(neighbor);
+                                }
+                            }
+                        }
+                    }
+                }
+
+                count++;
+                if (count >= maxTries)
+                    break;
+            }
+            return null;
+        }
+
+        private static Stack<Point> CompilePath(PathPoint current) {
+            var path = new Stack<Point>();
+            while (current != null) {
+                path.Push(current.Pos);
+                current = current.Parent;
+            }
+            return path;
+        }
+
+        public delegate int GetCost(Point pos);
+
+    }
+
+    public class PathPoint {
+
+        public readonly PathPoint Parent;
+        public readonly Point Pos;
+        public readonly int F;
+        public readonly int G;
+
+        public PathPoint(Point pos, Point goal, PathPoint parent, int terrainCostForThisPos, int defaultCost) {
+            this.Pos = pos;
+            this.Parent = parent;
+
+            this.G = (parent == null ? 0 : parent.G) + terrainCostForThisPos;
+            var manhattan = (Math.Abs(goal.X - pos.X) + Math.Abs(goal.Y - pos.Y)) * defaultCost;
+            this.F = this.G + manhattan;
+        }
+
+        public override bool Equals(object obj) {
+            if (obj == this)
+                return true;
+            return obj is PathPoint point && point.Pos.Equals(this.Pos);
+        }
+
+        public override int GetHashCode() {
+            return this.Pos.GetHashCode();
+        }
+
+    }
+}
\ No newline at end of file