/// <summary>
/// Add a node to the heap which will be sorted.
/// </summary>
/// <param name="node"> The node to add. </param>
/// <exception cref="IndexOutOfRangeException"> Throws if capacity reached. </exception>
public void Push(MinHeapNode node)
{
#if ENABLE_UNITY_COLLECTIONS_CHECKS
if (this.length == this.capacity)
{
throw new IndexOutOfRangeException("Capacity Reached");
}
AtomicSafetyHandle.CheckReadAndThrow(this.m_Safety);
#endif
if (this.head < 0)
{
this.head = this.length;
}
else if (node.ExpectedCost < this.Get(this.head).ExpectedCost)
{
node.Next = this.head;
this.head = this.length;
}
else
{
var currentPtr = this.head;
var current = this.Get(currentPtr);
while (current.Next >= 0 && this.Get(current.Next).ExpectedCost <= node.ExpectedCost)
{
currentPtr = current.Next;
current = this.Get(current.Next);
}
node.Next = current.Next;
current.Next = this.length;
UnsafeUtility.WriteArrayElement(this.buffer, currentPtr, current);
}
UnsafeUtility.WriteArrayElement(this.buffer, this.length, node);
this.length += 1;
}
bool ProcessPath(ref InstanceStash stash)
{
// Push the start to NativeMinHeap openSet
float hh = H(stash.Start, stash.Goal);
MinHeapNode head = new MinHeapNode(stash.Start, hh, hh);
stash.OpenSet.Push(head);
int iterations = this.Iterations;
MinHeapNode closest = head;
// While we still have potential nodes to explore
while (stash.OpenSet.HasNext())
{
MinHeapNode current = stash.OpenSet.Pop();
if (current.DistanceToGoal < closest.DistanceToGoal)
{
closest = current;
}
// Found our goal
if (current.Position.Equals(stash.Goal))
{
return(true);
}
// Path might still be obtainable but we've run out of allowed iterations
if (iterations == 0)
{
if (stash.Request.NavigateToBestIfIncomplete)
{
// Return the best result we've found so far
// Need to update goal so we can reconstruct the shorter path
stash.Goal = closest.Position;
return(true);
}
return(false);
}
iterations--;
var initialCost = stash.CostSoFar[this.GetIndex(current.Position)];
var fromIndex = this.GetIndex(current.Position);
// Loop our potential cells - generally neighbours but could include portals
for (var i = 0; i < this.Neighbors.Length; i++)
{
var neighbour = this.Neighbors[i];
var position = current.Position + neighbour.Offset;
// Make sure the node isn't outside our grid
if (position.x < 0 || position.x >= this.DimX || position.y < 0 || position.y >= this.DimY)
{
continue;
}
var index = this.GetIndex(position);
// Get the cost of going to this cell
var cellCost = this.GetCellCost(stash.Grid, stash.Capability, fromIndex, index, neighbour, true);
// Infinity means the cell is un-walkable, skip it
if (float.IsInfinity(cellCost))
{
continue;
}
var newCost = initialCost + (neighbour.Distance * cellCost);
var oldCost = stash.CostSoFar[index];
// If we've explored this cell before and it was a better path, ignore this route
if (!(oldCost <= 0) && !(newCost < oldCost))
{
continue;
}
// Update the costing and best path
stash.CostSoFar[index] = newCost;
stash.CameFrom[index] = current.Position;
// Push the node onto our heap
var h = H(position, stash.Goal);
var expectedCost = newCost + h;
stash.OpenSet.Push(new MinHeapNode(position, expectedCost, h));
}
}
if (stash.Request.NavigateToNearestIfBlocked)
{
stash.Goal = closest.Position;
return(true);
}
// All routes have been explored without finding a route to destination
return(false);
}
