原理视频
油管：https://youtu.be/i0x5fj4PqP4
别人的B站翻译：https://www.bilibili.com/video/BV1v44y1h7Dt?spm_id_from=333.999.0.0
基本概念
3个数值
G:起点到当前点步数：会随着路径的改变，例如找到一条更好的路了，A的点G = 原本A的G 和 cur路径到A的G（cur的G + cur到A的G）中的最小值
H：当前点到终点步数，乐观估计，无视障碍物。即实际的H（包含绕开障碍物的整条代价） > 按照无障碍物走的最短H
F：G+H
就是每一步都找最小F的走
每步移动代价
方便计算 * 10
Open表与Close表
var listOpen = new List() { startNode }; //开放集合,可以进行计算(但没有被选中)的节点 ,将初始节点加入到OpenSet当中。所有可能，每走一步都要放入
var listClose = new List();//封闭集合,所有已经计算过而且也被算中的节点 。表示改点已经走过，且是代价最小的点
伪代码
Init
得到StartNode与TargetNode
初始化OpenSet与CloseSet
将初始节点加入到OpenSet当中
Loop (OpenSet.Count > 0)
//找出OpenSet当中fCost最小的点,这个点就是被选中的将要行走的下一个点
currentNode = OpenSet当中fCost最小的点
//将这个点从OpenSet当中移除
OpenSet.Remove(currentNode);
//将这个点加入到ClostSet当中
ClosrSet.Add(currentNode);
if(currentNode == targetNode)
路径已经找到
//若路径没有找到,则查找当前选中的节点的邻居节点,
//计算他们的fCost并加入到OpenSet当中方便下一轮计算
foreach neighbour of the currentNode
//若这个节点是不可以行走,或者已经被算中过了,则跳过这个节点
if (!neighbour.walkable || closeSet.Contains(neighbour))
continue;
//计算这个这个邻居节点的gCost,若gCost更小则说明
//通过currentNode到这个节点的距离更加的短(代价更小)
newGCostToNeighbour = currentNode.gCost +
GetDistance(currentNode,neighbour);
//gCost若更小的话则需要重新计算这个点的fCost和他的父亲节点
//若这个节点不在OpenSet的话,则计算这个节点的fCost与设置父亲节点
//通过设置父亲节点,那么在之后找到路径之后可以通过父亲节点找到整条路径
if(newGCostToNeighbour < neighbour.gCost ||
!openSet.Contans(neighbour)){
neighour.fCost = evaluateFCost(neighour);
neighour.parentNode = currentNode;
if(!OpenSet.Contains(neighour)){
OpenSet.Add(neighbour);
}
}
核心代码
节点
public abstract class NodeBase : MonoBehaviour {
public List<NodeBase> Neighbors { get; protected set; }//周围的邻居
public NodeBase Connection { get; private set; }//上一个节点
public float G { get; private set; } //起点到当前：每次找邻居会更新, 取当前到这个邻居，和原本设定中的最小
public float H { get; private set; }//当前到达终点：乐观估计，会绕过障碍物，可能比真实到达的要小
public float F => G + H; //两者之和寻路
public static List<NodeBase> FindPath(NodeBase startNode, NodeBase targetNode) {
var listOpen = new List<NodeBase>() { startNode }; //开放集合,可以进行计算(但没有被选中)的节点 ,将初始节点加入到OpenSet当中。所有可能，每走一步都要放入
var listClose = new List<NodeBase>();//封闭集合,所有已经计算过而且也被算中的节点 。表示改点已经走过，且是代价最小的点
while (listOpen.Any()) {
//找到最小F todo：优化，每次节点多
var current = listOpen[0];
//找出OpenSet当中fCost最小的点,这个点就是被选中的将要行走的下一个点，如果F总和相同，找H最小，即到达终点最快（H是乐观估计，不算障碍物估计）
foreach (var t in listOpen)
if (t.F < current.F || ( t.F == current.F && t.H < current.H)) current = t;
//将这个点从OpenSet当中移除
listOpen.Remove(current);
//将这个点加入到ClostSet当中
listClose.Add(current);
current.SetColor(ClosedColor);
//路径已经到了
if (current == targetNode)
{
var currentPathTile = targetNode;
var path = new List<NodeBase>();
//var count = 100;
while (currentPathTile != startNode) {
path.Add(currentPathTile);
currentPathTile = currentPathTile.Connection; //cur的上一个连接点
//count--;
//if (count < 0) throw new Exception();
//Debug.Log("sdfsdf");
}
foreach (var tile in path) tile.SetColor(PathColor);
startNode.SetColor(PathColor);
Debug.Log(path.Count);
return path;
}
//找curNode的邻居，可到达和未在close表（即已经算过的不会再进行计算）
foreach (var neighbor in current.Neighbors.Where(t => t.Walkable && !listClose.Contains(t))) {
//是否未计算过这个邻居
var inSearch = listOpen.Contains(neighbor);
//从cur到邻居的G = curG+cur到邻居的G代价
var costToNeighbor = current.G + current.GetDistance(neighbor);
//路径发生改变，g会变化，例如找到一条更好的路
if (!inSearch || costToNeighbor < neighbor.G) {
neighbor.SetG(costToNeighbor);
neighbor.SetConnection(current);
//h是乐观估计，只需要算一遍
if (!inSearch) {
neighbor.SetH(neighbor.GetDistance(targetNode));
listOpen.Add(neighbor);
neighbor.SetColor(OpenColor);
}
}
}
}
return null;
}优化
Open表用二叉堆
二叉堆原理 父节点就是下标为i/2的节点。
插入节点(按照小堆为例子)
- 插入到最后一个位置，元素为a
- a跟父节点b比，如果插入的a更小，上浮，ab交换位置
删除节点 - 删除的是头节点
- 把最后一个临时a补到堆顶
- 跟左b右c比较，如果a比bc中最小的还小，a与其一换位置，为下沉操作
using System;
using System.Collections.Generic;
using System.Text;
namespace DataStructure
{
public enum HeapType
{
MinHeap,
MaxHeap
}
public class BinaryHeap<T> where T : IComparable<T>
{
public List<T> items;
public HeapType HType { get; private set; }
public T Root
{
get { return items[0]; }
}
public BinaryHeap(HeapType type)
{
items = new List<T>();
this.HType = type;
}
public bool Contains(T data)
{
return items.Contains(data);
}
public void Push(T item)
{
items.Add(item);
int i = items.Count - 1;
bool flag = HType == HeapType.MinHeap;
while (i > 0)
{
if ((items[i].CompareTo(items[(i - 1) / 2]) > 0) ^ flag)
{
T temp = items[i];
items[i] = items[(i - 1) / 2];
items[(i - 1) / 2] = temp;
i = (i - 1) / 2;
}
else
break;
}
}
private void DeleteRoot()
{
int i = items.Count - 1;
items[0] = items[i];
items.RemoveAt(i);
i = 0;
bool flag = HType == HeapType.MinHeap;
while (true)
{
int leftInd = 2 * i + 1;
int rightInd = 2 * i + 2;
int largest = i;
if (leftInd < items.Count)
{
if ((items[leftInd].CompareTo(items[largest]) > 0) ^ flag)
largest = leftInd;
}
if (rightInd < items.Count)
{
if ((items[rightInd].CompareTo(items[largest]) > 0) ^ flag)
largest = rightInd;
}
if (largest != i)
{
T temp = items[largest];
items[largest] = items[i];
items[i] = temp;
i = largest;
}
else
break;
}
}
public T PopRoot()
{
T result = items[0];
DeleteRoot();
return result;
}
}
}
源码
https://github.com/luoyikun/UnityForTest AStarDemo场景
