Unity中实现A*寻路算法参考

A*路径获取代码：

using System.Collections.Generic;
using UnityEngine;

public class AStarNode
{
	public int id { get; set; }
	public Vector3 position;
	public List<int> listNeighbor { get; set; }

	public AStarNode(int id, Vector3 position)
	{
		this.id = id;
		this.position = position;
		listNeighbor = new List<int>();
	}
}

public class AStarFindPath
{
	private static Dictionary<int, float> gScore;
	private static Dictionary<int, float> fScore;

	public static List<AStarNode> GetPath(Dictionary<int, AStarNode> nodes, int startId, int goalId)
	{
		var openSet = new SortedSet<(int, float)>(Comparer<(int, float)>.Create((a, b) =>
		{
			int compareResult = a.Item2.CompareTo(b.Item2);
			return compareResult == 0 ? a.Item1.CompareTo(b.Item1) : compareResult;
		}));

		var cameFrom = new Dictionary<int, int>();

		gScore = new Dictionary<int, float>();
		fScore = new Dictionary<int, float>();

		foreach (var node in nodes.Values)
		{
			gScore[node.id] = float.MaxValue;
			fScore[node.id] = float.MaxValue;
		}

		gScore[startId] = 0;
		fScore[startId] = (nodes[startId].position -nodes[goalId].position).magnitude;

		openSet.Add((startId, fScore[startId]));

		while (openSet.Count > 0)
		{
			var currentItem = openSet.Min;
			int current = currentItem.Item1;
			openSet.Remove(currentItem);

			if (current == goalId)
			{
				return ReconstructPath(cameFrom, nodes, current);
			}

			foreach (int neighborId in nodes[current].listNeighbor)
			{
				float tentativeGScore = gScore[current] + (nodes[current].position - nodes[neighborId].position).magnitude;

				if (tentativeGScore < gScore[neighborId])
				{
					cameFrom[neighborId] = current;
					gScore[neighborId] = tentativeGScore;
					fScore[neighborId] = gScore[neighborId] + (nodes[neighborId].position - nodes[goalId].position).magnitude;

					var neighborItem = (neighborId, fScore[neighborId]);
					if (!openSet.Contains(neighborItem))
					{
						openSet.Add(neighborItem);
					}
					else
					{
						openSet.Remove(neighborItem);
						openSet.Add(neighborItem);
					}
				}
			}
		}

		return null; // 找不到路径
	}

	private static List<AStarNode> ReconstructPath(Dictionary<int, int> cameFrom, Dictionary<int, AStarNode> nodes, int current)
	{
		var totalPath = new List<AStarNode> { nodes[current] };

		while (cameFrom.ContainsKey(current))
		{
			current = cameFrom[current];
			totalPath.Insert(0, nodes[current]);
		}

		return totalPath;
	}
}

在Unity中的测试代码：

using System.Collections.Generic;
using UnityEngine;

public class TestAStar : MonoBehaviour
{
	[SerializeField]
	Material material;

	List<GameObject> listNodeObj = new();

	void Start()
	{
		CreateAStarPath();
	}

	int nodeCount = 200;
	void CreateAStarPath()
	{
		Clear();

		for (int i = 0; i < nodeCount; i++)
		{
			GameObject obj = GameObject.CreatePrimitive(PrimitiveType.Sphere);
			obj.transform.SetParent(transform);
			obj.transform.localPosition = new Vector3(Random.Range(-100, 100), Random.Range(-10, 10), Random.Range(-100, 100));
			obj.name = i.ToString("D3");
			listNodeObj.Add(obj);
		}

		Dictionary<int, AStarNode> dicAStarNode = new();

		for (int i = 0; i < nodeCount; i++)
		{
			List<int> listNeighbor = new();
			Vector3 pos = listNodeObj[i].transform.position;
			for (int j = 0; j < nodeCount; j++)
			{
				if (i == j) continue;

				Vector3 posB = listNodeObj[j].transform.position;
				if ((pos - posB).sqrMagnitude < 900)
				{
					listNeighbor.Add(j);
				}
			}

			if (listNeighbor.Count > 0)
			{
				AStarNode aStarNode = new(i, pos)
				{
					listNeighbor = listNeighbor
				};
				dicAStarNode.Add(i, aStarNode);
			}
		}

		List<AStarNode> path = AStarFindPath.GetPath(dicAStarNode, 0, nodeCount-1);

		if (path != null && path.Count > 1)
		{
			GameObject pathObj = new("Path");
			pathObj.transform.SetParent(transform);
			LineRenderer lineRenderer = pathObj.AddComponent<LineRenderer>();
			Vector3[] points = new Vector3[path.Count];
			for (int i = 0; i < path.Count; i++)
			{
				points[i] = path[i].position;
			}
			lineRenderer.positionCount = points.Length;
			lineRenderer.SetPositions(points);
			lineRenderer.material = material;

			listNodeObj.Add(pathObj);
		}
	}

	void Clear()
	{
		for (int i = 0; i < listNodeObj.Count; i++)
		{
			Destroy(listNodeObj[i]);
		}
		listNodeObj.Clear();
	}
}