下面是算法主要源码:
- 在这个项目我事实上做了一些变通,Km和H的计算方式没有完全依据算法,另外去掉了一些感觉用不上的过程
还有队列的储存方式为了贪方便只是用了个单向链表,查找时还是很慢的,如果改变储存方式其实能有进一步提升(并不,半年后我实现了堆的优先队列版本,至少在110*100的地图下,链表略占优)
链表和堆实现优先队列的对比
算法需要用到的操作有几个:
- 不根据Key直接访问结点(Update函数)
- 不根据Key直接删除结点(Remove函数)
- 入队(插入并保持有序性)
- 出队(取出并删除最小值)
每次操作的时间复杂度如下:
其中n为存储的元素个数
储存方式 | 直接访问 | 直接删除 | 入队 | 出队 |
有序链表 | O(n) | O(n) | O(n) | O(1) |
小顶堆 | O(n) | O(n) | O(logn) | O(logn) |
(可删除)小顶堆 | O(n) | O(logn) | O(logn) | O(logn) |
- 有序链表前三者都是从头开始遍历,而出队直接取出首结点,然后头结点指向下个结点即可
- 小顶堆访问最小元素的复杂度是O(1),但是因为删除最小元素和插入任意元素要保证堆序,需要做O(logn)的调整,因此入队出队都是O(logn);而堆中其余元素的整体有序性并不好,不根据键值访问只能是遍历,因此是O(n)
- 鉴于小顶堆删除任意结点的时间复杂度高,又引入了可删除堆,原理是利用一个辅助堆记录删除的元素,当两堆顶元素相等时,同时Pop出舍弃,详见:
可以做到单纯删除(Pop)的操作时间复杂度为O(1),但是记录时和Pop出后,需要维护堆内其它元素顺序,要做O(logn)的维护操作,因此直接删除总体时间复杂度为:
入辅助堆+比较并在删除堆与队列堆同时Pop出删除结点+删除后两堆维护元素次序,即:
O(logn1) + O(1) + (O(logn1)+O(logn2)) = O(logn)
但是这个做法的弊端是会暂存删除的结点,使用的内存空间相对多亿点。 - 更新结点(Update函数)可以视为删除结点再马上入队新结点,因此可以用两个O(logn)的操作避免直接访问的O(n)操作
因此最终两个队列的有效操作时间复杂度如下:
其中n为存储的元素个数
储存方式 | 直接删除 | 入队 | 出队 |
有序链表 | O(n) | O(n) | O(1) |
(可删除)小顶堆 | O(logn) | O(logn) | O(logn) |
乍眼一看怎么都是下面的比较快,然而事实上在110*100地图的测试环境下,我写的两个版本中,小顶堆并无太大优势,大多数情况下链表更胜一筹。
根据测试,小顶堆中直接删除仍占大量时间
不论实现上的问题,从理论上可能是两个方面引起的时间频度的问题:
- 堆操作的时间常数比链表要大。 即同样执行一步循环时,链表更快。因为链表只需无脑访问Next,而堆往往要进行额外操作。特别对于删除函数:链表仍只需检测+无脑Next,找到了就直接删除,而可删除堆的删除共有3步组成,撇开O(1)操作不管,仍有3个O(logn)的操作;而且堆的每步操作都普遍比链表耗时长。时间复杂度只是描述耗时增长的速度,而每步循环都有自己本身的耗时,实际耗时应是:
单步循环时间×期望循环次数n,其中时间复杂度只衡量了期望循环次数,而忽略了前面的时间常量
举个例子,假设某O(n)操作单次循环需要运行1句(时间常数为1),而某O(logn)操作的单次循环需要运行x(>1)句(时间常数为x),我们虽然能保证n→∞时,n>xlogn,x∈N+,但我们能保证n很小时n>xlogn恒成立吗?
大规模问题的主导影响是时间复杂度,而对于小规模问题时间常数的影响不可忽视,显然在这里110*100还是受到了时间常数的影响了。 - 衡量链表、堆中的问题规模n并不一致。我们希望以实际入队元素个数来考察耗时问题,然而现实是:链表中元素个数n=算法意义上的入队结点数,而可删除堆中元素个数n>入队结点数。
听起来好像不可能,但可删除堆中确实是这样的:可删除堆并不是即时删除,而是继续存在堆中,并入队待删除堆,当两堆顶元素相等时,才实际移除并舍弃堆中元素。这里有个问题,就是会使堆中暂存许多待删除的无效结点。这导致了入队和出队时,仍不可避免地需要遍历这些无效结点,事实上是抬高了入队、出队操作的耗时。
入队出队O(logn)中的n不再代表入队结点数,而是入队结点+待删除结点数,但是我们希望的是以入队元素个数角度来衡量同一问题。因此为了让问题有可比性,统一用m代表入队结点(寻路算法意义上的入队,不包括队中待删除结点),然后在相同问题规模m下计算出两者的复杂度,至于如何确定n与m的关系,由于无效结点的加入依赖于算法,而算法比较复杂,这里只通过统计得出:
比如在如下情景:Remove+Enqueue函数代替了该算法的Update函数后,Remove需要频繁执行,导致大量待删除结点囤积在队列中。根据测试在地形不同时会分别导致堆中有大概3/10~7/10无效结点囤积。
取最坏情况:堆中7/10为无效结点,即(n-m)/n = 7/10,解得m:n=3:10,也就是有效入队结点m只占所有结点n的30%。
由:堆中元素个数为n,则入队出队时间复杂度为O(logn)
代入得:对m:n=3:10时,用m表示时间复杂度为O(log10m/3)(由于问题规模小,常数不忽略)
同理,由于链表中不存在无效结点,n=m,得出m规模下时间复杂度依然为O(m)
由此可见,对于同一问题规模m,堆优先队列的时间常数又双叒叕被抬升了,这意味着当问题规模小时,链表更可能优于堆实现的优先队列。
总而言之这大概是执行语句时间常数影响时间频度的问题。因为问题规模不够大,而堆队列的基本语句又比链表复杂,导致执行一次循环体的时间仍造成较大影响。
但是毕竟时间复杂度摆在这,在地图更大的时候,优先队列应该会比链表有更好的表现吧
Key类和U队列
using System.Collections;
using System.Collections.Generic;
using System.Diagnostics;
/// <summary>
/// 用于算法中的Key
/// </summary>
public class Key
{
static int idgiver;
public double Key1;
public double Key2;
public int id;
public Key() { }
public Key(double k1, double k2)
{
Key1 = k1;
Key2 = k2;
id = idgiver++;
}
/// <summary>
/// 绝对的偏序关系,内部维护确保不可能有两个Key相等
/// 当Key1,Key2均相等时,会由分配的id决定大小(这样设计是因为优先队列必须比出顺序)
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <returns></returns>
public static bool operator <(Key a, Key b)
{
if (a.Key1 < b.Key1)
return true;
else if (a.Key1 > b.Key1)
return false;
else if (a.Key2 < b.Key2)
return true;
else if (a.Key2 > b.Key2)
return false;
else if (a.id < b.id)
return true;
else
return false;
}
/// <summary>
/// 绝对的偏序关系,内部维护确保不可能有两个Key相等
/// 当Key1,Key2均相等时,会由分配的id决定大小(这样设计是因为优先队列必须比出顺序)
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <returns></returns>
public static bool operator >(Key a, Key b)
{
if (a.Key1 > b.Key1)
return true;
else if (a.Key1 < b.Key1)
return false;
else if (a.Key2 > b.Key2)
return true;
else if (a.Key2 < b.Key2)
return false;
else if (a.id > b.id)
return true;
else
return false;
}
/// <summary>
/// 用于比较Key值的小于关系(非小于等于)
/// 当key左 == key右的时候不相等,<=只是记号
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <returns></returns>
public static bool operator <=(Key a, Key b)
{
if (a.Key1 < b.Key1)
return true;
else if (a.Key1 > b.Key1)
return false;
else if (a.Key2 < b.Key2)
return true;
else
return false;
}
/// <summary>
/// 用于比较Key值的大于关系(非大于等于)
/// 当key左 == key右的时候不相等,>=只是记号
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <returns></returns>
public static bool operator >=(Key a, Key b)
{
if (a.Key1 > b.Key1)
return true;
else if (a.Key1 < b.Key1)
return false;
else if (a.Key2 > b.Key2)
return true;
else
return false;
}
public static bool operator ==(Key a, Key b)
{
if ((a as object) != null)
return a.Equals(b);
else
return (b as object) == null;
}
public static bool operator !=(Key a, Key b)
{
if ((a as object) != null)
return !a.Equals(b);
else
return (b as object) != null;
}
public override bool Equals(object obj)
{
if (obj == null)
{
return false;
}
if ((obj.GetType().Equals(this.GetType())) == false)
{
return false;
}
Key b = (Key)obj;
return Key1 == b.Key1 && Key2 == b.Key2 && id == b.id;
}
public override int GetHashCode()
{
return Key1.GetHashCode() ^ Key2.GetHashCode() ^ id.GetHashCode();
}
}
/// <summary>
/// 队列元素
/// </summary>
public class QueueElement
{
public Key key;
public Node node;
public QueueElement(Key k, Node n)
{
key = k;
node = n;
}
public static bool operator <(QueueElement x, QueueElement y)
{
if (x.key < y.key)
return true;
else
return false;
}
public static bool operator >(QueueElement x, QueueElement y)
{
if (x.key > y.key)
return true;
else
return false;
}
}
public class PriorityQueue
{ ///========================
///为了提高赋值效率,把部分的struct变量赋值改为ref赋值
///========================
public List<QueueElement> heap;//为减少几次运算,a[0]舍去,数据从a[1]开始记录
DeleteQueue deleteQueue;
Stack<int> TraversalIndex = new Stack<int>();
int id;
int count;
public int Count { get => count; }
public PriorityQueue(int queueId, Key key, Node node)
{
heap = new List<QueueElement>
{
null,
};
deleteQueue = new DeleteQueue();
id = queueId;
count = 0;
Enqueue(key, node);
}
public PriorityQueue(PriorityQueue priorityQueue)
{
heap = new List<QueueElement>(priorityQueue.heap);
count = priorityQueue.count;
id = priorityQueue.id;
}
/// <summary>
/// 在队列中加入指定键值对
/// 为提升效率,应在外部额外记录结点的插入情况
/// </summary>
/// <param name="key"></param>
/// <param name="node"></param>
public void Enqueue(Key key, Node node)
{
node.insertKey[id] = key;//记录插入时的key对象
count++;//先增加元素个数
if (count + 1 > heap.Count)//如果满了
heap.Add(new QueueElement(key, node));
else//否则直接加
heap[count] = new QueueElement(key, node);
//然后调整新结点在堆中的位置
AdjustInsert(count);
}
/// <summary>
/// 取出队列中最小的键值对,当队列为空时返回默认值(<0,0>,null)
/// 为提升效率,应在外部额外记录结点的插入情况
/// </summary>
/// <returns>(Key, Node)值元组</returns>
public (Key, Node) Dequeue()
{
//新增。若与删除堆的对顶相同,同时Pop出(舍弃)
while (count > 0 && deleteQueue.Count > 0 && deleteQueue.Top.key == heap[1].key && deleteQueue.Top.node == heap[1].node)
{
heap[1] = heap[count];
count--;
AdjustHeap(1);
deleteQueue.Dequeue();
}
//原有
if (count > 0)
{
QueueElement min = heap[1];
heap[1] = heap[count];
count--;
min.node.insertKey[id] = null;//记录移除
AdjustHeap(1);
return (min.key, min.node);
}
else
return (null, null);
}
/// <summary>
/// 移除指定结点,如不在队中则不做修改,但会耗费大量时间!
/// 所以最好请在移除前判断是否在队中!
/// 所以最好请在移除前判断是否在队中!
/// 另外,该函数是O(n)操作,已弃用,改用O1Remve
/// </summary>
/// <param name="node"></param>
public void Remove(Node node)
{
//========================= 先序遍历,提前折回版(如果Key不符合,则子树也不符合)========================
//if (node.inserted[id] == null)
// return;
//Node checkingNode;
//TraversalIndex.Clear(); //创建并初始化堆栈S
//int i = 1;
//while (true)
//{
// while (i <= count) //一直向左并将沿途节点访问(打印)后压入堆栈
// {
// checkingNode = heap[i];//取出访问
// if (checkingNode == node)//找到结点,则删除并返回
// {
// //不是最后一个元素则需要调整
// if (i != count)
// {
// if (heap[i].inserted[id] < heap[count].inserted[id])//最后一个元素>删除元素。代替后可能破坏子结点次序,视为根结点向下调整
// {
// heap[i] = heap[count];
// AdjustHeap(i);
// }
// else//否则最后一个元素≤删除元素。代替后可能破坏父结点次序,视为叶子结点向上调整
// {
// heap[i] = heap[count];
// AdjustInsert(i);
// }
// }
// count--;//减少长度
// //清除key
// checkingNode.inserted[id] = null;
// return;
// }
// else if (checkingNode.inserted[id] > node.inserted[id]) //当前结点>请求结点,则不可能在以该结点为根的子树出现
// {
// break;
// }
// else //当前结点<请求结点,继续监测子树
// {
// TraversalIndex.Push(i);
// i <<= 1;//左孩子
// }
// }
// if (TraversalIndex.Count > 0)
// {
// i = (TraversalIndex.Pop() << 1) + 1;//右孩子
// }
//}
//=================== 完全层次遍历版 ================
遍历表
//for (int i = 1; i <= count; i++)
//{
// //如果找到元素,则删除将用最后一个元素替代删除元素,并调整堆
// if (heap[i] == node)
// {
// stop.Start();
// //不是最后一个元素则需要调整
// if (i != count)
// {
// if (heap[i].insertKeys[id] < heap[count].insertKeys[id])//最后一个元素>删除元素。代替后可能破坏子结点次序,视为根结点向下调整
// {
// heap[i] = heap[count];
// AdjustHeap(i);
// }
// else//否则最后一个元素≤删除元素。代替后可能破坏父结点次序,视为叶子结点向上调整
// {
// heap[i] = heap[count];
// AdjustInsert(i);
// }
// }
// count--;//减少长度
// stop.Stop();
// }
//}
}
/// <summary>
/// 根据Node现在所记录的Key对象记录这次删除
/// 以标记代替删除,时间复杂度O(1),但大幅增加了空间使用
/// </summary>
/// <param name="node"></param>
public void O1Remove(Node node)
{
//若未插入(key空)或已标记移除,返回
if (node.insertKey[id] == null)
return;
deleteQueue.Enqueue(node.insertKey[id], node);
node.insertKey[id] = null;//记录移除
}
/// <summary>
/// 更新一个结点
/// O(n)操作,已弃用,改为O1Remove+Enqueue
/// </summary>
/// <param name="key"></param>
/// <param name="node"></param>
public void Update(Key key, Node node)
{
if (node.insertKey[id] == key)
return;
TraversalIndex.Clear(); //创建并初始化堆栈S
int i = 1;
while (true)
{
while (i <= count)//一直向左并将沿途节点访问后压入堆栈
{
if (heap[i].key == node.insertKey[id] && heap[i].node == node)//找到结点(并且不是待删除的),则更新key并返回
{
heap[i].node.insertKey[id] = key;
if (heap[i].key < key)//旧Key<新Key。代替后可能破坏子结点次序,视为根结点向下调整
{
heap[i].key = key;
AdjustHeap(i);
}
else//否则旧Key≥新Key。代替后可能破坏父结点次序,视为叶子结点向上调整
{
heap[i].key = key;
AdjustInsert(i);
}
return;
}
else if (heap[i].key > node.insertKey[id]) //当前结点>请求结点,则不可能在以该结点为根的子树出现
{
break;
}
else //当前结点<请求结点,继续监测子树
{
TraversalIndex.Push(i);
i <<= 1;//左孩子
}
}
if (TraversalIndex.Count > 0)
{
i = (TraversalIndex.Pop() << 1) + 1;//右孩子
}
}
}
/// <summary>
/// 自上而下调整一个结点在堆中的顺序
/// 适用于(相对的)根结点
/// </summary>
/// <param name="index"></param>
public void AdjustHeap(int index)
{
QueueElement temp = heap[index];
for (int child = index << 1; child <= count; index = child, child <<= 1)//index结点的左子结点开始
{
if (child + 1 <= count && heap[child] > heap[child + 1])//如果左子结点大于右子结点,child指向右子结点
child++;
if (heap[child] < temp)//如果子节点小于temp节点,子结点上升
{
heap[index] = heap[child];
}
else
{
break;
}
}
heap[index] = temp;//将temp值放到最终的位置
}
/// <summary>
/// 自下而上调整一个结点在堆中的顺序
/// 适用于(相对的)叶子结点
/// </summary>
/// <param name="index"></param>
public void AdjustInsert(int index)
{
QueueElement temp = heap[index];
for (int parent = index >> 1; parent >= 1; index = parent, parent >>= 1)
{
if (heap[parent] > temp)//父元素比该结点大,父结点下沉
{
heap[index] = heap[parent];
}
else//否则父结点≤该结点,在index位置插入该结点
{
break;
}
}
heap[index] = temp;//将temp值放到最终的位置
}
}
/// <summary>
/// 存储待删除数据,以实现以O(1)实现优先队列的任意结点删除(删除后的维护其实是O(logn))
/// 只辅助优先队列删除,不对Node造成任何实际更改
/// </summary>
public class DeleteQueue
{
List<QueueElement> heap;//为减少几次运算,a[0]舍去,数据从a[1]开始记录
int count;
public int Count { get => count; }
public QueueElement Top { get => count > 0 ? heap[1] : null; }
public DeleteQueue()
{
heap = new List<QueueElement>
{
null,
};
count = 0;
}
public void Enqueue(Key key, Node node)
{
count++;//先增加元素个数
if (count + 1 > heap.Count)//如果满了
heap.Add(new QueueElement(key, node));
else//否则直接加
heap[count] = new QueueElement(key, node);
//然后调整新结点在堆中的位置
AdjustInsert(count);
}
/// <summary>
/// 取出队列中最小的键值对,当队列为空时返回默认值(<0,0>,null)
/// 为提升效率,应在外部额外记录结点的插入情况
/// </summary>
/// <returns>(Key, Node)值元组</returns>
public void Dequeue()
{
if (count > 0)
{
heap[1] = heap[count];
count--;
AdjustHeap(1);
}
}
/// <summary>
/// 自上而下调整一个结点在堆中的顺序
/// 适用于(相对的)根结点
/// </summary>
/// <param name="index"></param>
public void AdjustHeap(int index)
{
QueueElement temp = heap[index];
for (int child = index << 1; child <= count; index = child, child <<= 1)//index结点的左子结点开始
{
if (child + 1 <= count && heap[child] > heap[child + 1])//如果左子结点大于右子结点,child指向右子结点
child++;
if (heap[child] < temp)//如果子节点小于temp节点,子结点上升
{
heap[index] = heap[child];
}
else
{
break;
}
}
heap[index] = temp;//将temp值放到最终的位置
}
/// <summary>
/// 自下而上调整一个结点在堆中的顺序
/// 适用于(相对的)叶子结点
/// </summary>
/// <param name="index"></param>
public void AdjustInsert(int index)
{
QueueElement temp = heap[index];
for (int parent = index >> 1; parent >= 1; index = parent, parent >>= 1)
{
if (heap[parent] > temp)//父元素比该结点大,父结点下沉
{
heap[index] = heap[parent];
}
else//否则父结点≤该结点,在index位置插入该结点
{
break;
}
}
heap[index] = temp;//将temp值放到最终的位置
}
}
Node(结点)类
using System.Collections;
using System.Collections.Generic;
using System.Diagnostics;
using UnityEngine;
public class Node
{
//====================变量====================
//记录地图块信息
public int x;
public int y;
//寻路地图到实际地图的映射关系,非Unity项目可直接删除
public Vector3 Position { get { return new Vector3(x * 2 + 1, y * 2 + 1, 0); } }
//记录算法主要数值
public bool enabled = true;
public HashSet<Node> neighbour = new HashSet<Node>();//相当于predecessors和successors的合并,由于不需要重复,所以使用不可重的hashset代替list
//由于是多个D*进程,所以会有多组算法变量,用List存储
//static List
public static List<double> km = new List<double>();
//List 一个结点储存多组寻路信息(以供多个单位)
public List<Key> insertKey = new List<Key>();
public List<bool> inRemoveQueue = new List<bool>();
public List<double> g = new List<double>();
public List<double> rhs = new List<double>();
public List<double> h = new List<double>();
//===================更新结点信息===================
public void AddNodeInfo()
{
insertKey.Add(null);
//inRemoveQueue.Add(false);
g.Add(double.PositiveInfinity);
rhs.Add(double.PositiveInfinity);
h.Add(0);
}
public void RenewNodeInfo(int i)//留下
{
insertKey[i] = null;
//inRemoveQueue.Add(false);
g[i] = double.PositiveInfinity;
rhs[i] = double.PositiveInfinity;
h[i] = 0;
}
//====================构造函数====================
public Node() {}
public Node(int inputX, int inputY)
{
x = inputX;
y = inputY;
}
//====================算法函数====================
public Key CalculateKey(int i)
{
return new Key((g[i] < rhs[i] ? g[i] : rhs[i]) + h[i] + km[i], g[i] < rhs[i] ? g[i] : rhs[i]);
}
public void UpdateVertex(int i, PriorityQueue U)
{
//代替下面
if (insertKey[i] != null)
{
U.O1Remove(this);
}
if (g[i] != rhs[i])
{
U.Enqueue(CalculateKey(i), this);
}
//较慢,已弃用
//if (g[i] != rhs[i])
//{
// if (insertKey[i] == null)
// U.Enqueue(CalculateKey(i), this);//不在队中,入队
// else
// U.Update(CalculateKey(i), this);//在队中,更新Key值
//}
//else if (insertKey[i] != null)
// U.O1Remove(this);//在队中,移除
}
//====================重载====================
public static bool operator !=(Node a, Node b)
{
if ((a as object) != null)
return !a.Equals(b);
else
return (b as object) != null;
}
public static bool operator ==(Node a, Node b)
{
if ((a as object) != null)
return a.Equals(b);
else
return (b as object) == null;
}
public override bool Equals(object obj)
{
if (obj == null)
{
return false;
}
Node nodeObj = obj as Node;
if ((object)nodeObj == null)
{
return false;
}
else if (x == nodeObj.x && y == nodeObj.y)
{
return true;
}
else
return false;
}
public override int GetHashCode()
{
return x.GetHashCode() ^ y.GetHashCode();
}
}
DStarLite类
using System.Collections;
using System.Collections.Generic;
using System.Diagnostics;
using UnityEngine;
public class DStarLite
{
int i;
///=================给自己看的提示=================
///该算法后来修复了穿墙角,共有两个部分共同组成
///第一部分在Cost函数,第二部分在GetFinalPath函数
///CalculateH必须要在结点更新前调用
///障碍物出现与消失需要外部手动调用该函数
///====================End提示====================
//====================委托====================
//将地图要素改变时,通过委托通知所有寻路实例
public delegate void MapChanges();
private static MapChanges reCalculateAllH;
private static MapChanges reCalculateAllPath;
public static MapChanges ReCalculateAllPath { get => reCalculateAllPath; private set => reCalculateAllPath = value; }
public static MapChanges ReCalculateAllH { get => reCalculateAllH; private set => reCalculateAllH = value; }
public delegate void NodeChanges(Node node);
private static NodeChanges onNodeDisabled;
private static NodeChanges onNodeEnabled;
public static NodeChanges OnNodeDisabled { get => onNodeDisabled; private set => onNodeDisabled = value; }
public static NodeChanges OnNodeEnabled { get => onNodeEnabled; private set => onNodeEnabled = value; }
//====================变量====================
//管理实例编号
private static int IdGiver = 0;
private static Queue<int> IdPool = new Queue<int>();
int id;
//储存全局的结点
public static Node[,] Nodes;
public static Node GlobalStart { get; private set; }
public static Node GlobalGoal { get; private set; }
//储存个体的结点
public Node lastStart;
public Node sStart;
public Node sGoal;
//public KeyQueue U;
public PriorityQueue U;
//寻路状态:初次?再次?,默认为初次
private bool FirstCalculte = true;
//最终路径
public Queue<Node> finalPath = new Queue<Node>();
/// <summary>
/// 示意寻路是否已达预定终点,为真时表示没有终点或未达终点
/// </summary>
public bool NotAchieveGoal
{
get
{
if (sStart != null && sGoal != null && sStart != sGoal)
return true;
else
return false;
}
}
//====================交互函数====================
/// <summary>
/// 构造函数会配合Node类一起,分配一个编号,并开辟储存g,h,rhs数值的空间
/// </summary>
public DStarLite()
{
if (IdPool.Count == 0)
{
//新增ID
//在Node中增加相关信息
Node.km.Add(0);
foreach (Node node in Nodes)
{
node.AddNodeInfo();
}
id = IdGiver++;
}
else
{
//重用ID
id = IdPool.Dequeue();
foreach (Node node in Nodes)
{
node.RenewNodeInfo(id);
}
}
}
/// <summary>
/// 对整个算法系统的初始化
/// 一般在新关卡开始时需要重新初始化变量
/// </summary>
public static void GlobalInitialize(Node globalStart, Node globalGoal)
{
//设置起点、终点
GlobalStart = globalStart;
GlobalGoal = globalGoal;
//重置分配id
IdPool.Clear();
IdGiver = 0;
//清空委托
ClearMapChanges(ref reCalculateAllPath);
ClearMapChanges(ref reCalculateAllH);
ClearNodeChanges(ref onNodeDisabled);
ClearNodeChanges(ref onNodeEnabled);
void ClearMapChanges(ref MapChanges mapChangeDelegate)
{
if (mapChangeDelegate == null)
return;
else
MonoBehaviour.print(mapChangeDelegate);
System.Delegate[] dlg = mapChangeDelegate.GetInvocationList();
for (int i = 0; i < dlg.Length; i++)
{
mapChangeDelegate -= dlg[i] as MapChanges;
}
MonoBehaviour.print(mapChangeDelegate);
}
void ClearNodeChanges(ref NodeChanges mapChangeDelegate)
{
if (mapChangeDelegate == null)
return;
System.Delegate[] dlg = mapChangeDelegate.GetInvocationList();
for (int i = 0; i < dlg.Length; i++)
{
mapChangeDelegate -= dlg[i] as NodeChanges;
}
}
}
/// <summary>
/// 表示已经到达finalPath的当前点,该函数将返回路径中该点并从finalPath移除,此时要更新部分数据
/// </summary>
public Node HaveGetToNextNode()
{
if (finalPath != null && finalPath.Count != 0)//获取下一个节点
{
Node to = finalPath.Dequeue();
Node.km[id] += System.Math.Abs((sStart.h[id] - to.h[id]));
sStart = to;
//如果获取后寻路结束
if (sStart == sGoal)
{
//==========回收ID==========
IdPool.Enqueue(id);
//==========寻路结束,从寻路事件中移除==========
ReCalculateAllPath -= ComputePath;
ReCalculateAllH -= CalculateAllH;
OnNodeDisabled -= NodeDisabled;
OnNodeEnabled -= NodeEnabled;
MonoBehaviour.print("到终点");
}
return to;
}
else//获取失败
{
return null;
}
}
//====================算法函数====================
public void ComputePath()
{
//==========================ForDebugStart==========================
//=====计时器
Stopwatch readdTime = new Stopwatch();
Stopwatch overConsistanceTime = new Stopwatch();
Stopwatch underConsistanceTime = new Stopwatch();
Stopwatch totalTime = new Stopwatch();
Stopwatch dequeueTime = new Stopwatch();
totalTime.Start();
//=====DequeueLoopCount 记录循环中队列Dequeue了多少次
int DequeueLoopCount = 0;
int readdToDeque = 0;
int overConsistance = 0;
int underConsistance = 0;
string timtText;
//===========================ForDebugEnd===========================
//对自身的初始化
if (FirstCalculte)
{
sStart = GlobalStart;
sGoal = GlobalGoal;
CalculateAllH();
#region 原算法的Initialize()
sGoal.rhs[id] = 0;
Node.km[id] = 0;
U = new PriorityQueue(id,sGoal.CalculateKey(id) , sGoal);
#endregion
//==========加入代理==========
ReCalculateAllPath += ComputePath;
ReCalculateAllH += CalculateAllH;
OnNodeDisabled += NodeDisabled;
OnNodeEnabled += NodeEnabled;
FirstCalculte = false;
}
//else
//{
// Node.km[id] += lastStart.h[id];
//}
//lastStart = sStart;
Key tempNewKey;
Key topKey;
Node Utop;
double tempRhs;
double gOld;
//进行节点队列处理
while (true)
{
//先取一个值,使用元组析构赋值
dequeueTime.Start();
(topKey, Utop) = U.Dequeue();
dequeueTime.Stop();
//空,溜
if (Utop == null) break;
//不满足条件,溜
if (!(topKey <= sStart.CalculateKey(id) || sStart.rhs[id] > sStart.g[id])) break;
DequeueLoopCount++;
//更新后Key值
tempNewKey = Utop.CalculateKey(id);
if (topKey <= tempNewKey)
{
readdTime.Start();
readdToDeque++;
U.Enqueue(tempNewKey, Utop);
}
//局部过一致
else if (Utop.g[id] > Utop.rhs[id])
{
overConsistanceTime.Start();
overConsistance++;
//设置为局部一致
Utop.g[id] = Utop.rhs[id];
//对九宫格内(除自己)的节点传递
foreach (Node predecessor in Utop.neighbour)
{
if (predecessor != sGoal)
{
//如果这个更新后的Utop节点是一个更好的successor,则把更低的Rhs赋值给自己(Rhs决定路径,即从结果而言这步是改变最佳路径)
tempRhs = Cost(predecessor, Utop) + Utop.g[id];
if (predecessor.rhs[id] > tempRhs)
{
predecessor.rhs[id] = tempRhs;
}
}
predecessor.UpdateVertex(id, U);
}
}
//局部欠一致(这里意味着g<rhs(因为进入U的都g!=rhs,且没有执行上一个if语句),则意味着出现了障碍)
else
{
underConsistanceTime.Start();
underConsistance++;
gOld = Utop.g[id];
Utop.g[id] = double.PositiveInfinity;
//对九宫格内(除自己)的节点传递
foreach (Node predecessor in Utop.neighbour)
{
if (predecessor != sGoal)
{
//如果rhs == Cost + gOld成立,意味着花费和gOld都没变
//这个rhs == Cost + gOld意味着在之前一种情况中,Utop → predecessor是一条最优回溯路径(即最终路径的一部分),而且障碍物出现后,Cost没有改变(障碍物没有直接阻挡这条路径)
//总体意思是:现在由于Utop节点被影响导致需要重计算g值。Utop单个节点虽然还是通的,但由于障碍物出现有可能导致这整条路径不再是最优路径了(甚至可能整体上不通),所以我们需要给这个点重新选择最优的路径(即重新选择successor)
if (predecessor.rhs[id] == Cost(predecessor, Utop) + gOld)
{
//重新寻找最优的successor
predecessor.rhs[id] = double.PositiveInfinity;
foreach (Node successor in predecessor.neighbour)
{
//==========计算稍微优化?==========
//根据随机频率测试,该判断大概可以有66%的概率避免if语句内的无效运算
if (successor.g[id] != double.PositiveInfinity)
{
tempRhs = Cost(successor, predecessor) + successor.g[id];
if (predecessor.rhs[id] > tempRhs)
{
predecessor.rhs[id] = tempRhs;
}
}
}
}
}
predecessor.UpdateVertex(id, U);
}
Utop.UpdateVertex(id, U);
}
readdTime.Stop();
overConsistanceTime.Stop();
underConsistanceTime.Stop();
}
SetFinalPath();
totalTime.Stop();
//==========================ForDebugStart==========================
timtText =
"总队列检测次数:" + DequeueLoopCount + "\n" +
"重新加入队列次数:" + readdToDeque + "\n" + "耗时(ms):" + readdTime.Elapsed.TotalMilliseconds + "\n" +
"局部过一致次数:" + overConsistance + "\n" + "耗时(ms):" + overConsistanceTime.Elapsed.TotalMilliseconds + "\n" +
"局部欠一致次数:" + underConsistance + "\n" + "耗时(ms):" + underConsistanceTime.Elapsed.TotalMilliseconds + "\n" +
"总耗时(ms):" + totalTime.Elapsed.TotalMilliseconds;
PrintUsingTime.ChangeTimeText(timtText);
MonoBehaviour.print("dequeueTime: " + dequeueTime.ElapsedMilliseconds);
//===========================ForDebugEnd===========================
}
/// <summary>
/// (计算h的函数,由于这个案例中km不用△h来计算,所以只有出现障碍物才需要再次计算h)
/// 在任意的结点被更新(包括障碍物出现、消失的结算)前,如果h过时,则需要调用该函数
/// </summary>
void CalculateAllH()
{
foreach (Node tempNode in Nodes)
{
tempNode.h[id] = (Mathf.Abs(tempNode.x - sStart.x) + Mathf.Abs(tempNode.y - sStart.y)) * 10;//用近似距离作为启发值
}
}
/// <summary>
/// 将寻路结果设置到finalPath变量
/// </summary>
void SetFinalPath()
{
finalPath.Clear();
//如果起点就是终点
if (sStart == sGoal)
return;
//初始化
Node tempStartNode = sStart;
Node tempBestSuccessor = null;
double tempG;
//===========debug
i = 0;
do
{
i++;
if (i > 100000)
{
MonoBehaviour.print("SetFinalPath DeadLoop");
break;
}
tempG = double.PositiveInfinity;
//寻找一个最好的successor
foreach (Node successor in tempStartNode.neighbour)
{
//贪婪选路+穿墙角修复
if (tempG > successor.g[id])
{
//穿墙角修复②:这里修补半穿墙角的情况,如:
//□■
//□□
int DeltaX = successor.x - tempStartNode.x;
int DeltaY = successor.y - tempStartNode.y;
if (Mathf.Abs(DeltaX) + Mathf.Abs(DeltaY) == 2)//判断斜角移动
{
//而且如果有障碍
if (!Nodes[tempStartNode.x + DeltaX, tempStartNode.y].enabled || !Nodes[tempStartNode.x, tempStartNode.y + DeltaY].enabled)
{
continue;//直接跳过
}
}
tempG = successor.g[id];
tempBestSuccessor = successor;
}
}
//如果到处碰壁
if (tempG == double.PositiveInfinity)
{
//返回空路径
finalPath.Clear();
return;
}
//否则添加最好successor至路径
else
{
finalPath.Enqueue(tempBestSuccessor);
tempStartNode = tempBestSuccessor;
}
}
while (tempStartNode != sGoal);
}
/// <summary>
/// 两个相邻节点之间的花费,包含穿墙角修复的一部分代码
/// </summary>
/// <param name="from">初始节点</param>
/// <param name="to">目标节点</param>
/// <returns>花费</returns>
double Cost(Node from, Node to)
{
if (!from.enabled || !to.enabled)
{
return double.PositiveInfinity;
}
else
{
int DeltaX = to.x - from.x;
int DeltaY = to.y - from.y;
switch (Mathf.Abs(DeltaX) + Mathf.Abs(DeltaY))
{
//用值模拟,为了和△h统一,所以没用√2,事实上用√2反而会出错
case 0: return 0;
case 1: return 10;
case 2:
//穿墙角修复①:这里修补半穿墙角的情况,如:
//□■
//■□
if (Nodes[from.x + DeltaX, from.y].enabled && Nodes[from.x, from.y + DeltaY].enabled)
{
return 20;
}
else
{
return double.PositiveInfinity;
}
default: return 0;
}
}
}
/// <summary>
/// 当节点需要被失效时调用,需要手动调用寻路函数
/// </summary>
/// <param name="disabled">失效的节点</param>
void NodeDisabled(Node disabled)
{
//需要在更新结点前重新计算H
//ReCalculateAllH(); 重要!:但是由于同一时间更改多个结点只需调用一次该函数,故需在外部手动调用
double cOld;
double tempRhs;
Node tempOldNode = new Node //注意这里将浅拷贝disabled,由于只是给cost函数调用,所以只拷贝关键的变量
{
x = disabled.x,
y = disabled.y,
//enabled = true
};
//对自己
disabled.rhs[id] = double.PositiveInfinity;
//对九宫格内所有节点
foreach (Node predecessor in disabled.neighbour)
{
cOld = Cost(predecessor, tempOldNode);
//如果predecessor.rhs == cOld + disabled.g,则意味着predecessor→disabled曾是最好路径
//所以现在要给这个predecessor重新找最好路径
if (predecessor.rhs[id] == cOld + disabled.g[id])
{
if (predecessor == sGoal)
{
//跳过终点的检测
continue;
}
//需要重新给予rhs值,所以先假设rhs最大,再取更小值
predecessor.rhs[id] = double.PositiveInfinity;
//对该predecessor的所有successor
foreach (Node successor in predecessor.neighbour)
{
tempRhs = Cost(predecessor, successor) + successor.g[id];
if (predecessor.rhs[id] > tempRhs)
{
predecessor.rhs[id] = tempRhs;
}
}
}
predecessor.UpdateVertex(id, U);
}
disabled.UpdateVertex(id, U);
}
/// <summary>
/// 当节点需要重新启用时调用,需要手动调用寻路函数
/// </summary>
/// <param name="enabled">生效的节点</param>
void NodeEnabled(Node enabled)
{
//需要在更新结点前重新计算H
//ReCalculateAllH(); 重要!:但是由于同一时间更改多个结点只需调用一次该函数,故需在外部手动调用
double tempCost;
double tempRhs;
//检测九宫格内所有节点
foreach (Node predecessor in enabled.neighbour)
{
tempCost = Cost(predecessor, enabled);
//对自己更新(注意:由于终点的设定是不能被disable或enable,所以这里对自己必然!=sGoal,就没有对 == sGoal的检测)
tempRhs = tempCost + predecessor.g[id];
if (enabled.rhs[id] > tempRhs)
{
enabled.rhs[id] = tempRhs;
}
//对predecessor更新
if (predecessor == sGoal)
{
continue;
}
//enabled.g一定还是无穷,这一步没有意义
//tempRhs = tempCost + enabled.g[id];
//if (predecessor.rhs[id] > tempRhs)
//{
// predecessor.rhs[id] = tempRhs;
//}
//predecessor.UpdateVertex(id, U);
}
enabled.UpdateVertex(id, U);
}
}
补充说明
每新建一个DstarLite对象即可生成一个寻路实例,其它交互函数见DstarLite类。编译时没有Unity库可以直接把错误的行注释掉,不影响算法。
使用示范
这个脚本需要Unity实现,但是可以参考实现思路。重点注意ReCalculateAllH函数的调用位置。
using System.Collections;
using System.Collections.Generic;
using System.Diagnostics;
using UnityEngine;
using UnityEngine.Tilemaps;
public class DStarLiteDebugger : MonoBehaviour
{
[SerializeField]
GameObject UI;
//=========预设值=============
int X = 110, Y = 100;
float blockPercentage = 0.2f;
//=========寻路实例============
Node sStart, sGoal;
DStarLite pathFinder;
Queue<Node> path;
Queue<Node> lastPath;
bool start;
//==========Tilemap===========
[SerializeField]
Tile blockTile;
[SerializeField]
Tile roadTile;
[SerializeField]
Tile startTile;
[SerializeField]
Tile goalTile;
[SerializeField]
Tile pathTile;
[SerializeField]
Tilemap tilemap;
void Start()
{
lastPath = new Queue<Node>();
//创建地图
DStarDebugMap();
}
void DStarDebugMap()
{
Node tempNode;
DStarLite.Nodes = new Node[X, Y];
//============创建Nodes并处理邻近结点====================
for (int x = 0; x < X; x++)
{
for (int y = 0; y < Y; y++)
{
tempNode = DStarLite.Nodes[x, y] = new Node(x, y);
//随机障碍
if (Random.value < blockPercentage)
{
tempNode.enabled = false;
tilemap.SetTile(new Vector3Int(x, y, 0), blockTile);
}
else
{
tilemap.SetTile(new Vector3Int(x, y, 0), roadTile);
}
}
}
for (int x = 0; x < X; x++)
{
for (int y = 0; y < Y; y++)
{
for (int i = x - 1; i <= x + 1; i++)
{
for (int j = y - 1; j <= y + 1; j++)
{
//确保在范围内
if ((i != x || j != y) && i >= 0 && j >= 0 && i < X && j < Y)
{
DStarLite.Nodes[x, y].neighbour.Add(DStarLite.Nodes[i, j]);
}
}
}
}
}
}
void Update()
{
//退出
Esc();
//右键中途取消设置障碍
RighClickCancelBlock();
//左键设置开始结束位置(第一次起点,第二次终点)
LeftClickSetStartAndEnd();
//中键设置障碍
MiddleClickSetBlock();
//显示路径
VisiblePath();
//空格键开始第一次寻路
PressSpaceStart();
}
void PressSpaceStart()
{
if (Input.GetKeyDown(KeyCode.Space))
{
if (!start)
{
if (sStart != null && sGoal != null)
{
DStarLite.GlobalInitialize(sStart, sGoal);
pathFinder = new DStarLite();
pathFinder.ComputePath();
path = pathFinder.finalPath;
StartCoroutine(Dequeue());
start = true;
}
}
else
{
//重新开始
UnityEngine.SceneManagement.SceneManager.LoadScene(0);
StopAllCoroutines();
sStart = null;
sGoal = null;
start = false;
}
}
}
IEnumerator Dequeue()
{
do
{
yield return new WaitForSeconds(1.0f);
if (path.Count != 0)
pathFinder.HaveGetToNextNode();
}
while (true);
}
void VisiblePath()
{
//先撤回上次更改
if (lastPath != null)
{
foreach (Node node in lastPath)
{
if (node.enabled == true)
tilemap.SetTile(new Vector3Int(node.x, node.y, 0), roadTile);
else
tilemap.SetTile(new Vector3Int(node.x, node.y, 0), blockTile);
}
}
//再重新修改路径颜色
if (path != null)
{
lastPath.Clear();
bool head = true;
foreach (Node node in path)
{
if (head)
{
tilemap.SetTile(new Vector3Int(node.x, node.y, 0), startTile);
head = false;
}
else
tilemap.SetTile(new Vector3Int(node.x, node.y, 0), pathTile);
lastPath.Enqueue(node);
}
}
}
void RighClickCancelBlock()
{
if (Input.GetMouseButton(1))
{
Node tempNode = GetNodeFromMouse();
if (tempNode != null)
{
if (!tempNode.enabled)
{
//更新D*结点前先更新全局H值
DStarLite.ReCalculateAllH?.Invoke();
tempNode.enabled = true;
//设置真假后需要调用结点更变委托
DStarLite.OnNodeEnabled?.Invoke(tempNode);
DStarLite.ReCalculateAllPath?.Invoke();
tilemap.SetTile(new Vector3Int(tempNode.x, tempNode.y, 0), roadTile);
}
}
}
}
void LeftClickSetStartAndEnd()
{
//左键设置开始结束位置
if (Input.GetMouseButtonDown(0))
{
if (sStart == null)
{
sStart = GetNodeFromMouse();
if (sStart != null)
{
tilemap.SetTile(new Vector3Int(sStart.x, sStart.y, 0), startTile);
}
}
else if (sGoal == null)
{
sGoal = GetNodeFromMouse();
if (sGoal != null)
{
//DStarLite全局初始化
tilemap.SetTile(new Vector3Int(sGoal.x, sGoal.y, 0), goalTile);
}
}
}
}
void MiddleClickSetBlock()
{
if (Input.GetMouseButton(2))
{
Node tempNode = GetNodeFromMouse();
if (tempNode != null)
{
if (tempNode.enabled)
{
//更新D*结点前先更新全局H值
DStarLite.ReCalculateAllH?.Invoke();
tempNode.enabled = false;
//设置真假后需要调用结点更变委托
DStarLite.OnNodeDisabled?.Invoke(tempNode);
DStarLite.ReCalculateAllPath?.Invoke();
tilemap.SetTile(new Vector3Int(tempNode.x, tempNode.y, 0), blockTile);
}
}
}
}
void Esc()
{
if (Input.GetKeyDown(KeyCode.Escape))
{
Application.Quit();
}
}
Node GetNodeFromMouse()
{
Vector3 tempVector = Camera.main.ScreenToWorldPoint(Input.mousePosition);
int x = (int)tempVector.x;
int y = (int)tempVector.y;
if (x >= 0 && x < X && y >= 0 && y < Y)
return DStarLite.Nodes[x, y];
else
return null;
}
}