private void DecreaseKey(PathBHNode node, int heapIndex)
{
var tempIndex = heapIndex;
uint nodeF = node.F;
uint nodeG = node.G;
while (tempIndex != 0)
{
int parentIndex = (tempIndex - 1) / ChildCount;
uint parentF = this.heap[parentIndex].F;
if (nodeF < parentF ||
(nodeF == parentF && nodeG > this.heap[parentIndex].G))
{
//如果F值比父亲节点小就交换位置
this.heap[tempIndex] = this.heap[parentIndex];
//this.heap[tempIndex].
tempIndex = parentIndex;
}
else
{
break;
}
}
this.heap[tempIndex] = node;
}
public int AddNode(PathNode node)
{
if (node.PathIndex < 0)
{
return(-1);
throw new Exception("Path Error");
}
var newNode = new PathBHNode
{
F = node.F,
G = node.G,
PathId = node.PathIndex
};
if (this.size >= this.heap.Capacity)
{
this.heap.Add(newNode);
}
this.DecreaseKey(newNode, this.size++);
return(-1);
}
