AStarPathfinding.cs

using System.Collections.Generic;
using UnityEngine;

namespace DeltaTimer
{
    public interface IPathfindingWorld<T>
    {
        bool IsPositionLegalToPassThrough(T unit, Vector2Int position);
    }

    public class AStarPathfinding<T>
    {
        private class Node
        {
            public Node parent;
            public Vector2Int position;
            public int g = int.MaxValue;
            public int h;

            public int f { get { return g + h; } }
        }

        private static readonly Vector2Int[] CHECK_DIRECTIONS = new Vector2Int[] {
            new Vector2Int( 0, 1 ), new Vector2Int( 0, -1 ),
            new Vector2Int( 1, 0 ), new Vector2Int( -1, 0 ),
            new Vector2Int( 1, 1 ), new Vector2Int( 1, -1 ),
            new Vector2Int( -1, 1 ), new Vector2Int( -1, -1 ),
        };

        private readonly List<Node> close = new List<Node>(32);
        private readonly List<Node> open = new List<Node>(32);
        private readonly List<Vector2Int> result = new List<Vector2Int>(32);

        public List<Vector2Int> FindPath(IPathfindingWorld<T> world, T unit, Vector2Int start, Vector2Int end)
        {
            close.Clear();
            open.Clear();
            result.Clear();

            open.Add(new Node { position = start, g = 0, });

            while (open.Count > 0)
            {
                open.Sort((Node a, Node b) =>
                {
                    return a.f.CompareTo(b.f);
                });

                Node lowestValue = open[0];
                if (lowestValue.position == end)  // Find the target position, let's get the path by iterating parents
                {
                    Node current = lowestValue;
                    result.Insert(0, current.position);
                    while (current.parent != null)
                    {
                        current = current.parent;
                        result.Insert(0, current.position);
                    }

                    return result;
                }
                else
                {
                    open.RemoveAt(0);
                    close.Add(lowestValue);

                    for (var i = 0; i < CHECK_DIRECTIONS.Length; i++)
                    {
                        Vector2Int checkPosition = lowestValue.position + CHECK_DIRECTIONS[i];

                        // If the position cannot be walked through, skip this position
                        if (world.IsPositionLegalToPassThrough(unit, checkPosition) == false)
                        {
                            continue;
                        }

                        var checkNode = new Node() { position = checkPosition };

                        if (close.Exists((Node node) => { return node.position == checkPosition; }))
                        {
                            continue;
                        }

                        if (open.Exists((Node node) => { return node.position == checkPosition; }) == false)
                        {
                            open.Add(checkNode);
                        }

                        int tempG = lowestValue.g + 1;
                        if (tempG >= checkNode.g)
                        {
                            continue;
                        }

                        checkNode.parent = lowestValue;
                        checkNode.g = tempG;
                    }
                }
            }

            // Failed to find any path, so returns an empty path
            return result;
        }
    }
}

AStarPathfindingTests.cs

using System.Collections.Generic;
using NUnit.Framework;
using UnityEngine;

namespace DeltaTimer
{
    public class AStarPathfindingTests
    {
        [Test]
        public void FindPath_OpenRount()
        {
            var world = new FakePathfindingWorld(10, 10);
            var pathfinding = new AStarPathfinding<object>();
            List<Vector2Int> actual = pathfinding.FindPath(world, new object(), new Vector2Int(2, 3), new Vector2Int(2, 5));
            var expected = new List<Vector2Int>
            {
                new Vector2Int(2, 3), new Vector2Int(2, 4), new Vector2Int(2, 5),
            };

            Assert.AreEqual(expected.Count, actual.Count, "Path count mismatch");
            for (var i = 0; i < expected.Count; i++)
            {
                Assert.AreEqual(expected[i], actual[i], "Mismatch at Node {0}", i);
            }
        }

        [Test]
        public void FindPath_HasObstacle()
        {
            var world = new FakePathfindingWorld(10, 10);
            world.SetObstacle(new Vector2Int(2, 4));

            var pathfinding = new AStarPathfinding<object>();
            List<Vector2Int> actual = pathfinding.FindPath(world, new object(), new Vector2Int(2, 3), new Vector2Int(2, 5));
            var expected = new List<Vector2Int>
            {
                new Vector2Int(2, 3), new Vector2Int(3, 4), new Vector2Int(2, 5),
            };

            Assert.AreEqual(expected.Count, actual.Count, "Path count mismatch");
            for (var i = 0; i < expected.Count; i++)
            {
                Assert.AreEqual(expected[i], actual[i], "Mismatch at Node {0}", i);
            }
        }

        [Test]
        public void FindPath_EndIsStart()
        {
            var world = new FakePathfindingWorld(10, 10);
            var pathfinding = new AStarPathfinding<object>();
            List<Vector2Int> actual = pathfinding.FindPath(world, new object(), new Vector2Int(2, 3), new Vector2Int(2, 3));
            var expected = new List<Vector2Int>
            {
                new Vector2Int(2, 3),
            };

            Assert.AreEqual(expected.Count, actual.Count, "Path count mismatch");
            for (var i = 0; i < expected.Count; i++)
            {
                Assert.AreEqual(expected[i], actual[i], "Mismatch at Node {0}", i);
            }
        }

        [Test]
        public void FindPath_CannotStepOnTarget()
        {
            var world = new FakePathfindingWorld(10, 10);
            world.SetObstacle(new Vector2Int(2, 5));

            var pathfinding = new AStarPathfinding<object>();
            List<Vector2Int> actual = pathfinding.FindPath(world, new object(), new Vector2Int(2, 3), new Vector2Int(2, 5));
            var expected = new List<Vector2Int>();

            Assert.AreEqual(expected.Count, actual.Count, "Path count mismatch");
            for (var i = 0; i < expected.Count; i++)
            {
                Assert.AreEqual(expected[i], actual[i], "Mismatch at Node {0}", i);
            }
        }

        [Test]
        public void FindPath_ObstacleSurrounded()
        {
            var world = new FakePathfindingWorld(10, 10);
            world.SetObstacle(new Vector2Int(1, 2));
            world.SetObstacle(new Vector2Int(1, 3));
            world.SetObstacle(new Vector2Int(1, 4));
            world.SetObstacle(new Vector2Int(2, 2));
            world.SetObstacle(new Vector2Int(2, 4));
            world.SetObstacle(new Vector2Int(3, 2));
            world.SetObstacle(new Vector2Int(3, 3));
            world.SetObstacle(new Vector2Int(3, 4));

            var pathfinding = new AStarPathfinding<object>();
            List<Vector2Int> actual = pathfinding.FindPath(world, new object(), new Vector2Int(2, 3), new Vector2Int(2, 5));
            var expected = new List<Vector2Int>();

            Assert.AreEqual(expected.Count, actual.Count, "Path count mismatch");
            for (var i = 0; i < expected.Count; i++)
            {
                Assert.AreEqual(expected[i], actual[i], "Mismatch at Node {0}", i);
            }
        }
    }

    public class FakePathfindingWorld : IPathfindingWorld<object>
    {
        private readonly Vector2Int m_size;
        private readonly HashSet<Vector2Int> m_obstacles = new HashSet<Vector2Int>();

        public FakePathfindingWorld(int sizeX, int sizeY)
        {
            m_size = new Vector2Int(sizeX, sizeY);
        }

        public void SetObstacle(Vector2Int position)
        {
            m_obstacles.Add(position);
        }

        public bool IsPositionLegalToPassThrough(object unit, Vector2Int position)
        {
            if (position.x < 0 || position.y < 0 || position.x >= m_size.x || position.y >= m_size.y)
            {
                return false;
            }

            return !m_obstacles.Contains(position);
        }
    }
}