Vector2Int in description

AStarSearchNonAlloc.cs

// This is a C# example from Red Blob Games (https://www.redblobgames.com/pathfinding/a-star/implementation.html#csharp). Remade to have no GCAllocs
// Changes:
// - Made to work in Unity and draw gizmos on screen instead of console text;
// - Cache all collections in Init(), then when Search() is called, no allocs are being made;
// - Uses Unity's Vector2Int that avoids boxing when comparing structs;
// - graph.Neighbors() no longer uses an IEnumerable<>. instead, fill a list with neighbors;

using System;
using System.Collections.Generic;
using UnityEngine;
using UnityEngine.Profiling;


// A* needs only a WeightedGraph and a Vector2Int type L, and does *not*
// have to be a grid. However, in the example code I am using a grid.
public interface WeightedGraph<L>
{
    double Cost(L a, L b);
    void NeighborsNonAlloc(ref List<L> list, L id);
    int TotalSize { get; }
}

public class SquareGrid : WeightedGraph<Vector2Int>
{
    // Implementation notes: I made the fields public for convenience,
    // but in a real project you'll probably want to follow standard
    // style and make them private.

    public readonly Vector2Int[] DIRS = new[]
        {
            new Vector2Int(1, 0),
            new Vector2Int(0, -1),
            new Vector2Int(-1, 0),
            new Vector2Int(0, 1)
        };

    public int width, height;
    public HashSet<Vector2Int> walls = new HashSet<Vector2Int>();
    public HashSet<Vector2Int> forests = new HashSet<Vector2Int>();

    public int TotalSize => width * height;

    public SquareGrid(int width, int height)
    {
        this.width = width;
        this.height = height;
    }

    public bool InBounds(Vector2Int id)
    {
        return 0 <= id.x && id.x < width
            && 0 <= id.y && id.y < height;
    }

    public bool Passable(Vector2Int id)
    {
        return !walls.Contains(id);
    }

    public double Cost(Vector2Int a, Vector2Int b)
    {
        return forests.Contains(b) ? 5 : 1;
    }

    public void NeighborsNonAlloc(ref List<Vector2Int> list, Vector2Int id)
    {
        list.Clear();

        for (int i = 0; i < DIRS.Length; i++)
        {
            Vector2Int next = new Vector2Int(id.x + DIRS[i].x, id.y + DIRS[i].y);
            if (InBounds(next) && Passable(next))
            {
                list.Add(next);
            }
        }
    }
}


public class PriorityQueue<T>
{
    // I'm using an unsorted array for this example, but ideally this
    // would be a binary heap. There's an open issue for adding a binary
    // heap to the standard C# library: https://github.com/dotnet/corefx/issues/574
    //
    // Until then, find a binary heap class:
    // * https://github.com/BlueRaja/High-Speed-Priority-Queue-for-C-Sharp
    // * http://visualstudiomagazine.com/articles/2012/11/01/priority-queues-with-c.aspx
    // * http://xfleury.github.io/graphsearch.html
    // * http://stackoverflow.com/questions/102398/priority-queue-in-net

    private List<(T, double)> elements = new List<(T, double)>();

    public int Count
    {
        get { return elements.Count; }
    }

    public void Enqueue(T item, double priority)
    {
        elements.Add((item, priority));
    }

    public T Dequeue()
    {
        int bestIndex = 0;

        for (int i = 0; i < elements.Count; i++)
        {
            if (elements[i].Item2 < elements[bestIndex].Item2)
            {
                bestIndex = i;
            }
        }

        T bestItem = elements[bestIndex].Item1;
        elements.RemoveAt(bestIndex);
        return bestItem;
    }

    public void Clear()
    {
        elements.Clear();
    }
}


/* NOTE about types: in the main article, in the Python code I just
 * use numbers for costs, heuristics, and priorities. In the C++ code
 * I use a typedef for this, because you might want int or double or
 * another type. In this C# code I use double for costs, heuristics,
 * and priorities. You can use an int if you know your values are
 * always integers, and you can use a smaller size number if you know
 * the values are always small. */

public class AStarSearch
{
    public Dictionary<Vector2Int, Vector2Int> cameFrom;
    public Dictionary<Vector2Int, double> costSoFar;
    PriorityQueue<Vector2Int> frontier;
    List<Vector2Int> neighbors;

    // Note: a generic version of A* would abstract over Vector2Int and
    // also Heuristic
    static public double Heuristic(Vector2Int a, Vector2Int b)
    {
        return Math.Abs(a.x - b.x) + Math.Abs(a.y - b.y);
    }

    public void Search(WeightedGraph<Vector2Int> graph, Vector2Int start, Vector2Int goal)
    {
        Profiler.BeginSample("Search");
        cameFrom.Clear();
        costSoFar.Clear();
        frontier.Clear();

        frontier.Enqueue(start, 0);

        cameFrom[start] = start;
        costSoFar[start] = 0;

        while (frontier.Count > 0)
        {
            var current = frontier.Dequeue();

            if (current.Equals(goal))
            {
                break;
            }

            graph.NeighborsNonAlloc(ref neighbors, current);

            for (int i = 0; i < neighbors.Count; i++)
            {
                var next = neighbors[i];

                double newCost = costSoFar[current]
                    + graph.Cost(current, next);
                if (!costSoFar.ContainsKey(next)
                    || newCost < costSoFar[next])
                {
                    costSoFar[next] = newCost;
                    double priority = newCost + Heuristic(next, goal);
                    frontier.Enqueue(next, priority);
                    cameFrom[next] = current;
                }
            }
        }
        Profiler.EndSample();
    }

    public AStarSearch(WeightedGraph<Vector2Int> graph, Vector2Int start, Vector2Int goal)
    {
        cameFrom = new Dictionary<Vector2Int, Vector2Int>(graph.TotalSize);
        costSoFar = new Dictionary<Vector2Int, double>(graph.TotalSize);
        frontier = new PriorityQueue<Vector2Int>();
        neighbors = new List<Vector2Int>(4);
        Search(graph, start, goal);
    }
}

public class TestAStar
{
    SquareGrid grid;
    AStarSearch astar;

    public Vector2Int source = new Vector2Int(1, 4);
    public Vector2Int target = new Vector2Int(8, 5);

    public void DrawGridGizmos()
    {
        DebugGrid(grid, astar);

        Gizmos.color = Color.cyan;
        Gizmos.DrawCube(new Vector3(source.x, source.y), Vector3.one);
        Gizmos.color = Color.green;
        Gizmos.DrawCube(new Vector3(target.x, target.y), Vector3.one);
    }

    public static void DebugGrid(SquareGrid grid, AStarSearch astar)
    {

        // Print out the cameFrom array
        for (var y = 0; y < 10; y++)
        {
            for (var x = 0; x < 10; x++)
            {
                var pos = new Vector3(x, y);
                Vector2Int id = new Vector2Int(x, y);
                Vector2Int ptr = id;
                if (!astar.cameFrom.TryGetValue(id, out ptr))
                {
                    ptr = id;
                }

                Vector3 ptrPos = new Vector3(ptr.x, ptr.y);



                if (grid.walls.Contains(id))
                {
                    //Console.Write("##");
                    Gizmos.color = Color.red;
                    Gizmos.DrawWireCube(new Vector3(id.x, id.y), Vector3.one);
                }
                else if (ptr.x == x + 1)
                {
                    //Console.Write("\u2192 ");
                    Gizmos.color = Color.yellow;
                    Gizmos.DrawLine(pos, ptrPos);
                }
                else if (ptr.x == x - 1)
                {
                    //Console.Write("\u2190 ");
                    Gizmos.color = Color.yellow;
                    Gizmos.DrawLine(pos, ptrPos);
                }
                else if (ptr.y == y + 1)
                {
                    //Console.Write("\u2193 ");
                    Gizmos.color = Color.yellow;
                    Gizmos.DrawLine(pos, ptrPos);
                }
                else if (ptr.y == y - 1)
                {
                    //Console.Write("\u2191 ");
                    Gizmos.color = Color.yellow;
                    Gizmos.DrawLine(pos, ptrPos);
                }
                else
                {
                    Gizmos.color = Color.grey;
                    Gizmos.DrawWireCube(pos + new Vector3(0, 0, 1), Vector3.one);
                    Console.Write("* ");
                }

                if (grid.forests.Contains(id))
                {
                    Gizmos.color = Color.green;
                    Gizmos.DrawWireCube(new Vector3(id.x, id.y), Vector3.one);
                }
            }
            Console.WriteLine();
        }
    }

    public void Init()
    {
        // Make "diagram 4" from main article
        grid = new SquareGrid(10, 10);
        for (var x = 1; x < 4; x++)
        {
            for (var y = 7; y < 9; y++)
            {
                grid.walls.Add(new Vector2Int(x, y));
            }
        }
        grid.forests = new HashSet<Vector2Int>
            {
                new Vector2Int(3, 4), new Vector2Int(3, 5),
                new Vector2Int(4, 1), new Vector2Int(4, 2),
                new Vector2Int(4, 3), new Vector2Int(4, 4),
                new Vector2Int(4, 5), new Vector2Int(4, 6),
                new Vector2Int(4, 7), new Vector2Int(4, 8),
                new Vector2Int(5, 1), new Vector2Int(5, 2),
                new Vector2Int(5, 3), new Vector2Int(5, 4),
                new Vector2Int(5, 5), new Vector2Int(5, 6),
                new Vector2Int(5, 7), new Vector2Int(5, 8),
                new Vector2Int(6, 2), new Vector2Int(6, 3),
                new Vector2Int(6, 4), new Vector2Int(6, 5),
                new Vector2Int(6, 6), new Vector2Int(6, 7),
                new Vector2Int(7, 3), new Vector2Int(7, 4),
                new Vector2Int(7, 5)
            };

        astar = new AStarSearch(grid,
            new Vector2Int(source.x, source.y),
            new Vector2Int(target.x, target.y));
    }

    public void Main()
    {
        // Run A*
        astar.Search(grid,
            new Vector2Int(source.x, source.y),
            new Vector2Int(target.x, target.y));

        //DrawGrid(grid, astar);
    }
}