public void InitializeGrid()
{
(int gridLayersIn, int gridSlicesIn) = grid.GetGridSize();
nodeGrid = new PathNodeHolder <GridCell> [gridLayersIn, gridSlicesIn];
gridLayers = gridLayersIn;
gridSlices = gridSlicesIn;
//Set up pathfinding nodes
foreach (GridCell cell in grid.gridCells)
{
PathNodeHolder <GridCell> holder = new PathNodeHolder <GridCell>();
holder.node = cell;
holder.gCost = int.MaxValue;
holder.hCost = cell.hCost;
holder.CalculateFCost();
holder.previousNodeHolder = null;
holder.SetNeighbors(cell.GetNeighbors());
holder.id = holder.node.GetInstanceID();
nodeGrid[cell.layer, cell.slice] = holder;
}
//Set up pathfinding nodes
foreach (PathNodeHolder <GridCell> node in nodeGrid)
{
List <PathNodeHolder <GridCell> > neighbors = new List <PathNodeHolder <GridCell> >();
int layer = node.node.layer;
int slice = node.node.slice;
if (layer - 1 >= 0)
{
neighbors.Add(nodeGrid[layer - 1, slice]);
}
if (layer + 1 < gridLayersIn)
{
neighbors.Add(nodeGrid[layer + 1, slice]);
}
if (slice - 1 >= 0)
{
neighbors.Add(nodeGrid[layer, slice - 1]);
}
if (slice - 1 == -1)
{
neighbors.Add(nodeGrid[layer, gridSlicesIn - 1]);
}
if (slice + 1 < gridSlicesIn)
{
neighbors.Add(nodeGrid[layer, slice + 1]);
}
if (slice + 1 == gridSlicesIn)
{
neighbors.Add(nodeGrid[layer, 0]);
}
node.SetNeighbors(neighbors);
}
}
public static List <GridCell> FindPath(Grid grid, int startX, int startZ, int endX, int endZ)
{
PathNodeHolder <GridCell> startCell = new PathNodeHolder <GridCell>();
PathNodeHolder <GridCell> endCell = new PathNodeHolder <GridCell>();
BinaryTree <PathNodeHolder <GridCell> > openCells = new BinaryTree <PathNodeHolder <GridCell> >();
HashSet <PathNodeHolder <GridCell> > closedCells = new HashSet <PathNodeHolder <GridCell> >();
int gridSizeX = grid.GetGridSize().x;
int gridSizeZ = grid.GetGridSize().z;
PathNodeHolder <GridCell>[,] nodeGrid = new PathNodeHolder <GridCell> [gridSizeX + 1, gridSizeZ + 1];
for (int x = 0; x <= gridSizeX; x++)
{
for (int z = 0; z <= gridSizeZ; z++)
{
GridCell cell = grid.GetGridCell(x, z);
PathNodeHolder <GridCell> holder = new PathNodeHolder <GridCell>();
holder.node = cell;
holder.gCost = int.MaxValue;
holder.hCost = cell.hCost;
holder.CalculateFCost();
holder.previousNodeHolder = null;
holder.SetNeighbors(cell.GetNeighbors());
holder.id = holder.node.GetInstanceID();
if (cell.x == startX && cell.z == startZ)
{
startCell = holder;
}
if (cell.x == endX && cell.z == endZ)
{
endCell = holder;
}
nodeGrid[x, z] = holder;
}
}
foreach (PathNodeHolder <GridCell> node in nodeGrid)
{
List <PathNodeHolder <GridCell> > neighbors = new List <PathNodeHolder <GridCell> >();
if (node.node.x - 1 >= 0)
{
neighbors.Add(nodeGrid[node.node.x - 1, node.node.z]);
}
if (node.node.x + 1 <= gridSizeX)
{
neighbors.Add(nodeGrid[node.node.x + 1, node.node.z]);
}
if (node.node.z - 1 >= 0)
{
neighbors.Add(nodeGrid[node.node.x, node.node.z - 1]);
}
if (node.node.z + 1 <= gridSizeZ)
{
neighbors.Add(nodeGrid[node.node.x, node.node.z + 1]);
}
node.SetNeighbors(neighbors);
}
startCell.gCost = 0;
startCell.hCost = CalculateDistance(startCell.node, endCell.node);
startCell.CalculateFCost();
openCells.Add(startCell);
while (openCells.Count() > 0)
{
PathNodeHolder <GridCell> currentCell = GetLowestFCostCell(openCells);
if (currentCell.node == endCell.node)
{
return(CalculatePath(endCell));
}
openCells.Remove(currentCell);
closedCells.Add(currentCell);
List <PathNodeHolder <GridCell> > currentCellNeighbors = currentCell.GetNeighbors();
foreach (PathNodeHolder <GridCell> neighbor in currentCellNeighbors)
{
if (closedCells.Contains(neighbor))
{
continue;
}
if (!neighbor.node.passable)
{
openCells.Remove(currentCell);
closedCells.Add(neighbor);
continue;
}
int tentativeGCost = CalculateDistance(currentCell.node, neighbor.node);
if (tentativeGCost < neighbor.gCost)
{
neighbor.previousNodeHolder = currentCell;
neighbor.gCost = tentativeGCost;
neighbor.hCost = CalculateDistance(neighbor.node, endCell.node);
neighbor.CalculateFCost();
if (!openCells.Contains(neighbor))
{
openCells.Add(neighbor);
}
}
}
}
return(null);
}