D*Lite Unity项目主要源码

点此查看D*Lite算法详解

点此下载Unity完整项目

下面是算法主要源码：

• 在这个项目我事实上做了一些变通，Km和H的计算方式没有完全依据算法，另外去掉了一些感觉用不上的过程
• 还有队列的储存方式为了贪方便只是用了个单向链表，查找时还是很慢的，如果改变储存方式其实能有进一步提升

Key类和Key的单向链表（U队列）

using System.Collections;
using System.Collections.Generic;


/// <summary>
/// 用于算法中的Key
/// </summary>
public class Key
{
	static int idGiver;
	public double Key1;
	public double Key2;
	public int id;//避免key冲突
	public Key()
	{
		id = idGiver;
		idGiver++;
	}

	public void CopyKey(Key copyFrom)
	{
		Key1 = copyFrom.Key1;
		Key2 = copyFrom.Key2;
		id = copyFrom.id;
	}

	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.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>
/// 单向链表，将从小到大排入或插入Key
/// </summary>
public class KeyQueue
{
	//====================内部构造函数====================
	private KeyQueue(KeyQueueElement givenHead)
	{
		head = givenHead;
	}
	//====================属性====================
	public int Count { get { return count; } }

	/// <summary>
	/// 获取第一个Node，不移除
	/// </summary>
	public Node PeekFirstNode
	{
		get
		{
			if (head != null)
			{
				return head.node;
			}
			else
			{
				return null;
			}
		}
	}

	/// <summary>
	/// 获取第一个Key，不移除
	/// </summary>
	public Key PeekFirstKey
	{
		get
		{
			if (head != null)
			{
				return head.key;
			}
			else
			{
				return null;
			}
		}
	}
	//====================链表成员====================
	//表头
	KeyQueueElement head;
	//长度
	int count = 0;

	private class KeyQueueElement
	{
		//数据域
		public Key key;
		public Node node;
		//指针域
		public KeyQueueElement nextElement;
	}
	//====================链表操作====================
	/// <summary>
	/// 创建链表并返回链表
	/// </summary>
	/// <param name="givenKey">Key值</param>
	/// <param name="givenNode">Key所对应的地图结点</param>
	/// <returns></returns>
	public static KeyQueue CreateList(Key givenKey, Node givenNode)
	{
		givenNode.InsertKey = givenKey;

		//创建表头并赋值
		KeyQueueElement creatingHead = new KeyQueueElement()
		{
			key = givenNode.InsertKey,
			node = givenNode,
		};
		//创建链表
		KeyQueue U = new KeyQueue(creatingHead);
		
		U.count++;

		//返回链表
		return U;
	}

	/// <summary>
	/// 按从小到大顺序插入Key
	/// </summary>
	/// <param name="givenKey"></param>
	/// <param name="givenNode"></param>
	public void Insert(Key givenKey, Node givenNode)
	{
		//数值处理
		givenNode.InsertKey = givenKey;
		count++;

		KeyQueueElement insertElement = new KeyQueueElement
		{
			key = givenNode.InsertKey,
			node = givenNode,
		};

		//如果头没了就输入为表头
		if (head == null)
		{
			head = insertElement;
			return;
		}
		//如果要从表头前插入，则需要更换表头
		else if (givenKey < head.key)
		{
			insertElement.nextElement = head;
			head = insertElement;
			//返回
			return;
		}
		//否则从第二个结点开始检索
		else
		{
			//对2~n-1个结点
			KeyQueueElement tempListElement = head;
			KeyQueueElement tempNextElement = head.nextElement;
			while (tempNextElement != null)
			{
				//符合条件则插入在第i与i+1个结点之间
				if (givenKey < tempNextElement.key)
				{
					
					insertElement.nextElement = tempNextElement;
					tempListElement.nextElement = insertElement;
					//返回
					return;
				}
				//向后检索
				else
				{
					tempListElement = tempNextElement;
					tempNextElement = tempListElement.nextElement;
				}
			}
			//循环没有return,则对第n个结点（插入到表尾）
			tempListElement.nextElement = insertElement;
			return;
		}
	}

	/// <summary>
	/// 移除第一个元素
	/// </summary>
	public void RemoveFirst()
	{
		//由于“移除” 设置为空
		head.node.InsertKey = null;
		count--;

		head = head.nextElement;
	}

	/// <summary>
	/// 如果包含所给Key的结点，则将其移除，否则无事发生
	/// </summary>
	/// <param name="givenKey"></param>
	public void Remove(Node givenNode)
	{
		//先读取InsertKey的值
		Key insertKey = givenNode.InsertKey;
		//如果不在表中则走人（进入列表都会有一个InsertKey）
		if(insertKey == null)
		{
			return;
		}
		//如果是空列表则走人
		if (head == null)
		{
			return;
		}

		//检测表头
		if (insertKey == head.key)
		{
			head = head.nextElement;
			//由于“移除” 设置为空
			givenNode.InsertKey = null;
			count--;
			return;
		}
		//检测其余
		else
		{
			KeyQueueElement lastListElement = head;
			KeyQueueElement tempListElement = head.nextElement;
			while (tempListElement != null)
			{
				//如果检索到insertKey已经大于givenKey，则意味着没有结果
				if (tempListElement.key > insertKey)
				{
					return;
				}
				else if (tempListElement.key == insertKey)
				{
					lastListElement.nextElement = tempListElement.nextElement;
					//由于“移除” 设置为空
					givenNode.InsertKey = null;
					count--;
					return;
				}
				lastListElement = tempListElement;
				tempListElement = tempListElement.nextElement;
			}
		}
	}
}

Node（结点）类

using System.Collections;
using System.Collections.Generic;
using UnityEngine;


public class Node
{
	//====================变量====================
	//记录地图块信息
	public int x;
	public int y;
	public GameObject tile;

	//记录算法主要数值
	public static double km;
	public double g = double.PositiveInfinity;
	public double rhs = double.PositiveInfinity;
	public double h;
	public bool enable = true;
	public List<Node> neighbour = new List<Node>();//相当于predecessors和successors的合并

	private bool insert;
	private Key insertKey = new Key();
	private Key tempKey = new Key();
	private static Key startKey = new Key();
	

	//====================三个获取Key的属性====================
	/// <summary>
	/// 插入/移出链表时使用的Key值，深拷贝赋值，如果未插入链表，则返回null
	/// </summary>
	public Key InsertKey
	{
		//为了避免赋值时多次构造insertKey的对象，将用另一种方式代替直接赋值
		//set块的逻辑意味着insertKey不能为null，但get块需要输出null来表示未插入链表，所以这里改成布尔判断和直接返回null
		get
		{
			if (insert == false)
			{
				//如果未插入链表则返回null，防止将null赋值给insertKey
				return null;
			}
			else
			{
				return insertKey;
			}
		}
		//这里将拦截null的赋值（避免丢失insertKey对象），改为记录状态，而非null赋值改用Key.CopyKey函数执行
		set
		{
			if(value == null)
			{
				//如果为null则不修改insertKey，而是记录状态
				insert = false;
			}
			else
			{
				//改变状态
				insert = true;
				//用CopyKey代替赋值
				insertKey.CopyKey(value);
			}
		}
	}
	/// <summary>
	/// 获取更新后的Key值
	/// </summary>
	public Key GetUpdatedKey
	{
		get
		{
			tempKey.Key1 = (g < rhs ? g : rhs) + h + km;
			tempKey.Key2 = (g < rhs ? g : rhs);
			return tempKey;
		}
	}
	/// <summary>
	/// start点的Key值，因为不希望无意地篡改Start结点的tempKey值，所以用了另一个变量来存储
	/// </summary>
	public Key GetStartUpdatedKey
	{
		get
		{
			//如果出现计算问题可能是由于startKey太小，可以尝试抬高key1数值。
			startKey.Key1 = (g < rhs ? g : rhs) + h + km;
			startKey.Key2 = (g < rhs ? g : rhs);
			return startKey;
		}
	}

	//====================构造函数====================
	public Node() { }
	public Node(int inputX, int inputY, GameObject gameObject)
	{
		x = inputX;
		y = inputY;
		tile = gameObject;
	}

	public void UpdateVertex()
	{
		DStarLite.U.Remove(this);
		if (g != rhs)
		{
			DStarLite.U.Insert(GetUpdatedKey, this);
		}
	}
}

DStarLite类

using System.Collections;
using System.Collections.Generic;
using System.Diagnostics;
using UnityEngine;

public class DStarLite
{
	public static Node sStart;
	public static Node sGoal;
	public static List<Node> nodes = new List<Node>();
	public static KeyQueue U;

	//获取路径
	public static Queue<Node> finalPath = new Queue<Node>();

	static void CalculateAllH()//（计算h的函数，由于这个案例中km不用△h来计算，所以只有出现障碍物才需要再次计算h）
	{

		foreach (Node tempNode in nodes)
		{
			tempNode.h = (Mathf.Abs(tempNode.x - sStart.x) + Mathf.Abs(tempNode.y - sStart.y)) * 10;//用近似距离作为启发值
		}

	}

	public static void Initialize()
	{
		sGoal.rhs = 0;
		Node.km = 0;

		CalculateAllH();
		
		U = KeyQueue.CreateList(sGoal.GetUpdatedKey, sGoal);
	}

	public static void ComputePath()
	{
		Key tempOldKey = new Key();
		Key tempNewKey;
		Node Utop;

		double tempRhs;
		double gOld;

		//进行节点队列处理
		//==========注意：这里用的是GetStartUpdatedKey属性，这个获取并不会改变本身的key值，可以理解为是深拷贝的另一个值===========
		//==========这样做的原因就是不希望篡改了里面的key值，而且可以对起点的Key做校正(可以放大起点key值以扩大此循环范围)==========
		//一定要判断队列是否为空
		while ((U.PeekFirstKey != null && (U.PeekFirstKey < sStart.GetStartUpdatedKey) || sStart.rhs != sStart.g))
		{
			//先拷贝Key里面的值
			tempOldKey.CopyKey(U.PeekFirstKey);
			//再取出队列中第一个元素，这一步将Remove队列中第一个元素
			Utop = U.PeekFirstNode;
			U.RemoveFirst();

			//再浅拷贝更新后Key值（只用作读）
			tempNewKey = Utop.GetUpdatedKey;

			if (tempOldKey < tempNewKey)
			{
				U.Insert(tempNewKey, Utop);
			}
			//局部过一致
			else if (Utop.g > Utop.rhs)
			{
				//设置为局部一致
				Utop.g = Utop.rhs;
				//对九宫格内(除自己)的节点传递
				foreach (Node predecessor in Utop.neighbour)
				{
					if (predecessor != sGoal)
					{
						//如果这个更新后的Utop节点是一个更好的successor，则把更低的Rhs赋值给自己(Rhs决定路径，即从结果而言这步是改变最佳路径)
						tempRhs = Cost(predecessor, Utop) + Utop.g;
						if (predecessor.rhs > tempRhs)
						{
							predecessor.rhs = tempRhs;
						}
					}
					predecessor.UpdateVertex();
				}
			}
			//局部欠一致（这里意味着g<rhs(因为进入U的都g!=rhs，且没有执行上一个if语句)，则意味着出现了障碍）
			else
			{
				gOld = Utop.g;
				Utop.g = double.PositiveInfinity;
				//对九宫格内(除自己)的节点传递
				foreach (Node predecessor in Utop.neighbour)
				{
					if (predecessor != sGoal)
					{
						//如果rhs == Cost + gOld成立，意味着花费和gOld都没变
						//这个rhs == Cost + gOld意味着在之前一种情况中，Utop → predecessor是一条最优回溯路径（即最终路径的一部分），而且障碍物出现后，Cost没有改变（障碍物没有直接阻挡这条路径）
						//总体意思是：现在由于节点被影响导致需要重计算g值。这个Utop节点虽然还是通的，但由于障碍物出现有可能导致这不再是最优路径了，所以我们需要给这个点重新选择最优的路径（即重新选择successor）
						if (predecessor.rhs == Cost(predecessor, Utop) + gOld)
						{

							//重新寻找最优的successor
							predecessor.rhs = double.PositiveInfinity;

							foreach (Node successor in predecessor.neighbour)
							{
								tempRhs = Cost(successor, predecessor) + successor.g;

								if (predecessor.rhs > tempRhs)
								{
									predecessor.rhs = tempRhs;
								}
							}
						}
					}
					predecessor.UpdateVertex();
				}
				//更新自己的结点
				//Utop.UpdateVertex();
			}
		}
		GetFinalPath();
	}

	static void GetFinalPath()
	{
		finalPath.Clear();
		Node tempStartNode = sStart;
		Node tempBestSuccessor = null;
		double tempG;
		//如果起点就是终点
		if (sStart == sGoal)
		{

			return;
		}
		do
		{
			tempG = double.PositiveInfinity;
			//贪婪地寻找一个最好的successor
			foreach (Node successor in tempStartNode.neighbour)
			{
				if (tempG > successor.g)
				{
					tempG = successor.g;
					tempBestSuccessor = successor;
				}
			}
			//如果到处碰壁
			if (tempG == double.PositiveInfinity)
			{
				//返回空路径
				finalPath.Clear();
				return;
			}
			//否则添加最好successor至路径
			else
			{
				finalPath.Enqueue(tempBestSuccessor);
				tempStartNode = tempBestSuccessor;
			}
		}
		while (tempStartNode != sGoal);
	}

	public static double Cost(Node from, Node to)
	{
		if (!from.enable || !to.enable)
		{
			return double.PositiveInfinity;
		}
		else
		{
			int DeltaX = Mathf.Abs(from.x - to.x);
			int DeltaY = Mathf.Abs(from.y - to.y);
			switch (DeltaX + DeltaY)
			{
				//用值模拟，为了和△h统一，所以没用√2
				case 0: return 0;
				case 1: return 10;
				case 2: return 20;
				default: return 0;
			}
		}
	}

	/// <summary>
	/// 返回路径中下一个节点，意味着向前运动，此时要更新部分数据
	/// </summary>
	public static Node GetNextPathNode()
	{
		if (finalPath != null && finalPath.Count != 0)
		{
			Node to = finalPath.Dequeue();
			Node.km += Cost(sStart, to);
			sStart = to;

			return to;
		}
		else
			return null;
	}

	/// <summary>
	/// 当节点需要被失效时调用，需要手动调用寻路函数
	/// </summary>
	/// <param name="disabled">失效的节点</param>
	public static void OnNodeDisabled(Node disabled)
	{

		double cOld;
		double tempRhs;
		Node tempOldNode = new Node //注意这里将深拷贝disabled，但由于只是给cost函数调用，所以只拷贝关键的变量
		{
			x = disabled.x,
			y = disabled.y,
		};

		//对自己
		disabled.enable = false;
		disabled.rhs = double.PositiveInfinity;

		//对全局(在这里计算H只是为了计算disabled到Start之间的h，其它值无需立即计算，所以如果想一帧内disabled多个结点后再计算路径，可以考虑这里只计算disabled到Start之间的h，而其它h在计算路径前计算，以避免多个结点Enabled时重复计算全图的h
		CalculateAllH();

		//对九宫格内所有节点
		foreach (Node predecessor in disabled.neighbour)
		{
			cOld = Cost(predecessor, tempOldNode);
			//如果predecessor.rhs == cOld + disabled.g，则意味着predecessor→disabled曾是最好路径
			//所以现在要给这个predecessor重新找最好路径
			if (predecessor.rhs == cOld + disabled.g)
			{
				if (predecessor == sGoal)
				{
					//跳过终点的检测
					continue;
				}
				
				//需要重新给予rhs值，所以先假设rhs最大，再取更小值
				predecessor.rhs = double.PositiveInfinity;

				//对该predecessor的所有successor
				foreach (Node successor in predecessor.neighbour)
				{
					tempRhs = Cost(predecessor, successor) + successor.g;
					if (predecessor.rhs > tempRhs)
					{
						predecessor.rhs = tempRhs;
					}
				}
			}
			predecessor.UpdateVertex();
		}
		disabled.UpdateVertex();
	}

	/// <summary>
	/// 当节点需要重新启用时调用，需要手动调用寻路函数
	/// </summary>
	/// <param name="enabled">生效的节点</param>
	public static void OnNodeEnabled(Node enabled)
	{
		//对自己
		enabled.enable = true;

		//对全局(在这里计算H只是为了计算Enabled到Start之间的h，其它值无需立即计算，所以如果想一帧内enable多个结点后再计算路径，可以考虑这里只计算Enabled到Start之间的h，而其它h在计算路径前计算，以避免多个结点Enabled时重复计算全图的h
		CalculateAllH();

		double tempCost;
		double tempRhs;
		//检测九宫格内所有节点
		foreach (Node predecessor in enabled.neighbour)
		{
			tempCost = Cost(predecessor, enabled);
			//对自己更新（注意：由于终点的设定是不能被disable或enable，所以这里对自己必然!=sGoal，就没有对 == sGoal的检测）
			tempRhs = tempCost + predecessor.g;
			if (enabled.rhs > tempRhs)
			{
				enabled.rhs = tempRhs;
			}
		}
		enabled.UpdateVertex();
	}
}