unity A*算法简单实现

项目要用A算法做运算所以边学边练。
http://edu.manew.com/course/44/task/432/show# 视屏
启发式搜索：在状态空间中搜索对每一个搜索位置进行评估，得到最好位置，再从这个位置进行搜索直到目标。省略大量无谓搜索路径，提高效率。
在启发式搜索中对位置评估十分重要。
估价函数：从当前节点移动到目标节点的预估费用。寻路常用曼哈顿估价函数。
A算法特点：理论上时间最优，缺点：空间增长指数级别。（优化：二叉堆）（不会）

储存列表

开启列表：待检查方格集合列表，寻找周围可达到的点，加入列表，并保存中心点为父节点。
关闭列表：列表中保存不需要再次检查的方格。

路径评分

G-与起始点的距离
H-与目标点的距离
F值=G+H；

F，G和H的评分被写在每个方格里

任意一方格F是中间的值，G在左上角，H右上角

红色的是下一步的走法 F越低越近

开始搜索

1 把起始格添加到开启列表
2 寻找起点周围所有可到达或者可通过的方格，把他们加入开启列表
3 从开启列表中删除点A，把它加到一个“关闭列表”，列表中保存所有不需要再次检查的方格。
4 把当前格子从开启列表删除，然后添加到关闭列表。
5 检查所有相邻格子。跳过那些已在关闭列表的或者不可通过的，把他们添加进开启列表，把选中的方格作为新的方格的父节点。
6 如果某个相邻格已在开启列表里了，检查现在的这条路径G值是否更低，如果新G值更低，那就把相邻方格的父节点改为目前选中的方格，重新计算F.G值。

尝试在红48选择

尝试在A左边的红48选择

从上图再往做走选择

蓝色是选择的路线

红色是选择过作为父节点的格子

总结

1 把起始格加入开启列表
2 重复如下
a 寻找开启列表中F值最低的格子。称当前格
b 把它切换到关闭列表
c 对相邻格中的每一个→
如果不可通过或已在关闭列表，略过。
反之
如果他不在开启列表，把他添加进去。把当前格作为父节点。记录这一格F.G.H值
如果他已经在开启列表，用G值参考检查新路径是否更好。更低的G值=更好的路径。如果这样，把这一格设为父节点，重新计算这一格G.F值。
d停止，当你
没把目标添加进关闭列表，这时候路径已经被找到
没找到目标格，开启列表空了。这时候路径不存在。
3 保存路径。从目标格开始，沿着每一格的父节点移动直到回到起始格。

伪代码实现

List开启
List关闭
把开始点加入开启集合

循环：
         当前点=开启集合中最小F_Cost的点
         把当前点移出开启集合
         将当前点添加到关闭集合
    
         如果当前是目标点，结束查询
      
         遍历当前点的每个相邻点
                如果相邻点不能访问或者相邻点在关闭集合中，跳过相邻点
          如果新路径到相邻点距离更短，或者相邻点不在开启集合中
          重新设置F_Cost
          重新设置当前点为父节点
          如果相邻点不在开启集合中
          添加相邻点到开启集合中

以下是可以运行的源码

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

public class Node{
    /// 

    /// 是否可以通过此路径
    /// 
    public bool _canWalk;
    /// 

    /// 保存节点位置
    /// 
    public Vector3 _worldPos;
    /// 

    /// 整个网格的索引
    /// 
    public int _gridX, _gridY;

    public int gCost;
    public int hCost;

    public int fCost
    {
        get { return gCost + hCost; }
    }

    public Node parent;

    public Node(bool _canWalk, Vector3 _worldPos, int _gridX, int _gridY)
    {
        this._canWalk = _canWalk;
        this._worldPos = _worldPos;
        this._gridX = _gridX;
        this._gridY = _gridY;
    }
}

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

public class Grid : MonoBehaviour
{

    Node[,] grid;
    /// 

    /// 保存网格大小
    /// 
    public Vector2 gridSize;
    /// 

    /// 节点半径
    /// 
    public float nodeRadius;
    /// 

    /// 节点直径
    /// 
    float nodeDiameter;
    /// 

    /// 射线图层
    /// 
    public LayerMask WhatLayer;

    public Transform player;

    /// 

    /// 每个方向网格数的个数
    /// 
    public int gridCntX, gridCntY;

    /// 

    /// 保存路径列表
    /// 
    public List path = new List();
    // Use this for initialization
    void Start()
    {
        nodeDiameter = nodeRadius * 2;
        gridCntX = Mathf.RoundToInt(gridSize.x / nodeDiameter);
        gridCntY = Mathf.RoundToInt(gridSize.y / nodeDiameter);
        grid = new Node[gridCntX, gridCntY];
        CreateGrid();
    }

    private void CreateGrid()
    {
        Vector3 startPoint = transform.position - gridSize.x * 0.5f * Vector3.right
            - gridSize.y * 0.5f * Vector3.forward;
        for (int i = 0; i < gridCntX; i++)
        {
            for (int j = 0; j < gridCntY; j++)
            {
                Vector3 worldPoint = startPoint + Vector3.right * (i * nodeDiameter + nodeRadius)
                    + Vector3.forward * (j * nodeDiameter + nodeRadius);
                //此节点是否可走
                bool walkable = !Physics.CheckSphere(worldPoint, nodeRadius, WhatLayer);
                //i，j是二维数组的索引
                grid[i, j] = new Node(walkable, worldPoint, i, j);
            }
        }
    }

    public Node GetFromPos(Vector3 pos)
    {
        float percentX = (pos.x + gridSize.x * 0.5f) / gridSize.x;
        float percentY = (pos.z + gridSize.y * 0.5f) / gridSize.y;

        percentX = Mathf.Clamp01(percentX);
        percentY = Mathf.Clamp01(percentY);

        int x = Mathf.RoundToInt((gridCntX - 1) * percentX);
        int y = Mathf.RoundToInt((gridCntY - 1) * percentY);

        return grid[x, y];
    }
    void OnDrawGizmos()
    {
        //画出网格边缘
        Gizmos.DrawWireCube(transform.position, new Vector3(gridSize.x, 1, gridSize.y));
        //画不可走网格
        if (grid == null)
            return;
        Node playerNode = GetFromPos(player.position);
        foreach (var item in grid)
        {
            Gizmos.color = item._canWalk ? Color.white : Color.red;
            Gizmos.DrawCube(item._worldPos, Vector3.one * (nodeDiameter - 0.1f));
        }
        //画路径
        if (path!=null)
        {
            foreach (var item in path)
            {
                Gizmos.color = Color.black;
                Gizmos.DrawCube(item._worldPos, Vector3.one * (nodeDiameter - 0.1f));
            }
        }
        //画玩家
        if (playerNode != null && playerNode._canWalk)
        {
            Gizmos.color = Color.cyan;
            Gizmos.DrawCube(playerNode._worldPos, Vector3.one * (nodeDiameter - 0.1f));
        }
    }

    public List GetNeibourhood(Node node)
    {
        List neibourhood = new List();
        //相邻上下左右格子
        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 < gridCntX && tempX > 0 && tempY > 0 && tempY < gridCntY)
                {
                    neibourhood.Add(grid[tempX, tempY]);
                }
            }
        }
        return neibourhood;
    }
}

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

public class FindPath : MonoBehaviour
{
    public Transform player, EndPoint;
    Grid grid;
    // Use this for initialization
    void Start()
    {
        grid = GetComponent();
    }

    // Update is called once per frame
    void Update()
    {
        FindingPath(player.position, EndPoint.position);
    }

    void FindingPath(Vector3 StarPos, Vector3 EndPos)
    {
        Node startNode = grid.GetFromPos(StarPos);
        Node endNode = grid.GetFromPos(EndPos);
        List openSet = new List();
        HashSet closeSet = new HashSet();
        openSet.Add(startNode);

        while (openSet.Count > 0)
        {
            Node currentNode = openSet[0];

            for (int i = 0; i < openSet.Count; i++)
            {
                if (openSet[i].fCost < currentNode.fCost || openSet[i].fCost == currentNode.fCost && openSet[i].hCost < currentNode.hCost)
                {
                    currentNode = openSet[i];
                }
            }

            openSet.Remove(currentNode);
            closeSet.Add(currentNode);

            if (currentNode == endNode)
            {
                GeneratePath(startNode,endNode);
                return;
            }
            //判断周围最优节点
            foreach (var item in grid.GetNeibourhood(currentNode))
            {
                if (!item._canWalk || closeSet.Contains(item))
                    continue;
                int newCost = currentNode.gCost + GetDistanceNodes(currentNode, item);
                if (newCost path = new List();
        Node temp = endNode;
        while (temp!=startNode)
        {
            path.Add(temp);
            temp = temp.parent;
        }
       //列表反转
        path.Reverse();
        grid.path = path;
    }

    int GetDistanceNodes(Node a, Node b)
    {
        int cntX = Mathf.Abs(a._gridX - b._gridY);
        int cntY = Mathf.Abs(a._gridY - b._gridX);
        if (cntX > cntY)
        {
            return 14 * cntY + 10 * (cntX - cntY);
        }
        else
        {
            return 14 * cntX + 10 * (cntY - cntX);
        }
    }
}

运行效果

脚本找个空节点挂上

效果图

这个算是个基本实现 图中可看出明明可以直线走却走斜线 同时4格两点之间直线最短

之后改良 发现是最后的对角线算法写反了

  int GetDistanceNodes(Node a, Node b)
    {
        //估算权值,对角线算法 看在X轴还是Y轴格子数多  可计算斜移动
        int cntX = Mathf.Abs(a._gridX - b._gridX);
        int cntY = Mathf.Abs(a._gridY - b._gridY);
        if (cntX > cntY)
        {
            return 14 * cntY + 10 * (cntX - cntY);
        }
        else
        {
            return 14 * cntX + 10 * (cntY - cntX);
        }

        //曼哈顿算法
        //return Mathf.Abs(a._gridX - b._gridX) * 10 + Mathf.Abs(a._gridY - b._gridY) * 10;
    }

已经不会有之前的先算斜线了

https://blog.csdn.net/denghecsdn/article/details/78778769
这个全面讲解A*