Unity中A*算法算法实现

A*算法（曼哈顿距离）

 

1. 地图分割：按照需求将地图分割为二维数组个格子
2. 计算公式：F = G + H，F:为预估代价，G:起点到当前位置的距离代价，H:当前位置到终点的估算代价
3. OpenList: 开放列表存放未考察的点；ClosedList: 存放已经考察过的点

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

public class Point
{
    public Point Parent { get; set; }
    public float F { get; set; }
    public float G { get; set; }
    public float H { get; set; }

    public int X { get; set; }
    public int Y { get; set; }

    public bool IsWall { get; set; }

    public Point(int x,int y,Point parent = null)
    {
        this.X = x;
        this.Y = y;
        this.Parent = parent;
        IsWall = false;
    }

    public void UpdateParent(Point parent,float g)
    {
        this.Parent = parent;
        this.G = g;
        F = G + H;
    }
}

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

public class AStar : MonoBehaviour
{
    private const int mapWith = 15;
    private const int mapHeight = 15;

    private Point[,] map = new Point[mapWith, mapHeight];

    // Use this for initialization
    void Start()
    {
        InitMap();
        Point start = map[2, 3];
        Point end = map[7, 4];
        FindPath(start, end);
        ShowPath(start,end);
        //List<Point> l = GetSurroundPoints(map[3, 3]);
        //foreach (Point p in l)
        //{
        //    Debug.Log(p.X + "-" + p.Y);
        //} 
    }

    /// <summary>
    /// 显示路径
    /// </summary>
    /// <param name="start"></param>
    /// <param name="end"></param>
    private void ShowPath(Point start, Point end)
    {
        Point temp = end;
        while (true)
        {
            //Debug.Log(temp.X + "," + temp.Y);
            Color c = Color.gray;
            if (temp == start)
            {
                c = Color.green;
            }
            else if (temp==end)
            {
                c = Color.red;
            }
            CreateCube(temp.X, temp.Y, c);
            if (temp.Parent == null)
            {
                break;
            }
            temp = temp.Parent;
        }

        for (int x = 0; x < mapWith; x++)
        {
            for (int y = 0; y < mapHeight; y++)
            {
                if (map[x,y].IsWall)
                {
                    CreateCube(x, y, Color.blue);
                }
            }
        }
    }

    private void CreateCube(int x,int y,Color color)
    {
        GameObject go = GameObject.CreatePrimitive(PrimitiveType.Cube);
        go.transform.position = new Vector3(x, y, 0);
        go.GetComponent<Renderer>().material.color = color;
    }

    /// <summary>
    /// 初始化地图
    /// </summary>
    private void InitMap()
    {
        for (int x = 0; x < mapWith; x++)
        {
            for (int y = 0; y < mapHeight; y++)
            {
                map[x, y] = new Point(x, y);
            }
        }
        map[4, 2].IsWall = true;
        map[4, 3].IsWall = true;
        map[4, 4].IsWall = true;
    }

    private void FindPath(Point start, Point end)
    {
        List<Point> openList = new List<Point>();       //开放列表
        List<Point> closeList = new List<Point>();      //关闭列表

        openList.Add(start);//将起点加入到开启列表中
        while (openList.Count > 0)
        {
            Point point = FindMinFOfPoint(openList);        //从开启列表中查找F值最小的点
            openList.Remove(point);                         //将查找到的F值最小的点从开启列表中移除
            closeList.Add(point);                           //将查找到的值加入到关闭列表中
            List<Point> surroundPoints = GetSurroundPoints(point);//查找周围的点
            PointsFilter(surroundPoints, closeList);        //将周围在关闭列表中的点去除

            //遍历周围的点
            foreach (Point surroundPoint in surroundPoints)
            {
                if (openList.IndexOf(surroundPoint) > -1)//如果开放列表中存在当前遍历到的点
                {
                    float nowG = CalcG(surroundPoint, point);       //当前点的G值
                    if (nowG < surroundPoint.G)                     //如果计算出的G值小于当前点的原本G值，就更新该点的G值和父节点
                    {
                        surroundPoint.UpdateParent(point, nowG);
                    }
                }
                else//如果不存在开放列表
                {
                    surroundPoint.Parent = point;           //设置父节点
                    CalcF(surroundPoint, end);              //计算F值
                    openList.Add(surroundPoint);            //添加到开放列表中
                }

            }
            //判断一下，如果结束点在开放列表中就结束路径查找   (路径已经查找到)
            if (openList.IndexOf(end) > -1)
            {
                break;
            }
        }
    }

    /// <summary>
    /// 过滤,将周围在关闭列表中的点去除
    /// </summary>
    /// <param name="src"></param>
    /// <param name="closeList"></param>
    private void PointsFilter(List<Point> src, List<Point> closeList)
    {
        foreach (Point p in closeList)
        {
            if (src.IndexOf(p) > -1)
            {
                src.Remove(p);
            }
        }
    }

    /// <summary>
    /// 查找周围的点
    /// </summary>
    /// <returns></returns>
    private List<Point> GetSurroundPoints(Point point)
    {
        Point up = null, down = null, left = null, right = null;
        Point lu = null, ru = null, ld = null, rd = null;
        if (point.Y < mapHeight - 1)
        {
            up = map[point.X, point.Y + 1];
        }

        if (point.Y > 0)
        {
            down = map[point.X, point.Y - 1];
        }

        if (point.X > 0)
        {
            left = map[point.X - 1, point.Y];
        }

        if (point.X < mapWith - 1)
        {
            right = map[point.X + 1, point.Y];
        }

        if (up != null && left != null)
        {
            lu = map[point.X - 1, point.Y + 1];
        }
        if (up != null && right != null)
        {
            ru = map[point.X + 1, point.Y + 1];
        }

        if (down != null && left != null)
        {
            ld = map[point.X - 1, point.Y - 1];
        }
        if (down != null && right != null)
        {
            rd = map[point.X + 1, point.Y - 1];
        }

        List<Point> list = new List<Point>();
        if (down != null && down.IsWall == false)
        {
            list.Add(down);
        }
        if (up != null && up.IsWall == false)
        {
            list.Add(up);
        }
        if (left != null && left.IsWall == false)
        {
            list.Add(left);
        }
        if (right != null && right.IsWall == false)
        {
            list.Add(right);
        }

        if (lu != null && lu.IsWall == false && left.IsWall == false && up.IsWall == false)
        {
            list.Add(lu);
        }
        if (ld != null && ld.IsWall == false && left.IsWall == false && down.IsWall == false)
        {
            list.Add(ld);
        }
        if (ru != null && ru.IsWall == false && right.IsWall == false && up.IsWall == false)
        {
            list.Add(ru);
        }
        if (rd != null && rd.IsWall == false && right.IsWall == false && down.IsWall == false)
        {
            list.Add(rd);
        }
        return list;
    }

    /// <summary>
    /// 查找开启列表中F值最小的点
    /// </summary>
    /// <param name="openList"></param>
    /// <returns></returns>
    private Point FindMinFOfPoint(List<Point> openList)
    {
        float f = float.MaxValue;
        Point temp = null;
        foreach (Point p in openList)
        {
            if (p.F < f)
            {
                temp = p;
                f = p.F;
            }
        }
        return temp;
    }
    
    /// <summary>
    /// 计算G值
    /// </summary>
    /// <param name="now"></param>
    /// <param name="parent"></param>
    /// <returns></returns>
    private float CalcG(Point now, Point parent)
    {

        return Vector2.Distance(new Vector2(now.X, now.Y), new Vector2(parent.X, parent.Y)) + parent.G;

    }

    /// <summary>
    /// 计算F值
    /// </summary>
    /// <param name="now"></param>
    /// <param name="end"></param>
    private void CalcF(Point now, Point end)
    {
        // F = G + H
        float h = Mathf.Abs(end.X - now.X) + Mathf.Abs(end.Y - now.Y);
        float g = 0;
        if (now.Parent == null)
        {
            g = 0;
        }
        else
        {
            g = Vector2.Distance(new Vector2(now.X, now.Y), new Vector2(now.Parent.X, now.Parent.Y)) + now.Parent.G;
        }
        float f = g + h;
        now.F = f;
        now.G = g;
        now.H = h;
    }
}