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;
}
@Override
public GridCell getNextMove(
GridDomain map, GridCell start, GridCell goal, int stepLeft, long stepTime, long timeLeft) {
try {
Trace.Enable(false);
GridCell nextStep = null;
boolean bReplan =
plan == null
|| // no last path stored, have yet notr planned before?
map.getChangedEdges().size() > 0
|| // map has had changes
!lastGoal.equals(goal)
|| // Goal has changed (equals not implemented?)
!plan.contains(start); // sNode is not in the path (sNode out of track)
if (bReplan) {
plan = generatePlan(map, start, goal);
stepNo = 0;
lastGoal = goal;
}
if (plan == null) {
System.out.println("Could not find goal");
return start;
}
if (stepNo < plan.getLength()) {
nextStep = (GridCell) plan.getStep(stepNo++);
if (nextStep == null) {
Trace.print("next step is null!");
}
}
return nextStep;
} catch (Exception e) {
e.printStackTrace();
return start;
}
}