A*算法，伪代码，源码

转载自博主 GottaYiWanLiu[点击查看原文](http://blog.csdn.net/gottayiwanliu/article/details/54881256)

A*   AStar   A星
2d游戏，或者网格游戏中

Cost f 总消耗
Cost g 距离起点的消耗
Cost h 距离终点的消耗

默认消耗，直走消耗10，斜着走消耗14

开启列表
关闭列表

父节点

//
开启列表
关闭列表

开始循环（开启列表有值）
 当前点 = 开启列表中最小的f_Cost 
 把当前点从开启列表删除
 把当前点添加到关闭列表
 
 If当前点是终点，跳出循环
 
点开一个红点，周围的点会得到一个新的花费
循环周围的点
 这个点不能走或者在关闭列表中，跳过这个点
 
如果新花费小于原来的花费，
 替换成新的花费
 将这个点（周围的点里面的）的父节点设置为当前点（新点开的红点）

这个点不再开启列表中
 这个点添加到开启列表中
  直接替换成新的花费
  将这个点（周围的点里面的）的父节点设置为当前点（新点开的红点）

源码：

[csharp] view plain copy

print ?

1. using UnityEngine;  
2. using System.Collections;  
3.   
4. public class Node  
5. {  
6.     /*逻辑中用的*/  
7.     public int gCost;  
8.     public int hCost;  
9.     public int fCost  
10.     {  
11.         get { return gCost + hCost; }  
12.     }  
13.     public Node parent;  
14.   
15.     /*在Unity当中用的*/  
16.     public bool canWalk;  
17.     //网格的下标  
18.     public int gridX;  
19.     public int gridY;  
20.     //节点的位置  
21.     public Vector3 worldPos;  
22.   
23.     public Node(bool _canWalk, Vector3 position, int x, int y)  
24.     {  
25.         canWalk = _canWalk;  
26.         worldPos = position;  
27.         gridX = x;  
28.         gridY = y;  
29.     }  
30. }  

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;
    }
}

[csharp] view plain copy

print ?

1. using UnityEngine;  
2. using System.Collections;  
3. using System.Collections.Generic;  
4.   
5. public class Grid : MonoBehaviour  
6. {  
7.     //存放点节点的数组  
8.     public Node[,] grid;  
9.   
10.     //网格的大小  
11.     public Vector2 gridSize;  
12.     //节点的大小  
13.     public float nodeRadius;  
14.     public float nodeDiameter;  
15.     //一个层，代表可不可以通过  
16.     public LayerMask cantLayer;  
17.   
18.     //x和y方向上各有多少个格子  
19.     public int gridContX;  
20.     public int gridContY;  
21.   
22.     //起点  
23.     public Transform start;  
24.   
25.     //用来保存路径的列表  
26.     public List<Node> path = new List<Node>();  
27.   
28.   
29.     void Start ()  
30.     {  
31.         cantLayer = LayerMask.GetMask(”CantWalk”);  
32.         nodeDiameter = nodeRadius * 2;  
33.         //gridContX = (int)(gridSize.x / nodeDiameter);  
34.         gridContX = Mathf.RoundToInt(gridSize.x / nodeDiameter);  
35.         gridContY = Mathf.RoundToInt(gridSize.y / nodeDiameter);  
36.   
37.         grid = new Node[gridContX, gridContY];  
38.         CreatGrid();  
39.     }  
40.       
41.     void Update ()  
42.     {  
43.       
44.     }  
45.     //创建格子  
46.     void CreatGrid()  
47.     {  
48.         //网格起点  
49.         Vector3 startPoint = transform.position - gridSize.y / 2 * Vector3.forward - gridSize.x / 2 * Vector3.right;  
50.   
51.         for (int i = 0; i < gridContX; i++)  
52.         {  
53.             for (int j = 0; j < gridContY; j++)  
54.             {  
55.                 Vector3 worldPos = startPoint + Vector3.right * (nodeDiameter * i + nodeRadius) + Vector3.forward * (nodeDiameter * j + nodeRadius);  
56.                 //检测有没有碰到不能走的层上的物体  
57.                 bool canwalk = !Physics.CheckSphere(worldPos, nodeRadius, cantLayer);  
58.   
59.                 grid[i, j] = new Node(canwalk, worldPos, i, j);  
60.             }  
61.         }  
62.     }  
63.   
64.     //Unity中的辅助类  
65.     void OnDrawGizmos()  
66.     {  
67.         if (grid == null)  
68.         {  
69.             return;  
70.         }  
71.         foreach (Node node in grid)  
72.         {  
73.             if (node.canWalk)  
74.             {  
75.                 Gizmos.color = Color.yellow;  
76.                 Gizmos.DrawCube(node.worldPos, (nodeDiameter - 0.02f) * new Vector3(1, 0.2f, 1));  
77.             }  
78.             else  
79.             {  
80.                 Gizmos.color = Color.red;  
81.                 Gizmos.DrawCube(node.worldPos, (nodeDiameter - 0.02f) * new Vector3(1, 0.2f, 1));  
82.             }  
83.         }  
84.   
85.         //画出起点的位置  
86.         Node startNode = FindWithPosition(start.position);  
87.         if (startNode.canWalk)  
88.         {  
89.             Gizmos.color = Color.black;  
90.             Gizmos.DrawCube(startNode.worldPos, (nodeDiameter - 0.02f) * new Vector3(1, 0.2f, 1));  
91.         }  
92.   
93.   
94.         //画路径  
95.         if(path != null)  
96.         {  
97.             foreach (var node in path)  
98.             {  
99.                 Gizmos.color = Color.blue;  
100.                 Gizmos.DrawCube(node.worldPos, (nodeDiameter - 0.02f) * new Vector3(1, 0.2f, 1));  
101.             }  
102.         }  
103.     }  
104.   
105.     //通过位置得到在哪一个格子  
106.     public Node FindWithPosition(Vector3 position)  
107.     {  
108.         //在x方向的占比  
109.         float percentX = (position.x + gridSize.x / 2) / gridSize.x;  
110.         float percentY = (position.z + gridSize.y / 2) / gridSize.y;  
111.   
112.         //算出在哪个格子  
113.         int x = Mathf.RoundToInt((gridContX - 1) * percentX);  
114.         int y = Mathf.RoundToInt((gridContY - 1) * percentY);  
115.   
116.         return grid[x, y];  
117.     }  
118.   
119.     //通过一个点寻找周围的点  
120.     public List<Node> GetAroundNode(Node node)  
121.     {  
122.         List<Node> aroundNodes = new List<Node>();  
123.   
124.         for (int i = -1; i <= 1; i++)  
125.         {  
126.             for (int j = -1; j <= 1; j++)  
127.             {  
128.                 //传进来的点的下标  跳过  
129.                 if(i == 0 && j == 0)  
130.                 {  
131.                     continue;  
132.                 }  
133.   
134.                 int tempX = node.gridX + i;  
135.                 int tempY = node.gridY + j;  
136.   
137.                 //判断有没有越界  
138.                 if (tempX >= 0 && tempX < gridContX && tempY >= 0 && tempY < gridContY)  
139.                 {  
140.                     aroundNodes.Add(grid[tempX, tempY]);  
141.                 }  
142.             }  
143.         }  
144.   
145.         return aroundNodes;  
146.     }  
147. }  

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;
    }
}

[csharp] view plain copy

print ?

1. using UnityEngine;  
2. using System.Collections;  
3. using System.Collections.Generic;  
4.   
5. public class FindPath_AStar : MonoBehaviour  
6. {  
7.     public Transform startPoint;  
8.     public Transform endPoint;  
9.   
10.   
11.     private Grid grid;  
12.     // Use this for initialization  
13.     void Start ()  
14.     {  
15.         grid = GetComponent<Grid>();  
16.   
17.     }  
18.       
19.     void Update ()  
20.     {  
21.         FindPath(startPoint.position, endPoint.position);  
22.   
23.     }  
24.   
25.     //  
26.     void FindPath(Vector3 startPos, Vector3 endPos)  
27.     {  
28.         //开启列表  
29.         List<Node> opentSet = new List<Node>();  
30.         //关闭列表  
31.         List<Node> closeSet = new List<Node>();  
32.   
33.         //起点格子  
34.         Node startNode = grid.FindWithPosition(startPos);  
35.         //终点格子  
36.         Node endNode = grid.FindWithPosition(endPos);  
37.   
38.         //把起点加入开启列表  
39.         opentSet.Add(startNode);  
40.   
41.         //开始循环（开启列表有值）  
42.         while (opentSet.Count > 0)  
43.         {  
44.             //当前点  
45.             Node currentNode = opentSet[0];  
46.             //开启列表中最小的f_Cost  
47.             for (int i = 0; i < opentSet.Count; i++)  
48.             {  
49.                 //如果总花费最小，并且离目标点最近  
50.                 if (opentSet[i].fCost <= currentNode.fCost && opentSet[i].hCost < currentNode.fCost)  
51.                 {  
52.                     currentNode = opentSet[i];  
53.                 }  
54.             }  
55.   
56.             //把这个点 点红  
57.             //把当前点从开启列表删除  
58.             opentSet.Remove(currentNode);  
59.             //把当前点添加到关闭列表  
60.             closeSet.Add(currentNode);  
61.   
62.             //If当前点是终点，跳出循环  
63.             if (currentNode == endNode)  
64.             {  
65.                 GetPath(startNode, endNode);  
66.                 return;  
67.             }  
68.   
69.             //周围的点  
70.             List<Node> around = grid.GetAroundNode(currentNode);  
71.             //循环周围的点  
72.             foreach (Node node in around)  
73.             {  
74.                 //这个点不能走或者在关闭列表中，跳过这个点  
75.                 if (!node.canWalk || closeSet.Contains(node))  
76.                 {  
77.                     continue;  
78.                 }  
79.                 //点开一个红点，周围的点会得到一个新的花费g  
80.                 int newCost_g = currentNode.gCost + GetCost(currentNode, node);  
81.                 //比较新花费和原来的花费，谁更小(谁离我们起点近) || 这个点不再开启列表中  
82.                 if (newCost_g < node.gCost || !opentSet.Contains(node))  
83.                 {  
84.                     //替换成新的花费  
85.                     node.gCost = newCost_g;  
86.                     node.hCost = GetCost(node, endNode);  
87.                     //将这个点（周围的点里面的）的父节点设置为当前点（新点开的红点）  
88.                     node.parent = currentNode;  
89.   
90.                     //这个点不再开启列表中  
91.                     if (!opentSet.Contains(node))  
92.                     {  
93.                         opentSet.Add(node);  
94.                     }  
95.                 }  
96.             }  
97.         }  
98.     }  
99.   
100.     //计算花费  
101.     int GetCost(Node a, Node b)  
102.     {  
103.         //等到两点之间的一个距离（x方向和y方向）  
104.         int coutX = Mathf.Abs(a.gridX - b.gridX);  
105.         int coutY = Mathf.Abs(a.gridY - b.gridY);  
106.   
107.         if(coutX > coutY)  
108.         {  
109.             return (coutX - coutY) * 10 + coutY * 14;  
110.         }  
111.         else  
112.         {  
113.             return (coutY - coutX) * 10 + coutX * 14;  
114.         }  
115.     }  
116.   
117.   
118.     //得到路径  
119.     void GetPath(Node startNode, Node endNode)  
120.     {  
121.         List<Node> path = new List<Node>();  
122.         Node temp = endNode;  
123.         while(temp != startNode)  
124.         {  
125.             path.Add(temp);  
126.             temp = temp.parent;  
127.         }  
128.         //列表转置  
129.         path.Reverse();  
130.         grid.path = path;  
131.     }  
132. }  

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;
    }
}