public void Push(ActiveNode node) {
if (numberOfItems == binaryHeap.Length) {
var newHeap = new ActiveNode[binaryHeap.Length * 2];
Array.Copy(binaryHeap, newHeap, binaryHeap.Length);
binaryHeap = newHeap;
}
var bubbleIndex = numberOfItems;
binaryHeap[bubbleIndex] = node;
node.Slot = bubbleIndex;
while (bubbleIndex != 1) {
var parentIndex = bubbleIndex / 2;
if (binaryHeap[parentIndex].F > node.F) {
binaryHeap[bubbleIndex] = binaryHeap[parentIndex];
binaryHeap[parentIndex] = node;
binaryHeap[parentIndex].Slot = parentIndex;
binaryHeap[bubbleIndex].Slot = bubbleIndex;
bubbleIndex = parentIndex;
}
else
break;
}
++numberOfItems;
}
public void Push(ActiveNode node)
{
if (numberOfItems == binaryHeap.Length)
{
var newHeap = new ActiveNode[binaryHeap.Length * 2];
Array.Copy(binaryHeap, newHeap, binaryHeap.Length);
binaryHeap = newHeap;
}
var bubbleIndex = numberOfItems;
binaryHeap[bubbleIndex] = node;
node.Slot = bubbleIndex;
while (bubbleIndex != 1)
{
var parentIndex = bubbleIndex / 2;
if (binaryHeap[parentIndex].F > node.F)
{
binaryHeap[bubbleIndex] = binaryHeap[parentIndex];
binaryHeap[parentIndex] = node;
binaryHeap[parentIndex].Slot = parentIndex;
binaryHeap[bubbleIndex].Slot = bubbleIndex;
bubbleIndex = parentIndex;
}
else
{
break;
}
}
++numberOfItems;
}
public void Update(ActiveNode node) {
var bubbleIndex = node.Slot;
while (bubbleIndex != 1) {
var parentIndex = bubbleIndex / 2;
if (binaryHeap[parentIndex].F > node.F) {
binaryHeap[bubbleIndex] = binaryHeap[parentIndex];
binaryHeap[parentIndex] = node;
binaryHeap[parentIndex].Slot = parentIndex;
binaryHeap[bubbleIndex].Slot = bubbleIndex;
bubbleIndex = parentIndex;
}
else
break;
}
}
public static ActiveNode Create(Short2 node) {
ActiveNode activeNode;
if (poolCount > 0) {
activeNode = pool.Pop();
poolCount -= 1;
}
else {
activeNode = new ActiveNode();
}
activeNode.Node = node;
activeNode.F = 0;
activeNode.G = 0;
activeNode.H = 0;
activeNode.Slot = 0;
activeNode.Parent = null;
return activeNode;
}
public static ActiveNode Create(Short2 node)
{
ActiveNode activeNode;
if (poolCount > 0)
{
activeNode = pool.Pop();
poolCount -= 1;
}
else
{
activeNode = new ActiveNode();
}
activeNode.Node = node;
activeNode.F = 0;
activeNode.G = 0;
activeNode.H = 0;
activeNode.Slot = 0;
activeNode.Parent = null;
return(activeNode);
}
public void Update(ActiveNode node)
{
var bubbleIndex = node.Slot;
while (bubbleIndex != 1)
{
var parentIndex = bubbleIndex / 2;
if (binaryHeap[parentIndex].F > node.F)
{
binaryHeap[bubbleIndex] = binaryHeap[parentIndex];
binaryHeap[parentIndex] = node;
binaryHeap[parentIndex].Slot = parentIndex;
binaryHeap[bubbleIndex].Slot = bubbleIndex;
bubbleIndex = parentIndex;
}
else
{
break;
}
}
}
public static int Cost(ActiveNode node, ActiveNode parent) {
return parent.G + (IsDiagonal(node.Node, parent.Node) ? 14 : 10);
}
public static void Recycle(ActiveNode node)
{
pool.Push(node);
}
public bool FindPath(Grid grid, Short2 from, Short2 to, out Short2[] foundPath)
{
var sw = System.Diagnostics.Stopwatch.StartNew();
foundPath = null;
var found = false;
int totalNodesSearched = 0;
ActiveNode connectionNode = null;
if (grid.IsClosed(to))
{
return(false);
}
var start = ActiveNode.Create(from);
openList.Push(start);
while (openList.NumberOfItems > 0)
{
var current = openList.Pop();
var numConnections = grid.GetConnections(current.Node);
for (var i = 0; i < numConnections; ++i)
{
var connection = grid.Connections[i];
if (grid.IsClosed(connection))
{
continue;
}
if (closedList.Contains(connection.Packed))
{
continue;
}
// Old Node
if (activeNodes.TryGetValue(connection.Packed, out connectionNode))
{
var newG = Grid.Cost(connectionNode, current);
if (newG < connectionNode.G)
{
connectionNode.G = newG;
connectionNode.F = connectionNode.H + newG;
connectionNode.Parent = current;
openList.Update(connectionNode);
}
}
// New Node
else
{
activeNodes[connection.Packed] = connectionNode = ActiveNode.Create(connection);
connectionNode.H = Grid.Heuristic(connection, to);
connectionNode.G = Grid.Cost(connectionNode, current);
connectionNode.F = connectionNode.H + connectionNode.G;
connectionNode.Parent = current;
openList.Push(connectionNode);
activeList.Add(connectionNode);
++totalNodesSearched;
}
}
if (current.Node.Packed == to.Packed)
{
while (current != null)
{
path.Add(current.Node);
current = current.Parent;
}
path.Reverse();
foundPath = path.ToArray();
if (grid.AutoSmooth)
{
foundPath = grid.Smooth(foundPath);
}
found = true;
break;
}
closedList.Add(current.Node.Packed);
}
for (var i = 0; i < activeList.Count; ++i)
{
ActiveNode.Recycle(activeList[i]);
}
sw.Stop();
openList.Clear();
closedList.Clear();
path.Clear();
activeList.Clear();
activeNodes.Clear();
return(found);
}
public static int Cost(ActiveNode node, ActiveNode parent)
{
return(parent.G + (IsDiagonal(node.Node, parent.Node) ? 14 : 10));
}
public static void Recycle(ActiveNode node) {
pool.Push(node);
}
