memaction.c

RoboCup 2D 仿真组冠军源代码之1998年冠军队——CMUnited98源代码

/* -*- Mode: C++ -*- */
#include "MemAction.h"

/* MemAction.C
 * CMUnited98 (soccer client for Robocup98)
 * Peter Stone <[e-mail]>
 * Computer Science Department
 * Carnegie Mellon University
 * Copyright (C) 1998 Peter Stone
 *
 * CMUnited-98 was created by Peter Stone, Manuela Veloso, and Patrick Riley
 *
 * 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.
 */

/* MemAction.C contains utility functions for ball interception
   Created by Patrick Riley
 */


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

void ActionInfo::Initialize()
{

#ifndef RELEASE_VERSION
  MyActionMode = AM_Unknown;
  PullOffside = FALSE;

  kick_in_progress = FALSE;
  goalie_last_catch_time = 0;
  GoalieWarnTime = 0;

  DribbleTargetAvailable = FALSE;
  DribbleTargetTime = -1;

  def_block_overrun_time = -1;
  def_block_overrun = FALSE;
#endif

  Stored_Fastest_Teammate_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;
  }

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

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




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

#ifdef DEBUG_OUTPUT
#define DebugInt(x) 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;
  //see if doing nothing will get us there next time
  if (MyPredictedPosition().dist(vBallPos) < SP_kickable_area) {
    info.res = BI_CanChase;
    info.numCyc = 1;
    info.pos = MyPos();
  }
  //see if a dash will get us there next time
  vNewPos = MyPredictedPosition(1, max_pow);
  if (vNewPos.dist(vBallPos) < SP_kickable_area) {
    info.res = BI_CanChase;
    info.numCyc = 1;
    //this'll make go_to_point dash
    info.pos = MyPos() + Polar2Vector(dash_dist, MyAng());
  }

  vBallPos += vBallVel;
  vBallVel *= SP_ball_decay;
  //now look two cycles ahead
  //try two dashes
  vNewPos = MyPredictedPosition(2, max_pow);
  if (vNewPos.dist(vBallPos) < SP_kickable_area) {
    info.res = BI_CanChase;
    info.numCyc = 2;
    //this'll make go_to_point dash
    info.pos = MyPos() + Polar2Vector(2*dash_dist, MyAng()); 
  }
  //try turn then dash
  float targ_ang = (vBallPos - MyPredictedPosition(2)).dir();
  if (fabs(targ_ang - MyAng()) > CP_max_go_to_point_angle_err) {
    vNewPos = MyPredictedPositionWithTurn(targ_ang - MyAng(), 2, max_pow);
    if (vNewPos.dist(vBallPos) < SP_kickable_area) {
      info.res = BI_CanChase;
      info.numCyc = 2;
      //this'll make go_to_point turn
      info.pos = MyPos() + Polar2Vector(dash_dist, targ_ang); 
    }
  }
  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 vPlayerAng, int PlayerAngValid,
			   bool IsThisMe)
{
  float at_point_buffer = 1;
  PlayerInterceptInfo info;
  Vector vPredPlayer = vPlayerPos + vPlayerVel;
  Vector vOldBallPos, vOldBallVel;
  float turn_angle;
  int cyc;
  int cyc_inc = CP_intercept_step;
  int max_cyc = (max_lookahead + CP_intercept_step - 1); 
  max_cyc -= (max_cyc % CP_intercept_step);
  /* 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() ));

  NormalizeAngleDeg(&vPlayerAng);
  
  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;
    /* it might be a good idea to aim one cycle ahead of the ball
       but this works pretty well */
    //DebugInt(printf(" angle to exp ball pos: %f\n", AngleTo(vBallPos)));
    turn_angle = ball_ang - vPlayerAng;
    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,
	 add 1 to our cycle count (for a turn in the middle)
	 and return sucess */
      if (dist_to_ball_after <= at_point_buffer)
	info.numCyc = cyc;
      else {	
	info.numCyc = cyc+1;
	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 */
	DebugInt(printf("Found answer, but going back for preciseness: %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 (PlayerFaceValid(PlayerSide, PlayerNum)) {
    AngValid = TRUE;
    PlayerAng = PlayerAbsoluteFace(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);
  
  DebugInt(printf("At BallIntercept_active  max_pow: %f, max_look: %d\n",
		  max_pow_to_use, max_lookahead));

  if (PlayerSide == MySide && PlayerNum == MyNumber)
    *pInfo = CloseBallInterception(max_pow_to_use, max_lookahead,
				   BallAbsolutePosition(), BallVel);
  
  if (pInfo->res == BI_None)
    *pInfo = 
      ActiveCanGetThere(max_pow_to_use, max_lookahead,
			BallAbsolutePosition(), BallVel,
			PlayerSide, PlayerNum,
			PlayerPos, PlayerVel, PlayerAng, AngValid,
			(PlayerSide == MySide && PlayerNum == MyNumber));
  else
    ;//{ printf("%d:%d Used Close Ball intercept\n",MyNumber,CurrentTime.t);}
} 

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

PlayerInterceptInfo* ActionInfo::GetPlayerIntInfo(char side, Unum num)
{
  if (side == MySide)
    return TeamIntInfo[num];
  else if (side == TheirSide)
    return OppIntInfo[num];
  else
    my_error("bad side passed to GetPlayerIntInfo");
  return NULL;
}

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

PlayerInterceptInfo* ActionInfo::VerifyIntInfo(char side, Unum num, float dash_pow)
{
  PlayerInterceptInfo* pInfo = GetPlayerIntInfo(side, num);
  /*SNORKprintf("pInfo1: %p\n", pInfo);*/
  if (pInfo == NULL) {
    my_error("Bad side or number passed to VerifyIntInfo");
    return NULL;
  }

  int lookahead;
  switch (InterceptLookahead) {
  case LA_Default: lookahead = CP_max_int_lookahead; break;
  case LA_BestSoFar:
    lookahead =
      (IntMinCycTime == CurrentTime) ? (IntMinCyc) : CP_max_int_lookahead;
    break;
  default: lookahead = InterceptLookahead; break;
    break;
  }
  if ( pInfo->time != CurrentTime || (pInfo->dash_pow-dash_pow)>FLOAT_EPS ||
       (pInfo->lookahead < lookahead && !IsSuccessRes(pInfo->res)) ) {
    /* set the info struct */
    DebugInt(printf("%d %d Data not current. Calling interception code\n", MyNumber, num));

    if (pInfo->time == CurrentTime && (pInfo->dash_pow-dash_pow)<=FLOAT_EPS)
      my_error("Recomputing %c %d because lookahead got bigger; old: %d\tnew: %d",
	       side,num,pInfo->lookahead, lookahead);
    
    /* let's do a real quick estimate to see if the player can make it there
       if player dist to ball > max ball dist will travel + max_dist we'll
       travel, then there's no way to get there */
    if (!PlayerPositionValid(side, num)) {      
      my_error("VerifyIntInfo: Can't give an answer if I don't know where player is");
      pInfo->res = BI_Invalid;
      return pInfo;
    }
    DebugInt(printf("Lookahead: %d\n", lookahead));
    float ball_travel = SumGeomSeries((BallVelocityValid() ? BallSpeed() : 0),
				      SP_ball_decay, lookahead);
    float player_travel = SP_player_speed_max * lookahead;
    float play_ball_dist = (PlayerAbsolutePosition(side, num) -
			    BallAbsolutePosition()).mod() ;
    if (play_ball_dist > player_travel + ball_travel) {
      pInfo->time = CurrentTime;
      pInfo->dash_pow = dash_pow;
      pInfo->lookahead = lookahead;
      pInfo->res = BI_Failure;
      DebugInt(printf("Interception: %d, %d Took shortcut to decide failure\n", MyNumber, num));
    } else {
      DebugInt(printf("Interception: %d, %d About to do actual calculation\n", MyNumber, num));
      BallIntercept_active( dash_pow, lookahead, side, num, pInfo);
      if (IsSuccessRes(pInfo->res))
	SetIntMinCyc(pInfo->numCyc);
      pInfo->time = CurrentTime;
      pInfo->dash_pow = dash_pow;
      pInfo->lookahead = lookahead;
    }    
  }
  else if ( IsSuccessRes(pInfo->res) )
    SetIntMinCyc(pInfo->numCyc);

  return pInfo;
}

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


InterceptRes ActionInfo::PlayerInterceptionResult(char side, Unum num,
				      float dash_pow)
{
  return (VerifyIntInfo(side, num, dash_pow))->res;
}

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

Bool ActionInfo::PlayerInterceptionAble(char side, Unum num, float dash_pow)
{
  return IsSuccessRes((VerifyIntInfo(side, num, dash_pow))->res) ? TRUE : FALSE;
}

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

int ActionInfo::PlayerInterceptionNumberCycles(char side, Unum num,
				   float dash_pow)
{
  PlayerInterceptInfo* pInfo = VerifyIntInfo(side, num, dash_pow);
  if (!IsSuccessRes(pInfo->res))
    my_error("Trying to get numer of cycles on invalid result");
  return pInfo->numCyc;
}

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

Vector ActionInfo::PlayerInterceptionPoint(char side, Unum num,
			       float dash_pow)
{
  PlayerInterceptInfo* pInfo = VerifyIntInfo(side, num, dash_pow);
  if (!IsSuccessRes(pInfo->res))
    my_error("Trying to get interception point on invalid result: %d", pInfo->res);
  return pInfo->pos;  
}

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