private static void GetAdjacentNodes(AStarGrid grid, AStarNode node, ref NativeArray <AStarNode> adjacentNodes, out int length)
{
length = 0;
// +X
if (!node.IsBlockedPosX)
{
int x = node.XCoord + 1;
int y = node.YCoord;
if (x < grid.LengthX && grid.HasNode(x, y) && grid[x, y].IsTraverseable && !grid[x, y].IsBlockedNegX)
{
adjacentNodes[length++] = grid[x, y];
}
}
// -X
if (!node.IsBlockedNegX)
{
int x = node.XCoord - 1;
int y = node.YCoord;
if (x >= 0 && grid.HasNode(x, y) && grid[x, y].IsTraverseable && !grid[x, y].IsBlockedPosX)
{
adjacentNodes[length++] = grid[x, y];
}
}
// +Y
if (!node.IsBlockedPosY)
{
int x = node.XCoord;
int y = node.YCoord + 1;
if (y < grid.LengthY && grid.HasNode(x, y) && grid[x, y].IsTraverseable && !grid[x, y].IsBlockedNegY)
{
adjacentNodes[length++] = grid[x, y];
}
}
// -Y
if (!node.IsBlockedNegY)
{
int x = node.XCoord;
int y = node.YCoord - 1;
if (y >= 0 && grid.HasNode(x, y) && grid[x, y].IsTraverseable && !grid[x, y].IsBlockedPosY)
{
adjacentNodes[length++] = grid[x, y];
}
}
}
public static bool SolvePath(AStarGrid grid, AStarNode start, AStarNode end, ref NativeList <AStarNode> pathNodes)
{
NativeList <AStarNode> openNodes = new NativeList <AStarNode>(Allocator.Temp);
NativeList <AStarNode> closedNodes = new NativeList <AStarNode>(Allocator.Temp);
AStarNode current = start;
openNodes.Add(start);
bool pathFound = false;
while (openNodes.Length > 0)
{
current = openNodes[0];
int minF = current.F;
for (int i = 1, n = openNodes.Length; i < n; i++)
{
if (minF > openNodes[i].F)
{
current = openNodes[i];
}
}
closedNodes.Add(current);
openNodes.RemoveAtSwapBack(openNodes.IndexOf(current));
if (current.Equals(end))
{
// Path to end found
pathFound = true;
break;
}
NativeArray <AStarNode> adjacentNodes = new NativeArray <AStarNode>(4, Allocator.Temp);
int length = 0;
GetAdjacentNodes(grid, current, ref adjacentNodes, out length);
for (int i = 0; i < length; i++)
{
AStarNode adjacent = adjacentNodes[i];
if (closedNodes.IndexOf(adjacent) < 0)
{
if (openNodes.IndexOf(adjacent) < 0)
{
adjacent.parentIndex = current.Index;
adjacent.G = DistanceToNode(adjacent, end);
adjacent.H = adjacent.TraversalCost + grid[adjacent.parentIndex].H;
openNodes.Add(adjacent);
grid[adjacent.XCoord, adjacent.YCoord] = adjacent;
}
}
}
adjacentNodes.Dispose();
}
if (pathFound)
{
while (current.parentIndex >= 0)
{
pathNodes.Add(current);
current = grid[current.parentIndex];
}
pathNodes.Add(start);
// Reverse
for (int i = 0, i2 = pathNodes.Length - 1; i < i2; i++, i2--)
{
AStarNode t = pathNodes[i];
pathNodes[i] = pathNodes[i2];
pathNodes[i2] = t;
}
}
openNodes.Dispose();
closedNodes.Dispose();
return(pathFound);
}
public void Execute(ArchetypeChunk chunk, int chunkIndex, int firstEntityIndex)
{
NativeArray <NavigationGridData> navigationGrids = chunk.GetNativeArray(this.navigationGridType);
BufferAccessor <AStarNodeElement> nodeBufferAccessor = chunk.GetBufferAccessor(this.nodeBufferType);
for (int ci = 0, cn = chunk.Count; ci < cn; ci++)
{
NavigationGridData navigationGrid = outNavigationGrid[0] = navigationGrids[ci];
DynamicBuffer <AStarNodeElement> nodeBuffer = nodeBufferAccessor[ci];
// Solve path if requested
if (navigationGrid.pathRequested)
{
// All nodes
NativeArray <AStarNode> nodes = nodeBuffer.Reinterpret <AStarNode>().ToNativeArray(Allocator.Temp);
// Create temp grid
AStarGrid aStarGrid = new AStarGrid(navigationGrid.lengthX, navigationGrid.lengthY, nodes);
// Start/end nodes
AStarNode start = navigationGrid.pathStart;
AStarNode end = navigationGrid.pathEnd;
// Solve path
NativeList <AStarNode> pathNodeList = new NativeList <AStarNode>(Allocator.Temp);
bool pathFound = AStarSolver.SolvePath(aStarGrid, start, end, ref pathNodeList);
int pathLength = pathNodeList.Length;
if (pathFound && navigationGrid.pathMustBeStraightLine)
{
// Check if path is straight line if specfied as a requirement
bool xDiverge = false;
bool yDiverge = false;
for (int i = 1, n = pathNodeList.Length; i < n; i++)
{
if (!xDiverge)
{
xDiverge = pathNodeList[i].XCoord != pathNodeList[i - 1].XCoord;
}
if (!yDiverge)
{
yDiverge = pathNodeList[i].YCoord != pathNodeList[i - 1].YCoord;
}
if (xDiverge && yDiverge)
{
pathFound = false;
break;
}
}
}
// Copy path node list to output array
NativeSlice <AStarNode> pathNodeListSlice = new NativeSlice <AStarNode>(pathNodeList.AsArray());
NativeSlice <AStarNode> pathNodeSlice = new NativeSlice <AStarNode>(this.outPathNodes, 0, pathLength);
pathNodeSlice.CopyFrom(pathNodeListSlice);
// Dispose native containers
pathNodeList.Dispose();
aStarGrid.Dispose();
nodes.Dispose();
this.outPathFound[0] = pathFound ? 1 : 0;
this.outPathFound[1] = pathLength;
navigationGrid.pathRequested = false;
navigationGrid.pathMustBeStraightLine = false;
// Apply changes
navigationGrids[ci] = navigationGrid;
}
}
}
