Unity a* road finding force planning

Unity A* Pathfinding

namespace AStar
{
    // Lattice type 
    public enum AStarType
    {
        WALK,
        STOP
    }
    public class AStarNode
    {
        public AStarNode() { }
        public AStarNode(int x, int y, AStarType type)
        {
            this.x = x;
            this.y = y;
            this.type = type;
        }

        // Parent lattice 
        public AStarNode father;
        // Grid location 
        public int x;
        public int y;
        //public Vector2 pos;
        // Map grid type 
        public AStarType type;
        // Pathfinding costs   Euler distance + Manhattan distance 
        public float f;
        // Distance from starting point   Euler distance 
        public float g;
        // Distance from the end point   Manhattan distance 
        public float h;
        // Reset node information 
        public void ResetNode()
        {
            this.father = null;
            this.f = 0;
            this.g = 0;
            this.h = 0;
        }
    }
}

using System.Collections.Generic;
using UnityEngine;

namespace AStar
{
    public class AStarMgr
    {
        private static AStarMgr instance = new AStarMgr();
        public static AStarMgr Instance { get => instance; set => instance = value; }

        private int mapW;

        private int mapH;
        // Map information 
        public AStarNode[,] nodes;
        // Open list 
        private List<AStarNode> openList;
        // Close list 
        private List<AStarNode> closeList;

        private List<AStarNode> path = new List<AStarNode>();

        // Load map information 
        public void InitMaps(int w, int h)
        {
            mapW = w;
            mapH = h;
            openList = new List<AStarNode>();
            closeList = new List<AStarNode>();
            nodes = new AStarNode[w, h];
            for (var i = 0; i < w; i++)
            {
                for (var j = 0; j < h; j++)
                {
                    var node = new AStarNode(i, j, Random.Range(0, 100) < 20 ? AStarType.STOP : AStarType.WALK);
                    nodes[i, j] = node;
                }
            }
        }
        // Pathfinding 
        public List<AStarNode> FindPath(Vector2 startPos, Vector2 endPos)
        {
            var startNode = GetNode(startPos);
            var endNode = GetNode(endPos);
            if (!CheckNode(startNode.x, startNode.y) || !CheckNode(endNode.x, endNode.y) || !CheckType(startNode) || !CheckType(endNode)) return null;
            startNode.ResetNode();
            openList.Clear();
            closeList.Clear();
            AddCloseList(startNode);
            while (true)
            {
                FineNearNodeToOpenList((int)startNode.x - 1, (int)startNode.y - 1, 1.4f, startNode, endNode);
                FineNearNodeToOpenList((int)startNode.x, (int)startNode.y - 1, 1f, startNode, endNode);
                FineNearNodeToOpenList((int)startNode.x + 1, (int)startNode.y - 1, 1.4f, startNode, endNode);
                FineNearNodeToOpenList((int)startNode.x - 1, (int)startNode.y, 1f, startNode, endNode);
                FineNearNodeToOpenList((int)startNode.x + 1, (int)startNode.y, 1f, startNode, endNode);
                FineNearNodeToOpenList((int)startNode.x - 1, (int)startNode.y + 1, 1.4f, startNode, endNode);
                FineNearNodeToOpenList((int)startNode.x, (int)startNode.y + 1, 1f, startNode, endNode);
                FineNearNodeToOpenList((int)startNode.x + 1, (int)startNode.y + 1, 1.4f, startNode, endNode);

                // Dead end 
                if (openList.Count == 0) return null;

                openList.Sort(SortOpenList);
                AddCloseList(openList[0]);
                startNode = openList[0];
                openList.RemoveAt(0);
                if (startNode == endNode)
                {
                    path.Clear();
                    path.Add(endNode);
                    while (endNode.father != null)
                    {
                        path.Add(endNode.father);
                        endNode = endNode.father;
                    }
                    path.Reverse();
                    return path;
                }
            }
        }
        // Get the corresponding... From the map information AStarNode
        private AStarNode GetNode(Vector2 pos)
        {
            return nodes[(int)pos.x, (int)pos.y];
        }
        // Judge legally 
        private bool CheckNode(int x, int y)
        {
            // In scope 
            if (x < 0 || x >= mapW || y < 0 || y >= mapH) return false;
            return true;
        }
        // Judgment type 
        private bool CheckType(AStarNode node)
        {
            if (node.type == AStarType.WALK) return true;
            return false;
        }
        // Add to open list 
        private void AddOpenList(AStarNode node)
        {

            if (!openList.Contains(node))
                openList.Add(node);
        }
        // Add to close list 
        private void AddCloseList(AStarNode node)
        {

            if (!closeList.Contains(node))
                closeList.Add(node);
        }

        // Get the surrounding points and add them to the open list 
        private void FineNearNodeToOpenList(int x, int y, float g, AStarNode father, AStarNode end)
        {
            if (!CheckNode(x, y)) return;
            var tempNode = nodes[x, y];
            if (tempNode == null || !CheckType(tempNode) || openList.Contains(tempNode) || closeList.Contains(tempNode))
                return;
            // Calculate pathfinding consumption 
            tempNode.father = father;
            tempNode.g = father.g + g;
            tempNode.h = Mathf.Abs(end.x - tempNode.x) + Mathf.Abs(end.y - tempNode.y);
            tempNode.f = tempNode.g + tempNode.h;
            openList.Add(tempNode);

        }

        private int SortOpenList(AStarNode a, AStarNode b)
        {
            if (a.f > b.f) return 1;
            else return -1;
        }
    }
}

using System.Collections.Generic;
using UnityEngine;

namespace AStar
{
    public class AStarTest : MonoBehaviour
    {
        public int BeginX = 3;
        public int BeginY = -5;
        public int MapW = 5;
        public int MapH = 5;
        public int OffetX = 2;
        public int OffetY = 2;
        private Vector2 beginPos = Vector2.right * -1;
        private Dictionary<string, GameObject> cubes = new Dictionary<string, GameObject>();
        private List<AStarNode> path;
        private void Start()
        {
            path = new List<AStarNode>();
            AStarMgr.Instance.InitMaps(MapW, MapH);
            for (int i = 0; i < MapW; i++)
            {
                for (int j = 0; j < MapH; j++)
                {
                    var obj = GameObject.CreatePrimitive(PrimitiveType.Cube);
                    obj.transform.position = new Vector3(BeginX + i * OffetX, BeginY + j * OffetY, 0);
                    obj.name = i + "_" + j;
                    cubes.Add(obj.name, obj);
                    var node = AStarMgr.Instance.nodes[i, j];
                    if (node.type == AStarType.STOP)
                    {
                        obj.GetComponent<MeshRenderer>().material.color = Color.red;
                    }
                }
            }
        }

        private void Update()
        {
            if (Input.GetMouseButtonDown(0))
            {
                RaycastHit hitInfo;
                var ray = Camera.main.ScreenPointToRay(Input.mousePosition);
                if (Physics.Raycast(ray, out hitInfo, 1000))
                {
                    if (beginPos == Vector2.right * -1)
                    {
                        if (path != null)
                        {
                            foreach (var item in path)
                            {
                                cubes[item.x + "_" + item.y].GetComponent<MeshRenderer>().material.color = Color.white;
                            }
                        }
                        string[] strs = hitInfo.collider.gameObject.name.Split('_');
                        beginPos = new Vector2(int.Parse(strs[0]), int.Parse(strs[1]));
                        hitInfo.collider.gameObject.GetComponent<MeshRenderer>().material.color = Color.yellow;
                    }
                    else
                    {
                        string[] strs = hitInfo.collider.gameObject.name.Split('_');
                        var endPos = new Vector2(int.Parse(strs[0]), int.Parse(strs[1]));
                        hitInfo.collider.gameObject.GetComponent<MeshRenderer>().material.color = Color.green;
                        // Pathfinding 
                        path = AStarMgr.Instance.FindPath(beginPos, endPos);
                        if (path != null)
                        {
                            foreach (var item in path)
                            {
                                cubes[item.x + "_" + item.y].GetComponent<MeshRenderer>().material.color = Color.green;
                            }
                        }
                        beginPos = Vector2.right * -1;
                    }
                }
            }
        }
    }
}