/* -*- Mode: C++ -*- *//* MemAction.C * CMUnited99 (soccer client for Robocup99) * Peter Stone <pstone@cs.cmu.edu> * 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) #endifvoid 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 */PlayerInterceptInfoActionInfo::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 */PlayerInterceptInfoActionInfo::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);