unity二维数组A*寻路

public class AStarData
{
    public float G;//到下一个的距离
    public float H;//到终点的步数
    public float F;//G+H
    public Vector2 pos;//坐标
    public AStarData last;//上一个

    public AStarData(Vector2 pos)
    {
        this.pos = pos;
    }
}

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

public class AStar : Singleton<AStar>
{
    int[,] arr;
    Vector2 begin;
    Vector2 end;
    bool isEight = false;//是否八方向寻路
    //取出数组中第一个元素out没有第一个元素的数组
    public AStarData Shift(List<AStarData> list, out List<AStarData> outlist)
    {
        AStarData data = list[0];
        List<AStarData> arr = new List<AStarData>();
        for (int i = 1; i < list.Count; i++)
        {
            arr.Add(list[i]);
        }
        outlist = arr;
        return data;
    }
    //获取点在数据集合中的位置
    public int GetIndex(Vector2 pos, List<AStarData> list)
    {
        for (int i = 0; i < list.Count; i++)
        {
            if (list[i].pos == pos)
            {
                return i;
            }
        }
        return -1;
    }
    //获取路径（起点，终点，二维数组地图）
    public List<Vector2> GetPath(Vector2 begin,Vector2 end, int[,] arr)
    {
        this.begin = begin;
        this.end = end;
        this.arr = arr;
        List<AStarData> openlist = new List<AStarData>();
        List<AStarData> closedlist = new List<AStarData>();
        List<Vector2> paths = new List<Vector2>();
        openlist.Add(new AStarData(begin));
        do
        {
            AStarData data = Shift(openlist, out List<AStarData> newlist);
            openlist = newlist;
            closedlist.Add(data);
            //找到终点就结束
            if (data.pos == end)
            {
                //查找所有父级返回路径
                AStarData last = data;
                do
                {
                    paths.Add(last.pos);
                    last = last.last;
                } while (last != null);
                break;
            }
            //获取所有可走的路
            List<Vector2> list = GetSetp(data.pos, closedlist);
            //想获取最短路径记得给list排序，目前是不排序直接返回第一条路径
            for (int i = 0; i < list.Count; i++)
            {
                Vector2 p = list[i];
                AStarData step = new AStarData(p);
                int index = GetIndex(p, openlist);
                //判断路是否存在open集合里，不存在则创建，存在则留下G比较小的
                if (index == -1)
                {
                    step.last = data;
                    step.G = data.G + Vector2.Distance(p, data.pos);
                    step.H = Mathf.Abs((int)end.x - (int)p.x) + Mathf.Abs((int)end.y - (int)p.y);
                    step.F = step.G + step.H;
                    openlist.Add(step);
                }
                else
                {
                    step = openlist[index];
                    if (data.G + Vector2.Distance(p, data.pos) < step.G)
                    {
                        step.G = data.G + Vector2.Distance(p, data.pos);
                        step.F = step.G + step.H;
                        openlist.Add(step);
                    }
                }
            }
            //重新排序open集合
            openlist.Sort((a, b) =>
            {
                if (a.F < b.F)
                {
                    return -1;
                }
                else if (a.F == b.F)
                {
                    return 0;
                }
                else
                {
                    return 1;
                }
            });
        } while (openlist.Count > 0);
        return paths;
    }
    public List<Vector2> GetSetp(Vector2 pos, List<AStarData> closedlist)
    {
        List<Vector2> list = new List<Vector2>();
        if (AddOpen(pos.x, pos.y + 1, closedlist))
        {
            list.Add(new Vector2(pos.x, pos.y + 1));
        }
        if (AddOpen(pos.x + 1, pos.y, closedlist))
        {
            list.Add(new Vector2(pos.x + 1, pos.y));
        }
        if (AddOpen(pos.x, pos.y - 1, closedlist))
        {
            list.Add(new Vector2(pos.x, pos.y - 1));
        }
        if (AddOpen(pos.x - 1, pos.y, closedlist))
        {
            list.Add(new Vector2(pos.x - 1, pos.y));
        }
        if (isEight)
        {
            if (AddOpen(pos.x + 1, pos.y + 1, closedlist))
            {
                list.Add(new Vector2(pos.x + 1, pos.y + 1));
            }
            if (AddOpen(pos.x + 1, pos.y - 1, closedlist))
            {
                list.Add(new Vector2(pos.x + 1, pos.y - 1));
            }
            if (AddOpen(pos.x - 1, pos.y - 1, closedlist))
            {
                list.Add(new Vector2(pos.x - 1, pos.y - 1));
            }
            if (AddOpen(pos.x - 1, pos.y + 1, closedlist))
            {
                list.Add(new Vector2(pos.x - 1, pos.y + 1));
            }
        }
        return list;
    }
    public bool AddOpen(float x, float y, List<AStarData> closedlist)
    {
        Vector2 pos = new Vector2(x, y);
        
        if (x >= 0 && x < arr.GetLength(0) && y >= 0 && y < arr.GetLength(1) && arr[(int)x, (int)y] == 0 && GetIndex(pos, closedlist) == -1)
        {
            return true;
        }
        return false;
    }
}