A算法学习总结

A*算法学习总结

思路：

​ 1.先初始化一个网格地图，根据需要设置障碍物的格子。

​ 2.根据传入开始位置和结束的位置获得网格上对应的点，设置两个列表，一个是待检查列表openlist ,一个检查结束列表closelist,把开始位置的点传入待检查列表，开始循环。

​ 3.取出待检查列表估值最小F的一个，放入closelist里面，并找到这一个点周边的所有点，把不在closelist列表并且不是障碍物的点家入待检测列表里 ，如果是待检测列表已经有的点 ，则评估从起始点通过当前点到这个点的值是否比他当前的值小，如果小则进行替换，如果大于等于则不做操作

​ 4.按3的流程循环直到待检查列表有结束的节点时退出循环，或者直到待检查列表为空时退出循环。

​ 5.根据终点的父节点一直寻找到开始节点就是两个点之间的路径

---

///地图节点信息
public class AStartNode 
{
    //地图上的位置标记
    public Vector2 id;
    //上一个节点
    public AStartNode parent = null;
    public float F=0,G=0,H = 0;
    public bool isObstacle = false;
    public Vector3 postion = Vector3.zero;


     

    public AStartNode(int x ,int y, Vector3 v)
    {
        id.x = x;
        id.y = y;
        postion = v;
    }

}

///A*寻路管理
public class AStart : MonoBehaviour
{
    public AStartNode startPoint;
    public int rowCount = 1;
    public int colCount = 1;
    public float wInterval=0;
    public float hInterval=0;
    public Vector3 mapOrigin;
    private AStartNode[,] maps=null;
    public List<AStartNode> openList = new List<AStartNode>();
    public List<AStartNode> closeList = new List<AStartNode>();
 


    public void InitMap(Vector3 origin,float width ,float hight ,float wInterval ,float hInterval )
    {
        rowCount =(int)(hight/wInterval);
        colCount =(int)(width/hInterval);
        this.hInterval = hInterval;
        this.wInterval = wInterval;
        mapOrigin = origin;
        maps = new AStartNode[rowCount, colCount];
        for(int i = 0; i < rowCount; i++)
        {
            for(int j = 0; j < colCount; j++)
            {
                Vector3 pos = origin + new Vector3(i * wInterval, 0, j * hInterval);
                maps[i, j] = new AStartNode(i, j, pos);
            }
        }
     
    }

    /// <summary>
    /// 寻找两点之间的路径
    /// </summary>
    /// <param name="start"></param>
    /// <param name="end"></param>
    /// <returns></returns>
    public List<Vector3> FindPath(Vector3 start, Vector3 end)
    {
        AStartNode startPoint = FindPoint(start);
        AStartNode endPoint = FindPoint(end);
        if (startPoint == null || endPoint == null) return null; 
        openList.Clear();
        closeList.Clear();
        AStartNode curPoint = startPoint;
        openList.Add(startPoint);
        
        while (openList.Count > 0)
        {
            openList.Sort((a, b) =>
            {
                return a.F.CompareTo(b.F);
            });
            curPoint = openList[0]; //FindMinPoint(openList);
            openList.Remove(curPoint);
            closeList.Add(curPoint);
            List<AStartNode> surrounds = GetSurroundPoints(curPoint);
            foreach(AStartNode point in surrounds)
            {
                if (openList.Contains(point))
                {
                    float g = CaculateG(curPoint, point);
                    if (g < point.G)
                    {
                        point.G = g;
                        point.F = g + point.H;
                        point.parent = curPoint;

                    }
                }
                else
                {
                    point.parent = curPoint;
                    point.G = CaculateG(curPoint, point);
                    point.H = Vector3.Distance(point.postion, endPoint.postion);
                    point.F = point.G + point.H;
                    openList.Add(point);

                }

                if (openList.Contains(endPoint))
                {
                    Debug.Log("find");
                    break;
                }
            }

        }
        startPoint.parent = null;
        return GetPath(endPoint);
    }

    /// <summary>
    /// 根据一个点获得到这个点的路径
    /// </summary>
    /// <param name="point"></param>
    /// <returns></returns>
    public List<Vector3> GetPath(AStartNode point)
    {
        AStartNode next = point;
        List<Vector3> points = new List<Vector3>();
        int count = 50;
        while (next != null)
        {
            Debug.Log("getPath:" + next.id);
            points.Add(next.postion);
            next = next.parent;
            count--;
            if (count < 0)
            {
                break;
            }
        }
        return points;
    }

    /// <summary>
    /// 根据坐标活动在网格上的点
    /// </summary>
    /// <param name="point"></param>
    /// <returns></returns>
    public AStartNode FindPoint(Vector3 point)
    {
        int x =(int)( point.x / wInterval);
        int y = (int)(point.z / hInterval);
        if (x < 0 || x > rowCount-1) return null;
        if (y < 0 || y > colCount-1) return null;
        if (maps[x, y] != null) return maps[x, y];
        return null;
    }

    /// <summary>
    /// 寻找一个点周围的点
    /// </summary>
    /// <param name="mainPoint"></param>
    /// <returns></returns>
    public List<AStartNode> GetSurroundPoints(AStartNode mainPoint)
    {
        int row =(int) mainPoint.id.x;
        int col =(int) mainPoint.id.y;
        List<AStartNode> points = new List<AStartNode>();
        for (int i=row -1;i<=row + 1; i++)
        {
            if ((i < 0 || i > rowCount-1)) continue;
            for (int j = col - 1; j <= col + 1; j++)
            {
                if ((j < 0 || j > colCount-1)) continue; 
                AStartNode point = maps[i, j];
                if (point == null) continue;
                if (point.isObstacle) continue;
                if (closeList.Contains(point)) continue;
                points.Add(point);

            }
        }
        return points;
    }

    /// <summary>
    /// 寻找F值最小的点
    /// </summary>
    /// <param name="nodes"></param>
    /// <returns></returns>
    public AStartNode FindMinPoint(List<AStartNode> nodes)
    {
        float minF = float.MaxValue;
        AStartNode minNode = null; ;
        foreach(AStartNode point in nodes)
        {
            if (point.F < minF)
            {
                minF = point.F;
                minNode = point;
            }
        }
        return minNode;
    }

    /// <summary>
    /// 计算G值
    /// </summary>
    /// <param name="a"></param>
    /// <param name="b"></param>
    /// <returns></returns>
    public float CaculateG(AStartNode a,AStartNode b)
    {
        float G = a.G + Vector3.Distance(a.postion, b.postion);
        return G;
    }
    
    /// <summary>
    /// 设置障碍物
    /// </summary>
    /// <param name="x"></param>
    /// <param name="y"></param>
    private void SetObstacle(int x,int y)
    {
        maps[x, y].isObstacle = true;
        GameObject cube = GameObject.CreatePrimitive(PrimitiveType.Cube);
        cube.transform.position = maps[x, y].postion;
    }

}