Unity A*算法实现

最近赶时间，字以后再码吧 ^_^

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

namespace Astar
{
    public class PathFinder : MonoBehaviour
    {
        List<Node> Open = new List<Node>();
        List<Node> Close = new List<Node>();
        List<Node> All = new List<Node>();

        Node start;
        Node end;

        public static bool LOG { get { return false; } }
        public static float F { get { return 5; } }

        void Start()
        {
            Init();
        }

        //初始化
        void Init()
        {
            Open.Clear();
            Close.Clear();
            for (int x = 0; x < Map.width; x++)
            {
                for (int z = 0; z < Map.deep; z++)
                {
                    All.Add(new Node(x, z));
                }
            }
        }


        void Reset()
        {
            Open.Clear();
            Close.Clear();
            foreach (var node in All)
            {
                node.Reset();
                Open.Add(node);
            }
        }

        public List<IntVector2> Find(IntVector2 from, IntVector2 to)
        {
            Reset();
            if (LOG) Debug.Log("find start.");
            start = GetNode(from);
            start.cost = 0;
            start.f = 0;
            end = GetNode(to);
            int count = 0;
            while (Open.Count != 0)
            {
                var N = GetMinFNode();
                if (LOG) Debug.Log("find step " + count + ":" + N.pos + " , f=" + N.f);
                var dirs = new IntVector2[4] { new IntVector2(0, 1), new IntVector2(0, -1), new IntVector2(1, 0), new IntVector2(-1, 0) };
                foreach (var dir in dirs)
                {
                    bool inclose;
                    var X = GetNode(N.pos + dir, out inclose);
                    //在地图外
                    if (X == null)
                    {
                        continue;
                    }
                    else if (X == end)
                    {
                        X.last = N;
                        List<IntVector2> res = new List<IntVector2>();
                        var cur = end;
                        while (cur.last != null)
                        {
                            res.Add(cur.pos);
                            cur = cur.last;
                        }
                        res.Reverse();
                        return res;
                    }
                    //在Open内
                    //Debug.LogError("运行到这里了");
                    else if (!inclose)
                    {
                        UpdateFValue(N, X);
                        continue;
                    }
                    //在Close内
                    else if (inclose)
                    {
                        bool reOpen = UpdateFValue(N, X);
                        if (reOpen)
                        {
                            Open.Add(X);
                            Close.Remove(X);
                        }
                        continue;
                    }
                }
                Debug.DrawLine((Vector3)N.pos, (Vector3)N.pos + Vector3.up, Color.red, 1);
                Close.Add(N);
                Open.Remove(N);
            }
            return null;
        }


        /// <summary>
        /// 对X的值进行更新。
        /// </summary>
        /// <param name="N">当前正在Close的点</param>
        /// <param name="X">刷新值的点</param>
        /// <returns>返回是否需要重新OPEN</returns>
        bool UpdateFValue(Node N, Node X)
        {
            if (X.cost > N.cost + 1) X.cost = N.cost + 1;
            if (X.left < 0) X.left = Node.distance(end, X);
            var newF = X.cost + X.left * F;
            if (newF < X.f)
            {
                X.f = newF;
                X.last = N;
                return true;
            }
            else return false;
        }


        Node GetNode(IntVector2 v, out bool inClose)
        {
            if (!Map.IsInMap(v))
            {
                inClose = false;
                return null;
            }
            if (!Map.Walkable(v.x, v.z))
            {
                inClose = false;
                return null;
            }
            for (int i = 0; i < Open.Count; i++)
            {
                if (Open[i].pos == v)
                {
                    inClose = false;
                    return Open[i];
                }
            }
            for (int i = 0; i < Close.Count; i++)
            {
                if (Close[i].pos == v)
                {
                    inClose = true;
                    return Close[i];
                }
            }
            Debug.Log("意料之外的情况");
            inClose = false;
            return null;
        }

        Node GetNode(IntVector2 v)
        {
            bool useless;
            return GetNode(v, out useless);
        }

        Node GetMinFNode()
        {
            if (Open.Count == 0) return null;
            Node result = Open[0];
            foreach (var node in Open)
            {
                if (node.f < result.f)
                {
                    result = node;
                }
            }
            return result;
        }

        void Update()
        {
            if (Input.GetKeyDown(KeyCode.F))
            {
                IntVector2 a = (IntVector2)GameObject.Find("a").transform.position;
                IntVector2 b = (IntVector2)GameObject.Find("b").transform.position;
                var path = Find(a, b);
                for (int i = 1; i < path.Count; i++)
                {
                    Debug.DrawLine((Vector3)path[i - 1], (Vector3)path[i], Color.yellow, 3f);
                }
                path = PathFixer.Fix(path, 0.3f);
                for (int i = 1; i < path.Count; i++)
                {
                    Debug.DrawLine((Vector3)path[i - 1] + Vector3.up * 0.2f, (Vector3)path[i] + Vector3.up * 0.2f, Color.red, 3f);
                }
            }
        }
    }
}