unity A*寻路算法

基本原理网上很多，这里不做多余赘述。

本文代码展示一个二维网格地图的A*寻路算法，并进行了简单的优化， 在插入元素到开启列表时进行了有序插入操作，代码如下：

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

public class PathMgr : Singleton<PathMgr>
{
    private Dictionary<KeyValuePair<Vector3, Vector3>, List<Vector3>> pathRecord = new Dictionary<KeyValuePair<Vector3, Vector3>, List<Vector3>>();

    public bool isReady = false;

    public void ResetPathRecord()
    {
        pathRecord.Clear();
    }

    public bool Contains(Vector3 startPos, Vector3 endPos)
    {
        KeyValuePair<Vector3, Vector3> key = new KeyValuePair<Vector3, Vector3>(startPos, endPos);
        return pathRecord.ContainsKey(key);
    }

    public List<Vector3> MarketFindPath(Vector3 startPos, Vector3 endPos)
    {
        List<Vector3> path = new List<Vector3>();
        KeyValuePair<Vector3, Vector3> key = new KeyValuePair<Vector3, Vector3>(startPos, endPos);
        if (pathRecord.ContainsKey(key))
        {
            path = pathRecord[key];
        }
        else
        {
            List<AStarPoint> points = FindPathPoints(startPos, endPos);

            if (points ==null || points.Count <= 0)
            {
                Debug.Log($"从{startPos}到{endPos}路径为空");
            }

            //只记录拐点
            if (points != null && points.Count >0)
            {
                //x相同为1  y相同为2
                int lastCom = 1;
                int nextCom = 2;
                for (int i = points.Count - 1; i >= 0; i--)
                {
                    if (0 < i && i < points.Count - 1)
                    {
                        lastCom = points[i].mPosition.x == points[i + 1].mPosition.x ? 1 : 2;
                        nextCom = points[i].mPosition.x == points[i - 1].mPosition.x ? 1 : 2;
                        if (lastCom != nextCom)
                            path.Add(points[i].mPosition);
                    }
                    else
                    {
                        path.Add(points[i].mPosition);
                    }
                }

                //string str = "";
                //foreach (Vector2 vec in path)
                //{
                //    str += $"{vec}  ";
                //}
                //Debug.Log(str);

            }

            if (!pathRecord.ContainsKey(key))
            {
                pathRecord.Add(key, path);
            }
        }

        return path;
    }

    public List<AStarPoint> FindPathPoints(Vector3 startPos, Vector3 targetPos)
    {
        AStarPoint startPoint = new AStarPoint(startPos);
        AStarPoint endPoint = new AStarPoint(targetPos);
        CalcF(startPoint, endPoint);

        if (endPoint.mIsObstacle || startPos == targetPos)
        {
            return null;
        }

        //开启列表
        List<AStarPoint> openPointList = new List<AStarPoint>();
        //关闭列表
        List<AStarPoint> closePointList = new List<AStarPoint>();

        openPointList.Add(startPoint);
        while (openPointList.Count > 0)
        {
            AStarPoint minFPoint = FindPointWithMinF(openPointList);
            openPointList.Remove(minFPoint);
            //有序插入  关闭列表
            OrderlyInsert(closePointList, minFPoint);

            List<AStarPoint> surroundPoints = FindSurroundPoint(minFPoint);

            foreach (var surroundPoint in surroundPoints)
            {
                CalcF(surroundPoint, endPoint);

                //如果关闭列表中存在，过滤掉
                if (closePointList.Find(item => item.mPosition == surroundPoint.mPosition) != null)//
                {
                    continue;
                }

                AStarPoint ap = openPointList.Find(item => item.mPosition == surroundPoint.mPosition);
                //FindPoint(openPointList, surroundPoint);
                if (ap != null)
                {
                    //计算下新路径下的G值（H值不变的，比较G相当于比较F值）
                    float newPathG = CalcG(surroundPoint, minFPoint);
                    if (newPathG < ap.mG)
                    {
                        ap.mG = newPathG;
                        ap.mF = surroundPoint.mG + surroundPoint.mH;
                        ap.mParentPoint = minFPoint;
                    }
                }
                else
                {
                    surroundPoint.mParentPoint = minFPoint;
                    //有序插入  开启列表
                    //PrintList(openPointList, surroundPoint);
                    OrderlyInsert(openPointList, surroundPoint);
                }
            }

            //如果开始列表中包含了终点，说明找到路径
            AStarPoint target = openPointList.Find((item) => item.mPosition == endPoint.mPosition);
            if (target != null)
            {
                endPoint = target;
                break;
            }
        }

        if (endPoint.mParentPoint != null)
        {
            return ShowPath(endPoint);
        }
        else
        {
            Debug.Log("找不到路");
            return null;
        }
    }

    private List<AStarPoint> ShowPath(AStarPoint end)
    {
        List<AStarPoint> mPathPosList = new List<AStarPoint>();

        AStarPoint temp = end;
        while (true)
        {
            mPathPosList.Add(temp);

            if (temp.mParentPoint == null)
                break;
            temp = temp.mParentPoint;
        }

        return mPathPosList;
    }

    #region 有序插入
    public int GetInsertIndex(List<AStarPoint> list, int low, int high, AStarPoint item)
    {
        int index = 0;
        if (list.Count > 0)
        {
            while (low < high)
            {
                index = (low + high) / 2;

                if (item.mF == list[index].mF)
                {
                    return index;
                }
                else
                {
                    if (item.mF > list[index].mF)
                    {
                        high = index - 1;
                    }
                    else
                    {
                        low = index + 1;
                    }
                }
            }

            index = low;

            if (item.mF > list[index].mF)
                index = (index == 0) ? 0 : --index;
            else
                index++;
        }

        return index;
    }

    public void PrintList(List<AStarPoint> list, AStarPoint point)
    {
        string str = "";
        foreach (AStarPoint s in list)
        {
            str += $"{s.mPosition}_{s.mF},";
        }
        str += $"-----{point.mPosition}_{point.mF}";
        Debug.Log(str);
    }

    public void OrderlyInsert(List<AStarPoint> list, AStarPoint item)
    {
        int index = 0;
        if (list.Count > 0)
        {
            index = GetInsertIndex(list, 0, list.Count - 1, item);
        }

        if (index > list.Count - 1)
        {
            list.Add(item);
        }
        else
        {
            list.Insert(index, item);
        }
    }

    #endregion

    //寻找期望值最小的格子
    private AStarPoint FindPointWithMinF(List<AStarPoint> openPointList)
    {
        AStarPoint temp = null;
        if (openPointList != null && openPointList.Count > 0)
        {
            //因为开启列表是f值降序排序，直接取最后一个就是最小值
            temp = openPointList[openPointList.Count - 1];
        }
        return temp;
    }

    private Vector2[] vectors = { Vector2.right, Vector2.down, Vector2.left, Vector2.up };
    //寻找周围的全部点
    private List<AStarPoint> FindSurroundPoint(AStarPoint point)
    {
        List<AStarPoint> list = new List<AStarPoint>();
        Vector2 selectPos;

        for (int i = 0; i < vectors.Length; i++)
        {
            selectPos = point.mPosition + vectors[i];
            if (!CheckObstacle(selectPos))
            {
                AStarPoint asp = new AStarPoint(selectPos);
                list.Add(asp);
            }
        }

        return list;
    }

    //计算最小期望值点G值
    private float CalcG(AStarPoint surround, AStarPoint minFPoint)
    {
        return CaluLength(surround, minFPoint) + minFPoint.mG;
    }

    //计算选中点的F值
    private void CalcF(AStarPoint now, AStarPoint end)
    {
        //F = G + H
        float h = CaluLength(now, end);
        float g = 0;
        if (now.mParentPoint == null)
        {
            g = 0;
        }
        else
        {
            g = CaluLength(now, now.mParentPoint) + now.mParentPoint.mG;
        }
        float f = g + h;
        now.mF = f;
        now.mG = g;
        now.mH = h;
    }

    private float CaluLength(AStarPoint one, AStarPoint two)
    {
        return Mathf.Abs(one.mPosition.x - two.mPosition.x) + Mathf.Abs(one.mPosition.y - two.mPosition.y);
    }

    public bool InDoor(Vector2 pos)
    {
        return Market.Instance.InDoor(pos);
    }

    public bool CheckObstacle(Vector2 pos)
    {
        //根据具体情况做判断
		if (true)
			return true;
		else 
			return false;
    }
}


/// <summary>
/// 存储寻路点信息
/// </summary>
public class AStarPoint
{
    public AStarPoint mParentPoint;
    public Vector2 mPosition;

    //从起点经过这个点到终点的消耗预估值
    public float mF;

    //起点到这个点的预估值
    public float mG;
    //这个点到终点的预估值
    public float mH;

    public bool mIsObstacle;

    public AStarPoint(Vector2 pos)
    {
        this.mPosition = pos;
        this.mParentPoint = null;

        mIsObstacle = PathMgr.Instance.CheckObstacle(mPosition);
    }
}