├── Docs ├── PathingExample.png └── PathingExample.drawio ├── AStar ├── Heuristics │ ├── ICalculateHeuristic.cs │ ├── HeuristicFormula.cs │ ├── Manhattan.cs │ ├── MaxDXDY.cs │ ├── Euclidean.cs │ ├── EuclideanNoSQR.cs │ ├── Custom1.cs │ ├── DiagonalShortcut.cs │ └── HeuristicFactory.cs ├── Collections │ ├── PriorityQueue │ │ ├── IModelAPriorityQueue.cs │ │ └── SimplePriorityQueue.cs │ ├── PathFinder │ │ ├── IModelAGraph.cs │ │ ├── ComparePathFinderNodeByFValue.cs │ │ ├── PathFinderNode.cs │ │ └── PathFinderGraph.cs │ └── MultiDimensional │ │ ├── IModelAGrid.cs │ │ ├── GridOffsets.cs │ │ └── Grid.cs ├── PositionExtensions.cs ├── AStar.csproj ├── IFindAPath.cs ├── Options │ └── PathFinderOptions.cs ├── WorldGrid.cs ├── Position.cs └── PathFinder.cs ├── AStar.sln.DotSettings ├── AStar.Tests ├── AStar.Tests.csproj ├── PointPathingTest.cs ├── WeightedPathingTests.cs ├── OptionsTest.cs ├── PathingTests.cs ├── PunishChangeDirectionIssueTest.cs ├── PunishChangeDirectionTests.cs ├── GridTests.cs ├── Helper.cs ├── PathfinderTests.cs └── LongerPathingTests.cs ├── .github └── workflows │ └── main.yml ├── LICENSE ├── AStar.sln ├── README.md └── .gitignore /Docs/PathingExample.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/valantonini/AStar/HEAD/Docs/PathingExample.png -------------------------------------------------------------------------------- /AStar/Heuristics/ICalculateHeuristic.cs: -------------------------------------------------------------------------------- 1 | namespace AStar.Heuristics 2 | { 3 | public interface ICalculateHeuristic 4 | { 5 | int Calculate(Position source, Position destination); 6 | } 7 | } -------------------------------------------------------------------------------- /AStar/Heuristics/HeuristicFormula.cs: -------------------------------------------------------------------------------- 1 | namespace AStar.Heuristics 2 | { 3 | public enum HeuristicFormula 4 | { 5 | Manhattan = 1, 6 | MaxDXDY = 2, 7 | DiagonalShortCut = 3, 8 | Euclidean = 4, 9 | EuclideanNoSQR = 5, 10 | Custom1 = 6 11 | } 12 | } -------------------------------------------------------------------------------- /AStar/Collections/PriorityQueue/IModelAPriorityQueue.cs: -------------------------------------------------------------------------------- 1 | namespace AStar.Collections.PriorityQueue 2 | { 3 | internal interface IModelAPriorityQueue 4 | { 5 | int Push(T item); 6 | T Pop(); 7 | T Peek(); 8 | 9 | void Clear(); 10 | int Count { get; } 11 | } 12 | } 13 | -------------------------------------------------------------------------------- /AStar/Collections/PathFinder/IModelAGraph.cs: -------------------------------------------------------------------------------- 1 | using System.Collections.Generic; 2 | 3 | namespace AStar.Collections.PathFinder 4 | { 5 | internal interface IModelAGraph 6 | { 7 | bool HasOpenNodes { get; } 8 | IEnumerable GetSuccessors(T node); 9 | T GetParent(T node); 10 | void OpenNode(T node); 11 | T GetOpenNodeWithSmallestF(); 12 | } 13 | } -------------------------------------------------------------------------------- /AStar.sln.DotSettings: -------------------------------------------------------------------------------- 1 | 2 | True -------------------------------------------------------------------------------- /AStar/Heuristics/Manhattan.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | 3 | namespace AStar.Heuristics 4 | { 5 | public class Manhattan : ICalculateHeuristic 6 | { 7 | public int Calculate(Position source, Position destination) 8 | { 9 | var heuristicEstimate = 2; 10 | var h = heuristicEstimate * (Math.Abs(source.Row - destination.Row) + Math.Abs(source.Column - destination.Column)); 11 | return h; 12 | } 13 | } 14 | } -------------------------------------------------------------------------------- /AStar/Heuristics/MaxDXDY.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | 3 | namespace AStar.Heuristics 4 | { 5 | public class MaxDXDY : ICalculateHeuristic 6 | { 7 | public int Calculate(Position source, Position destination) 8 | { 9 | var heuristicEstimate = 2; 10 | var h = heuristicEstimate * (Math.Max(Math.Abs(source.Row - destination.Row), Math.Abs(source.Column - destination.Column))); 11 | return h; 12 | } 13 | } 14 | } -------------------------------------------------------------------------------- /AStar/PositionExtensions.cs: -------------------------------------------------------------------------------- 1 | using System.Drawing; 2 | 3 | namespace AStar 4 | { 5 | public static class PositionExtensions 6 | { 7 | public static Point ToPoint(this Position position) 8 | { 9 | return new Point(position.Column, position.Row); 10 | } 11 | 12 | public static Position ToPosition(this Point point) 13 | { 14 | return new Position(point.Y, point.X); 15 | } 16 | } 17 | } -------------------------------------------------------------------------------- /AStar/Collections/MultiDimensional/IModelAGrid.cs: -------------------------------------------------------------------------------- 1 | using System.Collections.Generic; 2 | 3 | namespace AStar.Collections.MultiDimensional 4 | { 5 | public interface IModelAGrid 6 | { 7 | int Height { get; } 8 | int Width { get; } 9 | T this[int row, int column] { get; set; } 10 | T this[Position position] { get; set; } 11 | IEnumerable GetSuccessorPositions(Position node, bool optionsUseDiagonals = false); 12 | } 13 | } -------------------------------------------------------------------------------- /AStar/Heuristics/Euclidean.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | 3 | namespace AStar.Heuristics 4 | { 5 | public class Euclidean : ICalculateHeuristic 6 | { 7 | public int Calculate(Position source, Position destination) 8 | { 9 | var heuristicEstimate = 2; 10 | var h = (int)(heuristicEstimate * Math.Sqrt(Math.Pow((source.Row - destination.Row), 2) + Math.Pow((source.Column - destination.Column), 2))); 11 | return h; 12 | } 13 | } 14 | } -------------------------------------------------------------------------------- /AStar/Heuristics/EuclideanNoSQR.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | 3 | namespace AStar.Heuristics 4 | { 5 | public class EuclideanNoSQR : ICalculateHeuristic 6 | { 7 | public int Calculate(Position source, Position destination) 8 | { 9 | var heuristicEstimate = 2; 10 | var h = (int)(heuristicEstimate * (Math.Pow((source.Row - destination.Row), 2) + Math.Pow((source.Column - destination.Column), 2))); 11 | return h; 12 | } 13 | } 14 | } -------------------------------------------------------------------------------- /AStar/Collections/PathFinder/ComparePathFinderNodeByFValue.cs: -------------------------------------------------------------------------------- 1 | using System.Collections.Generic; 2 | 3 | namespace AStar.Collections.PathFinder 4 | { 5 | internal class ComparePathFinderNodeByFValue : IComparer 6 | { 7 | public int Compare(PathFinderNode a, PathFinderNode b) 8 | { 9 | if (a.F > b.F) 10 | { 11 | return 1; 12 | } 13 | 14 | if (a.F < b.F) 15 | { 16 | return -1; 17 | } 18 | 19 | return 0; 20 | } 21 | } 22 | } -------------------------------------------------------------------------------- /AStar/Heuristics/Custom1.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | 3 | namespace AStar.Heuristics 4 | { 5 | public class Custom1 : ICalculateHeuristic 6 | { 7 | public int Calculate(Position source, Position destination) 8 | { 9 | var heuristicEstimate = 2; 10 | var dxy = new Position(Math.Abs(destination.Row - source.Row), Math.Abs(destination.Column - source.Column)); 11 | var Orthogonal = Math.Abs(dxy.Row - dxy.Column); 12 | var Diagonal = Math.Abs(((dxy.Row + dxy.Column) - Orthogonal) / 2); 13 | var h = heuristicEstimate * (Diagonal + Orthogonal + dxy.Row + dxy.Column); 14 | return h; 15 | } 16 | } 17 | } -------------------------------------------------------------------------------- /AStar.Tests/AStar.Tests.csproj: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | net9.0 5 | false 6 | latest 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /AStar/Heuristics/DiagonalShortcut.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | 3 | namespace AStar.Heuristics 4 | { 5 | public class DiagonalShortcut : ICalculateHeuristic 6 | { 7 | public int Calculate(Position source, Position destination) 8 | { 9 | var hDiagonal = Math.Min(Math.Abs(source.Row - destination.Row), 10 | Math.Abs(source.Column - destination.Column)); 11 | var hStraight = (Math.Abs(source.Row - destination.Row) + Math.Abs(source.Column - destination.Column)); 12 | var heuristicEstimate = 2; 13 | var h = (heuristicEstimate * 2) * hDiagonal + heuristicEstimate * (hStraight - 2 * hDiagonal); 14 | return h; 15 | } 16 | } 17 | } -------------------------------------------------------------------------------- /AStar/AStar.csproj: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | AStarLite 5 | AStar Lite 6 | Val Antonini 7 | https://github.com/valantonini/AStar/blob/master/LICENSE 8 | https://github.com/valantonini/AStar 9 | https://github.com/valantonini/AStar 10 | false 11 | A lightweight, 2D A* (A Star) algorithm. 12 | Pathfinding;A*;AStar;Algorithms 13 | AStar 14 | latest 15 | netstandard2.0;net5.0;net6.0;net7.0;net8.0;net9.0 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /AStar/IFindAPath.cs: -------------------------------------------------------------------------------- 1 | using System.Drawing; 2 | 3 | namespace AStar 4 | { 5 | public interface IFindAPath 6 | { 7 | /// 8 | /// Determines a path between 2 positions 9 | /// 10 | /// start/current position 11 | /// target position 12 | /// An array of positions from the start to end position or empty[] if unreachable 13 | Position[] FindPath(Position start, Position end); 14 | 15 | /// 16 | /// Determines a path between 2 positions where the point's X 17 | /// represents the column and the point's Y represents the row 18 | /// 19 | /// start position 20 | /// target position 21 | /// An array of points from the start to end points or empty[] if unreachable 22 | Point[] FindPath(Point start, Point end); 23 | } 24 | } 25 | -------------------------------------------------------------------------------- /AStar/Options/PathFinderOptions.cs: -------------------------------------------------------------------------------- 1 | using AStar.Heuristics; 2 | 3 | namespace AStar.Options 4 | { 5 | public class PathFinderOptions 6 | { 7 | public HeuristicFormula HeuristicFormula { get; set; } 8 | 9 | public bool UseDiagonals { get; set; } 10 | 11 | public bool PunishChangeDirection { get; set; } 12 | 13 | public int SearchLimit { get; set; } 14 | 15 | public bool IgnoreClosedEndCell { get; set; } 16 | 17 | public Weighting Weighting {get;set;} 18 | 19 | public PathFinderOptions() 20 | { 21 | HeuristicFormula = HeuristicFormula.Manhattan; 22 | UseDiagonals = true; 23 | SearchLimit = 2000; 24 | } 25 | } 26 | 27 | public enum Weighting { 28 | // The number in the grid will not influence the path. 29 | None, 30 | // Higher open values will be favoured and applied to the new h value. 31 | Positive, 32 | // Lower open values will be favoured and applied to the new h value. 33 | Negative 34 | } 35 | } 36 | -------------------------------------------------------------------------------- /AStar/Heuristics/HeuristicFactory.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | 3 | namespace AStar.Heuristics 4 | { 5 | public static class HeuristicFactory 6 | { 7 | public static ICalculateHeuristic Create(HeuristicFormula heuristicFormula) 8 | { 9 | switch (heuristicFormula) 10 | { 11 | case HeuristicFormula.Manhattan: 12 | return new Manhattan(); 13 | case HeuristicFormula.MaxDXDY: 14 | return new MaxDXDY(); 15 | case HeuristicFormula.DiagonalShortCut: 16 | return new DiagonalShortcut(); 17 | case HeuristicFormula.Euclidean: 18 | return new Euclidean(); 19 | case HeuristicFormula.EuclideanNoSQR: 20 | return new EuclideanNoSQR(); 21 | case HeuristicFormula.Custom1: 22 | return new Custom1(); 23 | default: 24 | throw new ArgumentOutOfRangeException(nameof(heuristicFormula), heuristicFormula, null); 25 | } 26 | } 27 | } 28 | } -------------------------------------------------------------------------------- /.github/workflows/main.yml: -------------------------------------------------------------------------------- 1 | name: .NET Core CI 2 | 3 | on: [push] 4 | 5 | jobs: 6 | build: 7 | runs-on: ubuntu-latest 8 | 9 | steps: 10 | - uses: actions/checkout@v2 11 | 12 | - name: Setup .NET Core 13 | uses: actions/setup-dotnet@v1 14 | with: 15 | dotnet-version: 9.0.x 16 | 17 | - name: Restore dependencies 18 | run: dotnet restore 19 | 20 | - name: Build 21 | run: dotnet build --no-restore 22 | 23 | - name: Test 24 | run: dotnet test --no-build --verbosity normal 25 | 26 | package: 27 | needs: build 28 | if: startsWith(github.ref, 'refs/tags/v') 29 | runs-on: ubuntu-latest 30 | 31 | steps: 32 | - uses: actions/checkout@v2 33 | 34 | - name: Setup .NET Core 35 | uses: actions/setup-dotnet@v1 36 | with: 37 | dotnet-version: 9.0.x 38 | 39 | - name: Pack 40 | run: dotnet pack -c Release /p:PackageVersion=${GITHUB_REF#refs/tags/v} 41 | 42 | - name: Push 43 | run: dotnet nuget push AStar/bin/Release/AStarLite.${GITHUB_REF#refs/tags/v}.nupkg -s https://www.nuget.org/api/v2/package -k ${{ secrets.NUGET_API_KEY }} 44 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 Val Antonini 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /AStar.Tests/PointPathingTest.cs: -------------------------------------------------------------------------------- 1 | using System.Drawing; 2 | using AStar.Options; 3 | using NUnit.Framework; 4 | using Shouldly; 5 | 6 | namespace AStar.Tests 7 | { 8 | [TestFixture] 9 | public class PointPathingTests 10 | { 11 | private WorldGrid _world; 12 | 13 | [SetUp] 14 | public void SetUp() 15 | { 16 | var level = @"XXXXXXX 17 | X11X11X 18 | X11111X 19 | XXXXXXX"; 20 | 21 | _world = Helper.ConvertStringToPathfinderGrid(level); 22 | } 23 | 24 | [Test] 25 | public void ShouldPathPredictablyByPoint() 26 | { 27 | var pathfinder = new PathFinder(_world, new PathFinderOptions { UseDiagonals = false }); 28 | 29 | var path = pathfinder.FindPath(new Point(1, 1), new Point(5, 1)); 30 | 31 | path.ShouldBe(new[] { 32 | new Point(1, 1), 33 | new Point(2, 1), 34 | new Point(2, 2), 35 | new Point(3, 2), 36 | new Point(4, 2), 37 | new Point(5, 2), 38 | new Point(5, 1), 39 | }); 40 | } 41 | } 42 | } -------------------------------------------------------------------------------- /Docs/PathingExample.drawio: -------------------------------------------------------------------------------- 1 | 7Vnfb9owEP5reASRmNDwWChsUztpaidNfULGMYmHk4scp8D++tmJHUh/AJVAoQgeIPf5Yp/vu7tcTAuN4tU3gdPoJwSUt9xusGqhu5brDnp99a2BdQl4qFcCoWBBCTkb4In9owbsGjRnAc1qihKAS5bWQQJJQomsYVgIWNbV5sDrq6Y4pG+AJ4L5W/QPC2RUor57s8G/UxZGdmWnPyhHYmyVzU6yCAew3ILQuIVGAkCWV/FqRLn2nfVLed/kg9HKMEETecgN0+WgjR5n4/Hk9/Piefa0fszv2245ywvmudmwMVaurQdeqJBMOeQBzyj/BRmTDBI1NAMpIW6hYbWxrhICnEU0MALmLNSqRJlIhQIiGXMlO+rSTntrdCSkei4pYFF52ilnT7Ul8SrUAdaJgSzytFMIjGSdjMUppz+ItmmYLagkkVl9zjgfAQdR7AMFHvWDXrWGHUkgUdMPjR+UUXT1oYOdijYV7hRiKsVaqZgbkI1ZE+qOb+TlJnA8A0VbMWMxbEI1rGbesKkuDKGfIBddGLk13hSjM9/ree8wPfcJJeR1OByDYd87L4Z7F8Zwk+nb659Z+vYvjNyzS1/X85pl2DK6i2IaqH7EiCBkBCEkmI836FBAngQVrxudB9C0FUT9pVKuTXOFcwl1uss19UK7HavsglwQumNHvmnRsAip3KHndN9nSlCOJXupG3J0v/sXllnn1PUg1HDZHFzJPR65rypm4+S6B3Q8X6ti2vTZXzIHTZZMa+Y1rU7QajaeVs4BRdO6l8XFkcl+hvaGA9cDQ0wWYZGQ2y1g8VEqxWK3WVoe7RSRYoU5W+nwGRp77iIp9ZnQrfaEOyFB4naY4nrOVKqLDlErupMAS6x+NJ7pX8jaN20rae9OlDrLoumc47DtuH4nTcLXgbsrdCrDIcWESc0vOlL3enbvJ3YLl1OL3UNrsd9kKXYPKMVfLlnR3mQNKV3oI62J7gEmdxDD9D5PpiTCAhP10DhSuvaP9bJ5bqeBN9cH+Mke4I2fJHhXck/2RntCcpW4+QOoGNv6Fw2N/wM= -------------------------------------------------------------------------------- /AStar/Collections/PathFinder/PathFinderNode.cs: -------------------------------------------------------------------------------- 1 | using System.Runtime.InteropServices; 2 | 3 | namespace AStar.Collections.PathFinder 4 | { 5 | [StructLayout(LayoutKind.Sequential, Pack = 1)] 6 | internal readonly struct PathFinderNode 7 | { 8 | /// 9 | /// The position of the node 10 | /// 11 | public Position Position { get; } 12 | 13 | /// 14 | /// Distance from home 15 | /// 16 | public int G { get; } 17 | 18 | /// 19 | /// Heuristic 20 | /// 21 | public int H { get; } 22 | 23 | /// 24 | /// This nodes parent 25 | /// 26 | public Position ParentNodePosition { get; } 27 | 28 | /// 29 | /// Gone + Heuristic (H) 30 | /// 31 | public int F { get; } 32 | 33 | /// 34 | /// If the node has been considered yet 35 | /// 36 | public bool HasBeenVisited => F > 0; 37 | 38 | public PathFinderNode(Position position, int g, int h, Position parentNodePosition) 39 | { 40 | Position = position; 41 | G = g; 42 | H = h; 43 | ParentNodePosition = parentNodePosition; 44 | 45 | F = g + h; 46 | } 47 | } 48 | } 49 | -------------------------------------------------------------------------------- /AStar/WorldGrid.cs: -------------------------------------------------------------------------------- 1 | using AStar.Collections.MultiDimensional; 2 | 3 | namespace AStar 4 | { 5 | /// 6 | /// A world grid consisting of integers where a closed cell is represented by 0 7 | /// 8 | public class WorldGrid : Grid 9 | { 10 | /// 11 | /// Creates a new world with the given dimensions initialised to closed 12 | /// 13 | /// height of the world (Position.Row / Point.Y) 14 | /// width of the world (Position.Column / Point.X) 15 | public WorldGrid(int height, int width) : base(height, width) 16 | { 17 | } 18 | 19 | /// 20 | /// Creates a new world with values set from the provided 2d array. 21 | /// Height will be first dimension, and Width will be the second, 22 | /// e.g [4,2] will have a height of 4 and a width of 2. 23 | /// 24 | /// A 2 dimensional array of short where 0 indicates a closed node 25 | public WorldGrid(short[,] worldArray) : base(worldArray.GetLength(0), worldArray.GetLength(1)) 26 | { 27 | for (var row = 0; row < worldArray.GetLength(0); row++) 28 | { 29 | for (var column = 0; column < worldArray.GetLength(1); column++) 30 | { 31 | this[row, column] = worldArray[row, column]; 32 | } 33 | } 34 | } 35 | } 36 | } -------------------------------------------------------------------------------- /AStar/Collections/MultiDimensional/GridOffsets.cs: -------------------------------------------------------------------------------- 1 | using System.Collections.Generic; 2 | using System.Linq; 3 | 4 | namespace AStar.Collections.MultiDimensional 5 | { 6 | public static class GridOffsets 7 | { 8 | private static IEnumerable<(sbyte row, sbyte column)> CardinalDirectionOffsets 9 | { 10 | get 11 | { 12 | yield return (0, -1); 13 | yield return (1, 0); 14 | yield return (0, 1); 15 | yield return (-1, 0); 16 | } 17 | } 18 | 19 | private static IEnumerable<(sbyte row, sbyte column)> DiagonalsOffsets 20 | { 21 | get 22 | { 23 | yield return (1, -1); 24 | yield return (1, 1); 25 | yield return (-1, 1); 26 | yield return (-1, -1); 27 | } 28 | } 29 | 30 | public static IEnumerable<(sbyte row, sbyte column)> GetOffsets(bool withDiagonals = false) 31 | { 32 | return withDiagonals 33 | ? CardinalDirectionOffsets.Concat(DiagonalsOffsets) 34 | : CardinalDirectionOffsets; 35 | } 36 | 37 | public static bool IsCardinalOffset((sbyte row, sbyte column) offset) 38 | { 39 | return offset.row != 0 && offset.column != 0; 40 | } 41 | 42 | public static bool IsDiagonal((sbyte row, sbyte column) offset) 43 | { 44 | return offset.row != 0 || offset.column != 0; 45 | } 46 | } 47 | } -------------------------------------------------------------------------------- /AStar.Tests/WeightedPathingTests.cs: -------------------------------------------------------------------------------- 1 | using AStar.Options; 2 | using NUnit.Framework; 3 | using Shouldly; 4 | 5 | namespace AStar.Tests 6 | { 7 | [TestFixture] 8 | public class WeightedPathingTests 9 | { 10 | [Test] 11 | public void ShouldPathWithWeight() 12 | { 13 | var level = @"1111115 14 | 1511151 15 | 1155511 16 | 1111111"; 17 | var world = Helper.ConvertStringToPathfinderGrid(level); 18 | var opts = new PathFinderOptions { Weighting = Weighting.Positive }; 19 | var pathfinder = new PathFinder(world, opts); 20 | 21 | var path = pathfinder.FindPath(new Position(1, 1), new Position(1, 5)); 22 | 23 | path.ShouldBe(new[] { 24 | new Position(1, 1), 25 | new Position(2, 2), 26 | new Position(2, 3), 27 | new Position(2, 4), 28 | new Position(1, 5), 29 | }); 30 | } 31 | 32 | [Test] 33 | public void ShouldPathWithInvertedWeight() 34 | { 35 | var level = @"9999995 36 | 9599959 37 | 9955599 38 | 9999999"; 39 | 40 | var world = Helper.ConvertStringToPathfinderGrid(level); 41 | var opts = new PathFinderOptions { Weighting = Weighting.Negative }; 42 | var pathfinder = new PathFinder(world, opts); 43 | 44 | var path = pathfinder.FindPath(new Position(1, 1), new Position(1, 5)); 45 | 46 | path.ShouldBe(new[] { 47 | new Position(1, 1), 48 | new Position(2, 2), 49 | new Position(2, 3), 50 | new Position(2, 4), 51 | new Position(1, 5), 52 | }); 53 | } 54 | } 55 | } 56 | -------------------------------------------------------------------------------- /AStar/Position.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | 3 | namespace AStar 4 | { 5 | /// 6 | /// A point in a matrix. P(row, column) 7 | /// 8 | public readonly struct Position 9 | { 10 | /// 11 | /// The row in the matrix 12 | /// 13 | public int Row { get; } 14 | 15 | /// 16 | /// The column in the matrix 17 | /// 18 | public int Column { get; } 19 | 20 | public Position(int row = 0, int column = 0) 21 | { 22 | Row = row; 23 | Column = column; 24 | } 25 | 26 | public bool IsDiagonalTo(Position other) 27 | { 28 | // return Row - other.Row != 0 || 29 | // Column - other.Column != 0; 30 | 31 | return Row != other.Row && 32 | Column != other.Column; 33 | } 34 | 35 | public static bool operator ==(Position a, Position b) 36 | { 37 | return a.Equals(b); 38 | } 39 | 40 | public static bool operator !=(Position a, Position b) 41 | { 42 | return !a.Equals(b); 43 | } 44 | 45 | public override bool Equals(Object other) 46 | { 47 | if (other is Position otherPoint) 48 | { 49 | return Row == otherPoint.Row && Column == otherPoint.Column; 50 | } 51 | 52 | return false; 53 | } 54 | 55 | public override int GetHashCode() 56 | { 57 | unchecked 58 | { 59 | var hash = 17; 60 | hash = hash * 23 + Row.GetHashCode(); 61 | hash = hash * 23 + Column.GetHashCode(); 62 | return hash; 63 | } 64 | } 65 | 66 | public override string ToString() 67 | { 68 | return $"[{Row}.{Column}]"; 69 | } 70 | } 71 | } -------------------------------------------------------------------------------- /AStar.Tests/OptionsTest.cs: -------------------------------------------------------------------------------- 1 | using AStar.Options; 2 | using NUnit.Framework; 3 | using Shouldly; 4 | 5 | namespace AStar.Tests 6 | { 7 | [TestFixture] 8 | public class OptionsTest 9 | { 10 | private WorldGrid _world; 11 | 12 | [SetUp] 13 | public void SetUp() 14 | { 15 | var level = @"XXXXXXX 16 | X11X11X 17 | X11111X 18 | XXXXXXX"; 19 | 20 | _world = Helper.ConvertStringToPathfinderGrid(level); 21 | } 22 | 23 | [Test] 24 | public void ShouldEnforceSearchLimit() 25 | { 26 | var pathfinder = new PathFinder(_world, new PathFinderOptions { SearchLimit = 2 }); 27 | 28 | var path = pathfinder.FindPath(new Position(1, 1), new Position(1, 5)); 29 | 30 | path.ShouldBeEmpty(); 31 | } 32 | 33 | [Test] 34 | public void ShouldStartOnClosed() 35 | { 36 | var pathfinder = new PathFinder(_world, new PathFinderOptions { UseDiagonals = false }); 37 | 38 | var path = pathfinder.FindPath(new Position(0, 1), new Position(2, 3)); 39 | 40 | path.ShouldBe(new[] { 41 | new Position(0, 1), 42 | new Position(1, 1), 43 | new Position(2, 1), 44 | new Position(2, 2), 45 | new Position(2, 3), 46 | }); 47 | } 48 | 49 | [Test] 50 | public void ShouldEndOnClosed() 51 | { 52 | var pathfinder = new PathFinder(_world, new PathFinderOptions { IgnoreClosedEndCell = true }); 53 | 54 | var path = pathfinder.FindPath(new Position(1, 0), new Position(1, 3)); 55 | 56 | path.ShouldBe(new[] { 57 | new Position(1, 0), 58 | new Position(1, 1), 59 | new Position(1, 2), 60 | new Position(1, 3), 61 | }); 62 | } 63 | } 64 | } 65 | -------------------------------------------------------------------------------- /AStar.Tests/PathingTests.cs: -------------------------------------------------------------------------------- 1 | using AStar.Options; 2 | using NUnit.Framework; 3 | using Shouldly; 4 | 5 | namespace AStar.Tests 6 | { 7 | [TestFixture] 8 | public class PathingTests 9 | { 10 | private WorldGrid _world; 11 | 12 | [SetUp] 13 | public void SetUp() 14 | { 15 | var level = @"XXXXXXX 16 | X11X11X 17 | X11111X 18 | XXXXXXX"; 19 | 20 | _world = Helper.ConvertStringToPathfinderGrid(level); 21 | } 22 | 23 | [Test] 24 | public void ShouldPathPredictably() 25 | { 26 | var pathfinder = new PathFinder(_world); 27 | 28 | var path = pathfinder.FindPath(new Position(1, 1), new Position(2, 3)); 29 | 30 | path.ShouldBe(new[] { 31 | new Position(1, 1), 32 | new Position(2, 2), 33 | new Position(2, 3), 34 | }); 35 | } 36 | 37 | [Test] 38 | public void ShouldPathPredictably2() 39 | { 40 | var pathfinder = new PathFinder(_world); 41 | 42 | var path = pathfinder.FindPath(new Position(1, 1), new Position(1, 5)); 43 | 44 | path.ShouldBe(new[] { 45 | new Position(1, 1), 46 | new Position(1, 2), 47 | new Position(2, 3), 48 | new Position(1, 4), 49 | new Position(1, 5), 50 | }); 51 | 52 | } 53 | 54 | [Test] 55 | public void ShouldPathPredictably3() 56 | { 57 | var pathfinder = new PathFinder(_world, new PathFinderOptions { UseDiagonals = false }); 58 | 59 | var path = pathfinder.FindPath(new Position(1, 1), new Position(1, 5)); 60 | 61 | path.ShouldBe(new[] { 62 | new Position(1, 1), 63 | new Position(1, 2), 64 | new Position(2, 2), 65 | new Position(2, 3), 66 | new Position(2, 4), 67 | new Position(2, 5), 68 | new Position(1, 5), 69 | }); 70 | } 71 | } 72 | } -------------------------------------------------------------------------------- /AStar/Collections/PathFinder/PathFinderGraph.cs: -------------------------------------------------------------------------------- 1 | using System.Collections.Generic; 2 | using System.Linq; 3 | using AStar.Collections.MultiDimensional; 4 | using AStar.Collections.PriorityQueue; 5 | 6 | namespace AStar.Collections.PathFinder 7 | { 8 | internal class PathFinderGraph : IModelAGraph 9 | { 10 | private readonly bool _allowDiagonalTraversal; 11 | private readonly Grid _internalGrid; 12 | private readonly SimplePriorityQueue _open = new SimplePriorityQueue(new ComparePathFinderNodeByFValue()); 13 | 14 | public bool HasOpenNodes 15 | { 16 | get 17 | { 18 | return _open.Count > 0; 19 | } 20 | } 21 | public PathFinderGraph(int height, int width, bool allowDiagonalTraversal) 22 | { 23 | _allowDiagonalTraversal = allowDiagonalTraversal; 24 | _internalGrid = new Grid(height, width); 25 | Initialise(); 26 | } 27 | 28 | private void Initialise() 29 | { 30 | for (var row = 0; row < _internalGrid.Height; row++) 31 | { 32 | for (var column = 0; column < _internalGrid.Width; column++) 33 | { 34 | _internalGrid[row, column] = new PathFinderNode(position: new Position(row, column), 35 | g: 0, 36 | h: 0, 37 | parentNodePosition: default); 38 | } 39 | } 40 | 41 | _open.Clear(); 42 | } 43 | 44 | public IEnumerable GetSuccessors(PathFinderNode node) 45 | { 46 | return _internalGrid 47 | .GetSuccessorPositions(node.Position, _allowDiagonalTraversal) 48 | .Select(successorPosition => _internalGrid[successorPosition]); 49 | } 50 | 51 | public PathFinderNode GetParent(PathFinderNode node) 52 | { 53 | return _internalGrid[node.ParentNodePosition]; 54 | } 55 | 56 | public void OpenNode(PathFinderNode node) 57 | { 58 | _internalGrid[node.Position] = node; 59 | _open.Push(node); 60 | } 61 | 62 | public PathFinderNode GetOpenNodeWithSmallestF() 63 | { 64 | return _open.Pop(); 65 | } 66 | } 67 | } -------------------------------------------------------------------------------- /AStar/Collections/MultiDimensional/Grid.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Collections.Generic; 3 | using System.Drawing; 4 | 5 | namespace AStar.Collections.MultiDimensional 6 | { 7 | public class Grid : IModelAGrid 8 | { 9 | private readonly T[] _grid; 10 | public Grid(int height, int width) 11 | { 12 | if (height <= 0) 13 | { 14 | throw new ArgumentOutOfRangeException(nameof(height)); 15 | } 16 | 17 | if (width <= 0) 18 | { 19 | throw new ArgumentOutOfRangeException(nameof(width)); 20 | } 21 | 22 | Height = height; 23 | Width = width; 24 | 25 | _grid = new T[height * width]; 26 | } 27 | 28 | public int Height { get; } 29 | 30 | public int Width { get; } 31 | 32 | public IEnumerable GetSuccessorPositions(Position node, bool optionsUseDiagonals = false) 33 | { 34 | var offsets = GridOffsets.GetOffsets(optionsUseDiagonals); 35 | foreach (var neighbourOffset in offsets) 36 | { 37 | var successorRow = node.Row + neighbourOffset.row; 38 | 39 | if (successorRow < 0 || successorRow >= Height) 40 | { 41 | continue; 42 | 43 | } 44 | 45 | var successorColumn = node.Column + neighbourOffset.column; 46 | 47 | if (successorColumn < 0 || successorColumn >= Width) 48 | { 49 | continue; 50 | } 51 | 52 | yield return new Position(successorRow, successorColumn); 53 | } 54 | } 55 | 56 | public T this[Point point] 57 | { 58 | get 59 | { 60 | return this[point.ToPosition()]; 61 | } 62 | set 63 | { 64 | this[point.ToPosition()] = value; 65 | } 66 | } 67 | public T this[Position position] 68 | { 69 | get 70 | { 71 | return _grid[ConvertRowColumnToIndex(position.Row, position.Column)]; 72 | } 73 | set 74 | { 75 | _grid[ConvertRowColumnToIndex(position.Row, position.Column)] = value; 76 | } 77 | } 78 | public T this[int row, int column] 79 | { 80 | get 81 | { 82 | return _grid[ConvertRowColumnToIndex(row, column)]; 83 | } 84 | set 85 | { 86 | _grid[ConvertRowColumnToIndex(row, column)] = value; 87 | } 88 | } 89 | 90 | private int ConvertRowColumnToIndex(int row, int column) 91 | { 92 | return Width * row + column; 93 | } 94 | } 95 | } -------------------------------------------------------------------------------- /AStar.sln: -------------------------------------------------------------------------------- 1 | 2 | Microsoft Visual Studio Solution File, Format Version 12.00 3 | # Visual Studio 15 4 | VisualStudioVersion = 15.0.26124.0 5 | MinimumVisualStudioVersion = 15.0.26124.0 6 | Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "AStar", "AStar\AStar.csproj", "{3EBF7A9E-F5EC-431E-A8E7-CC98C05D02CA}" 7 | EndProject 8 | Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "AStar.Tests", "AStar.Tests\AStar.Tests.csproj", "{907F3E85-08B9-4609-9209-2D5FC5695BA6}" 9 | EndProject 10 | Project("{2150E333-8FDC-42A3-9474-1A3956D46DE8}") = "ReadMe", "ReadMe", "{240B9410-9B7D-49A2-9431-B45FA3003686}" 11 | ProjectSection(SolutionItems) = preProject 12 | README.md = README.md 13 | LICENSE = LICENSE 14 | EndProjectSection 15 | EndProject 16 | Global 17 | GlobalSection(SolutionConfigurationPlatforms) = preSolution 18 | Debug|Any CPU = Debug|Any CPU 19 | Debug|x64 = Debug|x64 20 | Debug|x86 = Debug|x86 21 | Release|Any CPU = Release|Any CPU 22 | Release|x64 = Release|x64 23 | Release|x86 = Release|x86 24 | EndGlobalSection 25 | GlobalSection(SolutionProperties) = preSolution 26 | HideSolutionNode = FALSE 27 | EndGlobalSection 28 | GlobalSection(ProjectConfigurationPlatforms) = postSolution 29 | {3EBF7A9E-F5EC-431E-A8E7-CC98C05D02CA}.Debug|Any CPU.ActiveCfg = Debug|Any CPU 30 | {3EBF7A9E-F5EC-431E-A8E7-CC98C05D02CA}.Debug|Any CPU.Build.0 = Debug|Any CPU 31 | {3EBF7A9E-F5EC-431E-A8E7-CC98C05D02CA}.Debug|x64.ActiveCfg = Debug|Any CPU 32 | {3EBF7A9E-F5EC-431E-A8E7-CC98C05D02CA}.Debug|x64.Build.0 = Debug|Any CPU 33 | {3EBF7A9E-F5EC-431E-A8E7-CC98C05D02CA}.Debug|x86.ActiveCfg = Debug|Any CPU 34 | {3EBF7A9E-F5EC-431E-A8E7-CC98C05D02CA}.Debug|x86.Build.0 = Debug|Any CPU 35 | {3EBF7A9E-F5EC-431E-A8E7-CC98C05D02CA}.Release|Any CPU.ActiveCfg = Release|Any CPU 36 | {3EBF7A9E-F5EC-431E-A8E7-CC98C05D02CA}.Release|Any CPU.Build.0 = Release|Any CPU 37 | {3EBF7A9E-F5EC-431E-A8E7-CC98C05D02CA}.Release|x64.ActiveCfg = Release|Any CPU 38 | {3EBF7A9E-F5EC-431E-A8E7-CC98C05D02CA}.Release|x64.Build.0 = Release|Any CPU 39 | {3EBF7A9E-F5EC-431E-A8E7-CC98C05D02CA}.Release|x86.ActiveCfg = Release|Any CPU 40 | {3EBF7A9E-F5EC-431E-A8E7-CC98C05D02CA}.Release|x86.Build.0 = Release|Any CPU 41 | {907F3E85-08B9-4609-9209-2D5FC5695BA6}.Debug|Any CPU.ActiveCfg = Debug|Any CPU 42 | {907F3E85-08B9-4609-9209-2D5FC5695BA6}.Debug|Any CPU.Build.0 = Debug|Any CPU 43 | {907F3E85-08B9-4609-9209-2D5FC5695BA6}.Debug|x64.ActiveCfg = Debug|Any CPU 44 | {907F3E85-08B9-4609-9209-2D5FC5695BA6}.Debug|x64.Build.0 = Debug|Any CPU 45 | {907F3E85-08B9-4609-9209-2D5FC5695BA6}.Debug|x86.ActiveCfg = Debug|Any CPU 46 | {907F3E85-08B9-4609-9209-2D5FC5695BA6}.Debug|x86.Build.0 = Debug|Any CPU 47 | {907F3E85-08B9-4609-9209-2D5FC5695BA6}.Release|Any CPU.ActiveCfg = Release|Any CPU 48 | {907F3E85-08B9-4609-9209-2D5FC5695BA6}.Release|Any CPU.Build.0 = Release|Any CPU 49 | {907F3E85-08B9-4609-9209-2D5FC5695BA6}.Release|x64.ActiveCfg = Release|Any CPU 50 | {907F3E85-08B9-4609-9209-2D5FC5695BA6}.Release|x64.Build.0 = Release|Any CPU 51 | {907F3E85-08B9-4609-9209-2D5FC5695BA6}.Release|x86.ActiveCfg = Release|Any CPU 52 | {907F3E85-08B9-4609-9209-2D5FC5695BA6}.Release|x86.Build.0 = Release|Any CPU 53 | EndGlobalSection 54 | EndGlobal 55 | -------------------------------------------------------------------------------- /AStar.Tests/PunishChangeDirectionIssueTest.cs: -------------------------------------------------------------------------------- 1 | using AStar.Heuristics; 2 | using AStar.Options; 3 | using NUnit.Framework; 4 | using Shouldly; 5 | 6 | namespace AStar.Tests 7 | { 8 | [TestFixture] 9 | public class PunishChangeDirectionIssueTests 10 | { 11 | private WorldGrid _world; 12 | 13 | [SetUp] 14 | public void SetUp() 15 | { 16 | var level = @" 11111111111111111111 17 | 11111111111111111111 18 | 11111111111111X11111 19 | 11111111X1X111X11111 20 | 11111111X1111XXXXX11 21 | 11XXX111X1X11XXXXX11 22 | 111111111111111XXX11 23 | 11111111111111111111 24 | 11111111111111111111"; 25 | 26 | _world = Helper.ConvertStringToPathfinderGrid(level); 27 | } 28 | 29 | [Test] 30 | public void ShouldPunishChangingDirectionsIssue() 31 | { 32 | var pathFinderOptions = new PathFinderOptions { UseDiagonals = false, PunishChangeDirection = false, HeuristicFormula = HeuristicFormula.Euclidean}; 33 | var pathfinder = new PathFinder(_world, pathFinderOptions); 34 | 35 | var path = pathfinder.FindPath(new Position(7, 2), new Position(1, 17)); 36 | 37 | path.ShouldBe(new[] { 38 | new Position(7, 2), 39 | new Position(7, 3), 40 | new Position(7, 4), 41 | new Position(7, 5), 42 | new Position(7, 6), 43 | new Position(7, 7), 44 | new Position(7, 8), 45 | new Position(7, 9), 46 | new Position(7, 10), 47 | new Position(7, 11), 48 | new Position(7, 12), 49 | new Position(6, 12), 50 | new Position(5, 12), 51 | new Position(4, 12), 52 | new Position(3, 12), 53 | new Position(3, 13), 54 | new Position(2, 13), 55 | new Position(1, 13), 56 | new Position(1, 14), 57 | new Position(1, 15), 58 | new Position(1, 16), 59 | new Position(1, 17), 60 | }); 61 | } 62 | 63 | [Test] 64 | public void ShouldCorrectIssue() 65 | { 66 | var pathFinderOptions = new PathFinderOptions { UseDiagonals = true, PunishChangeDirection = false}; 67 | var pathfinder = new PathFinder(_world, pathFinderOptions); 68 | 69 | var path = pathfinder.FindPath(new Position(1, 2), new Position(8, 14)); 70 | 71 | path.ShouldBe(new[] { 72 | new Position(1, 2), 73 | new Position(2, 3), 74 | new Position(3, 4), 75 | new Position(4, 5), 76 | new Position(5, 6), 77 | new Position(6, 7), 78 | new Position(7, 8), 79 | new Position(8, 9), 80 | new Position(8, 10), 81 | new Position(8, 11), 82 | new Position(8, 12), 83 | new Position(8, 13), 84 | new Position(8, 14), 85 | }); 86 | } 87 | } 88 | } -------------------------------------------------------------------------------- /AStar.Tests/PunishChangeDirectionTests.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using AStar.Heuristics; 3 | using AStar.Options; 4 | using NUnit.Framework; 5 | using Shouldly; 6 | 7 | namespace AStar.Tests 8 | { 9 | [TestFixture] 10 | public class PunishChangeDirectionTests 11 | { 12 | [Test] 13 | public void ShouldPunishChangingDirections() 14 | { 15 | var level = @" 111111111X 16 | 111111111X 17 | 11111X111X 18 | 11111X111X 19 | 11111X111X 20 | 111111111X 21 | 111111111X 22 | 111111111X 23 | 111XXX111X 24 | 111111111X 25 | 111X1X111X 26 | 111111111X 27 | 111111111X 28 | 1111XX111X 29 | 11XXXX111X 30 | 1111XXX11X 31 | 1111XXX11X 32 | 1111XXX11X 33 | 111111111X 34 | 111111111X"; 35 | 36 | var world = Helper.ConvertStringToPathfinderGrid(level); 37 | 38 | var pathFinderOptions = new PathFinderOptions { UseDiagonals = true, PunishChangeDirection = true}; 39 | var pathfinder = new PathFinder(world, pathFinderOptions); 40 | 41 | var path = pathfinder.FindPath(new Position(2, 9), new Position(15, 3)); 42 | 43 | path.ShouldBe(new[] 44 | { 45 | new Position(2, 9), 46 | new Position(3, 8), 47 | new Position(4, 7), 48 | new Position(5, 6), 49 | new Position(6, 5), 50 | new Position(7, 5), 51 | new Position(8, 6), 52 | new Position(9, 5), 53 | new Position(10, 4), 54 | new Position(11, 3), 55 | new Position(12, 3), 56 | new Position(13, 2), 57 | new Position(14, 1), 58 | new Position(15, 2), 59 | new Position(15, 3), 60 | }); 61 | } 62 | 63 | [Test] 64 | public void ShouldCalculateAdjacentCorrectly() 65 | { 66 | var level = @" 110111 67 | 110111 68 | 100111 69 | 111111 70 | 101111 71 | 111111"; 72 | 73 | var world = Helper.ConvertStringToPathfinderGrid(level); 74 | var pathfinder = new PathFinder(world, new PathFinderOptions { UseDiagonals = false, PunishChangeDirection = true, HeuristicFormula = HeuristicFormula.MaxDXDY }); 75 | 76 | var path = pathfinder.FindPath(new Position(4, 4), new Position(1, 1)); 77 | 78 | var expected = new[] 79 | { 80 | new Position(4, 4), 81 | new Position(3, 4), 82 | new Position(3, 3), 83 | new Position(3, 2), 84 | new Position(3, 1), 85 | new Position(3, 0), 86 | new Position(2, 0), 87 | new Position(1, 0), 88 | new Position(1, 1), 89 | }; 90 | 91 | Console.WriteLine("actual"); 92 | Console.WriteLine("expected"); 93 | 94 | path.ShouldBe(expected); 95 | } 96 | } 97 | } -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | A 2D A* (A Star) algorithm for C# 2 | ===== 3 | 4 | ![Travis (.com) branch](https://img.shields.io/travis/com/valantonini/AStar/master?style=for-the-badge) 5 | [![NuGet](https://img.shields.io/nuget/v/AStarLite.svg?style=for-the-badge)](https://www.nuget.org/packages/AStarLite/) 6 | 7 | The world is represented by a WorldGrid that is essentially a matrix of the C# short data type. 8 | A value of 0 indicates the cell is closed / blocked. Any other number indicates the cell is open and traversable. 9 | It is recommended to use 1 for open cells as numbers greater and less than 0 may be used to apply penalty or 10 | priority to movement through those nodes in the future. 11 | 12 | The WorldGrid can be indexed via either: 13 | 14 | 1) The provided Position struct where a row represents the vertical axis and column the horizontal axis 15 | (Similar to indexing into a matrix Prc). 16 | 17 | 2) The C# Point struct that operates like a cartesian co-ordinate system where 18 | X represents the horizontal axis and Y represents the vertical axis (Pxy). 19 | 20 | Paths can be found using either Positions (matrix indexing) or Points (cartesian indexing). 21 | 22 | [A Go version is also in to works](https://github.com/valantonini/go-astar) 23 | ## Example usage 24 | ![PathingExample](Docs/PathingExample.png "Pathing Example") 25 | 26 | ```csharp 27 | var pathfinderOptions = new PathFinderOptions { 28 | PunishChangeDirection = true, 29 | UseDiagonals = false, 30 | }; 31 | 32 | var tiles = new short[,] { 33 | { 1, 0, 1 }, 34 | { 1, 0, 1 }, 35 | { 1, 1, 1 }, 36 | }; 37 | 38 | var worldGrid = new WorldGrid(tiles); 39 | var pathfinder = new PathFinder(worldGrid, pathfinderOptions); 40 | 41 | // The following are equivalent: 42 | 43 | // matrix indexing 44 | Position[] path = pathfinder.FindPath(new Position(0, 0), new Position(0, 2)); 45 | 46 | // point indexing 47 | Point[] path = pathfinder.FindPath(new Point(0, 0), new Point(2, 0)); 48 | ``` 49 | 50 | ## Options 51 | - Allowing / restricting diagonal movement 52 | - A choice of heuristic (Manhattan, MaxDxDy, Euclidean, Diagonal shortcut) 53 | - The option to punish direction changes. 54 | - A search limit to short circuit the search 55 | 56 | ## FAQ 57 | 58 | q. why doesn't this algorithm always find the shortest path? 59 | 60 | a. A* optimises speed over accuracy. Because the algorithm relies on a 61 | heuristic to determine the distances from start and finish, it won't necessarily 62 | produce the shortest path to the target. 63 | 64 | ## Changes from 1.1.0 to 1.3.0 65 | - Introduced path weighting to favour or penalize cells. This is off by default and 66 | can be opted into using the new options. See [this blog post for more info](https://valantonini.com/posts/20210401/) 67 | ```csharp 68 | var level = @"1111115 69 | 1511151 70 | 1155511 71 | 1111111"; 72 | var world = Helper.ConvertStringToPathfinderGrid(level); 73 | var opts = new PathFinderOptions { Weighting = Weighting.Positive }; 74 | var pathfinder = new PathFinder(world, opts); 75 | ``` 76 | - Introduced `IgnoreClosedEndCell` option to allow pathing to a closed cell 77 | 78 | ## Changes from 1.0.0 to 1.1.0 79 | - Reimplemented the punish change direction to perform more consistently 80 | 81 | ## Changes from 0.1.x to 1.0.0 82 | - The world is now represented by a WorldGrid that uses shorts internally instead of bytes 83 | - If no path is found, the algorithm now reports an empty array instead of null 84 | - Moved out of the AStar.Core namespace into simply AStar 85 | - Replaced former Point class with the new Position class that uses Row / Column instead of X / Y to avoid confusion with cartesian co-ordinates 86 | - Implemented support for Point class indexing and pathing which represent a traditional cartesian co-ordinate system 87 | - Changed path from List to Array and changed type from PathFinderNode to Position or Point 88 | - Reversed the order of the returned path to start at the start node 89 | - Rationalised and dropped buggy options (heavy diagonals) 90 | -------------------------------------------------------------------------------- /AStar.Tests/GridTests.cs: -------------------------------------------------------------------------------- 1 | using System.Drawing; 2 | using System.Linq; 3 | using NUnit.Framework; 4 | using Shouldly; 5 | 6 | namespace AStar.Tests 7 | { 8 | [TestFixture] 9 | public class GridTests 10 | { 11 | [Test] 12 | public void ShouldInstantiateWithCorrectDimensions() 13 | { 14 | var grid = new WorldGrid(12, 10); 15 | 16 | grid.Height.ShouldBe(12); 17 | grid.Width.ShouldBe(10); 18 | } 19 | 20 | [Test] 21 | public void ShouldReadAndWriteByIndex() 22 | { 23 | var grid = new WorldGrid(2, 3) 24 | { 25 | [0, 0] = 1, 26 | [0, 1] = 2, 27 | [0, 2] = 3, 28 | [1, 0] = 4, 29 | [1, 1] = 5, 30 | [1, 2] = 6, 31 | }; 32 | 33 | grid[0, 0].ShouldBe((short)1); 34 | grid[0, 1].ShouldBe((short)2); 35 | grid[0, 2].ShouldBe((short)3); 36 | 37 | grid[1, 0].ShouldBe((short)4); 38 | grid[1, 1].ShouldBe((short)5); 39 | grid[1, 2].ShouldBe((short)6); 40 | } 41 | 42 | [Test] 43 | public void ShouldInstantiateWith2DArray() 44 | { 45 | var grid = new WorldGrid(new short[,] 46 | { 47 | { 1, 2, 3 }, 48 | { 4, 5, 6 }, 49 | { 7, 8, 9 }, 50 | }); 51 | 52 | grid[0, 0].ShouldBe((short)1); 53 | grid[0, 1].ShouldBe((short)2); 54 | grid[0, 2].ShouldBe((short)3); 55 | 56 | grid[1, 0].ShouldBe((short)4); 57 | grid[1, 1].ShouldBe((short)5); 58 | grid[1, 2].ShouldBe((short)6); 59 | } 60 | 61 | [Test] 62 | public void ShouldReadAndWriteByPoint() 63 | { 64 | var grid = new WorldGrid(2, 3); 65 | 66 | grid[new Position(0, 0)] = 1; 67 | grid[new Position(0, 1)] = 2; 68 | grid[new Position(0, 2)] = 3; 69 | grid[new Position(1, 0)] = 4; 70 | grid[new Position(1, 1)] = 5; 71 | grid[new Position(1, 2)] = 6; 72 | 73 | grid[new Point(0, 0)].ShouldBe((short)1); 74 | grid[new Point(1, 0)].ShouldBe((short)2); 75 | grid[new Point(2, 0)].ShouldBe((short)3); 76 | grid[new Point(0, 1)].ShouldBe((short)4); 77 | grid[new Point(1, 1)].ShouldBe((short)5); 78 | grid[new Point(2, 1)].ShouldBe((short)6); 79 | } 80 | 81 | [Test] 82 | public void ShouldGetCardinalSuccessorPositions() 83 | { 84 | var grid = new WorldGrid(3, 3); 85 | 86 | var successors = grid 87 | .GetSuccessorPositions(new Position(1,1)) 88 | .ToArray(); 89 | 90 | successors.Length.ShouldBe(4); 91 | 92 | successors[0].ShouldBe(new Position(1, 0)); 93 | successors[1].ShouldBe(new Position(2, 1)); 94 | successors[2].ShouldBe(new Position(1, 2)); 95 | successors[3].ShouldBe(new Position(0, 1)); 96 | } 97 | 98 | [Test] 99 | public void ShouldGetCardinalAndDiagonalSuccessorPositions() 100 | { 101 | var grid = new WorldGrid(3, 3); 102 | 103 | var successors = grid 104 | .GetSuccessorPositions(new Position(1,1), true) 105 | .ToArray(); 106 | 107 | successors.Length.ShouldBe(8); 108 | 109 | successors[0].ShouldBe(new Position(1, 0)); 110 | successors[1].ShouldBe(new Position(2, 1)); 111 | successors[2].ShouldBe(new Position(1, 2)); 112 | successors[3].ShouldBe(new Position(0, 1)); 113 | 114 | successors[4].ShouldBe(new Position(2, 0)); 115 | successors[5].ShouldBe(new Position(2, 2)); 116 | successors[6].ShouldBe(new Position(0, 2)); 117 | successors[7].ShouldBe(new Position(0, 0)); 118 | } 119 | 120 | [Test] 121 | public void ShouldGetSuccessorsWithoutGoingOutOfBounds() 122 | { 123 | var grid = new WorldGrid(3, 3); 124 | 125 | var successors = grid 126 | .GetSuccessorPositions(new Position(2,2), true) 127 | .ToArray(); 128 | 129 | successors.Length.ShouldBe(3); 130 | 131 | successors[0].ShouldBe(new Position(2, 1)); 132 | successors[1].ShouldBe(new Position(1, 2)); 133 | successors[2].ShouldBe(new Position(1, 1)); 134 | } 135 | } 136 | } -------------------------------------------------------------------------------- /AStar/Collections/PriorityQueue/SimplePriorityQueue.cs: -------------------------------------------------------------------------------- 1 | using System.Collections.Generic; 2 | 3 | namespace AStar.Collections.PriorityQueue 4 | { 5 | internal class SimplePriorityQueue : IModelAPriorityQueue 6 | { 7 | private readonly List _innerList = new List(); 8 | private readonly IComparer _comparer; 9 | 10 | public SimplePriorityQueue(IComparer comparer = null) 11 | { 12 | _comparer = comparer ?? Comparer.Default;; 13 | } 14 | 15 | public T Peek() 16 | { 17 | return _innerList.Count > 0 ? _innerList[0] : default(T); 18 | } 19 | 20 | public void Clear() 21 | { 22 | _innerList.Clear(); 23 | } 24 | 25 | public int Count 26 | { 27 | get { return _innerList.Count; } 28 | } 29 | 30 | public int Push(T item) 31 | { 32 | var p = _innerList.Count; 33 | _innerList.Add(item); // E[p] = O 34 | 35 | do 36 | { 37 | if (p == 0) 38 | { 39 | break; 40 | } 41 | 42 | var p2 = (p - 1) / 2; 43 | 44 | if (OnCompare(p, p2) < 0) 45 | { 46 | SwitchElements(p, p2); 47 | p = p2; 48 | } 49 | else 50 | { 51 | break; 52 | } 53 | 54 | } while (true); 55 | 56 | return p; 57 | } 58 | 59 | public T Pop() 60 | { 61 | var result = _innerList[0]; 62 | var p = 0; 63 | 64 | _innerList[0] = _innerList[_innerList.Count - 1]; 65 | _innerList.RemoveAt(_innerList.Count - 1); 66 | 67 | do 68 | { 69 | var pn = p; 70 | var p1 = 2 * p + 1; 71 | var p2 = 2 * p + 2; 72 | 73 | if (_innerList.Count > p1 && OnCompare(p, p1) > 0) 74 | { 75 | p = p1; 76 | } 77 | if (_innerList.Count > p2 && OnCompare(p, p2) > 0) 78 | { 79 | p = p2; 80 | } 81 | 82 | if (p == pn) 83 | { 84 | break; 85 | } 86 | 87 | SwitchElements(p, pn); 88 | 89 | } while (true); 90 | 91 | return result; 92 | } 93 | 94 | public T this[int index] 95 | { 96 | get 97 | { 98 | return _innerList[index]; 99 | } 100 | set 101 | { 102 | _innerList[index] = value; 103 | Update(index); 104 | } 105 | } 106 | 107 | private void Update(int i) 108 | { 109 | var p = i; 110 | int p2; 111 | 112 | do 113 | { 114 | if (p == 0) 115 | { 116 | break; 117 | } 118 | 119 | p2 = (p - 1) / 2; 120 | 121 | if (OnCompare(p, p2) < 0) 122 | { 123 | SwitchElements(p, p2); 124 | p = p2; 125 | } 126 | else 127 | { 128 | break; 129 | } 130 | 131 | } while (true); 132 | 133 | if (p < i) 134 | { 135 | return; 136 | } 137 | 138 | do 139 | { 140 | var pn = p; 141 | var p1 = 2 * p + 1; 142 | p2 = 2 * p + 2; 143 | 144 | if (_innerList.Count > p1 && OnCompare(p, p1) > 0) 145 | { 146 | p = p1; 147 | } 148 | 149 | if (_innerList.Count > p2 && OnCompare(p, p2) > 0) 150 | { 151 | p = p2; 152 | } 153 | 154 | if (p == pn) 155 | { 156 | break; 157 | } 158 | 159 | SwitchElements(p, pn); 160 | 161 | } while (true); 162 | } 163 | 164 | private void SwitchElements(int i, int j) 165 | { 166 | var h = _innerList[i]; 167 | _innerList[i] = _innerList[j]; 168 | _innerList[j] = h; 169 | } 170 | 171 | private int OnCompare(int i, int j) 172 | { 173 | return _comparer.Compare(_innerList[i], _innerList[j]); 174 | } 175 | } 176 | } 177 | -------------------------------------------------------------------------------- /AStar.Tests/Helper.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Drawing; 3 | using System.Linq; 4 | using System.Text; 5 | 6 | namespace AStar.Tests 7 | { 8 | public static class Helper 9 | { 10 | public static string PrintGrid(WorldGrid worldGrid, bool appendSpace = true) 11 | { 12 | var s = new StringBuilder(); 13 | 14 | for (var row = 0; row < worldGrid.Height; row++) 15 | { 16 | for (var column = 0; column < worldGrid.Width; column++) 17 | { 18 | s.Append(worldGrid[row, column]); 19 | if (appendSpace) 20 | { 21 | s.Append(' '); 22 | } 23 | } 24 | s.Append(Environment.NewLine); 25 | } 26 | 27 | return s.ToString(); 28 | } 29 | 30 | public static string PrintPath(WorldGrid world, Position[] path, bool appendSpace = true) 31 | { 32 | var s = new StringBuilder(); 33 | 34 | for (var row = 0; row < world.Height; row++) 35 | { 36 | for (var column = 0; column < world.Width; column++) 37 | { 38 | if (path.Any(n => n.Row == row && n.Column == column)) 39 | { 40 | s.Append("*"); 41 | } 42 | else 43 | { 44 | s.Append(world[row, column]); 45 | } 46 | s.Append(' '); 47 | } 48 | s.Append(Environment.NewLine); 49 | } 50 | return s.ToString(); 51 | } 52 | 53 | public static string PrintPath(WorldGrid world, Point[] path, bool appendSpace = true) 54 | { 55 | var s = new StringBuilder(); 56 | 57 | for (var y = 0; y < world.Height; y++) 58 | { 59 | for (var x = 0; x < world.Width; x++) 60 | { 61 | if (path.Any(n => n.Y == y && n.X == x)) 62 | { 63 | s.Append("_"); 64 | } 65 | else 66 | { 67 | s.Append(world[y, x]); 68 | } 69 | s.Append(' '); 70 | } 71 | s.Append(Environment.NewLine); 72 | } 73 | return s.ToString(); 74 | } 75 | 76 | public static void Print(WorldGrid world, Position[] path) 77 | { 78 | Console.WriteLine(PrintGrid(world)); 79 | Console.WriteLine(Environment.NewLine); 80 | Console.WriteLine(Environment.NewLine); 81 | Console.WriteLine(PrintPath(world, path)); 82 | 83 | PrintAssertions(path); 84 | } 85 | 86 | public static void Print(WorldGrid world, Point[] path) 87 | { 88 | Print(world, path.Select(p => p.ToPosition()).ToArray()); 89 | } 90 | 91 | public static void PrintAssertions(Position[] path) 92 | { 93 | StringBuilder s = new StringBuilder(); 94 | s.AppendLine("path.ShouldBe(new[] {"); 95 | foreach (var position in path) 96 | { 97 | s.AppendLine($"new Position({position.Row}, {position.Column}),"); 98 | } 99 | s.AppendLine("});"); 100 | Console.WriteLine(s.ToString()); 101 | } 102 | 103 | public static void PrintAssertions(Point[] path) 104 | { 105 | for (var i = 0; i < path.Length; i++) 106 | { 107 | Console.WriteLine("path[{0}].X.ShouldBe({1});", i, path[i].X); 108 | Console.WriteLine("path[{0}].Y.ShouldBe({1});", i, path[i].Y); 109 | } 110 | } 111 | 112 | public static WorldGrid ConvertStringToPathfinderGrid(string level) 113 | { 114 | var closedCharacter = 'X'; 115 | 116 | var splitLevel = level.Split('\n') 117 | .Select(row => row.Trim()) 118 | .ToList(); 119 | 120 | var world = new WorldGrid(splitLevel.Count, splitLevel[0].Length); 121 | 122 | for (var row = 0; row < splitLevel.Count; row++) 123 | { 124 | for (var column = 0; column < splitLevel[row].Length; column++) 125 | { 126 | if (splitLevel[row][column] != closedCharacter) 127 | { 128 | world[row, column] = short.Parse(splitLevel[row][column].ToString()); 129 | } 130 | } 131 | } 132 | 133 | return world; 134 | } 135 | } 136 | } 137 | -------------------------------------------------------------------------------- /AStar.Tests/PathfinderTests.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using AStar.Options; 3 | using NUnit.Framework; 4 | using Shouldly; 5 | 6 | namespace AStar.Tests 7 | { 8 | [TestFixture] 9 | public class PathfinderTests 10 | { 11 | private WorldGrid _world; 12 | private PathFinder _pathFinder; 13 | 14 | [SetUp] 15 | public void SetUp() 16 | { 17 | _world = CreateGridInitializedToOpen(8, 8); 18 | _pathFinder = new PathFinder(_world); 19 | } 20 | 21 | [Test] 22 | public void ShouldPathRectangleGrid() 23 | { 24 | var grid = CreateGridInitializedToOpen(3, 5); 25 | var pathfinder = new PathFinder(grid); 26 | 27 | var path = pathfinder.FindPath(new Position(0, 0), new Position(2, 4)); 28 | 29 | path.ShouldBe(new[] { 30 | new Position(0, 0), 31 | new Position(1, 1), 32 | new Position(2, 2), 33 | new Position(2, 3), 34 | new Position(2, 4), 35 | }); 36 | } 37 | 38 | [Test] 39 | public void ShouldPathToSelf() 40 | { 41 | var path = _pathFinder.FindPath(new Position(1, 1), new Position(1, 1)); 42 | 43 | path.ShouldBe(new[] { 44 | new Position(1, 1), 45 | }); 46 | } 47 | 48 | [Test] 49 | public void ShouldPathToAdjacent() 50 | { 51 | var path = _pathFinder.FindPath(new Position(1, 1), new Position(2, 1)); 52 | 53 | path.ShouldBe(new[] { 54 | new Position(1, 1), 55 | new Position(2, 1), 56 | }); 57 | } 58 | 59 | [Test] 60 | public void ShouldDoSimplePath() 61 | { 62 | var path = _pathFinder.FindPath(new Position(1, 1), new Position(4, 2)); 63 | 64 | path.ShouldBe(new[] { 65 | new Position(1, 1), 66 | new Position(2, 2), 67 | new Position(3, 2), 68 | new Position(4, 2), 69 | }); 70 | } 71 | 72 | [Test] 73 | public void ShouldDoSimplePathWithNoDiagonal() 74 | { 75 | var pathfinderOptions = new PathFinderOptions { UseDiagonals = false }; 76 | _pathFinder = new PathFinder(_world, pathfinderOptions); 77 | 78 | var path = _pathFinder.FindPath(new Position(1, 1), new Position(4, 2)); 79 | 80 | path.ShouldBe(new[] { 81 | new Position(1, 1), 82 | new Position(2, 1), 83 | new Position(3, 1), 84 | new Position(4, 1), 85 | new Position(4, 2), 86 | }); 87 | } 88 | 89 | [Test] 90 | public void ShouldDoSimplePathWithNoDiagonalAroundObstacle() 91 | { 92 | var pathfinderOptions = new PathFinderOptions { UseDiagonals = false }; 93 | _pathFinder = new PathFinder(_world, pathfinderOptions); 94 | 95 | _world[2, 0] = 0; 96 | _world[2, 1] = 0; 97 | _world[2, 2] = 0; 98 | 99 | var path = _pathFinder.FindPath(new Position(1, 1), new Position(4, 2)); 100 | 101 | path.ShouldBe(new[] { 102 | new Position(1, 1), 103 | new Position(1, 2), 104 | new Position(1, 3), 105 | new Position(2, 3), 106 | new Position(3, 3), 107 | new Position(3, 2), 108 | new Position(4, 2), 109 | }); 110 | } 111 | [Test] 112 | public void ShouldPathAroundObstacle() 113 | { 114 | _world[2, 0] = 0; 115 | _world[2, 1] = 0; 116 | _world[2, 2] = 0; 117 | _world[2, 3] = 0; 118 | 119 | var path = _pathFinder.FindPath(new Position(1, 1), new Position(4, 2)); 120 | 121 | path.ShouldBe(new[] { 122 | new Position(1, 1), 123 | new Position(1, 2), 124 | new Position(1, 3), 125 | new Position(2, 4), 126 | new Position(3, 3), 127 | new Position(4, 2), 128 | }); 129 | } 130 | 131 | [Test] 132 | public void ShouldReturnEmptyPathIfUnreachable() 133 | { 134 | _world[2, 0] = 0; 135 | _world[2, 1] = 0; 136 | _world[2, 2] = 0; 137 | _world[2, 3] = 0; 138 | _world[2, 4] = 0; 139 | _world[2, 5] = 0; 140 | _world[2, 6] = 0; 141 | _world[2, 7] = 0; 142 | var path = _pathFinder.FindPath(new Position(1, 1), new Position(4, 2)); 143 | path.ShouldBeEmpty(); 144 | } 145 | 146 | private static WorldGrid CreateGridInitializedToOpen(int height, int width) 147 | { 148 | var grid = new WorldGrid(height, width); 149 | 150 | for (var row = 0; row < grid.Height; row++) 151 | { 152 | for (var column = 0; column < grid.Width; column++) 153 | { 154 | grid[row, column] = 1; 155 | } 156 | } 157 | 158 | return grid; 159 | } 160 | 161 | private static void PrintCoordinates(Position[] path) 162 | { 163 | foreach (var node in path) 164 | { 165 | Console.WriteLine(node.Row); 166 | Console.WriteLine(node.Column); 167 | Console.WriteLine(Environment.NewLine); 168 | } 169 | } 170 | } 171 | } 172 | -------------------------------------------------------------------------------- /AStar.Tests/LongerPathingTests.cs: -------------------------------------------------------------------------------- 1 | using AStar.Options; 2 | using NUnit.Framework; 3 | using Shouldly; 4 | 5 | namespace AStar.Tests 6 | { 7 | [TestFixture] 8 | public class LongerPathingTests 9 | { 10 | private WorldGrid _world; 11 | 12 | [SetUp] 13 | public void SetUp() 14 | { 15 | var level = @"XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX 16 | X111XXXX11111111111111111111111X 17 | X111XXXX11111111111111111111111X 18 | X111XXXX11111111111111111111111X 19 | X111XXXX11111111111111111111111X 20 | X111XXXX11111111111111111111111X 21 | X111XXXX11111111111111111111111X 22 | X111XXXX11111111111111111111111X 23 | X111111111111111111111111111111X 24 | X111111111111111111111111111111X 25 | X111111111111111111111111111111X 26 | XXXXXXXXXXXXXXXXXXXXXX111111111X 27 | XXXXXXXXXXXXXXXXXXXXXX111111111X 28 | XXXXXXXXXXXXXXXXXXXXXX111111111X 29 | X1111111111111111XXXXX111111111X 30 | X1111111111111111XXXXX111111111X 31 | X1111111111111111XXXXX111111111X 32 | X1111111111111111XXXXX111111111X 33 | X1111111111111111XXXXX111111111X 34 | X1111111111111111XXXXX111111111X 35 | X1111111111111111XXXXX111111111X 36 | X1111111111111111XXXXX111111111X 37 | X1111111111111111XXXXX111111111X 38 | X1111111111111111XXXXX111111111X 39 | X1111111111111111XXXXX111111111X 40 | X1111111111111111XXXXX111111111X 41 | X111111111111111111111111111111X 42 | X111111111111111111111111111111X 43 | X111111XXXXXXXXXXXXXXXXXXXXXXXXX 44 | X111111XXXXXXXXXXXXXXXXXXXXXXXXX 45 | X111111111111111111111111111111X 46 | XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX"; 47 | 48 | _world = Helper.ConvertStringToPathfinderGrid(level); 49 | } 50 | 51 | [Test] 52 | public void TestPathingOptions() 53 | { 54 | var pathfinderOptions = new PathFinderOptions { PunishChangeDirection = true }; 55 | 56 | var pathfinder = new PathFinder(_world, pathfinderOptions); 57 | var path = pathfinder.FindPath(new Position(1, 1), new Position(30, 30)); 58 | 59 | } 60 | 61 | [Test] 62 | public void ShouldPathEnvironment() 63 | { 64 | var pathfinder = new PathFinder(_world); 65 | 66 | var path = pathfinder.FindPath(new Position(1, 1), new Position(30, 30)); 67 | 68 | 69 | 70 | path.ShouldBe(new[] { 71 | new Position(1, 1), 72 | new Position(2, 2), 73 | new Position(3, 3), 74 | new Position(4, 3), 75 | new Position(5, 3), 76 | new Position(6, 3), 77 | new Position(7, 3), 78 | new Position(8, 4), 79 | new Position(9, 5), 80 | new Position(10, 6), 81 | new Position(10, 7), 82 | new Position(10, 8), 83 | new Position(10, 9), 84 | new Position(10, 10), 85 | new Position(10, 11), 86 | new Position(10, 12), 87 | new Position(10, 13), 88 | new Position(10, 14), 89 | new Position(10, 15), 90 | new Position(10, 16), 91 | new Position(10, 17), 92 | new Position(10, 18), 93 | new Position(10, 19), 94 | new Position(10, 20), 95 | new Position(10, 21), 96 | new Position(11, 22), 97 | new Position(12, 23), 98 | new Position(13, 24), 99 | new Position(14, 25), 100 | new Position(15, 26), 101 | new Position(16, 27), 102 | new Position(17, 28), 103 | new Position(18, 29), 104 | new Position(19, 28), 105 | new Position(20, 27), 106 | new Position(21, 26), 107 | new Position(22, 25), 108 | new Position(23, 24), 109 | new Position(24, 23), 110 | new Position(25, 22), 111 | new Position(26, 21), 112 | new Position(27, 20), 113 | new Position(27, 19), 114 | new Position(27, 18), 115 | new Position(27, 17), 116 | new Position(27, 16), 117 | new Position(27, 15), 118 | new Position(27, 14), 119 | new Position(27, 13), 120 | new Position(27, 12), 121 | new Position(27, 11), 122 | new Position(27, 10), 123 | new Position(27, 9), 124 | new Position(27, 8), 125 | new Position(27, 7), 126 | new Position(28, 6), 127 | new Position(29, 6), 128 | new Position(30, 7), 129 | new Position(30, 8), 130 | new Position(30, 9), 131 | new Position(30, 10), 132 | new Position(30, 11), 133 | new Position(30, 12), 134 | new Position(30, 13), 135 | new Position(30, 14), 136 | new Position(30, 15), 137 | new Position(30, 16), 138 | new Position(30, 17), 139 | new Position(30, 18), 140 | new Position(30, 19), 141 | new Position(30, 20), 142 | new Position(30, 21), 143 | new Position(30, 22), 144 | new Position(30, 23), 145 | new Position(30, 24), 146 | new Position(30, 25), 147 | new Position(30, 26), 148 | new Position(30, 27), 149 | new Position(30, 28), 150 | new Position(30, 29), 151 | new Position(30, 30), 152 | }); 153 | } 154 | } 155 | } 156 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | 2 | # Created by https://www.gitignore.io/api/visualstudio 3 | 4 | ### VisualStudio ### 5 | ## Ignore Visual Studio temporary files, build results, and 6 | ## files generated by popular Visual Studio add-ons. 7 | ## 8 | ## Get latest from https://github.com/github/gitignore/blob/master/VisualStudio.gitignore 9 | 10 | #OSX 11 | .DS_Store 12 | old 13 | 14 | # User-specific files 15 | *.suo 16 | *.user 17 | *.userosscache 18 | *.sln.docstates 19 | 20 | # User-specific files (MonoDevelop/Xamarin Studio) 21 | *.userprefs 22 | 23 | # Build results 24 | [Dd]ebug/ 25 | [Dd]ebugPublic/ 26 | [Rr]elease/ 27 | [Rr]eleases/ 28 | x64/ 29 | x86/ 30 | bld/ 31 | [Bb]in/ 32 | [Oo]bj/ 33 | [Ll]og/ 34 | 35 | # Visual Studio 2015/2017 cache/options directory 36 | .vs/ 37 | # Uncomment if you have tasks that create the project's static files in wwwroot 38 | #wwwroot/ 39 | 40 | # Visual Studio 2017 auto generated files 41 | Generated\ Files/ 42 | 43 | # MSTest test Results 44 | [Tt]est[Rr]esult*/ 45 | [Bb]uild[Ll]og.* 46 | 47 | # NUNIT 48 | *.VisualState.xml 49 | TestResult.xml 50 | 51 | # Build Results of an ATL Project 52 | [Dd]ebugPS/ 53 | [Rr]eleasePS/ 54 | dlldata.c 55 | 56 | # Benchmark Results 57 | BenchmarkDotNet.Artifacts/ 58 | 59 | # .NET Core 60 | project.lock.json 61 | project.fragment.lock.json 62 | artifacts/ 63 | 64 | # StyleCop 65 | StyleCopReport.xml 66 | 67 | # Files built by Visual Studio 68 | *_i.c 69 | *_p.c 70 | *_h.h 71 | *.ilk 72 | *.meta 73 | *.obj 74 | *.iobj 75 | *.pch 76 | *.pdb 77 | *.ipdb 78 | *.pgc 79 | *.pgd 80 | *.rsp 81 | *.sbr 82 | *.tlb 83 | *.tli 84 | *.tlh 85 | *.tmp 86 | *.tmp_proj 87 | *.log 88 | *.vspscc 89 | *.vssscc 90 | .builds 91 | *.pidb 92 | *.svclog 93 | *.scc 94 | 95 | # Chutzpah Test files 96 | _Chutzpah* 97 | 98 | # Visual C++ cache files 99 | ipch/ 100 | *.aps 101 | *.ncb 102 | *.opendb 103 | *.opensdf 104 | *.sdf 105 | *.cachefile 106 | *.VC.db 107 | *.VC.VC.opendb 108 | 109 | # Visual Studio profiler 110 | *.psess 111 | *.vsp 112 | *.vspx 113 | *.sap 114 | 115 | # Visual Studio Trace Files 116 | *.e2e 117 | 118 | # TFS 2012 Local Workspace 119 | $tf/ 120 | 121 | # Guidance Automation Toolkit 122 | *.gpState 123 | 124 | # ReSharper is a .NET coding add-in 125 | _ReSharper*/ 126 | *.[Rr]e[Ss]harper 127 | *.DotSettings.user 128 | 129 | # JustCode is a .NET coding add-in 130 | .JustCode 131 | 132 | # TeamCity is a build add-in 133 | _TeamCity* 134 | 135 | # DotCover is a Code Coverage Tool 136 | *.dotCover 137 | 138 | # AxoCover is a Code Coverage Tool 139 | .axoCover/* 140 | !.axoCover/settings.json 141 | 142 | # Visual Studio code coverage results 143 | *.coverage 144 | *.coveragexml 145 | 146 | # NCrunch 147 | _NCrunch_* 148 | .*crunch*.local.xml 149 | nCrunchTemp_* 150 | 151 | # MightyMoose 152 | *.mm.* 153 | AutoTest.Net/ 154 | 155 | # Web workbench (sass) 156 | .sass-cache/ 157 | 158 | # Installshield output folder 159 | [Ee]xpress/ 160 | 161 | # DocProject is a documentation generator add-in 162 | DocProject/buildhelp/ 163 | DocProject/Help/*.HxT 164 | DocProject/Help/*.HxC 165 | DocProject/Help/*.hhc 166 | DocProject/Help/*.hhk 167 | DocProject/Help/*.hhp 168 | DocProject/Help/Html2 169 | DocProject/Help/html 170 | 171 | # Click-Once directory 172 | publish/ 173 | 174 | # Publish Web Output 175 | *.[Pp]ublish.xml 176 | *.azurePubxml 177 | # Note: Comment the next line if you want to checkin your web deploy settings, 178 | # but database connection strings (with potential passwords) will be unencrypted 179 | *.pubxml 180 | *.publishproj 181 | 182 | # Microsoft Azure Web App publish settings. Comment the next line if you want to 183 | # checkin your Azure Web App publish settings, but sensitive information contained 184 | # in these scripts will be unencrypted 185 | PublishScripts/ 186 | 187 | # NuGet Packages 188 | *.nupkg 189 | # The packages folder can be ignored because of Package Restore 190 | **/[Pp]ackages/* 191 | # except build/, which is used as an MSBuild target. 192 | !**/[Pp]ackages/build/ 193 | # Uncomment if necessary however generally it will be regenerated when needed 194 | #!**/[Pp]ackages/repositories.config 195 | # NuGet v3's project.json files produces more ignorable files 196 | *.nuget.props 197 | *.nuget.targets 198 | 199 | # Microsoft Azure Build Output 200 | csx/ 201 | *.build.csdef 202 | 203 | # Microsoft Azure Emulator 204 | ecf/ 205 | rcf/ 206 | 207 | # Windows Store app package directories and files 208 | AppPackages/ 209 | BundleArtifacts/ 210 | Package.StoreAssociation.xml 211 | _pkginfo.txt 212 | *.appx 213 | 214 | # Visual Studio cache files 215 | # files ending in .cache can be ignored 216 | *.[Cc]ache 217 | # but keep track of directories ending in .cache 218 | !*.[Cc]ache/ 219 | 220 | # Others 221 | ClientBin/ 222 | ~$* 223 | *~ 224 | *.dbmdl 225 | *.dbproj.schemaview 226 | *.jfm 227 | *.pfx 228 | *.publishsettings 229 | orleans.codegen.cs 230 | 231 | # Including strong name files can present a security risk 232 | # (https://github.com/github/gitignore/pull/2483#issue-259490424) 233 | #*.snk 234 | 235 | # Since there are multiple workflows, uncomment next line to ignore bower_components 236 | # (https://github.com/github/gitignore/pull/1529#issuecomment-104372622) 237 | #bower_components/ 238 | 239 | # RIA/Silverlight projects 240 | Generated_Code/ 241 | 242 | # Backup & report files from converting an old project file 243 | # to a newer Visual Studio version. Backup files are not needed, 244 | # because we have git ;-) 245 | _UpgradeReport_Files/ 246 | Backup*/ 247 | UpgradeLog*.XML 248 | UpgradeLog*.htm 249 | ServiceFabricBackup/ 250 | *.rptproj.bak 251 | 252 | # SQL Server files 253 | *.mdf 254 | *.ldf 255 | *.ndf 256 | 257 | # Business Intelligence projects 258 | *.rdl.data 259 | *.bim.layout 260 | *.bim_*.settings 261 | *.rptproj.rsuser 262 | 263 | # Microsoft Fakes 264 | FakesAssemblies/ 265 | 266 | # GhostDoc plugin setting file 267 | *.GhostDoc.xml 268 | 269 | # Node.js Tools for Visual Studio 270 | .ntvs_analysis.dat 271 | node_modules/ 272 | 273 | # Visual Studio 6 build log 274 | *.plg 275 | 276 | # Visual Studio 6 workspace options file 277 | *.opt 278 | 279 | # Visual Studio 6 auto-generated workspace file (contains which files were open etc.) 280 | *.vbw 281 | 282 | # Visual Studio LightSwitch build output 283 | **/*.HTMLClient/GeneratedArtifacts 284 | **/*.DesktopClient/GeneratedArtifacts 285 | **/*.DesktopClient/ModelManifest.xml 286 | **/*.Server/GeneratedArtifacts 287 | **/*.Server/ModelManifest.xml 288 | _Pvt_Extensions 289 | 290 | # Paket dependency manager 291 | .paket/paket.exe 292 | paket-files/ 293 | 294 | # FAKE - F# Make 295 | .fake/ 296 | 297 | # JetBrains Rider 298 | .idea/ 299 | *.sln.iml 300 | 301 | # CodeRush 302 | .cr/ 303 | 304 | # Python Tools for Visual Studio (PTVS) 305 | __pycache__/ 306 | *.pyc 307 | 308 | # Cake - Uncomment if you are using it 309 | # tools/** 310 | # !tools/packages.config 311 | 312 | # Tabs Studio 313 | *.tss 314 | 315 | # Telerik's JustMock configuration file 316 | *.jmconfig 317 | 318 | # BizTalk build output 319 | *.btp.cs 320 | *.btm.cs 321 | *.odx.cs 322 | *.xsd.cs 323 | 324 | # OpenCover UI analysis results 325 | OpenCover/ 326 | 327 | # Azure Stream Analytics local run output 328 | ASALocalRun/ 329 | 330 | # MSBuild Binary and Structured Log 331 | *.binlog 332 | 333 | # NVidia Nsight GPU debugger configuration file 334 | *.nvuser 335 | 336 | # MFractors (Xamarin productivity tool) working folder 337 | .mfractor/ 338 | 339 | # Local History for Visual Studio 340 | .localhistory/ 341 | 342 | 343 | # End of https://www.gitignore.io/api/visualstudio -------------------------------------------------------------------------------- /AStar/PathFinder.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Collections.Generic; 3 | using System.Drawing; 4 | using System.Linq; 5 | using AStar.Collections.PathFinder; 6 | using AStar.Heuristics; 7 | using AStar.Options; 8 | 9 | namespace AStar 10 | { 11 | public class PathFinder : IFindAPath 12 | { 13 | private const int ClosedValue = 0; 14 | private const int DistanceBetweenNodes = 1; 15 | private readonly PathFinderOptions _options; 16 | private readonly WorldGrid _world; 17 | private readonly ICalculateHeuristic _heuristic; 18 | 19 | public PathFinder(WorldGrid worldGrid, PathFinderOptions pathFinderOptions = null) 20 | { 21 | _world = worldGrid ?? throw new ArgumentNullException(nameof(worldGrid)); 22 | _options = pathFinderOptions ?? new PathFinderOptions(); 23 | _heuristic = HeuristicFactory.Create(_options.HeuristicFormula); 24 | } 25 | 26 | /// 27 | public Point[] FindPath(Point start, Point end) 28 | { 29 | return FindPath(new Position(start.Y, start.X), new Position(end.Y, end.X)) 30 | .Select(position => new Point(position.Column, position.Row)) 31 | .ToArray(); 32 | } 33 | 34 | /// 35 | public Position[] FindPath(Position start, Position end) 36 | { 37 | var nodesVisited = 0; 38 | IModelAGraph graph = new PathFinderGraph(_world.Height, _world.Width, _options.UseDiagonals); 39 | 40 | var startNode = new PathFinderNode(position: start, g: 0, h: 2, parentNodePosition: start); 41 | graph.OpenNode(startNode); 42 | 43 | while (graph.HasOpenNodes) 44 | { 45 | var q = graph.GetOpenNodeWithSmallestF(); 46 | 47 | if (q.Position == end) 48 | { 49 | return OrderClosedNodesAsArray(graph, q); 50 | } 51 | 52 | if (nodesVisited > _options.SearchLimit) 53 | { 54 | return new Position[0]; 55 | } 56 | 57 | foreach (var successor in graph.GetSuccessors(q)) 58 | { 59 | if (_world[successor.Position] == ClosedValue) 60 | { 61 | 62 | // https://github.com/valantonini/AStar/issues/11 63 | if (successor.Position == end && _options.IgnoreClosedEndCell) 64 | { 65 | //fallthrough so we find it on next iteration 66 | } 67 | else 68 | { 69 | // node is closed, check next successor 70 | continue; 71 | } 72 | } 73 | 74 | var newG = q.G + DistanceBetweenNodes; 75 | 76 | if (_options.PunishChangeDirection) 77 | { 78 | newG += CalculateModifierToG(q, successor, end); 79 | } 80 | 81 | var newH = _heuristic.Calculate(successor.Position, end); 82 | switch (_options.Weighting) 83 | { 84 | case Weighting.Positive: 85 | newH -= _world[successor.Position]; 86 | break; 87 | case Weighting.Negative: 88 | newH += _world[successor.Position]; 89 | break; 90 | case Weighting.None: 91 | default: 92 | break; 93 | } 94 | 95 | var updatedSuccessor = new PathFinderNode( 96 | position: successor.Position, 97 | g: newG, 98 | h: newH, 99 | parentNodePosition: q.Position); 100 | 101 | if (BetterPathToSuccessorFound(updatedSuccessor, successor)) 102 | { 103 | graph.OpenNode(updatedSuccessor); 104 | } 105 | } 106 | 107 | nodesVisited++; 108 | } 109 | 110 | return new Position[0]; 111 | } 112 | 113 | private int CalculateModifierToG(PathFinderNode q, PathFinderNode successor, Position end) 114 | { 115 | if (q.Position == q.ParentNodePosition) 116 | { 117 | return 0; 118 | } 119 | 120 | var gPunishment = Math.Abs(successor.Position.Row - end.Row) + Math.Abs(successor.Position.Column - end.Column); 121 | 122 | var successorIsVerticallyAdjacentToQ = successor.Position.Row - q.Position.Row != 0; 123 | 124 | if (successorIsVerticallyAdjacentToQ) 125 | { 126 | var qIsVerticallyAdjacentToParent = q.Position.Row - q.ParentNodePosition.Row == 0; 127 | if (qIsVerticallyAdjacentToParent) 128 | { 129 | return gPunishment; 130 | } 131 | } 132 | 133 | var successorIsHorizontallyAdjacentToQ = successor.Position.Row - q.Position.Row != 0; 134 | 135 | if (successorIsHorizontallyAdjacentToQ) 136 | { 137 | var qIsHorizontallyAdjacentToParent = q.Position.Row - q.ParentNodePosition.Row == 0; 138 | if (qIsHorizontallyAdjacentToParent) 139 | { 140 | return gPunishment; 141 | } 142 | } 143 | 144 | if (_options.UseDiagonals) 145 | { 146 | var successorIsDiagonallyAdjacentToQ = (successor.Position.Column - successor.Position.Row) == (q.Position.Column - q.Position.Row); 147 | if (successorIsDiagonallyAdjacentToQ) 148 | { 149 | var qIsDiagonallyAdjacentToParent = (q.Position.Column - q.Position.Row) == (q.ParentNodePosition.Column - q.ParentNodePosition.Row) 150 | && IsStraightLine(q.ParentNodePosition, q.Position, successor.Position); 151 | if (qIsDiagonallyAdjacentToParent) 152 | { 153 | return gPunishment; 154 | } 155 | } 156 | } 157 | 158 | return 0; 159 | } 160 | 161 | private bool IsStraightLine(Position a, Position b, Position c) 162 | { 163 | // area of triangle == 0 164 | return (a.Column * (b.Row - c.Row) + b.Column * (c.Row - a.Row) + c.Column * (a.Row - b.Row)) / 2 == 0; 165 | } 166 | 167 | private bool BetterPathToSuccessorFound(PathFinderNode updateSuccessor, PathFinderNode currentSuccessor) 168 | { 169 | return !currentSuccessor.HasBeenVisited || 170 | (currentSuccessor.HasBeenVisited && updateSuccessor.F < currentSuccessor.F); 171 | } 172 | 173 | private static Position[] OrderClosedNodesAsArray(IModelAGraph graph, PathFinderNode endNode) 174 | { 175 | var path = new Stack(); 176 | 177 | var currentNode = endNode; 178 | 179 | while (currentNode.Position != currentNode.ParentNodePosition) 180 | { 181 | path.Push(currentNode.Position); 182 | currentNode = graph.GetParent(currentNode); 183 | } 184 | 185 | path.Push(currentNode.Position); 186 | 187 | return path.ToArray(); 188 | } 189 | } 190 | } 191 | --------------------------------------------------------------------------------