人工智能4_寻路之A形算法

 

F为该点到父节点一直走到起点的距离，H为该点到重点的距离；

从中心向周围所有的点计算，跳出F的最小，第一轮为C点

以C点为圆心，除去C点，起点，障碍物后再计算一圈，发现从C走到D，比走到D的计算出的F值还大，所以变更路线，第一步直接到D点，然后以D为中心，围绕着D的一圈（除去C，除去起点）继续计算，算出D下面的一点

 坐标点类

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;
        this.IsWall = false;
    } 
    public void UpdateParent(Point parent, float G)
    {
        this.Parent = parent;
        this.G = G;
        F = G + H;
    }
}

 

A形算法具体实现

public class Astar : MonoBehaviour {

    public int mapWidth = 8;
    public int mapHeight = 6;
    private Point[,] map = new Point[8,6];

    void Start () 
    {
        InitMap();
        Point start = map[2, 3];
        Point end = map[6, 3];
        FindPath(start,end);
        //显示最短路径
        ShowPath(start, end);
	}

    //初始化地图
    private void InitMap()
    {
        for (int x = 0; x < mapWidth; x++)
        {
            for (int y = 0; y < mapHeight; y++)
            {
                map[x, y] = new Point(x,y);
                if (x == 4 && y <= 4 && y >= 2)
                {
                    map[x, y].IsWall = true;
                }
            }
            
        }
    }
    
    //寻路
    private void FindPath(Point start, Point end)
    {
        //开启集合，相当于周围的8个点
        List<Point> openList = new List<Point>();
        //关闭集合，相当于走过的点
        List<Point> closeList = new List<Point>();
        openList.Add(start);
       
        while (openList.Count>0)
        {
            Point point = FindMinPoint(openList);
            openList.Remove(point);
            closeList.Add(point);
            List<Point> surroudPoints = GetSurrountPints(point);
            FilterPoints(surroudPoints, closeList);
            foreach (Point surrondPoint in surroudPoints)
            {
                if (openList.IndexOf(surrondPoint) > -1)
                {
                    float nowG = CalculateG(surrondPoint,point);
                    if (nowG < surrondPoint.G)
                    {
                        surrondPoint.UpdateParent(point, nowG);
                    }
                }
                else
                {
                    surrondPoint.Parent = point;
                    CalculateF(surrondPoint, end);
                    openList.Add(surrondPoint);
                }
            }
            //终点出现在周围的点中
            if (openList.IndexOf(end) > -1)
            {
                break;
            }
        }
    }

    private void ShowPath(Point start,Point end)
    {
        Point temp = end;
        while (true)
        {
            if (temp.Parent == null) 
                break;
            temp = temp.Parent;
            Debug.Log("最近路径的坐标  " + temp.X + "____" + temp.Y);
        }
    }


    //计算距离F
    private void CalculateF(Point now, Point end)
    {
        //F=G+H;
        //now到end的距离
        float h = Mathf.Abs(end.X - now.X) + Mathf.Abs(end.Y - now.Y);
        //now到start的距离
        float g = 0;
        if (now.Parent == null)
        {
            g = 0;
        }
        else
        {
            //now到start的距离
            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;
    }

    private float CalculateG(Point now,Point parent)
    {
        return Vector2.Distance(new Vector2(now.X, now.Y), new Vector2(parent.X, parent.Y)) + parent.G;
    }

    //从开启的列表中找f值最小的
    private Point FindMinPoint(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;
    }

    //得到某个点周围的点的集合
    private List<Point> GetSurrountPints(Point point)
    {
        List<Point> list = new List<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 (!up.IsWall)
            {
                list.Add(up);
            }
        }
        if (point.Y > 0)
        {
            down = map[point.X, point.Y - 1];
            if (!down.IsWall)
            {
                list.Add(down);
            }
        }
        if (point.X > 0)
        {
            left = map[point.X - 1, point.Y];
            if (!left.IsWall)
            {
                list.Add(left);
            }
        }
        if (point.X < mapWidth - 1)
        {
            right = map[point.X + 1, point.Y];
            if (!right.IsWall)
            {
                list.Add(right);
            }
        }
        if (up != null && left != null)
        {
            lu = map[point.X - 1, point.Y + 1];
            //左边不是障碍物，上边不是障碍物，左上也不是障碍物
            if (!lu.IsWall&&!up.IsWall&&!left.IsWall) 
            {
                list.Add(lu);
            }
        }
        if (up != null && right != null)
        {
            ru = map[point.X + 1, point.Y + 1];
            if (!ru.IsWall&&!up.IsWall&&!right.IsWall)
            {
                list.Add(ru);
            }
        }
        if (down != null && left != null)
        {
            ld = map[point.X - 1, point.Y - 1];
            if (!ld.IsWall&&!down.IsWall&&!left.IsWall)
            {
                list.Add(ld);
            }
        }
        if (down != null && right != null)
        {
            rd = map[point.X + 1, point.Y - 1];
            if (!rd.IsWall&&!down.IsWall&&!right.IsWall)
            {
                list.Add(rd);
            }
        }
        return list;
    }

    //过滤点的集合
    private void FilterPoints(List<Point> src, List<Point> ClosList)
    {
        foreach (Point p in ClosList)
        {
            if (src.IndexOf(p) > -1)
            {
                src.Remove(p);
            }
        }
    }
}