C# A*算法

自己用Unity实现了一下
https://gitee.com/cqmyg13/Astar
主要关键词有开表，闭表，优先级队列，广度优先搜索，排序，启发式搜索，麦哈顿距离
引用: https://www.redblobgames.com/pathfinding/a-star/implementation.html#csharp
上面链接的代码不完整，我对其进行了修改完善，效果如下：

输出("P"是路径, “#” 是墙, "*"是树)：

using System;
using System.Collections.Generic;


// A* 寻路不一定是网格，可以是其他类型，这里是网格

// A到B的权重图接口
public interface WeightedGraph<L>
{
    double Cost(Location a, Location b);
    IEnumerable<Location> Neighbors(Location id);
}

// 地理坐标 (x, y)
public struct Location
{
    // Implementation notes: I am using the default Equals but it can
    // be slow. You'll probably want to override both Equals and
    // GetHashCode in a real project.

    public readonly int x, y;
    public Location(int x, int y)
    {
        this.x = x;
        this.y = y;
    }

    // 重写 equals
    public override bool Equals(Object obj)
    {
        Location p = (Location)obj;
        return (this.x == p.x) && (this.y == p.y);
    }

    public override int GetHashCode()
    {
        return base.GetHashCode();
    }

}


public class SquareGrid : WeightedGraph<Location>
{
    //  真实项目最好设为private
    public static readonly Location[] DIRS = new[]
        {
            new Location(1, 0),
            new Location(0, -1),
            new Location(-1, 0),
            new Location(0, 1)
        };

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

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

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

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

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

    public IEnumerable<Location> Neighbors(Location id)
    {
        foreach (var dir in DIRS)
        {
            Location next = new Location(id.x + dir.x, id.y + dir.y);
            if (InBounds(next) && Passable(next))
            {
                yield return 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<Tuple<T, double>> elements = new List<Tuple<T, double>>();

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

    public void Enqueue(T item, double priority)
    {
        elements.Add(Tuple.Create(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;
    }
}


/* 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<Location, Location> cameFrom
        = new Dictionary<Location, Location>();
    public Dictionary<Location, double> costSoFar
        = new Dictionary<Location, double>();

    // Note: a generic version of A* would abstract over Location and
    // also Heuristic
    // 启发函数 曼哈顿距离 出租车几何
    static public double Heuristic(Location a, Location b)
    {
        return Math.Abs(a.x - b.x) + Math.Abs(a.y - b.y);
    }

    public AStarSearch(WeightedGraph<Location> graph, Location start, Location goal)
    {
        var frontier = new PriorityQueue<Location>();
        frontier.Enqueue(start, 0);

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

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

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

            foreach (var next in graph.Neighbors(current))
            {
                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;
                }
            }
        }
    }
}

public class Test
{
    // 初始化
    static void DrawGridPlant(SquareGrid grid, AStarSearch astar, List<Location> path)
    {
        Console.WriteLine("A* Init:");
        // Print out the cameFrom array
        for (var y = 0; y < 10; y++)
        {
            for (var x = 0; x < 10; x++)
            {
                Location id = new Location(x, y);
                Location ptr = id;
                if (!astar.cameFrom.TryGetValue(id, out ptr))
                {
                    ptr = id;
                }
                if (grid.walls.Contains(id)) { Console.Write("##"); }
                else if (grid.forests.Contains(id)) { Console.Write("* "); }
                else { Console.Write(". "); }
            }
            Console.WriteLine();
        }
    }

    // 全部路径
    static void DrawGrid(SquareGrid grid, AStarSearch astar, List<Location> path)
    {
        Console.WriteLine("All path:");
        // Print out the cameFrom array
        for (var y = 0; y < 10; y++)
        {
            for (var x = 0; x < 10; x++)
            {
                Location id = new Location(x, y);
                Location ptr = id;
                if (!astar.cameFrom.TryGetValue(id, out ptr))
                {
                    ptr = id;
                }
                if (grid.walls.Contains(id)) { Console.Write("##"); }
                else if (ptr.x == x + 1) { Console.Write("\u2192"); }
                else if (ptr.x == x - 1) { Console.Write("\u2190"); }
                else if (ptr.y == y + 1) { Console.Write("\u2193"); }
                else if (ptr.y == y - 1) { Console.Write("\u2191"); }
                else if (grid.forests.Contains(id)) { Console.Write("* "); }
                else { Console.Write(". "); }
            }
            Console.WriteLine();
        }
    }

    // 最短路径
    // 一个汉字占两个字节 一个ascii 占一个字节 特殊字符也是两个字节
    static void DrawGridPath(SquareGrid grid, AStarSearch astar, List<Location> path)
    {
        Console.WriteLine("Shorter path:");
        // Print out the cameFrom array
        for (var y = 0; y < 10; y++)
        {
            for (var x = 0; x < 10; x++)
            {
                Location id = new Location(x, y);
                Location ptr = id;
                if (!astar.cameFrom.TryGetValue(id, out ptr))
                {
                    ptr = id;
                }
                if (grid.walls.Contains(id)) { Console.Write("##"); }       // 墙
                else if (path.Contains(id)) { Console.Write("P "); }
                else if (grid.forests.Contains(id)) { Console.Write("* "); } // 森林
                else if (ptr.x == x + 1) { Console.Write(". "); }           // 地面
                else if (ptr.x == x - 1) { Console.Write(". "); }
                else if (ptr.y == y + 1) { Console.Write(". "); }
                else if (ptr.y == y - 1) { Console.Write(". "); }
                else { Console.Write(". "); };                    
            }
            Console.WriteLine();
        }
    }

    static void Main()
    {
        // Make "diagram 4" from main article
        var grid = new SquareGrid(10, 10);
        for (var x = 1; x < 4; x++)
        {
            for (var y = 7; y < 9; y++)
            {
                grid.walls.Add(new Location(x, y));
            }
        }

        // 森林其实就相当于 粘稠的可通过区域 walkable
        grid.forests = new HashSet<Location>
            {
                new Location(3, 4), new Location(3, 5),
                new Location(4, 1), new Location(4, 2),
                new Location(4, 3), new Location(4, 4),
                new Location(4, 5), new Location(4, 6),
                new Location(4, 7), new Location(4, 8),
                new Location(5, 1), new Location(5, 2),
                new Location(5, 3), new Location(5, 4),
                new Location(5, 5), new Location(5, 6),
                new Location(5, 7), new Location(5, 8),
                new Location(6, 2), new Location(6, 3),
                new Location(6, 4), new Location(6, 5),
                new Location(6, 6), new Location(6, 7),
                new Location(7, 3), new Location(7, 4),
                new Location(7, 5)
            };

        // Run A*

        Location start = new Location(1, 4);
        Location goal = new Location(9, 4);

        var astar = new AStarSearch(grid, start, goal);

        Location current = goal;

        // 寻路路径
        List<Location> path = new List<Location>();
        while (!current.Equals(start))
        {
            path.Add(current);
            current = astar.cameFrom[current];
        }
        path.Add(start);

        // 打印路径
        //foreach (Location l in path) {
        //    Console.WriteLine(l.x + " "+l.y );
        //}

        DrawGridPlant(grid, astar, path);
        Console.WriteLine("");
        DrawGrid(grid, astar, path);
        Console.WriteLine("");
        DrawGridPath(grid, astar, path);

        Console.ReadLine();
    }
}

函数

 // 启发式算法
    public float Heuristic(Node a, Node b)
    {
        //return (a.transform.position - b.transform.position).magnitude;
        return Mathf.Abs(a.transform.position.x - b.transform.position.x) + Mathf.Abs(a.transform.position.z - b.transform.position.z);
    }


    // Djsktra最短路径
    public float CostDjsktra(Node node, float newCost)
    {
        return newCost;
    }

    // 贪婪
    public float CostBestFirst(Node node, float newCost)
    {
        return Heuristic(node, m_GoalNode);
    }

    // A*
    public float CostAStar(Node node, float newCost)
    {
        return newCost + Heuristic(node, m_GoalNode);
    }