permhomog.java

一个多机器人的仿真平台

package Domains.SoccerBots.teams;

/**<pre>
===============================================================================
    File: PermHomoG.java
      By: Gita Sukthankar               ([e-mail])
===============================================================================
</pre>*/

import  EDU.gatech.cc.is.util.Vec2;
import  EDU.gatech.cc.is.abstractrobot.*;


public class PermHomoG extends ControlSystemSS {

    public  boolean BehindBall(Vec2 ball, Vec2 opp_goal) {
      if (ball.x*opp_goal.x > 0) {
        return true;
      } else {
        return false;
      }
    }

    public  boolean hasArrived(Vec2 target) {
      if (target.r < 0.5*abstract_robot.RADIUS) {
        return true;
      } else {
        return false;
      }
    }

    public double distance (Vec2 a, Vec2 b) {
      Vec2 t=new Vec2(a);
      t.sub(b);
      return(t.r);
    }

    public double evaluate(int[] perm_row, Vec2[] all_team, Vec2[] all_pos) {
      double sum=0.0;
      for (int i=0; i < perm_row.length; i++) {
        sum+=distance(all_team[i], all_pos[perm_row[i]]);
      }
      return(sum);
    }
    public int optimize(Vec2[] all_team, Vec2[] all_pos) {
      int strategy=0;
      double least_move=9999.0;

      for (int i=0; i < 120; i++) {
        double buf=evaluate(permutation[i], all_team, all_pos);
        if (buf < least_move) {
          strategy=i;
          least_move=buf;
        }
      }
        return(strategy);
    }

    public void Configure() { }
                
        
    public int TakeStep() {
      Vec2      result = new Vec2(0,0);
      long      curr_time = abstract_robot.getTime();


      //retrieve all all the possible sensor information
      Vec2 ball = abstract_robot.getBall(curr_time);
      Vec2 me = abstract_robot.getPosition(curr_time);

      Vec2 ourgoal = abstract_robot.getOurGoal(curr_time);
      Vec2 theirgoal = abstract_robot.getOpponentsGoal(curr_time);

      Vec2[] teammates = abstract_robot.getTeammates(curr_time);
      Vec2[] opponents = abstract_robot.getOpponents(curr_time);

      //heading is given in radians
      double heading=abstract_robot.getSteerHeading(curr_time);

      //compute useful features in the environment
      Vec2 center=new Vec2(ourgoal);
      center.add(theirgoal);
      center.setr(center.r/2);  //scale it by half
      Vec2 center_disp=new Vec2(ourgoal);
      center_disp.sub(theirgoal);
      center_disp.setr(abstract_robot.RADIUS);
      center.add(center_disp);

      boolean behindBall=BehindBall(ball, theirgoal);

      Vec2 defspot=new Vec2(ball.x, ball.y);
      defspot.add(ourgoal);
      defspot.setr(defspot.r/2);

      //determine most dangeorus opponent (opponent closest to ball)
      Vec2 min = new Vec2(99999,0);
      Vec2 minarg = new Vec2();

      for (int i=0; i< opponents.length; i++) {
        Vec2 buf = new Vec2(opponents[i]);
        buf.sub(ball);
        if (buf.r < min.r) {
          min=buf;
          minarg=opponents[i];
        }
      }
      Vec2 blockspot=new Vec2(ball.x, ball.y);
      blockspot.add(minarg);
      blockspot.setr(blockspot.r/2);

      Vec2 closestteammate = new Vec2(99999,0);
      Vec2 closestopponent = new Vec2(99999,0);
      for (int i=0; i< teammates.length; i++) {
        if (teammates[i].r < closestteammate.r) {
          closestteammate = teammates[i];
        }
      }
      for (int i=0; i< opponents.length; i++) {
        if (opponents[i].r < closestopponent.r) {
          closestopponent = opponents[i];
        }
      }


      // compute a point one robot radius behind ball as best kickspot
      Vec2 kickspot = new Vec2(ball.x, ball.y);
      kickspot.sub(theirgoal); //subtract their goal
      kickspot.setr(abstract_robot.RADIUS);
      kickspot.add(ball);
      Vec2 goalie = new Vec2(ourgoal.x, ball.y);

      Vec2 goalie1 = new Vec2(ourgoal.x,
                      ourgoal.y-abstract_robot.RADIUS);
      Vec2 goalie2 = new Vec2(ourgoal.x,
                      ourgoal.y+abstract_robot.RADIUS);

      Vec2 backspot = new Vec2(ball.x, ball.y);
      backspot.sub(theirgoal);
      backspot.setr(abstract_robot.RADIUS*5);
      backspot.add(ball);

      //get setup for iterating through the permutation array
      Vec2[] all_team=new Vec2[5] ;
      Vec2[] all_pos=new Vec2[5] ;
      all_team[0]=new Vec2(0,0);  //myself
      if (teammates.length==0) {
        return(CSSTAT_OK);
      }
      for (int i=0; i< teammates.length; i++) {
        all_team[i+1]=new Vec2(teammates[i]);
      }
      all_pos[0]=new Vec2(kickspot);
      all_pos[1]=new Vec2(defspot);
      all_pos[2]=new Vec2(blockspot);
      all_pos[3]=new Vec2(goalie);
      all_pos[4]=new Vec2(backspot);
      int strategy=optimize(all_team, all_pos);
      result=all_pos[permutation[strategy][0]];


      //only kick if behind the Ball
      if ((behindBall==true) &&
        abstract_robot.canKick(curr_time)) {
        abstract_robot.kick(curr_time);
        abstract_robot.setDisplayString("good position");
      } else if (abstract_robot.canKick(curr_time)) {
        abstract_robot.setDisplayString("can kick");
      } else {
        abstract_robot.setDisplayString("");
      }

      abstract_robot.setDisplayString("" +  permutation[strategy][0]);


      if (hasArrived(result)) {
        abstract_robot.setSpeed(curr_time, 0.0);
        abstract_robot.setSteerHeading(curr_time, theirgoal.t);
      } else {
        boolean isBlocked=false;
        if ((ball.r < 2*abstract_robot.RADIUS) &&
            (Math.abs(ball.t - result.t) < Math.PI/8)) {
          isBlocked=true;
        }
        for (int i=0; i < teammates.length; i++) {
          if (isBlocked==true) {
            break;
          }
          if ((teammates[i].r < 2*abstract_robot.RADIUS) &&
              (Math.abs(teammates[i].t - result.t) < Math.PI/8)) {
            isBlocked=true;
          }
        }
        for (int i=0; i < opponents.length; i++) {
          if (isBlocked==true) {
            break;
          }
          if ((opponents[i].r < 2*abstract_robot.RADIUS) &&
              (Math.abs(opponents[i].t - result.t) < Math.PI/8)) {
            isBlocked=true;
          }
        }

        double offset= isBlocked ? -1*Math.PI/8: 0.0;
        offset=((strategy%2==0) ? offset : -offset);
        abstract_robot.setSteerHeading(curr_time, result.t+offset);
        abstract_robot.setSpeed(curr_time, 1.0);
      }

      return(CSSTAT_OK);
    }

    public static final int permutation[][] = {
      {0, 1, 2, 3, 4},
      {0, 1, 2, 4, 3},
      {0, 1, 3, 2, 4},
      {0, 1, 4, 2, 3},
      {0, 1, 3, 4, 2},
      {0, 1, 4, 3, 2},
      {0, 2, 1, 3, 4},
      {0, 2, 1, 4, 3},
      {0, 3, 1, 2, 4},
      {0, 4, 1, 2, 3},
      {0, 3, 1, 4, 2},
      {0, 4, 1, 3, 2},
      {0, 2, 3, 1, 4},
      {0, 2, 4, 1, 3},
      {0, 3, 2, 1, 4},
      {0, 4, 2, 1, 3},
      {0, 3, 4, 1, 2},
      {0, 4, 3, 1, 2},
      {0, 2, 3, 4, 1},
      {0, 2, 4, 3, 1},
      {0, 3, 2, 4, 1},
      {0, 4, 2, 3, 1},
      {0, 3, 4, 2, 1},
      {0, 4, 3, 2, 1},
      {1, 0, 2, 3, 4},
      {1, 0, 2, 4, 3},
      {1, 0, 3, 2, 4},
      {1, 0, 4, 2, 3},
      {1, 0, 3, 4, 2},
      {1, 0, 4, 3, 2},
      {2, 0, 1, 3, 4},
      {2, 0, 1, 4, 3},
      {3, 0, 1, 2, 4},
      {4, 0, 1, 2, 3},
      {3, 0, 1, 4, 2},
      {4, 0, 1, 3, 2},
      {2, 0, 3, 1, 4},
      {2, 0, 4, 1, 3},
      {3, 0, 2, 1, 4},
      {4, 0, 2, 1, 3},
      {3, 0, 4, 1, 2},
      {4, 0, 3, 1, 2},
      {2, 0, 3, 4, 1},
      {2, 0, 4, 3, 1},
      {3, 0, 2, 4, 1},
      {4, 0, 2, 3, 1},
      {3, 0, 4, 2, 1},
      {4, 0, 3, 2, 1},
      {1, 2, 0, 3, 4},
      {1, 2, 0, 4, 3},
      {1, 3, 0, 2, 4},
      {1, 4, 0, 2, 3},
      {1, 3, 0, 4, 2},
      {1, 4, 0, 3, 2},
      {2, 1, 0, 3, 4},
      {2, 1, 0, 4, 3},
      {3, 1, 0, 2, 4},
      {4, 1, 0, 2, 3},
      {3, 1, 0, 4, 2},
      {4, 1, 0, 3, 2},
      {2, 3, 0, 1, 4},
      {2, 4, 0, 1, 3},
      {3, 2, 0, 1, 4},
      {4, 2, 0, 1, 3},
      {3, 4, 0, 1, 2},
      {4, 3, 0, 1, 2},
      {2, 3, 0, 4, 1},
      {2, 4, 0, 3, 1},
      {3, 2, 0, 4, 1},
      {4, 2, 0, 3, 1},
      {3, 4, 0, 2, 1},
      {4, 3, 0, 2, 1},
      {1, 2, 3, 0, 4},
      {1, 2, 4, 0, 3},
      {1, 3, 2, 0, 4},
      {1, 4, 2, 0, 3},
      {1, 3, 4, 0, 2},
      {1, 4, 3, 0, 2},
      {2, 1, 3, 0, 4},
      {2, 1, 4, 0, 3},
      {3, 1, 2, 0, 4},
      {4, 1, 2, 0, 3},
      {3, 1, 4, 0, 2},
      {4, 1, 3, 0, 2},
      {2, 3, 1, 0, 4},
      {2, 4, 1, 0, 3},
      {3, 2, 1, 0, 4},
      {4, 2, 1, 0, 3},
      {3, 4, 1, 0, 2},
      {4, 3, 1, 0, 2},
      {2, 3, 4, 0, 1},
      {2, 4, 3, 0, 1},
      {3, 2, 4, 0, 1},
      {4, 2, 3, 0, 1},
      {3, 4, 2, 0, 1},
      {4, 3, 2, 0, 1},
      {1, 2, 3, 4, 0},
      {1, 2, 4, 3, 0},
      {1, 3, 2, 4, 0},
      {1, 4, 2, 3, 0},
      {1, 3, 4, 2, 0},
      {1, 4, 3, 2, 0},
      {2, 1, 3, 4, 0},
      {2, 1, 4, 3, 0},
      {3, 1, 2, 4, 0},
      {4, 1, 2, 3, 0},
      {3, 1, 4, 2, 0},
      {4, 1, 3, 2, 0},
      {2, 3, 1, 4, 0},
      {2, 4, 1, 3, 0},
      {3, 2, 1, 4, 0},
      {4, 2, 1, 3, 0},
      {3, 4, 1, 2, 0},
      {4, 3, 1, 2, 0},
      {2, 3, 4, 1, 0},
      {2, 4, 3, 1, 0},
      {3, 2, 4, 1, 0},
      {4, 2, 3, 1, 0},
      {3, 4, 2, 1, 0},
      {4, 3, 2, 1, 0},
    };
}