#35 AStar

Provided simple realization for A* algo. Added 2d Matrix playground for test purposes.

Author: Leefrost

Algorithms/Algorithms.Tests/AStarTests/AStarTests.cs

@@ -0,0 +1,29 @@
+using Algorithms.AStar;
+using Algorithms.Tests.AStarTests.TestField;
+using FluentAssertions;
+using Xunit;
+
+namespace Algorithms.Tests.AStarTests
+{
+    public class AStarTests
+    {
+        [Fact]
+        void AStar_RunOnStable3On3Filed_ReturnCorrectPath()
+        {
+            /* The way
+             * |F.W|
+             * |W..|
+             * |WWF|
+             */
+
+            var matrix = new MatrixField();
+            var search = new AStarSearch(matrix.Start, matrix.Goal);
+            var expectedPath = "s.W\nW..\nWWg\n";
+
+            search.Run();
+            var foundPath = matrix.Print(search.GetPath());
+
+            foundPath.Should().Be(expectedPath);
+        }
+    }
+}

Algorithms/Algorithms.Tests/AStarTests/TestField/Matrix.cs

@@ -0,0 +1,59 @@
+using System;
+using System.Collections.Generic;
+using Algorithms.AStar;
+
+namespace Algorithms.Tests.AStarTests.TestField
+{
+    public class MatrixField
+    {
+        public MatrixNode[][] Matrix;
+
+        public int Width { get { return Matrix.Length; } }
+        public int Height { get { return Matrix[0].Length; } }
+
+        public MatrixNode Start;
+        public MatrixNode Goal;
+
+        /// <example>
+        /// |S W|
+        /// |W  |
+        /// |W F|
+        /// </example>
+        public MatrixField()
+        {
+            Start = new MatrixNode(this, 0, 0, false);
+            Goal = new MatrixNode(this, 2, 2, false);
+
+            Matrix = new MatrixNode[3][];
+            for (var i = 0; i < 3; i++)
+                Matrix[i] = new MatrixNode[3];
+
+            Matrix[Start.X][Start.Y] = Start;
+            Matrix[Goal.X][Goal.Y] = Goal;
+
+            Matrix[0][1] = new MatrixNode(this, 0, 2, false);
+            Matrix[0][2] = new MatrixNode(this, 0, 2, true);
+
+            Matrix[1][0] = new MatrixNode(this, 1, 0, true);
+            Matrix[1][1] = new MatrixNode(this, 1, 0, false);
+            Matrix[1][2] = new MatrixNode(this, 1, 0, false);
+
+            Matrix[2][0] = new MatrixNode(this, 2, 0, true);
+            Matrix[2][1] = new MatrixNode(this, 2, 1, true);
+        }
+
+        public string Print(IEnumerable<IWayNode> path)
+        {
+            var output = "";
+            for (var i = 0; i < Width; i++)
+            {
+                for (var j = 0; j < Height; j++)
+                {
+                    output += Matrix[i][j].Print(Start, Goal, path);
+                }
+                output += "\n";
+            }
+            return output;
+        }
+    }
+}

Algorithms/Algorithms.Tests/AStarTests/TestField/MatrixNode.cs

@@ -0,0 +1,133 @@
+using System;
+using System.Collections.Generic;
+using Algorithms.AStar;
+
+namespace Algorithms.Tests.AStarTests.TestField
+{
+    public class MatrixNode : IWayNode
+    {
+        private MatrixField Matrix;
+
+        private bool isOpenList = false;
+        private bool isClosedList = false;
+        
+        private static int[] childXPos = new int[] { 0, -1, 1, 0, };
+        private static readonly int[] childYPos = new int[] { -1, 0, 0, 1, };
+
+        public int X { get; private set; }
+        public int Y { get; private set; }
+        public bool IsWall { get; set; }
+        
+        public MatrixNode(MatrixField matrix, int x, int y, bool isWall)
+        {
+            Matrix = matrix;
+            X = x;
+            Y = y;
+            IsWall = isWall;
+        }
+
+        public IWayNode Parent { get; set; }
+
+        public IEnumerable<IWayNode> Children
+        {
+            get
+            {
+                var children = new List<MatrixNode>();
+
+                for (int i = 0; i < childXPos.Length; i++)
+                {
+                    // skip any nodes out of bounds.
+                    if (X + childXPos[i] >= Matrix.Width || Y + childYPos[i] >= Matrix.Height)
+                        continue;
+                    if (X + childXPos[i] < 0 || Y + childYPos[i] < 0)
+                        continue;
+
+                    children.Add(Matrix.Matrix[X + childXPos[i]][Y + childYPos[i]]);
+                }
+
+                return children;
+            }
+        }
+
+        public int TotalCost { get { return MovementCost + EstimatedCost; } }
+
+        public int MovementCost { get; private set; }
+
+        public int EstimatedCost { get; private set; }
+
+        public bool IsOpenNodes(IEnumerable<IWayNode> openNodes)
+        {
+            return isOpenList;
+        }
+
+        public void SetOpenNode(bool value)
+        {
+            isOpenList = value;
+        }
+
+        public bool IsClosedNodes(IEnumerable<IWayNode> closedNodes)
+        {
+            return IsWall || isClosedList;
+        }
+
+        public void SetClosedNode(bool value)
+        {
+            isClosedList = value;
+        }
+
+        public void SetMovementCost(IWayNode parent)
+        {
+            MovementCost = parent.MovementCost + 1;
+        }
+
+        public void SetEstimatedCost(IWayNode goal)
+        {
+            var goalNode = (MatrixNode)goal;
+            EstimatedCost = Math.Abs(X - goalNode.X) + Math.Abs(Y - goalNode.Y);
+        }
+
+        public bool IsGoal(IWayNode goal)
+        {
+            return IsEqual((MatrixNode)goal);
+        }
+
+        public bool IsEqual(MatrixNode node)
+        {
+            return (this == node) || (this.X == node.X && this.Y == node.Y);
+        }
+
+        public string Print(MatrixNode start, MatrixNode goal, IEnumerable<IWayNode> path)
+        {
+            if (IsWall)
+            {
+                return "W";
+            }
+            else if (IsEqual(start))
+            {
+                return "s";
+            }
+            else if (IsEqual(goal))
+            {
+                return "g";
+            }
+            else if (IsInPath(path))
+            {
+                return ".";
+            }
+            else
+            {
+                return " ";
+            }
+        }
+
+        private bool IsInPath(IEnumerable<IWayNode> path)
+        {
+            foreach (var node in path)
+            {
+                if (IsEqual((MatrixNode)node))
+                    return true;
+            }
+            return false;
+        }
+    }
+}

Algorithms/Algorithms/AStar/AStar.cs

@@ -0,0 +1,127 @@
+using System.Collections.Generic;
+
+namespace Algorithms.AStar
+{
+    public enum State
+    {
+        Searching,
+        GoalFound,
+        Failed
+    }
+
+    internal class DuplicateComparer : IComparer<int>
+    {
+        public int Compare(int x, int y)
+        {
+            return x <= y ? -1 : 1;
+        }
+    }
+
+    public class AStarSearch
+    {
+        private readonly SortedList<int, IWayNode> _openNodes;
+        private readonly SortedList<int, IWayNode> _closeNodes;
+
+        public IEnumerable<IWayNode> OpenNodes => _openNodes.Values;
+        public IEnumerable<IWayNode> ClosedNodes => _closeNodes.Values;
+
+        public IWayNode Current { get; private set; }
+        public IWayNode Goal { get; private set; }
+
+        public int Steps { get; private set; }
+
+        public AStarSearch(IWayNode start, IWayNode goal)
+        {
+            var duplicateComparer = new DuplicateComparer();
+            _openNodes = new SortedList<int, IWayNode>(duplicateComparer);
+            _closeNodes = new SortedList<int, IWayNode>(duplicateComparer);
+
+            Reset(start, goal);
+        }
+
+        public void Reset(IWayNode start, IWayNode goal)
+        {
+            _openNodes.Clear();
+            _closeNodes.Clear();
+
+            Current = start;
+            Goal = goal;
+
+            _openNodes.Add(Current);
+            Current.SetOpenNode(true);
+        }
+
+        public State Run()
+        {
+            while (true)
+            {
+                var state = NextStep();
+                if (state != State.Searching)
+                    return state;
+            }
+        }
+
+        private State NextStep()
+        {
+            Steps++;
+            while (true)
+            {
+                if (_openNodes.IsEmpty())
+                {
+                    return State.Failed;
+                }
+
+                Current = _openNodes.Pop();
+                if (Current.IsClosedNodes(ClosedNodes))
+                {
+                    continue;
+                }
+
+                break;
+            }
+
+            Current.SetOpenNode(false);
+            _closeNodes.Add(Current);
+            Current.SetClosedNode(true);
+
+            if (Current.IsGoal(Goal))
+            {
+                return State.GoalFound;
+            }
+
+            foreach (var child in Current.Children)
+            {
+                if (child.IsOpenNodes(OpenNodes) || child.IsClosedNodes(ClosedNodes))
+                {
+                    continue;
+                }
+
+                child.Parent = Current;
+                child.SetMovementCost(Current);
+                child.SetEstimatedCost(Goal);
+
+                _openNodes.Add(child);
+                child.SetOpenNode(true);
+            }
+
+            return State.Searching;
+        }
+
+        public IEnumerable<IWayNode> GetPath()
+        {
+            if (Current != null)
+            {
+                var next = Current;
+                var path = new List<IWayNode>();
+                while (next != null)
+                {
+                    path.Add(next);
+                    next = next.Parent;
+                }
+                path.Reverse();
+                return path.ToArray();
+            }
+            return null;
+        }
+    }
+}

Algorithms/Algorithms/AStar/Extensions.cs

@@ -0,0 +1,24 @@
+using System.Collections.Generic;
+
+namespace Algorithms.AStar
+{
+    internal static class Extensions
+    {
+        internal static bool IsEmpty<TKey, TValue>(this SortedList<TKey, TValue> sortedList)
+        {
+            return sortedList.Count == 0;
+        }
+
+        internal static void Add(this SortedList<int, IWayNode> sortedList, IWayNode wayNode)
+        {
+            sortedList.Add(wayNode.TotalCost, wayNode);
+        }
+
+        internal static IWayNode Pop(this SortedList<int, IWayNode> sortedList)
+        {
+            var top = sortedList.Values[0];
+            sortedList.RemoveAt(0);
+            return top;
+        }
+    }
+}