route.java

2004年robotcup世界冠军源代码

// Copyright (C) 2002 Takeshi Morimoto <[e-mail]>
// All rights reserved.
package yab.agent;

import java.util.*;
import yab.agent.Util.*;
import yab.agent.object.*;
import MRL.Utilities.Partitioning.Path;


public class Route /* implements MRL.Utilities.Partitioning.Partitionable */ {
  public final MotionlessObject end;
  public final float cost;
  private Route m_previous;
  private boolean changed = true;

//    protected MotionlessObject middle; // Represents A Location Aproximately In The Middle Of The Route ;
  public Route previous() {
    return m_previous;
  }

  /*public*/
  void setPrevious(Route previous) {
    m_previous = previous;
  }

  public Route(MotionlessObject end) {
    this(end, 0, null);
  }

  private Route(MotionlessObject end, float cost, Route previous) {
    this.end = end;
    this.cost = cost;
    m_previous = previous;
  }

  public Route expandedRoute(MotionlessObject next,
                             Router.CostFunction costFunc) { // newExpandedRoute In MRL Uses Null CostFunctions
    return new Route(next, cost + costFunc.cost(end, next), this);
  }

  public boolean equalsRoute(Route target) {

    for (Route rt = this, rtT = target; ; ) {
      if (rt.end != rtT.end)return false;
      rtT = rtT.previous();
      rt = rt.previous();
      if (rt == null) {
        if (rtT != null)return false;
        else return true;
      }
      else if (rtT == null)return false;
    }
  }

  public Route merge(Route start) { // Results A Route Where Starts With start.firstRoute and Ends With this.end
    Route res = changeStartTo(this,start);
    if(res == null)
      throw new Error("Cannot Merge : " + this + " With Start : " + start);
    return res;

    /*
    Route temp;
    Node node, tn, hn;
    Road road;
    MotionlessObject first = null;


    for (temp = this; temp != null; temp = temp.previous()) {
      if (temp.previous() == null || temp.previous().end == this.end)break;
    }
    temp.m_previous = this;

    return target;

    /*
          if(this.end instanceof Node){
            if(first instanceof Node){
              if(first == end){
                temp.setPrevious(this.previous());
                return validateAndSubmit(target);
              }
            }
          }
          else{
            if(first instanceof Node){
              if(((Node)first).getConnectedRoads().contains(end)){
                temp.setPrevious(this);
                return validateAndSubmit(target);
              }
            }
            else {
              road = (Road)first;
              tn = (Node) road.tail();
              hn = (Node)road.head();
              if(tn.getConnectedRoads().contains(end) ){
                Route res = new Route (tn,0f,this );
                temp.setPrevious(res);
                return validateAndSubmit(target);

              }
              else if ( hn.getConnectedRoads().contains(end)){
                Route res = new Route (hn,0f,this );
                temp.setPrevious(res);
                return validateAndSubmit(target);
              }
            }
          }*/
    //throw new Error("Cannot Merge Routes : " + this + " , " + target);
  }

  public void validateAndSubmit() {
    if (!validate()) {
      throw new Error("Bad Merged Route : " + this);
    }
  }

  public int size() {
    int result = 0;
    for (Route rt = this; rt != null; rt = rt.previous())
      result++;
    return result;
  }

  public int[] toIDs() {
    int i = size();
    int[] ids = new int[i];
    for (Route rt = this; rt != null; rt = rt.previous())
      ids[--i] = rt.end.id;
    return ids;
  }

  public String toString() {
    //return Util.classBaseName(this) + "(cost:" + cost + ",end:" +end+")";
    String str = "";
    for (Route rt = this; rt != null; rt = rt.previous())
      str = rt.end + "," + str;
    return str;
  }

  public boolean validate() {
    boolean bPN = false;
    for (Route rt = this; rt != null; rt = rt.previous()) {
      if (rt.end instanceof Node) {
        if (bPN)
          return false;
        else
          bPN = true;
      }
      else bPN = false;
    }
    return true;
  }

  public boolean isContaining(Path path){
    HashSet pathRoads = new HashSet(path);
    MotionlessObject mo;
    Road road;
    for(Route rt = this ; rt != null ; rt = rt.previous()){
      mo = rt.end;
      if(mo instanceof Road){
        road = (Road) mo;
        pathRoads.remove(road);
      }
    }
    boolean res = pathRoads.isEmpty();
    pathRoads = null;
    return res;
  }

  public boolean isContaining(Route route) {
    MotionlessObject mo;
    Collection origs = new HashSet();
    for (Route rt = this; rt != null; rt = rt.previous()) {
      mo = rt.end;
      origs.add(mo);
    }
    for (Route rt = route; rt != null; rt = rt.previous()) {
      mo = rt.end;
      if (!origs.contains(mo)) {
        origs = null;
        return false;
      }
    }
    origs = null;
    return true;
  }

  public boolean isContaining(Collection targets) {
    Collection col = new HashSet(targets);
    for (Route rt = this; rt != null; rt = rt.previous()) {
      col.remove(rt.end);
    }
    boolean res = false;
    res = !col.isEmpty();
    col = null;
    return res;
  }

  public boolean isContaining(MotionlessObject mo) {
    for (Route rt = this; rt != null; rt = rt.previous()) {
      if (rt.end == mo)return true;
    }
    return false;
  }

  public int getContainingCount(Collection targets) {
    int res = 0;
    for (Route rt = this; rt != null; rt = rt.previous()) {
      if (targets.contains(rt.end)) res++;
    }
    return res;

  }

  /*
      private void setMiddle(){
        int distance,middist= (int) (getLength(this) / 2),bestmatch=middist *2;
        Route best= this;
        for (Route rt = this;  rt != null;  rt = rt.previous()){
          distance  = getLength(rt);
          if(Math.abs(distance - middist) < Math.abs(bestmatch - middist)){
            bestmatch = distance;
            best = rt;
          }
        }
        middle = best.end;
        changed=false;
      }

      public int x(){
        if(changed) setMiddle();
        return middle.x();
      }

      public int y(){
        if(changed) setMiddle();
        return middle.y();
      }
   */
  public static int getLength(Route route) {
    int distance = 0;
    Road road;
    for (Route rt = route; rt != null; rt = rt.previous()) {
      if (rt.previous() == null)break;
//      distance += Util.distance(rt.end, rt.previous().end);
      if(! (rt.end instanceof Road )) continue;
      road = (Road) rt.end;
      distance += road.length();
    }
    return distance;
  }

  public static int getRouteCount(Route route) {
    int res = 0;
    for (Route rt = route; rt != null; rt = rt.previous()) {
      res++;
    }
    return res;
  }

  public int getLength() {
    return getLength(this);
  }

  private ArrayList m_col = null;
  public ArrayList getObjects() {
    if (m_col == null) {
      m_col = new ArrayList();
      for (Route rt = this; rt != null; rt = rt.previous()) {
        m_col.add(rt.end);
      }
    }
    return m_col;
  }

  public Route reverse() {
    Route route = null, prev;
    for (Route rt = this; rt != null; rt = rt.previous()) {
      if (rt.previous() == null) {
        route = new Route(rt.end, rt.cost, route);
        break;
      }
      prev = rt.previous();
      route = new Route(rt.end, rt.cost - prev.cost, route);
    }
    return route;
  }

  public Route modifyStartTo(Route route) {
    Route tRoute = Clone();
    tRoute.getFirstRoute().setPrevious(route);
    return tRoute;
  }

  public Route trimStartTo(MotionlessObject pos) { // New Route Contains POS
    Route tRoute = Clone();
    for (Route rt = tRoute; rt != null; rt = rt.previous()) {
      if (rt.end == pos) {
        rt.setPrevious(null);
        break;
      }
    }
    return tRoute;
  }

  public Route trimEndTo(MotionlessObject pos){
    Route tRoute = Clone();
    for (Route rt = tRoute; rt != null; rt = rt.previous()) {
      if (rt.end == pos) {
        return rt;
      }
    }
    return tRoute;
  }


  public Route getFirstRoute() {
    Route rt = this;
    while (rt.previous() != null) rt = rt.previous();
    return rt;
  }

  public int getIndexOf(MotionlessObject mo) {
//    int tC = getRouteCount()-1;
//    for (Route rt = this; rt != null; rt = rt.previous()) {
//      if (rt.end == mo) break;
//      else tC--;
//    }
//    return tC;
    int res = 0;
    for(Route rt = this ; rt != null ; rt = rt.previous())
      if(rt.end == mo ) break;
      else res++;
    return res;
  }

  public MotionlessObject getObject(int indx){
    int size = getRouteCount();
    if(indx >= size) return  null;
    Route tmp = this;
    for(int i = 0 ; i < indx ; i++){
      tmp = tmp.previous();
    }
    return tmp.end;
  }

  public static Route changeStartTo(Route route , Route start){
    if(start.end != route.getFirstRoute().end)     throw new Error ("Cannot Merge : " + route + " With : " + start);
    int sRC = start.getRouteCount() , rRC = route.getRouteCount();
    if(sRC == 1 && rRC == 1 )
      if(start.equalsRoute(route)) return route;
      else return null;
    else if(sRC == 1)  return route;
    else if (rRC ==1 ) {
      return start;
    }
    Route tRoute = route.Clone();
    for(Route rt = tRoute ; rt != null ; rt = rt.previous()){
      if(rt.previous() == null ){
        rt.setPrevious(start.previous());
        return tRoute;
      }
    }
    throw new Error ("WHA!");
  }

  public int getRouteCount(){
    return getRouteCount(this);
  }

  public Route Clone(){
    Route res=null, rev = reverse();
    for(Route rt = rev;rt != null;rt = rt.previous()){
      res = new Route (rt.end,rt.cost,res);
    }
//    if(!equalsRoute(res)) throw new Error("Bad Cloned Route");
//    System.out.println("Cloned : " + res + "\nFrom : " + this);
    return res;
  }
}