public Stack <Node> SeekPath2(Node start, Node target, Dictionary <NodeType, float> costMap)
{
Stack <Node> result = new Stack <Node>();
closeNode.Clear();
openList.Clear();
SeekNode realTarget = AddNodeToList(start, target, SeekNode.StartParent, costMap, 0);
int safeFlag = 0;
while (openList.Count > 0 && safeFlag++ < 10000)
{
SeekNode curNode = openList[0];
openList.RemoveAt(0);
closeNode[curNode.GetKey()] = true;
List <Node> neighbours = curNode.SelfNode.Neighbours;
for (int i = 0; i < neighbours.Count; i++)
{
Node sonNode = neighbours[i];
if (closeNode.ContainsKey(sonNode.GetKey()))
{
continue;
}
SeekNode seekNode = AddNodeToList(sonNode, target, curNode, costMap, i);
if (seekNode != null)
{
if (seekNode.H < realTarget.H)
{
realTarget = seekNode;
}
}
if (sonNode == target)
{
heap.Clear();
break;
}
}
}
if (safeFlag >= 10000)
{
Debug.LogError("逻辑错误");
}
SeekNode temp = realTarget;
while (temp.H != -1)
{
result.Push(temp.SelfNode);
temp.PutBack();
temp = temp.Parent;
}
return(result);
}
void InsertSort(SeekNode node)
{
int index = 0;
for (; index < openList.Count; index++)
{
if (openList[index].G > node.G)
{
break;
}
}
openList.Insert(index, node);
}
public IEnumerator SeekPathLog(Node start, Node target, Dictionary <NodeType, float> costMap)
{
Stack <Node> result = new Stack <Node>();
heap.Clear();
closeNode.Clear();
SeekNode realTarget = AddNodeToHeap(start, target, SeekNode.StartParent, costMap, 0);
realTarget.SelfNode.MarkSelf(Color.yellow, 99999);
int safeFlag = 0;
while (heap.Length > 0 && safeFlag++ < 10000)
{
SeekNode curNode = heap.GetMin();
closeNode[curNode.GetKey()] = true;
List <Node> neighbours = curNode.SelfNode.Neighbours;
for (int i = 0; i < neighbours.Count; i++)
{
Node sonNode = neighbours[i];
if (closeNode.ContainsKey(sonNode.GetKey()))
{
continue;
}
SeekNode seekNode = AddNodeToHeap(sonNode, target, curNode, costMap, i);
if (seekNode != null)
{
seekNode.SelfNode.MarkSelf(Color.yellow, 99999);
yield return(new WaitForSeconds(0.1f));
}
if (seekNode != null)
{
if (seekNode.H < realTarget.H)
{
realTarget = seekNode;
}
}
if (sonNode == target)
{
heap.Clear();
break;
}
}
}
if (safeFlag >= 10000)
{
Debug.LogError("逻辑错误");
}
}
public void AddOne(SeekNode nodeAdded)
{
Length++;
sortNodes[Length] = nodeAdded;
if (Length != 1)
{
int currentIndex = Length;
while (currentIndex != 1 && nodeAdded.F <= sortNodes[currentIndex >> 1].F)
{
sortNodes[currentIndex] = sortNodes[currentIndex >> 1];
currentIndex >>= 1;
}
sortNodes[currentIndex] = nodeAdded;
}
}
public static SeekNode GetNode(Node selfNode)
{
lock (lockFlag)
{
if (pool.Count > 0)
{
SeekNode result = pool.Dequeue();
result.SelfNode = selfNode;
return(result);
}
}
return(new SeekNode()
{
SelfNode = selfNode
});
}
SeekNode AddNodeToHeap(Node selfNode, Node target, SeekNode parent, Dictionary <NodeType, float> costMap, int index)
{
SeekNode node = SeekNode.GetNode(selfNode);
node.H = Mathf.Abs(target.X - selfNode.X) + Mathf.Abs(target.Z - selfNode.Z);
float e;
node.G = costMap[selfNode.Type];
if (parent != null)
{
node.G += parent.G;
e = parent.H * index * 0.01f;
}
else
{
e = 0;
}
//e = 0;
node.F = node.G + node.H + e;
float oldG;
int selfKey = selfNode.GetKey();
if (node2G.TryGetValue(selfKey, out oldG))
{
if (oldG <= node.G)
{
return(null);
}
}
node2G[selfKey] = node.G;
if (node.F > 100)
{
node.PutBack();
return(null);
}
else
{
node.Parent = parent;
heap.AddOne(node);
return(node);
}
}
SeekNode AddNodeToList(Node selfNode, Node target, SeekNode parent, Dictionary <NodeType, float> costMap, int index)
{
int selfKey = selfNode.GetKey();
int oldIndex = openList.FindIndex(t => t.GetKey() == selfKey);
float curG = costMap[selfNode.Type] + parent.G;
if (oldIndex >= 0)
{//节点原本就在开启列表,则检查更新
SeekNode oldNode = openList[oldIndex];
if (oldNode.G > curG)
{//新的路径G值小,则使用新的更新数据
oldNode.F -= oldNode.G - curG;
oldNode.G = curG;
openList.RemoveAt(oldIndex);
oldNode.Parent = parent;
InsertSort(oldNode);
}
return(null);
}
SeekNode node = SeekNode.GetNode(selfNode);
node.H = Mathf.Abs(target.X - selfNode.X) + Mathf.Abs(target.Z - selfNode.Z);
node.G = costMap[selfNode.Type] + parent.G;
node.F = node.G + node.H + parent.H * index * 0.01f;
if (node.F > 100)
{
node.PutBack();
return(null);
}
else
{
node.Parent = parent;
InsertSort(node);
//selfNode.MarkSelf(Color.yellow, 99999);
return(node);
}
}
/// <summary>
/// 获取堆中Node的F值最小的Node
/// </summary>
/// <returns></returns>
public SeekNode GetMin()
{
SeekNode result = sortNodes[1];
SeekNode lastNode = sortNodes[Length];
Length--;
int currentIndex = 1;
int leftSon = 2, rightSon = 3;
bool isOver = false;
sortNodes[1] = lastNode;
while (leftSon <= Length && !isOver)
{ //当前节点的左子节点小于长度,则表明当前节点不是叶子点
if (lastNode.F > sortNodes[leftSon].F || lastNode.F > sortNodes[rightSon].F)
{ //如果当前节点元素的F值小于任意一个子节点,则表示需要换位(因为可能发生连续换位,这里不直接换位),应该选择与两个子节点中较小的那个换
if (sortNodes[leftSon].F < sortNodes[rightSon].F)
{ //如果左子节点小,则应该将左子节点与父节点换位,并更新当前节点索引为此左子节点
sortNodes[currentIndex] = sortNodes[leftSon];
currentIndex = leftSon;
}
else
{
sortNodes[currentIndex] = sortNodes[rightSon];
currentIndex = rightSon;
}
}
else
{//如果当前节点小于它的两个子节点,则表示完成调整
isOver = true;
}
leftSon = currentIndex << 1;
rightSon = leftSon + 1;
}
sortNodes[currentIndex] = lastNode; //补充添加当前节点的值
return(result);
}