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; } } }