示例#1
0
  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;
  }
示例#2
0
  @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;
    }
  }