From a9593ccb74532147aed78073ddcd005d1cda0ac1 Mon Sep 17 00:00:00 2001 From: Ellpeck Date: Sun, 18 Aug 2019 15:14:35 +0200 Subject: [PATCH] overhauled pathfinding and added a demo for it --- Demos/PathfindingDemo.cs | 97 ++++++++++++++++++++++++++++++++++++++ Demos/Program.cs | 1 + MLEM/Pathfinding/AStar.cs | 93 ++++++++++++++++++++---------------- MLEM/Pathfinding/AStar2.cs | 35 ++++++++++++++ MLEM/Pathfinding/AStar3.cs | 47 ++++++++++++++++++ 5 files changed, 232 insertions(+), 41 deletions(-) create mode 100644 Demos/PathfindingDemo.cs create mode 100644 MLEM/Pathfinding/AStar2.cs create mode 100644 MLEM/Pathfinding/AStar3.cs diff --git a/Demos/PathfindingDemo.cs b/Demos/PathfindingDemo.cs new file mode 100644 index 0000000..e52390b --- /dev/null +++ b/Demos/PathfindingDemo.cs @@ -0,0 +1,97 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using Microsoft.Xna.Framework; +using Microsoft.Xna.Framework.Graphics; +using MLEM.Extensions; +using MLEM.Input; +using MLEM.Pathfinding; +using MLEM.Startup; +using MonoGame.Extended; + +namespace Demos { + public class PathfindingDemo : MlemGame { + + private bool[,] world; + private AStar2 pathfinder; + private List path; + + private void Init() { + // generate a simple random world for testing, where true is walkable area, and false is a wall + var random = new Random(); + this.world = new bool[50, 50]; + for (var x = 0; x < 50; x++) { + for (var y = 0; y < 50; y++) { + if (random.NextDouble() >= 0.25) + this.world[x, y] = true; + } + } + + // Create a cost function, which determines how expensive (or difficult) it should be to move from a given position + // to the next, adjacent position. In our case, the only restriction should be walls and out-of-bounds positions, which + // both have a cost of float.MaxValue, meaning they are completely unwalkable. + // If your game contains harder-to-move-on areas like, say, a muddy pit, you can return a higher cost value for those + // locations. If you want to scale your cost function differently, you can specify a different default cost in your + // pathfinder's constructor + AStar.GetCost cost = (pos, nextPos) => { + if (nextPos.X < 0 || nextPos.Y < 0 || nextPos.X >= 50 || nextPos.Y >= 50) + return float.MaxValue; + return this.world[nextPos.X, nextPos.Y] ? 1 : float.MaxValue; + }; + // Actually initialize the pathfinder with the cost function, as well as specify if moving diagonally between tiles should be + // allowed or not (in this case it's not) + this.pathfinder = new AStar2(cost, false); + + // Now find a path from the top left to the bottom right corner and store it in a variable + // If no path can be found after the maximum amount of tries (10000 by default), the pathfinder will abort and return no path (null) + var foundPath = this.pathfinder.FindPath(Point.Zero, new Point(49, 49)); + this.path = foundPath != null ? foundPath.ToList() : null; + + // print out some info + Console.WriteLine("Pathfinding took " + this.pathfinder.LastTriesNeeded + " tries"); + if (this.path == null) + Console.WriteLine("Couldn't find a path, press the left mouse button to try again"); + } + + protected override void LoadContent() { + base.LoadContent(); + this.Init(); + } + + protected override void Update(GameTime gameTime) { + base.Update(gameTime); + + // when pressing the left mouse button, generate a new world and find a new path + if (Input.IsMouseButtonPressed(MouseButton.Left)) { + this.Init(); + } + } + + protected override void DoDraw(GameTime gameTime) { + this.GraphicsDevice.Clear(Color.White); + + this.SpriteBatch.Begin(SpriteSortMode.Deferred, null, SamplerState.PointClamp, transformMatrix: Matrix.CreateScale(14)); + // draw the world with simple shapes + for (var x = 0; x < 50; x++) { + for (var y = 0; y < 50; y++) { + if (!this.world[x, y]) { + this.SpriteBatch.FillRectangle(new Vector2(x, y), new Size2(1, 1), Color.Black); + } + } + } + // draw the path + // in a real game, you'd obviously make your characters walk along the path instead of drawing it + if (this.path != null) { + for (var i = 1; i < this.path.Count; i++) { + var first = this.path[i - 1]; + var second = this.path[i]; + this.SpriteBatch.DrawLine(new Vector2(first.X + 0.5F, first.Y + 0.5F), new Vector2(second.X + 0.5F, second.Y + 0.5F), Color.Blue, 0.25F); + } + } + this.SpriteBatch.End(); + + base.DoDraw(gameTime); + } + + } +} \ No newline at end of file diff --git a/Demos/Program.cs b/Demos/Program.cs index be70952..50716e3 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -12,6 +12,7 @@ namespace Demos { Demos.Add("Ui", () => new UiDemo()); Demos.Add("AutoTiling", () => new AutoTilingDemo()); Demos.Add("Animation", () => new AnimationDemo()); + Demos.Add("Pathfinding", () => new PathfindingDemo()); } public static void Main(string[] args) { diff --git a/MLEM/Pathfinding/AStar.cs b/MLEM/Pathfinding/AStar.cs index f3923d9..e054006 100644 --- a/MLEM/Pathfinding/AStar.cs +++ b/MLEM/Pathfinding/AStar.cs @@ -1,34 +1,39 @@ -using System; using System.Collections.Generic; -using System.Linq; -using Microsoft.Xna.Framework; namespace MLEM.Pathfinding { - public static class AStar { + public abstract class AStar { - private static readonly Point[] AdjacentDirections = { - new Point(1, 0), - new Point(-1, 0), - new Point(0, 1), - new Point(0, -1) - }; + public readonly T[] AllDirections; + public readonly T[] AdjacentDirections; + public GetCost DefaultCostFunction; + public float DefaultCost; + public int DefaultMaxTries; + public bool DefaultAllowDiagonals; + public int LastTriesNeeded { get; private set; } - 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(); + protected AStar(T[] allDirections, T[] adjacentDirections, GetCost defaultCostFunction, bool defaultAllowDiagonals, float defaultCost = 1, int defaultMaxTries = 10000) { + this.AllDirections = allDirections; + this.AdjacentDirections = adjacentDirections; + this.DefaultCostFunction = defaultCostFunction; + this.DefaultCost = defaultCost; + this.DefaultMaxTries = defaultMaxTries; + this.DefaultAllowDiagonals = defaultAllowDiagonals; + } - public static Stack FindPath(Point start, Point goal, int defaultCost, GetCost getCost, int maxTries = 10000, bool allowDiagonals = false) { - var open = new List(); - var closed = new List(); - open.Add(new PathPoint(start, goal, null, 0, defaultCost)); + public Stack FindPath(T start, T goal, GetCost costFunction = null, float? defaultCost = null, int? maxTries = null, bool? allowDiagonals = null) { + var getCost = costFunction ?? this.DefaultCostFunction; + var diags = allowDiagonals ?? this.DefaultAllowDiagonals; + var tries = maxTries ?? this.DefaultMaxTries; + var defCost = defaultCost ?? this.DefaultCost; + + var open = new List>(); + var closed = new List>(); + open.Add(new PathPoint(start, this.GetManhattanDistance(start, goal), null, 0, defCost)); var count = 0; while (open.Count > 0) { - PathPoint current = null; - var lowestF = int.MaxValue; + PathPoint current = null; + var lowestF = float.MaxValue; foreach (var point in open) if (point.F < lowestF) { current = point; @@ -40,15 +45,17 @@ namespace MLEM.Pathfinding { open.Remove(current); closed.Add(current); - if (current.Pos.Equals(goal)) + if (current.Pos.Equals(goal)) { + this.LastTriesNeeded = count; return CompilePath(current); + } - var dirsUsed = allowDiagonals ? AllDirections : AdjacentDirections; + var dirsUsed = diags ? this.AllDirections : this.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); + var neighborPos = this.AddPositions(current.Pos, dir); + var cost = getCost(current.Pos, neighborPos); + if (cost < float.MaxValue) { + var neighbor = new PathPoint(neighborPos, this.GetManhattanDistance(neighborPos, goal), current, cost, defCost); if (!closed.Contains(neighbor)) { var alreadyIndex = open.IndexOf(neighbor); if (alreadyIndex < 0) { @@ -65,14 +72,19 @@ namespace MLEM.Pathfinding { } count++; - if (count >= maxTries) + if (count >= tries) break; } + this.LastTriesNeeded = count; return null; } - private static Stack CompilePath(PathPoint current) { - var path = new Stack(); + protected abstract T AddPositions(T first, T second); + + protected abstract float GetManhattanDistance(T first, T second); + + private static Stack CompilePath(PathPoint current) { + var path = new Stack(); while (current != null) { path.Push(current.Pos); current = current.Parent; @@ -80,30 +92,29 @@ namespace MLEM.Pathfinding { return path; } - public delegate int GetCost(Point pos); + public delegate float GetCost(T currPos, T nextPos); } - public class PathPoint { + public class PathPoint { - public readonly PathPoint Parent; - public readonly Point Pos; - public readonly int F; - public readonly int G; + public readonly PathPoint Parent; + public readonly T Pos; + public readonly float F; + public readonly float G; - public PathPoint(Point pos, Point goal, PathPoint parent, int terrainCostForThisPos, int defaultCost) { + public PathPoint(T pos, float distance, PathPoint parent, float terrainCostForThisPos, float 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; + this.F = this.G + distance * defaultCost; } public override bool Equals(object obj) { if (obj == this) return true; - return obj is PathPoint point && point.Pos.Equals(this.Pos); + return obj is PathPoint point && point.Pos.Equals(this.Pos); } public override int GetHashCode() { diff --git a/MLEM/Pathfinding/AStar2.cs b/MLEM/Pathfinding/AStar2.cs new file mode 100644 index 0000000..0122c76 --- /dev/null +++ b/MLEM/Pathfinding/AStar2.cs @@ -0,0 +1,35 @@ +using System; +using System.Linq; +using Microsoft.Xna.Framework; + +namespace MLEM.Pathfinding { + public class AStar2 : AStar { + + private static readonly Point[] AdjacentDirs = { + new Point(1, 0), + new Point(-1, 0), + new Point(0, 1), + new Point(0, -1) + }; + + private static readonly Point[] AllDirs = AdjacentDirs.Concat(new[] { + new Point(1, 1), + new Point(-1, 1), + new Point(1, -1), + new Point(-1, -1) + }).ToArray(); + + public AStar2(GetCost defaultCostFunction, bool defaultAllowDiagonals, float defaultCost = 1, int defaultMaxTries = 10000) : + base(AllDirs, AdjacentDirs, defaultCostFunction, defaultAllowDiagonals, defaultCost, defaultMaxTries) { + } + + protected override Point AddPositions(Point first, Point second) { + return first + second; + } + + protected override float GetManhattanDistance(Point first, Point second) { + return Math.Abs(second.X - first.X) + Math.Abs(second.Y - first.Y); + } + + } +} \ No newline at end of file diff --git a/MLEM/Pathfinding/AStar3.cs b/MLEM/Pathfinding/AStar3.cs new file mode 100644 index 0000000..b1de642 --- /dev/null +++ b/MLEM/Pathfinding/AStar3.cs @@ -0,0 +1,47 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using Microsoft.Xna.Framework; + +namespace MLEM.Pathfinding { + public class AStar3 : AStar { + + private static readonly Vector3[] AdjacentDirs = { + new Vector3(1, 0, 0), + new Vector3(-1, 0, 0), + new Vector3(0, 1, 0), + new Vector3(0, -1, 0), + new Vector3(0, 0, 1), + new Vector3(0, 0, -1) + }; + + private static readonly Vector3[] AllDirs; + + static AStar3() { + var dirs = new List(); + for (var x = -1; x <= 1; x++) { + for (var y = -1; y <= 1; y++) { + for (var z = -1; z <= 1; z++) { + if (x == 0 && y == 0 && z == 0) + continue; + dirs.Add(new Vector3(x, y, z)); + } + } + } + AllDirs = dirs.ToArray(); + } + + public AStar3(GetCost defaultCostFunction, bool defaultAllowDiagonals, float defaultCost = 1, int defaultMaxTries = 10000) : + base(AllDirs, AdjacentDirs, defaultCostFunction, defaultAllowDiagonals, defaultCost, defaultMaxTries) { + } + + protected override Vector3 AddPositions(Vector3 first, Vector3 second) { + return first + second; + } + + protected override float GetManhattanDistance(Vector3 first, Vector3 second) { + return Math.Abs(second.X - first.X) + Math.Abs(second.Y - first.Y) + Math.Abs(second.Z - first.Z); + } + + } +} \ No newline at end of file