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);
}
public List <GridCell> FindPath(int startLayer, int startSlice, int endLayer, int endSlice)
{
ResetCellCosts();
PathNodeHolder <GridCell> startNodeHolder = nodeGrid[startLayer, startSlice];
PathNodeHolder <GridCell> endNodeHolder = nodeGrid[endLayer, endSlice];
BinaryTree <PathNodeHolder <GridCell> > openCells = new BinaryTree <PathNodeHolder <GridCell> >();
HashSet <PathNodeHolder <GridCell> > closedCells = new HashSet <PathNodeHolder <GridCell> >();
startNodeHolder.gCost = 0;
startNodeHolder.hCost = CalculateDistance(startNodeHolder.node, endNodeHolder.node);
startNodeHolder.CalculateFCost();
openCells.Add(startNodeHolder);
int numberOfLoops = 0;
while (openCells.Count() > 0 && numberOfLoops < 1000)
{
PathNodeHolder <GridCell> currentCell = GetLowestFCostCell(openCells);
if (currentCell.node == endNodeHolder.node)
{
return(CalculatePath(endNodeHolder));
}
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.Selectable != null || neighbor.node.ResourceDeposit != null) ||
neighbor.node.isEdgeCell) &&
!(neighbor == endNodeHolder))
{
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, endNodeHolder.node);
neighbor.CalculateFCost();
if (!openCells.Contains(neighbor))
{
openCells.Add(neighbor);
}
}
}
numberOfLoops++;
}
return(null);
}