memaction.c

卡内基梅隆大学99年机器人足球世界杯2D仿真组源代码。卡内基梅隆大学在人工智能界的巨牛

/* -*- Mode: C++ -*- */

/* MemAction.C
 * CMUnited99 (soccer client for Robocup99)
 * Peter Stone <[e-mail]>
 * Computer Science Department
 * Carnegie Mellon University
 * Copyright (C) 1999 Peter Stone
 *
 * CMUnited-99 was created by Peter Stone, Patrick Riley, and Manuela Veloso
 *
 * You may copy and distribute this program freely as long as you retain this notice.
 * If you make any changes or have any comments we would appreciate a message.
 * For more information, please see http://www.cs.cmu.edu/~robosoccer/
 */

#include "MemAction.h"

#ifdef DEBUG_OUTPUT
#define DebugClear(x) 
#else
#define DebugClear(x) 
#endif

void ActionInfo::Initialize()
{

  Stored_Fastest_Teammate_Time = 0;
  Stored_Fastest_Opponent_Time = 0;

  for (int i = 1; i <= SP_team_size; i++) {
    TeamIntInfo[i] = new PlayerInterceptInfo;
    TeamIntInfo[i]->time = -1;
    OppIntInfo[i] = new PlayerInterceptInfo;
    OppIntInfo[i]->time = -1;
  }

  kick_in_progress = FALSE;

  InterceptLookahead = LA_Default;
  IntMinCyc = -1;
  IntMinCycTime = -1;

  HKTime = -1;
  HKStep = -1;
  HKStepNext = -1;
  HKrot = TURN_CW;

}




/*****************************************************************************************/
/*****************************************************************************************/
/*****************************************************************************************/





/*****************************************************************************************/
/*****************************************************************************************/
/*****************************************************************************************/

#ifdef DEBUG_OUTPUT
#define DebugInt(x) 
#else
#define DebugInt(x) 
#endif

/* only used for this player */
PlayerInterceptInfo
ActionInfo::CloseBallInterception(float max_pow, int max_lookahead,
				  Vector vBallPos, Vector vBallVel)
{
  Vector vNewPos;
  PlayerInterceptInfo info;
  float dash_dist = max_pow * SP_dash_power_rate;
  info.dash_pow = max_pow;
  info.lookahead = max_lookahead;
  info.res = BI_None;  
  
  vBallPos += vBallVel;
  vBallVel *= SP_ball_decay;
  /* we need to figure out the right dash power so that the ball ends up right in
     front of us. Like many things, this ends up as a LineCircleIntersect problem */
  Vector vMyPred = MyPredictedPosition();
  Ray    rDash(vMyPred, MyBodyAng());
  Line   lDash   = LineFromRay(rDash);
  Vector sol1, sol2;
  
  int num_sol = LineCircleIntersect(lDash, SP_player_size + SP_ball_size + CP_collision_buffer,
				    vBallPos, &sol1, &sol2);
  if (num_sol >= 1) {
    /* we'll make sure that the right answer is in sol1 */
    if (num_sol == 2) {
      /* we have to pick which point is better */
      if (fabs(GetNormalizeAngleDeg((vBallPos - sol2).dir() - MyBodyAng())) < 90) {
	sol1 = sol2; //sol2 is the right solution, put it in sol1
      } else if (!(fabs(GetNormalizeAngleDeg((vBallPos-sol1).dir() - MyBodyAng())) < 90)) {
	my_error("CloseBallInterception: 1 ahead, neither solution looks good %.2f %.2f",
		 GetNormalizeAngleDeg((vBallPos - sol2).dir() - MyBodyAng()),
		 GetNormalizeAngleDeg((vBallPos - sol1).dir() - MyBodyAng()));
      }
    }

    /* now figure out the dash power based on that point */
    float dash_pow = vMyPred.dist(sol1) / SP_dash_power_rate;
    dash_pow = MinMax(SP_min_power, dash_pow, SP_max_power);
    if (!rDash.InRightDir(sol1))
      dash_pow = -dash_pow;

    if (vBallPos.dist(MyPredictedPosition(1, dash_pow)) < SP_kickable_area) {
      /* this works! */
      info.res = BI_CanChase;
      info.numCyc = 1;
      info.dash_pow_to_use = dash_pow;
      //this'll make go_to_point dash
      info.pos = vMyPred + Polar2Vector(signf(dash_pow)*dash_dist, MyBodyAng());
      LogAction5(70, "CloseBallInterception: One dash and we're there: %.2f to (%.2f, %.2f)",
		 dash_pow, sol1.x, sol1.y);
      return info;
    }
    
    
  }
  
  vBallPos += vBallVel;
  vBallVel *= SP_ball_decay;
  //now look two cycles ahead
  //try turn then dash
  float targ_ang = (vBallPos - MyPredictedPosition(2)).dir();
  if (fabs(targ_ang - MyBodyAng()) > CP_max_go_to_point_angle_err) {
    vNewPos = MyPredictedPositionWithTurn(targ_ang - MyBodyAng(), 2, max_pow);
    if (vNewPos.dist(vBallPos) < SP_kickable_area) {
      info.res = BI_CanChase;
      info.numCyc = 2;
      info.dash_pow_to_use = max_pow;
      //this'll make go_to_point turn
      //info.pos = MyPos() + Polar2Vector(dash_dist, targ_ang); 
      info.pos = MyPredictedPosition() + Polar2Vector(dash_dist, targ_ang); 
      LogAction2(70, "CloseBallInterception: Turn then dash and we're there");
      return info;
    }
  }
  //try two dashes
  vNewPos = MyPredictedPosition(2, max_pow);
  if (vNewPos.dist(vBallPos) < SP_kickable_area) {
    info.res = BI_CanChase;
    info.numCyc = 2;
    info.dash_pow_to_use = max_pow;
    //this'll make go_to_point dash
    //info.pos = MyPos() + Polar2Vector(2*dash_dist, MyBodyAng()); 
    info.pos = MyPredictedPosition() + Polar2Vector(dash_dist, MyBodyAng());
    LogAction2(70, "CloseBallInterception: Two dashes and we're there");
    return info;
  }
  return info;
}

/*****************************************************************************************/

/* does not set time field */
/* cyc inc is initially CP_intercept_step, but when an answer is found, we
   then bring cyc back a step, and go up by ones */
PlayerInterceptInfo
ActionInfo::ActiveCanGetThere(float max_pow, int max_lookahead,
			   Vector vBallPos, Vector vBallVel,
			   char side, Unum num,
			   Vector vPlayerPos, Vector vPlayerVel,
			   float fPlayerAng, int PlayerAngValid,
			   bool IsThisMe)
{
  float at_point_buffer = 1;
  PlayerInterceptInfo info;
  //Vector vPredPlayer = vPlayerPos + vPlayerVel;
  Vector vPredPlayer = vPlayerPos +
    vPlayerVel * (SumInfGeomSeries(vPlayerVel.mod(), SP_player_decay));
  Vector vOldBallPos, vOldBallVel;
  float turn_angle;
  int cyc;
  int cyc_inc = (IsThisMe ? CP_my_intercept_step : CP_intercept_step); 
  int max_cyc = (max_lookahead + cyc_inc - 1);  
  max_cyc -= (max_cyc % cyc_inc); 
  /* max_cyc is so that we don't miss an interception if CP_intercept_step is not
     1. For example, if CP_intercept_step is 5, max_look is 14, and we can
     intercept in 13, we would return no intercept if we just used max_lookahead */
  
  DebugInt(printf(" ACGT: BallPos.vel.mod: %f\n", vBallVel.mod() ));

  info.dash_pow_to_use = max_pow;
  NormalizeAngleDeg(&fPlayerAng);

  /* we want to aim a little ahead of the ball, so advance it a little */
  for (int i=0; i < CP_intercept_aim_ahead; i++) {
    vBallPos += vBallVel;
    vBallVel *= SP_ball_decay;
  }

  if (IsThisMe) LogAction4(140, "ActiveBallIntercept: %d %d", (int)max_pow, max_lookahead);
  
  for (cyc=0; cyc<=max_cyc; cyc += cyc_inc) {

    if (!IsPointInBounds(vBallPos,-3)) {  /* expand the field by 3 meters so we don't give up to soon */
      DebugInt(printf("The ball will go out of bounds before we can get it\n"));
      break;
    }
    
    /* decide if we need to turn to ball */
    float ball_ang = (vBallPos - vPredPlayer).dir();
    Vector vEndSpot;
    /* SMURF - we should probably aim for ball 1 cycle ahead or something
       like that */
    //DebugInt(printf(" angle to exp ball pos: %f\n", AngleTo(vBallPos)));
    turn_angle = ball_ang - fPlayerAng;
    if (fabs(turn_angle) < CP_max_go_to_point_angle_err)
      turn_angle = 0.0;      
    if (IsThisMe) {
      vEndSpot = MyPredictedPositionWithTurn(turn_angle, cyc, max_pow,(turn_angle != 0.0));
    } else {
      int run_cyc = cyc;
      if (PlayerAngValid) {
	if (turn_angle != 0.0)
	  run_cyc--;
	run_cyc = Max(0, run_cyc);
      }
      Vector PlayerDash =
	Polar2Vector(max_pow*SP_dash_power_rate, ball_ang);
      vEndSpot =
	PlayerPredictedPosition(side, num, run_cyc, PlayerDash);
    }
    

    float dist_to_ball_after = (vBallPos - vEndSpot).mod();
    /* if we can make it there */
    /* SMURF- is this too lenient? */
    if (dist_to_ball_after <= at_point_buffer || 
        (vEndSpot - vPredPlayer).mod() > (vBallPos - vPredPlayer).mod() + SP_kickable_area) {
      /* we can get to the ball! */
      /* OR we travelled far enough, but somehow missed the ball,
	 return sucess */
      if (dist_to_ball_after <= at_point_buffer) {
	if (IsThisMe) LogAction4(100, "Found a ball interception by being close (%.2f, %.2f)",
				vBallPos.x, vBallPos.y);
	info.numCyc = cyc;
      } else {	
	if (IsThisMe) LogAction4(100, "Found a ball interception by going far (%.2f, %.2f)",
				vBallPos.x, vBallPos.y);
	info.numCyc = cyc;
	//vBallPos += vBallVel; /* advance one spot for that turn*/
      }      
      
      if (cyc_inc > 1 && cyc != 0) {
	/* we want the best answer- go back and go up by ones */
	if (IsThisMe)
	  LogAction2(100, "Found a ball interception, but goign back for accuracy");	
	DebugInt(printf("Found answer, but going back for accuracy: %d\n", cyc));
	cyc -= cyc_inc;
	vBallPos = vOldBallPos;
	vBallVel = vOldBallVel;
	cyc_inc = 1;
	max_cyc = max_lookahead; // don;t need to go above this anymore
      } else {

	if (info.numCyc > max_lookahead) {
	  info.res = BI_Failure;
	} else {
	  info.res = BI_CanChase;
	  info.pos = vBallPos;
	}	
	return info;
      }      
    }
    
    /* update ball position estimate */
    vOldBallPos = vBallPos;
    vOldBallVel = vBallVel;
    for (int i=0; i < cyc_inc; i++) {
      vBallPos += vBallVel;
      vBallVel *= SP_ball_decay;
    }
      
    
  } /* cycle loop */

  info.res = BI_Failure; // can't make it to ball before max_lookahead
  return info;
}

/*****************************************************************************************/

void ActionInfo::BallIntercept_active(float max_pow_to_use, int max_lookahead,
				   char PlayerSide, Unum PlayerNum,
				   PlayerInterceptInfo* pInfo)
{
  Vector PlayerPos;
  Vector PlayerVel;
  float PlayerAng;
  int AngValid = FALSE;
  Vector BallVel;

  pInfo->res = BI_None;

  if (!BallPositionValid()) {
    my_error("BallIntercept_active: Can't get to ball if I don;t know where it is");
    pInfo->res = BI_Invalid;
    return;
  }
  
  if (!PlayerPositionValid(PlayerSide, PlayerNum)) {
    my_error("BallIntercept_active: Can't give an answer if I don't know where player is");
    pInfo->res = BI_Invalid;
    return;
  }
  PlayerPos = PlayerAbsolutePosition(PlayerSide, PlayerNum);
  //DebugInt(cout << "PlayerPos: " << PlayerPos << endl);
  
  if (PlayerVelocityValid(PlayerSide, PlayerNum)) {
    PlayerVel = PlayerAbsoluteVelocity(PlayerSide, PlayerNum);
  } else {    
    PlayerVel = Vector(0,0);
  }
  
  if (PlayerBodyAngleValid(PlayerSide, PlayerNum)) {
    AngValid = TRUE;
    PlayerAng = PlayerAbsoluteBodyAngle(PlayerSide, PlayerNum);
  } else
    PlayerAng = 0;
    
  if ((PlayerPos - BallAbsolutePosition()).mod() <
      SP_kickable_area) {
    pInfo->res = BI_ReadyToKick;
    pInfo->numCyc = 0;
    pInfo->pos = PlayerPos;
    return;
  }

  if (BallVelocityValid())
    BallVel = BallAbsoluteVelocity();
  else
    BallVel = Vector(0,0);