（C#）A*算法伪代码及源码

using UnityEngine;using System.Collections;public class Node{    /*逻辑中用的*/    public int gCost;    public int hCost;    public int fCost    {        get { return gCost + hCost; }    }    public Node parent;    /*在Unity当中用的*/    public bool canWalk;    //网格的下标    public int gridX;    public int gridY;    //节点的位置    public Vector3 worldPos;    public Node(bool _canWalk, Vector3 position, int x, int y)    {        canWalk = _canWalk;        worldPos = position;        gridX = x;        gridY = y;    }}
using UnityEngine;using System.Collections;using System.Collections.Generic;public class Grid : MonoBehaviour{    //存放点节点的数组    public Node[,] grid;    //网格的大小    public Vector2 gridSize;    //节点的大小    public float nodeRadius;    public float nodeDiameter;    //一个层，代表可不可以通过    public LayerMask cantLayer;    //x和y方向上各有多少个格子    public int gridContX;    public int gridContY;    //起点    public Transform start;    //用来保存路径的列表    public List<Node> path = new List<Node>();    void Start ()    {        cantLayer = LayerMask.GetMask("CantWalk");        nodeDiameter = nodeRadius * 2;        //gridContX = (int)(gridSize.x / nodeDiameter);        gridContX = Mathf.RoundToInt(gridSize.x / nodeDiameter);        gridContY = Mathf.RoundToInt(gridSize.y / nodeDiameter);        grid = new Node[gridContX, gridContY];        CreatGrid();    }		void Update ()    {		}    //创建格子    void CreatGrid()    {        //网格起点        Vector3 startPoint = transform.position - gridSize.y / 2 * Vector3.forward - gridSize.x / 2 * Vector3.right;        for (int i = 0; i < gridContX; i++)        {            for (int j = 0; j < gridContY; j++)            {                Vector3 worldPos = startPoint + Vector3.right * (nodeDiameter * i + nodeRadius) + Vector3.forward * (nodeDiameter * j + nodeRadius);                //检测有没有碰到不能走的层上的物体                bool canwalk = !Physics.CheckSphere(worldPos, nodeRadius, cantLayer);                grid[i, j] = new Node(canwalk, worldPos, i, j);            }        }    }    //Unity中的辅助类    void OnDrawGizmos()    {        if (grid == null)        {            return;        }        foreach (Node node in grid)        {            if (node.canWalk)            {                Gizmos.color = Color.yellow;                Gizmos.DrawCube(node.worldPos, (nodeDiameter - 0.02f) * new Vector3(1, 0.2f, 1));            }            else            {                Gizmos.color = Color.red;                Gizmos.DrawCube(node.worldPos, (nodeDiameter - 0.02f) * new Vector3(1, 0.2f, 1));            }        }        //画出起点的位置        Node startNode = FindWithPosition(start.position);        if (startNode.canWalk)        {            Gizmos.color = Color.black;            Gizmos.DrawCube(startNode.worldPos, (nodeDiameter - 0.02f) * new Vector3(1, 0.2f, 1));        }        //画路径        if(path != null)        {            foreach (var node in path)            {                Gizmos.color = Color.blue;                Gizmos.DrawCube(node.worldPos, (nodeDiameter - 0.02f) * new Vector3(1, 0.2f, 1));            }        }    }    //通过位置得到在哪一个格子    public Node FindWithPosition(Vector3 position)    {        //在x方向的占比        float percentX = (position.x + gridSize.x / 2) / gridSize.x;        float percentY = (position.z + gridSize.y / 2) / gridSize.y;        //算出在哪个格子        int x = Mathf.RoundToInt((gridContX - 1) * percentX);        int y = Mathf.RoundToInt((gridContY - 1) * percentY);        return grid[x, y];    }    //通过一个点寻找周围的点    public List<Node> GetAroundNode(Node node)    {        List<Node> aroundNodes = new List<Node>();        for (int i = -1; i <= 1; i++)        {            for (int j = -1; j <= 1; j++)            {                //传进来的点的下标  跳过                if(i == 0 && j == 0)                {                    continue;                }                int tempX = node.gridX + i;                int tempY = node.gridY + j;                //判断有没有越界                if (tempX >= 0 && tempX < gridContX && tempY >= 0 && tempY < gridContY)                {                    aroundNodes.Add(grid[tempX, tempY]);                }            }        }        return aroundNodes;    }}
using UnityEngine;using System.Collections;using System.Collections.Generic;public class FindPath_AStar : MonoBehaviour{    public Transform startPoint;    public Transform endPoint;    PRivate Grid grid;	// Use this for initialization	void Start ()    {        grid = GetComponent<Grid>();    }		void Update ()    {        FindPath(startPoint.position, endPoint.position);    }    //    void FindPath(Vector3 startPos, Vector3 endPos)    {        //开启列表        List<Node> opentSet = new List<Node>();        //关闭列表        List<Node> closeSet = new List<Node>();        //起点格子        Node startNode = grid.FindWithPosition(startPos);        //终点格子        Node endNode = grid.FindWithPosition(endPos);        //把起点加入开启列表        opentSet.Add(startNode);        //开始循环（开启列表有值）        while (opentSet.Count > 0)        {            //当前点            Node currentNode = opentSet[0];            //开启列表中最小的f_Cost            for (int i = 0; i < opentSet.Count; i++)            {                //如果总花费最小，并且离目标点最近                if (opentSet[i].fCost <= currentNode.fCost && opentSet[i].hCost < currentNode.fCost)                {                    currentNode = opentSet[i];                }            }            //把这个点 点红            //把当前点从开启列表删除            opentSet.Remove(currentNode);            //把当前点添加到关闭列表            closeSet.Add(currentNode);            //If当前点是终点，跳出循环            if (currentNode == endNode)            {                GetPath(startNode, endNode);                return;            }            //周围的点            List<Node> around = grid.GetAroundNode(currentNode);            //循环周围的点            foreach (Node node in around)            {                //这个点不能走或者在关闭列表中，跳过这个点                if (!node.canWalk || closeSet.Contains(node))                {                    continue;                }                //点开一个红点，周围的点会得到一个新的花费g                int newCost_g = currentNode.gCost + GetCost(currentNode, node);                //比较新花费和原来的花费，谁更小(谁离我们起点近) || 这个点不再开启列表中                if (newCost_g < node.gCost || !opentSet.Contains(node))                {                    //替换成新的花费                    node.gCost = newCost_g;                    node.hCost = GetCost(node, endNode);                    //将这个点（周围的点里面的）的父节点设置为当前点（新点开的红点）                    node.parent = currentNode;                    //这个点不再开启列表中                    if (!opentSet.Contains(node))                    {                        opentSet.Add(node);                    }                }            }        }    }    //计算花费    int GetCost(Node a, Node b)    {        //等到两点之间的一个距离（x方向和y方向）        int coutX = Mathf.Abs(a.gridX - b.gridX);        int coutY = Mathf.Abs(a.gridY - b.gridY);        if(coutX > coutY)        {            return (coutX - coutY) * 10 + coutY * 14;        }        else        {            return (coutY - coutX) * 10 + coutX * 14;        }    }    //得到路径    void GetPath(Node startNode, Node endNode)    {        List<Node> path = new List<Node>();        Node temp = endNode;        while(temp != startNode)        {            path.Add(temp);            temp = temp.parent;        }        //列表转置        path.Reverse();        grid.path = path;    }}