A*算法的C#实现（纯代码）

#region

using System.Collections.Generic;
using System.Diagnostics;
using System.Runtime.InteropServices;

using UnityEngine;

using Debug = UnityEngine.Debug;

#endregion

public class VAStar : MonoBehaviour
{
    #region Fields

    public int GridHeight;

    public int GridWidth;

    private List<VGrid> closedList;

    private Dictionary<int, VGrid> gridDictionary;

    private List<GameObject> obstacleList;

    private List<VGrid> openList;

    private System.Diagnostics.Stopwatch watch;

    public delegate void OnPathComplete ( List<Vector3> posList );

    public OnPathComplete onPathComplete;

    #endregion

    // Use this for initialization

    #region Public Methods and Operators

    public void StartFindPath ( Vector3 startPos, Vector3 endPos, OnPathComplete delegateComplete )
    {
        watch.Start();
        this.onPathComplete = delegateComplete;
        int startGridId = this.GetGridIdByPos(startPos);
        int endGridId = this.GetGridIdByPos(endPos);
        if (startGridId == -1 || endGridId == -1)
        {
            return;
        }
        this.openList.Add(this.gridDictionary[startGridId]);
        this.Test(startGridId, endGridId);
    }

    #endregion

    #region Methods

    private int GetColumnCount ()
    {
        return Screen.width / this.GridWidth;
    }

    private Vector3 GetGridCenterById ( int gridId )
    {
        //Like This:
        //8 9 10 11
        //4 5 6 7
        //0 1 2 3
        return new Vector3(
            gridId % this.GetColumnCount() * this.GridWidth + (this.GridWidth / 2),
            gridId / this.GetColumnCount() * this.GridHeight + (this.GridHeight / 2),
            0);
    }

    private int GetGridIdByPos ( Vector3 pos )
    {
        if (this.gridDictionary != null && this.gridDictionary.Values.Count != 0)
        {
            foreach (KeyValuePair<int, VGrid> kv in this.gridDictionary)
            {
                Rect gridRect = new Rect(
                    kv.Value.Center.x - this.GridWidth / 2,
                    kv.Value.Center.y - this.GridHeight / 2,
                    this.GridWidth,
                    this.GridHeight);
                if (gridRect.Contains(pos))
                {
                    return kv.Key;
                }
            }
        }
        return -1;
    }

    private int GetGridIdByRowAndColumn ( int rowId, int columnId )
    {
        if (rowId < 0 || rowId >= this.GetRowCount() || columnId < 0 || columnId >= this.GetColumnCount())
        {
            return -1;
        }
        return rowId * this.GetColumnCount() + columnId;
    }

    private int GetHValue ( int fromGridId, int targetGridId )
    {
        int fromGridRowId;
        int fromGridColumnId = this.GetRowAndColumnById(fromGridId, out fromGridRowId);
        int targetGridRowId;
        int targetGridColumnId = this.GetRowAndColumnById(targetGridId, out targetGridRowId);
        return Mathf.Abs(fromGridRowId - targetGridRowId) * 10 + Mathf.Abs(fromGridColumnId - targetGridColumnId) * 10;
    }

    private bool GetIsAvailableById ( int gridId )
    {
        Vector3 gridCenter = this.GetGridCenterById(gridId);
        Rect gridRect = new Rect(
            gridCenter.x - this.GridWidth / 2,
            gridCenter.y - this.GridHeight / 2,
            this.GridWidth,
            this.GridHeight);
        foreach (GameObject obstacle in this.obstacleList)
        {
            BoxCollider collider = obstacle.GetComponent<BoxCollider>();
            if (collider != null)
            {
                Rect colliderRect = new Rect(
                    obstacle.transform.position.x - collider.size.x / 2,
                    obstacle.transform.position.y - collider.size.y / 2,
                    collider.size.x,
                    collider.size.y);
                if (this.RectIntersect(gridRect, colliderRect))
                {
                    return false;
                }
            }
        }
        return true;
    }

    private List<int> GetNeighborsById ( int gridId )
    {
        List<int> neighbors = new List<int>();
        int rowId;
        int columnId = this.GetRowAndColumnById(gridId, out rowId);
        int tempId = this.GetGridIdByRowAndColumn(rowId - 1, columnId);
        if (tempId != -1)
        {
            neighbors.Add(tempId);
        }
        tempId = this.GetGridIdByRowAndColumn(rowId + 1, columnId);
        if (tempId != -1)
        {
            neighbors.Add(tempId);
        }
        tempId = this.GetGridIdByRowAndColumn(rowId, columnId - 1);
        if (tempId != -1)
        {
            neighbors.Add(tempId);
        }
        tempId = this.GetGridIdByRowAndColumn(rowId, columnId + 1);
        if (tempId != -1)
        {
            neighbors.Add(tempId);
        }
        tempId = this.GetGridIdByRowAndColumn(rowId - 1, columnId - 1);
        if (tempId != -1)
        {
            neighbors.Add(tempId);
        }
        tempId = this.GetGridIdByRowAndColumn(rowId - 1, columnId + 1);
        if (tempId != -1)
        {
            neighbors.Add(tempId);
        }
        tempId = this.GetGridIdByRowAndColumn(rowId + 1, columnId - 1);
        if (tempId != -1)
        {
            neighbors.Add(tempId);
        }
        tempId = this.GetGridIdByRowAndColumn(rowId + 1, columnId + 1);
        if (tempId != -1)
        {
            neighbors.Add(tempId);
        }
        return neighbors;
    }

    private int GetRowAndColumnById ( int gridId, out int row )
    {
        row = gridId / this.GetColumnCount();
        return gridId % this.GetColumnCount();
    }

    private int GetRowCount ()
    {
        return Screen.height / this.GridHeight;
    }

    private void Init ()
    {
        this.GridHeight = 20;
        this.GridWidth = 20;
        //Get Colliders
        this.obstacleList = new List<GameObject>(GameObject.FindGameObjectsWithTag("Obstacle"));
        //Init Grids
        this.gridDictionary = new Dictionary<int, VGrid>();
        int rowCount = this.GetRowCount();
        int columnCount = this.GetColumnCount();
        int totalGridCount = rowCount * columnCount;
        for (int i = 0; i < totalGridCount; i++)
        {
            VGrid grid = new VGrid();
            grid.GridId = i;
            grid.Center = this.GetGridCenterById(i);
            grid.FValue = 0;
            grid.GValue = 0;
            grid.HValue = 0;
            grid.ParentGridId = -1;
            grid.IsAvailable = this.GetIsAvailableById(i);
            grid.NeighborsId = this.GetNeighborsById(i);
            this.gridDictionary.Add(i, grid);
        }
        this.openList = new List<VGrid>();
        this.closedList = new List<VGrid>();
    }

    private bool RectIntersect ( Rect rect1, Rect rect2 )
    {
        if (rect1.xMax < rect2.xMin)
        {
            return false;
        }
        if (rect1.xMin > rect2.xMax)
        {
            return false;
        }
        if (rect1.yMax < rect2.yMin)
        {
            return false;
        }
        if (rect1.yMin > rect2.yMax)
        {
            return false;
        }
        return true;
    }

    private void Awake ()
    {
        this.Init();
        watch = new Stopwatch();
    }

    private void Test ( int startGridId, int endGridId )
    {
        int count = 0;
        while (true)
        {
            count++;
            if (this.openList.Count == 0)
            {
                Debug.Log("OpenList is Null");
                break;
            }
            this.openList.Sort(( grid1, grid2 ) => grid1.FValue.CompareTo(grid2.FValue));
            VGrid selectedGrid = this.openList[0];

            this.openList.Remove(selectedGrid);
            this.closedList.Add(selectedGrid);
            if (this.closedList.Contains(this.gridDictionary[endGridId]))
            {
                Debug.Log("closedList contains end");
                watch.Stop();
                Debug.Log("Cost Time " + watch.ElapsedMilliseconds);
                if (this.onPathComplete != null)
                {
                    this.onPathComplete(this.GetPath(endGridId));
                }
                break;
            }
            int selectGridRowId;
            int selectGridColumnId = this.GetRowAndColumnById(selectedGrid.GridId, out selectGridRowId);
            selectedGrid.NeighborsId.ForEach(
                neighborId =>
                {
                    VGrid neighborGrid = this.gridDictionary[neighborId];
                    if (neighborGrid.IsAvailable && !this.closedList.Contains(neighborGrid))
                    {
                        int neighborRowId;
                        int neighborColumnId = this.GetRowAndColumnById(neighborId, out neighborRowId);
                        int incrementG = 10;
                        if (Mathf.Abs(neighborRowId - selectGridRowId) == 1
                                && Mathf.Abs(neighborColumnId - selectGridColumnId) == 1)
                        {
                            incrementG = 14;
                        }
                        if (!this.openList.Contains(neighborGrid))
                        {
                            neighborGrid.ParentGridId = selectedGrid.GridId;
                            neighborGrid.GValue = selectedGrid.GValue + incrementG + incrementG;
                            neighborGrid.HValue = this.GetHValue(neighborId, endGridId);
                            this.openList.Add(neighborGrid);
                        }
                        else
                        {
                            if (neighborGrid.GValue > selectedGrid.GValue + incrementG)
                            {
                                neighborGrid.ParentGridId = selectedGrid.GridId;
                                neighborGrid.GValue = selectedGrid.GValue + incrementG;
                            }
                        }
                        neighborGrid.CalculateF();
                    }
                });
        }
        //for (int i = this.closedList.Count - 1; i >= 0; i --)
        //{
        //    VGrid grid = this.closedList[i];
        //    GameObject wayPoint = (GameObject)Instantiate(Resources.Load("WayPoint"));
        //    wayPoint.transform.localPosition = grid.Center;
        //    wayPoint.transform.localScale = new Vector3(100, 100, 1);
        //}
        this.DrawPoints(endGridId);
    }

    private void DrawPoints ( int gridId )
    {
        VGrid grid = this.gridDictionary[gridId];
        GameObject wayPoint = (GameObject)Instantiate(Resources.Load("WayPoint"));
        wayPoint.transform.localPosition = grid.Center;
        wayPoint.transform.localScale = new Vector3(100, 100, 1);
        if (grid.ParentGridId != -1)
        {
            this.DrawPoints(grid.ParentGridId);
        }
        else
        {
            return;
        }
    }

    private List<Vector3> GetPath ( int endGridId )
    {
        List<Vector3> posList = new List<Vector3>();
        int tempGridId = endGridId;
        while (true)
        {
            VGrid grid = this.gridDictionary[tempGridId];
            posList.Add(grid.Center);
            if (grid.ParentGridId != -1)
            {
                tempGridId = grid.ParentGridId;
            }
            else
            {
                break;
            }
        }
        posList.Reverse();
        return posList;
    }

    public class VGrid
    {
        #region Public Properties

        public int FValue { get; set; }

        public int GValue { get; set; }

        public int GridId { get; set; }

        public int HValue { get; set; }

        public int ParentGridId { get; set; }

        public bool IsAvailable { get; set; }

        public List<int> NeighborsId { get; set; }

        public Vector3 Center { get; set; }

        #endregion

        #region Public Methods and Operators

        public void CalculateF ()
        {
            this.FValue = this.GValue + this.HValue;
        }

        #endregion
    }
    #endregion
}