A*算法，（C#，Unity版本）

A*算法，（C#，Unity版本）

流程：

1. 创建open和closed集合，将起始点加入open集合
2. 定义启发函数 F= G+H （G：从起点到当前的路径代价，H：不考虑不通过区域，从当前节点到终点的路径代价）
3. 重复下面步骤：
   ①.当open集合为空时，循环结束。
   ②.寻找open集合中，以F排序的最小的节点作为当前节点。
   ③.从open中移除当前节点，并把当前节点加入到closed集合中。
   ④.以当前节点为中心，找周围最多8个方位（九宫格除了中间）的节点（该节点可通过且不在open和closed集合中，否则不要），更新他们的G值，将他们的父节点设置为当前节点，并加入open集合中。
   ⑤.若当前节点为目标点，则结束。此时，从当前节点开始逐级追溯父节点，直到回溯到起点，此时回溯的各个节点即为路径。

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

public class AStarNode:IComparable,IEquatable<AStarNode>
{
    private float f, g, h;
    public AStarNode parent = null;
    public int x, y;
    public float F => f;

    public float G
    {
        set
        {
            if (Mathf.Approximately(g, value)) return;
            g = value;
            f = g + h;
        }
        get => g;
    }

    public float H
    {
        set
        {
            if (Mathf.Approximately(h, value)) return;
            h = value;
            f = g + h;
        }
        get => h;
    }

    public float Distance(AStarNode aStarNode)
    {
        return Mathf.Sqrt((aStarNode.x - x) * (aStarNode.x - x) + (aStarNode.y - y) * (aStarNode.y - y));
    }
    public AStarNode(int x,int y)
    {
        this.x = x;
        this.y = y;
        Reset();
    }
    public AStarNode(int x,int y,AStarNode parent)
    {
        Reset();
        this.parent = parent;
        this.x = x;
        this.y = y;
    }

    public void Reset()
    {
        g = 0;
        h = 0;
        f = 0;
        parent = null;
    }
    public int CompareTo(object obj)
    {
        AStarNode aStarNode = obj as AStarNode;
        return F.CompareTo(aStarNode.F);
    }

    public override bool Equals(object obj)
    {
        if (ReferenceEquals(null, obj)) return false;
        if (ReferenceEquals(this, obj)) return true;
        if (obj.GetType() != this.GetType()) return false;
        return Equals((AStarNode)obj);
    }

    public Vector2 Position => new Vector2(x, y);

    public bool Equals(AStarNode other)
    {
        return x == other.x && y == other.y;
    }
}

public struct NodeInfo
{
    public int x;
    public int y;
    public float deltaG;
}

public class AStarManager
{
    private static AStarManager instance;

    public static AStarManager Instance => instance ??= new AStarManager();
    private AStarNode[,] aStarNodes;
    private GameObject[,] objects;
    private int row = 10;
    private int col = 10;
    private int[,] maps;

    private List<AStarNode> openSet = new List<AStarNode>();
    private List<AStarNode> closedSet = new List<AStarNode>();
    private AStarNode startNode;
    private AStarNode endNode;
    private AStarNode currentNode;
    private List<AStarNode> paths = new List<AStarNode>();
    public List<AStarNode> Path => paths;

    public bool Find => paths is { Count: > 0 };
    private readonly float sqr2 = Mathf.Sqrt(2);

    /// <summary>
    /// 
    /// </summary>
    /// <param name="maps">0 可行走，1表示障碍</param>
    public void SetMap(int[,] maps)
    {
        if (objects != null)
        {
            for (int i = 0; i < row; i++)
            {
                for (int j = 0; j < col; j++)
                {
                    UnityEngine.Object.Destroy(objects[i,j]);
                }
            }

            objects = null;
        }
        paths.Clear();
        this.maps = maps;
        row = this.maps.GetLength(0);
        col = this.maps.GetLength(1);
        Debug.Log($"row:{row} col:{col}");
    }

    public void SetStartPosition(int x, int y)
    {
        startNode = new AStarNode(x, y);
        Debug.Log($"start pos:{x}-{y}");
    }
    public void SetEndPosition(int x, int y)
    {
        endNode = new AStarNode(x, y);
        Debug.Log($"end pos:{x}-{y}");
    }
    
    public void FindPath()
    {
        if(startNode == null || endNode == null) 
            return ;
        List<AStarNode> nodes = new List<AStarNode>();
        openSet.Clear();
        closedSet.Clear();
        openSet.Add(startNode);
        while (openSet.Count>0)
        {
            openSet.Sort();
            currentNode = openSet[0];
            openSet.Remove(currentNode);
            closedSet.Add(currentNode);
            Queue<NodeInfo> queue = AddSurroundingNodes();

            while (queue.Count>0)
            {
                NodeInfo nodeInfo = queue.Dequeue();
                if (maps[nodeInfo.x, nodeInfo.y] == 1) continue;
                AStarNode node = new AStarNode(nodeInfo.x, nodeInfo.y, currentNode);
                if (openSet.Contains(node) || closedSet.Contains(node)) continue;
                if (node.Equals(endNode))
                {
                    currentNode = node;
                    while (currentNode.parent!=null)
                    {
                        paths.Add(currentNode);
                        currentNode = currentNode.parent;
                    }
                    paths.Reverse();
                    openSet.Clear();
                    queue.Clear();
                    Debug.Log($"找到路径,路径点个数: {paths.Count}");
                    return;
                }
                node.G = currentNode.G + nodeInfo.deltaG;
                // node.G = node.Distance(startNode);
                node.H = node.Distance(endNode);
                openSet.Add(node);
            }
        }
    }
    
    private Queue<NodeInfo> AddSurroundingNodes()
    {
        Queue<NodeInfo> queue = new Queue<NodeInfo>();
        int x = currentNode.x;
        int y = currentNode.y - 1;

        if (y >= 0)
        {
            queue.Enqueue(new NodeInfo()
            {
                x=x,y=y,deltaG = 1
            });
        }
        y = currentNode.y + 1;
        if (y < col)
        {
            queue.Enqueue(new NodeInfo()
            {
                x=x,y=y,deltaG = 1
            });
        }
        x = currentNode.x - 1;
        if (x >= 0)
        {
            y = currentNode.y - 1;
            if (y >= 0)
            {
                queue.Enqueue(new NodeInfo()
                {
                    x=x,y=y,deltaG = sqr2
                });
            }
            queue.Enqueue(new NodeInfo()
            {
                x=x,y=currentNode.y,deltaG = 1
            });
            y = currentNode.y + 1;
            if (y < col)
            {
                queue.Enqueue(new NodeInfo()
                {
                    x=x,y=y,deltaG = sqr2
                });
            }
        }
        x = currentNode.x + 1;
        if (x < row)
        {
            y = currentNode.y - 1;
            if (y >= 0)
            {
                queue.Enqueue(new NodeInfo()
                {
                    x=x,y=y,deltaG = sqr2
                });
            }
            queue.Enqueue(new NodeInfo()
            {
                x=x,y=currentNode.y,deltaG = 1
            });
            y = currentNode.y + 1;
            if (y < col)
            {
                queue.Enqueue(new NodeInfo()
                {
                    x=x,y=y,deltaG = sqr2
                });
            }
        }

        return queue;
    }
    public IEnumerator StepShowPath()
    {
        if(startNode == null || endNode == null) 
            yield return null;
        ShowMap();
        List<AStarNode> nodes = new List<AStarNode>();
        openSet.Clear();
        closedSet.Clear();
        openSet.Add(startNode);
        
        while (openSet.Count>0)
        {
            openSet.Sort();
            currentNode = openSet[0];
            objects[currentNode.x, currentNode.y].GetComponent<MeshRenderer>().material.color = Color.blue;
            yield return new WaitForSeconds(1f);
            openSet.Remove(currentNode);
            closedSet.Add(currentNode);
            Queue<NodeInfo> queue = AddSurroundingNodes();

            while (queue.Count>0)
            {
                NodeInfo nodeInfo = queue.Dequeue();
                if (maps[nodeInfo.x, nodeInfo.y] == 1) continue;
                AStarNode node = new AStarNode(nodeInfo.x, nodeInfo.y, currentNode);
                if (openSet.Contains(node) || closedSet.Contains(node)) continue;
                if (node.Equals(endNode))
                {
                    currentNode = node;
                    while (currentNode.parent!=null)
                    {
                        paths.Add(currentNode);
                        currentNode = currentNode.parent;
                    }
                    paths.Reverse();
                    openSet.Clear();
                    queue.Clear();
                    Debug.Log($"找到路径,路径点个数: {paths.Count}");
                    ShowPath();
                    yield break;
                }
                node.G = currentNode.G + nodeInfo.deltaG;
                // node.G = node.Distance(startNode);
                node.H = node.Distance(endNode);
                openSet.Add(node);
                objects[node.x, node.y].GetComponent<MeshRenderer>().material.color = Color.cyan;
                yield return new WaitForSeconds(0.5f);
            }
        }
    }
    
    public void ShowMap()
    {
        if (objects == null)
        {
            objects = new GameObject[row, col];
        }
        for (int i = 0; i < row; i++)
        {
            for (int j = 0; j < col; j++)
            {
                GameObject obj = GameObject.CreatePrimitive(PrimitiveType.Cube);
                objects[i, j] = obj;
                obj.name = $"{i}-{j}";
                obj.GetComponent<MeshRenderer>().material = new Material(Shader.Find("Unlit/SimpleColor"));
                obj.GetComponent<MeshRenderer>().material.color = maps[i, j] == 0 ? Color.white : Color.red;
                obj.transform.position = new Vector3(j,i,0);
            }
        }

        if (startNode != null)
        {
            objects[startNode.x,startNode.y].GetComponent<MeshRenderer>().material.color = Color.magenta;
        }
        if (endNode != null)
        {
            objects[endNode.x,endNode.y].GetComponent<MeshRenderer>().material.color = Color.yellow;
        }
    }

    public void ShowPath()
    {
        if(objects == null || paths == null)return;
        foreach (var item in paths)
        {
            objects[item.x,item.y].GetComponent<MeshRenderer>().material.color = Color.green;
        }
        if (startNode != null)
        {
            objects[startNode.x,startNode.y].GetComponent<MeshRenderer>().material.color = Color.magenta;
        }
        if (endNode != null)
        {
            objects[endNode.x,endNode.y].GetComponent<MeshRenderer>().material.color = Color.yellow;
        }
    }
}

using UnityEngine;

public class AStarTest : MonoBehaviour
{
    // Start is called before the first frame update
    void Start()
    {
        AStarManager.Instance.SetMap(new int[,]
        {
            {1,1,1,1,1,1,1,1,1,1},
            {1,0,0,0,0,0,0,0,0,1},
            {1,0,0,0,1,0,0,0,1,1},
            {1,0,0,0,1,0,1,0,1,1},
            {1,0,0,0,1,0,1,0,0,1},
            {1,0,0,0,0,0,1,1,0,1},
            {1,0,0,0,0,0,0,1,0,1},
            {1,0,0,0,0,0,0,0,0,1},
            {1,1,1,1,1,1,1,1,1,1},
        });
        AStarManager.Instance.SetStartPosition(1,1);
        AStarManager.Instance.SetEndPosition(7,7);
        // AStarManager.Instance.SetEndPosition(4,7);
        // AStarManager.Instance.FindPath();
        // AStarManager.Instance.ShowMap();
        // AStarManager.Instance.ShowPath();
        StartCoroutine(AStarManager.Instance.StepShowPath());
    }
}