基本原理网上很多,这里不做多余赘述。
本文代码展示一个二维网格地图的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);
}
}