private ComputedPlan generatePlan(GridDomain map, GridCell start, GridCell goal) {
mapinfo = new MapInfo(map);
// initialize open set with start node
mapinfo.add(start, 0f, map.hCost(start, goal));
// repeat while states are left in open set
while (mapinfo.isOpenEmpty() == false) {
GridCell current = mapinfo.closeCheapestOpen();
// timer.getCurrentNanotime();
// timer.getCurrentNanotime();
// timer.getCurrentNanotime();
Trace.print(current);
// if goal has been reached, return path
if (current == goal) {
Trace.print("found goal!");
return mapinfo.computePlan(goal);
}
// iterate through neighboring nodes
for (State neighbor : map.getSuccessors(current)) {
// threadMX.getCurrentThreadCpuTime();
// threadMX.getCurrentThreadCpuTime();
// threadMX.getCurrentThreadCpuTime();
// consider node if it can be entered and is not in closed list
if (map.isBlocked(neighbor) == false && mapinfo.isClosed((GridCell) neighbor) == false) {
// get g cost of neighbor
float gCost = mapinfo.getGCost(current) + map.cost(current, neighbor);
if (mapinfo.isOpen((GridCell) neighbor) == false) {
// node not previously encountered, add it to the open set
mapinfo.add((GridCell) neighbor, gCost, map.hCost(neighbor, goal), current);
} else if (gCost < mapinfo.getGCost((GridCell) neighbor)) {
// more direct route to node found, update it
// NOTE: this can never happen with an admissible heuristic!
// System.out.println("failing now..." + gCost + " < " + mapinfo.getGCost((GridCell)
// neighbor));
// mapInfo.setGCost(neighbor, gCost);
// mapInfo.setParent(neighbor, current);
}
}
}
}
// no goal found
return null;
}
