public void findPath(GridCell startCell, GridCell goalCell, GridCell[,,] grid)
{
this.grid = grid;
gridWidth = grid.GetLength(0);
gridHeight = grid.GetLength(1);
gridDepth = grid.GetLength(2);
start = new Node(startCell.getCell()[0], startCell.getCell()[1], startCell.getCell()[2], 0, 0, 0, null, startCell);
end = new Node(goalCell.getCell()[0], goalCell.getCell()[1], goalCell.getCell()[2], 0, 0, 0, null, goalCell);
openList.Add(start);
bool keepSearching = true;
bool pathExists = true;
while (keepSearching && pathExists) {
Node curNode = getBestFromOpenList();
if (curNode == null) {
pathExists = false;
break;
}
closeList.Add(curNode);
if (nodeIsGoal(curNode)) {
keepSearching = false;
} else {
findValidNeighbors(curNode);
foreach (Node n in neighbors) {
if (findInCloseList(n) != null) {
continue;
}
Node inOpenList = findInOpenList(n);
if (inOpenList == null) {
openList.Add(n);
} else {
if (n.G < inOpenList.G) {
inOpenList.G = n.G;
inOpenList.F = inOpenList.G + inOpenList.H;
inOpenList.parent = curNode;
}
}
}
}
}
if (pathExists) {
Node n = findInCloseList(end);
while (n != null) {
finalPath.Add(n);
n = n.parent;
}
}
}
