void CreatePath(Cell origin, Cell destination)
{
List <PathStep <Cell> > openSteps = new List <PathStep <Cell> >();
HashSet <PathStep <Cell> > closedSteps = new HashSet <PathStep <Cell> >();
openSteps.Add(new PathStep <Cell>(origin));
do
{
// Pop the next step in openSteps
PathStep <Cell> curStep = openSteps.First();
Cell curCell = curStep.data;
openSteps.RemoveAt(0);
closedSteps.Add(curStep);
// Destination reached, carve the path.
if (curStep.data.Equals(destination))
{
do
{
if (curStep.parent != null)
{
curCell = curStep.data;
curCell.Empty();
}
curStep = curStep.parent;
} while (curStep != null);
return;
}
List <Cell> adjacentCells = GetAdjacentCells(curCell);
// Perform A* search to find shortest path.
foreach (Cell adjCell in adjacentCells)
{
PathStep <Cell> step = new PathStep <Cell>(adjCell);
Cell stepCell = step.data;
// Only proceed if the nextStep is not already in the closedSteps.
if (!closedSteps.Contains(step))
{
float moveCost = GetCostForStep(curCell, stepCell);
step.parent = curStep;
step.gCost = curStep.gCost + moveCost;
// Check if the nextStep is already in the openSteps
int stepIndex = openSteps.IndexOf(step);
// nextStep is not in openSteps, so add it.
if (stepIndex == -1)
{
step.hCost = GetCostForStep(stepCell, destination);
InsertPathStep(step, openSteps);
}
// nextStep is already in openSteps, so update its score.
else
{
// If the current path gives nextStep a better gScore than its existing
// gScore, remove the existing and insert step.
if (curStep.gCost + moveCost < openSteps[stepIndex].gCost)
{
step.hCost = openSteps[stepIndex].hCost;
openSteps.RemoveAt(stepIndex);
InsertPathStep(step, openSteps);
}
}
}
}
} while (openSteps.Count > 0);
}
