二维平面简单A*算法的实现（不理解也能直接用！）

算法的具体思路在网上一搜一大把，我这里就不献丑了，写了也是CV的，下面直接放算法实现，方便急用又不想了解具体算法的同学~

适用范围

1. 二维平面
2. 以int整数为坐标表示

实现算法的环境

Unity、C#

实现思路

在实现方式上对算法本身以及数据来源进行了拆分，数据来源以接口的形式去实现，可以灵活用于不同的情况，例如四连通还是八连通、不同格子移动代价不同等等。

完整算法代码

using System.Collections.Generic;
using UnityEngine;

public class Simple2DAStarPathFinding
{
	private struct PathNode
	{
		public Vector2Int Position;
		public Vector2Int ParentPosition;
		public float CostFromStart;
		public float EstimatedCostToEnd;
		public float CostSum => CostFromStart + EstimatedCostToEnd;

		public PathNode(Vector2Int position, Vector2Int parentPosition, float costFromStart, float estimatedCostToEnd)
		{
			Position = position;
			ParentPosition = parentPosition;
			CostFromStart = costFromStart;
			EstimatedCostToEnd = estimatedCostToEnd;
		}
	}

	private Dictionary<Vector2Int, PathNode> _openList = new Dictionary<Vector2Int, PathNode>();
	private Dictionary<Vector2Int, PathNode> _closeList = new Dictionary<Vector2Int, PathNode>();

	private Vector2Int _end;

	private ISimple2DAStarPathFindingDataSource _pathFindingAStarDataSource;

	public const float UnreachableCost = -1;

	public Simple2DAStarPathFinding(ISimple2DAStarPathFindingDataSource pathFindingAStarDataSource)
	{
		_pathFindingAStarDataSource = pathFindingAStarDataSource;
	}

	public bool FindPath(Vector2Int start, Vector2Int end, out List<Vector2Int> path)
	{
		_end = end;
		_openList.Clear();
		_closeList.Clear();
		path = null;

		if (!_pathFindingAStarDataSource.IsValidNode(start) || !_pathFindingAStarDataSource.IsValidNode(end))
			return false;

		var currentRowAndColumn = start;
		int costFromStart = 0;
		_openList.Add(currentRowAndColumn, new PathNode(currentRowAndColumn, currentRowAndColumn, costFromStart, EstimateCostToEnd(currentRowAndColumn)));
		while (_openList.Count > 0)
		{
			var nextNode = ChooseMinimumCostNodeFromOpenList();
			MoveToCloseList(nextNode);
			AppendNearConnectableIntoOpenList(nextNode);
			if (_openList.ContainsKey(_end))
			{
				path = GeneratePath(start);
				Debug.Log("Find path successfully.");
				return true;
			}
		}
		Debug.Log("Find path failure.");
		return false;
	}

	private List<Vector2Int> GeneratePath(Vector2Int start)
	{
		List<Vector2Int> path = new List<Vector2Int>();
		var node = _openList[_end];
		path.Insert(0, node.Position);
		while (node.Position != start)
		{
			node = _closeList[node.ParentPosition];
			path.Insert(0, node.Position);
		}

		return path;
	}

	private void AppendNearConnectableIntoOpenList(Vector2Int currentRowAndColumn)
	{
		var nearNodes = _pathFindingAStarDataSource.GetNearNodes(currentRowAndColumn);
		foreach (var nearNode in nearNodes)
		{
			if (nearNode.Value < 0)
				continue;

			var nodePosition = nearNode.Key;
			if (_closeList.ContainsKey(nodePosition))
				continue;

			if (!_pathFindingAStarDataSource.IsValidNode(nodePosition))
				continue;

			AddToOpenListOrRefreshCost(currentRowAndColumn, nodePosition, nearNode.Value);
		}
	}

	private void AddToOpenListOrRefreshCost(Vector2Int currentPosition, Vector2Int anotherPosition, float cost)
	{
		var parentNode = _closeList[currentPosition];
		if (_openList.TryGetValue(anotherPosition, out var leftNode))
		{
			if (parentNode.CostFromStart + cost < leftNode.CostFromStart)
			{
				leftNode.ParentPosition = currentPosition;
				leftNode.CostFromStart = parentNode.CostFromStart + cost;
			}
		}
		else
		{
			PathNode pathNode = new PathNode(anotherPosition, currentPosition, parentNode.CostFromStart + cost, EstimateCostToEnd(anotherPosition));
			_openList.Add(anotherPosition, pathNode);
			Debug.LogFormat("Add position {0} into openlist", anotherPosition);
		}
	}

	private void MoveToCloseList(Vector2Int nodePosition)
	{
		Debug.Assert(_openList.ContainsKey(nodePosition));

		var node = _openList[nodePosition];
		_openList.Remove(nodePosition);
		_closeList.Add(nodePosition, node);
		Debug.LogFormat("move position {0} into closelist", nodePosition);
	}

	private int EstimateCostToEnd(Vector2Int position)
	{
		return Mathf.Abs(_end.x - position.x) + Mathf.Abs(_end.y - position.y);
	}

	private Vector2Int ChooseMinimumCostNodeFromOpenList()
	{
		Vector2Int? result = null;
		foreach (var node in _openList)
		{
			if (result == null)
			{
				result = node.Key;
				continue;
			}
			if (_openList[result.Value].CostSum > node.Value.CostSum)
				result = node.Key;
		}
		return result.Value;
	}
}

在使用时实现以下接口即可，建议拥有完整地图坐标数据的类去实现，至于是四连通还是八连通，以及相邻格间的代价如何计算（例如沼泽、水域、沙地）取决于GetNearNodes如何实现。

public interface ISimple2DAStarPathFindingDataSource
{
	public bool IsValidNode(Vector2Int nodePos);

	/// <returns>PathNodes and cost from <paramref name="nodePos"/> to which. If unreachable, use <see cref="Simple2DAStarPathFinding.UnreachableCost"/>.</returns>
	public Dictionary<Vector2Int, float> GetNearNodes(Vector2Int nodePos);
}

使用示例

ISimple2DAStarPathFindingDataSource impl; // 建议拥有完整地图坐标数据的类去实现
Simple2DAStarPathFinding pathFinding = new Simple2DAStarPathFinding(impl);
Vector2Int startPos = xxx; // 以任意方式获取的开始点
Vector2Int endPos = xxx; // 以任意方式获取的结束点
if (pathFinding.FindPath(startPos, endPos, out var path)) // 得出完整路径则为true，如果找不到路径则为false
{
    // TODO 拿着path去做你想做的事情吧
}

拓展思考

1. 如果是连续坐标而非离散坐标（整数坐标），该如何实现。
2. 可否实现为二维三维通用的算法。

一点小唠叨

提取接口的初衷，其实就是项目里面我接手过的不同模块都用到了寻路算法。然后一想，我要是复制代码大佬肯定忍不了呀，而且CV大法虽然好，但也显得有些蠢，所以还是通过提取接口的方式，让我能脱胎于算法本身，专注于功能实现，嘿嘿嘿~