void ReconstructPath(InstanceStash stash)
{
var current = stash.CameFrom[this.GetIndex(stash.Goal)];
var from = this.GetPosition(stash.Grid, current);
stash.Waypoints.Add(from);
var next = this.GetPosition(stash.Grid, current);
while (!current.Equals(stash.Start))
{
current = stash.CameFrom[this.GetIndex(current)];
var tmp = next;
next = this.GetPosition(stash.Grid, current);
if (!this.IsWalkable(stash.Grid, from.xy, next.xy))
{
// skip
stash.Waypoints.Add(tmp);
from = tmp;
}
}
stash.Waypoints.Reverse();
stash.Request.fufilled = true;
}
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);
}
public void Execute(ArchetypeChunk chunk, int chunkIndex, int firstEntityIndex)
{
//BufferAccessor<Cell> accessor = this.GridChunks[0].GetBufferAccessor(this.CellTypeRO);
//DynamicBuffer<Cell> grid = accessor[0].Reinterpret<Cell>();
int size = DimX * DimY;
BufferAccessor <Waypoint> Waypoints = chunk.GetBufferAccessor(WaypointChunkBuffer);
NativeArray <PathRequest> PathRequests = chunk.GetNativeArray(PathRequestsChunkComponent);
NativeArray <Translation> Translations = chunk.GetNativeArray(TranslationsChunkComponent);
NativeArray <NavigationCapabilities> NavigationCapabilities = chunk.GetNativeArray(NavigationCapabilitiesChunkComponent);
NativeArray <float> CostSoFar = new NativeArray <float>(size * chunk.Count, Allocator.Temp);
NativeArray <int2> CameFrom = new NativeArray <int2>(size * chunk.Count, Allocator.Temp);
NativeMinHeap OpenSet = new NativeMinHeap((Iterations + 1) * Neighbors.Length * chunk.Count, Allocator.Temp);
for (int i = chunkIndex; i < chunk.Count; i++)
{
NativeSlice <float> costSoFar = CostSoFar.Slice(i * size, size);
NativeSlice <int2> cameFrom = CameFrom.Slice(i * size, size);
int openSetSize = (Iterations + 1) * NeighborCount;
NativeMinHeap openSet = OpenSet.Slice(i * openSetSize, openSetSize);
PathRequest request = PathRequests[i];
// Clear our shared data
//var buffer = costSoFar.GetUnsafePtr();
//UnsafeUtility.MemClear(buffer, (long)costSoFar.Length * UnsafeUtility.SizeOf<float>());
//openSet.Clear();
Translation currentPosition = Translations[i];
NavigationCapabilities capability = NavigationCapabilities[i];
// cache these as they're used a lot
int2 start = currentPosition.Value.xy.FloorToInt();
int2 goal = request.end;
DynamicBuffer <float3> waypoints = Waypoints[i].Reinterpret <float3>();
waypoints.Clear();
// Special case when the start is the same point as the goal
if (start.Equals(goal))
{
// We just set the destination as the goal, but need to get the correct height
int gridIndex = this.GetIndex(goal);
Cell cell = CellArray[gridIndex];
float3 point = new float3(request.Destination.x, request.Destination.y, cell.Height);
waypoints.Add(point);
continue;
}
var stash = new InstanceStash
{
Grid = CellArray,
CameFrom = cameFrom,
CostSoFar = costSoFar,
OpenSet = openSet,
Request = request,
Capability = capability,
CurrentPosition = currentPosition,
Start = start,
Goal = goal,
Waypoints = waypoints,
};
if (this.ProcessPath(ref stash))
{
this.ReconstructPath(stash);
}
}
CostSoFar.Dispose();
CameFrom.Dispose();
OpenSet.Dispose();
}