if (!Mem->BallVelocityValid()) {    DebugKick(printf("In dokick with velocity not valid. Assuming it's 0."));    Mem->LogAction2(130, "dokick: assuming vel 0");  }    if (Mem->BallVelocityValid() &&      Mem->BallAbsoluteVelocity() != 0) { /* correct for ball velocity */    Vector tmp = Polar2Vector(v0, ddir) - Mem->BallRelativeToBodyVelocity();    kick_dir = tmp.dir();    power = tmp.mod() / Mem->BallKickRate();    DebugKick(printf(" Correcting for ball velocity# vel.x: %f\tvel.y:%f\n",	  Mem->BallRelativeVelocity().x, Mem->BallRelativeVelocity().y));    DebugKick(printf(" New stuff# power: %f\t ddir:%f\n", power, kick_dir));    Mem->LogAction4(130, "dokick: vel corr: %f at %f", power, kick_dir);  } else {    Mem->LogAction2(130, "dokick: vel is 0");    power = v0  / Mem->BallKickRate();  }    if (power > Mem->SP_max_power) {    DebugKick(printf("Trying to kick over SP_max_power! Correcting...\n"));    //Mem->LogAction2(130, "dokick: trying to kick too hard, correcting");    if (!Mem->BallVelocityValid() || Mem->BallAbsoluteVelocity() == 0) {      power = Mem->SP_max_power;      Mem->LogAction4(130, "dokick: max_pow corr: (stopped ball) %f at %f",		      power, kick_dir);    } else if (ddist == 0) {      /* this is probably a stop_ball kick, but ddir is meaningless for	 the RayCircleIntersection below.	 Therefore, we just kick against the ball's velocity */      kick_dir = GetNormalizeAngleDeg(Mem->BallRelativeToBodyHeading() + 180);      power = Mem->SP_max_power;      Mem->LogAction4(130, "dokick: max_pow corr: (ddist = 0) %f at %f",		      power, kick_dir);    } else {      /* this is a ray circle intersection problem         We want to kick in the right direction, but not as hard as desired */      Vector sol1, sol2;      int numSol;      Vector vNewTraj;      numSol =	RayCircleIntersect(Ray(Mem->BallRelativeToBodyPosition(), ddir),			   Mem->SP_max_power * Mem->BallKickRate(),			   Mem->Global2RelativeToMyBody(Mem->BallPredictedPosition()),			   &sol1, &sol2);      /* we want the solution that's furthest along the ray - that's sol2	 if there are two solution */      if (numSol == 0) {	/* we can't kick ball to desired trajectory, so let's just slow it	   down by kicking directly against velocity	   It might be better to return an error and have the caller decide	   what to do, but this works pretty well */	DebugKick(printf("Can't kick ball to right trajectory!\n"));	power = Mem->SP_max_power;	kick_dir = Mem->BallRelativeToBodyHeading() + 180;	if (pCanKickToTraj)	  *pCanKickToTraj = FALSE;	Mem->LogAction4(130, "dokick: max_pow corr: can't get to trajectory! %.2f at %.2f",			power, kick_dir);      } else {	if (numSol == 1) {	  vNewTraj = sol1;	} else if (numSol == 2) {	  /* we want the solution closer to the target point we wanted */	  Vector targ = Mem->BallRelativeToBodyPosition() + Polar2Vector(v0, ddir);	  Mem->LogAction8(130, "dokick: 2 solutions (%.2f, %.2f) (%.2f, %.2f) targ (%.2f, %.2f)",			 sol1.x, sol1.y, sol2.x, sol2.y, targ.x, targ.y);	  if (sol1.dist2(targ) < sol2.dist(targ)) {	    Mem->LogAction4(140, "Picked sol1: %.2f < %.2f",			    sol1.dist2(targ), sol2.dist2(targ));	    vNewTraj = sol1;	  } else {	    Mem->LogAction4(140, "Picked sol2: %.2f > %.2f",			    sol1.dist2(targ), sol2.dist2(targ));	    	    vNewTraj = sol2;	  }	  	} else {	  my_error("dokick: How many solutions to RayCircleIntersection? %d", numSol);	}		Vector vNewKick = vNewTraj -	  Mem->Global2RelativeToMyBody(Mem->BallPredictedPosition());	power = vNewKick.mod() / Mem->BallKickRate();	kick_dir = vNewKick.dir();	DebugKick(printf(" Correcting for ball velocity AND max power# vel.x: %f\tvel.y:%f\n",	       Mem->BallRelativeVelocity().x, Mem->BallRelativeVelocity().y));	DebugKick(printf("New stuff# power: %f\t dir:%f\n", power, kick_dir));	Mem->LogAction4(130, "dokick: max_pow corr: %f at %f", power, kick_dir);      }    }      }   power = Min(Round(power, -2), Mem->SP_max_power);  kick_dir = Round(kick_dir, -2);  NormalizeAngleDeg(&kick_dir);  DebugKick(printf("kicking with power: %f at dir: %f\n", power, kick_dir));  com.time = Mem->CurrentTime;  com.power = power;  com.angle = kick_dir;  return com;}/* all the turnball reasoning is done in relative (to body) coordinates   call this before calling dokick to correct for movement of the player */void PlayerMovementCorrection(AngleDeg* pdir, float* pdist){  Mem->LogAction4(110, "Player movement correction: before dist: %f at dir: %f",		 *pdist, *pdir);  DebugKick(printf("Before corr- dir: %f\t dist: %f\n", *pdir, *pdist));  Vector vRelBallTarg = Mem->BallRelativeToBodyPosition() +    Polar2Vector(*pdist, *pdir);  Vector vNewRelTarg = Mem->MyPredictedPosition() +    vRelBallTarg - Mem->MyPos();  Vector vNewRelTraj = vNewRelTarg - Mem->BallRelativeToBodyPosition();  *pdir  = vNewRelTraj.dir();  *pdist = vNewRelTraj.mod();  DebugKick(printf("After corr- dir: %f\t dist: %f\n", *pdir, *pdist));  Mem->LogAction4(110, "Player movement correction: after dist: %f at dir: %f",		 *pdist, *pdir);}/* kick ball so that it is at angle ddir and dist EndDist   If you have to kick around the player, kick rotway(clockwise or counter-)   *//* we always reason about the right trajectory for the ball leave velocity   correction for dokick */KickToRes turnball_kick(AngleDeg target_dir, TurnDir rotate, 		  Bool StopBall, TurnKickCommand* pCom,		  float EndDist, float closeMarg, float kickFac){  float dir;  float dist;  Vector btraj;  pCom->time = -1;  pCom->turn_neck = FALSE;    DebugKick(printf("\nat turnball_kick: target_dir: %f\n", target_dir));  Mem->LogAction4(60, "Turnball_kick: targ_dir: %.1f  dir: %d", target_dir, (int)rotate);    NormalizeAngleDeg(&target_dir);    //DebugKick(printf("HERE Time: %d\n", Mem->CurrentTime.t));  /* the pos valid is not really right - if we are turning the ball and didn't     actually see it last time, then there's a problem */  if ( !Mem->BallPositionValid() || !Mem->BallKickable() ) {    Mem->LogAction2(90, "turnball_kick: lost the ball");    return KT_LostBall;  }  /* if the velocity isn's valid, turn to face ball */  if ( !Mem->BallVelocityValid() ) {    float ball_ang_from_body = Mem->BallAngleFromBody(); /* for efficiency */    Mem->LogAction2(90, "turnball_kick: vel is not valid, looking at it");    DebugKick(printf("turning to face ball\n"));    if (Mem->CanSeeBallWithNeck()) {      pCom->time = Mem->CurrentTime;      pCom->type = CMD_kick;      pCom->angle = ball_ang_from_body + 180;      pCom->power = Mem->CP_stop_ball_power;      pCom->turn_neck = TRUE;      pCom->turn_neck_angle = Mem->LimitTurnNeckAngle(Mem->BallAngleFromNeck());    } else {      /* turn body to face ball, and turn neck to straight ahead */      pCom->time = Mem->CurrentTime;      pCom->type = CMD_turn;	pCom->turn_neck = TRUE;      if (fabs(ball_ang_from_body) > Mem->MaxEffectiveTurn()) {	/* out body can't get to where we want to go */	pCom->angle = 180; /* get our maximum effective turn */	pCom->turn_neck_angle = ball_ang_from_body -	  signf(ball_ang_from_body)*Mem->MaxEffectiveTurn();      } else {	pCom->angle = ball_ang_from_body;	pCom->turn_neck_angle = -Mem->MyNeckRelAng();      }          }        return KT_TurnedToBall;  }     DebugKick(printf(" ball.dist: %f\t.dir: %f\n",	 Mem->BallDistance(), Mem->BallAngle()));  DebugKick(printf(" HERE ball.vel.x: %f\t.y: %f\tmod: %f\n",	 Mem->BallRelativeVelocity().x, Mem->BallRelativeVelocity().y,	 Mem->BallSpeed()));  DebugKick(printf(" ball.rpos.x: %f\t.y: %f\n",	 Mem->BallRelativePosition().x, Mem->BallRelativePosition().y));  DebugKick(printf(" target_dir: %f\n", target_dir));  if ( fabs(GetNormalizeAngleDeg(target_dir - Mem->BallAngleFromBody())) < Mem->CP_KickTo_err) {    /* Do something to indicate we are done */    if (!StopBall || Mem->BallSpeed() < Mem->CP_max_ignore_vel)      return KT_DidNothing;    Mem->LogAction2(90, "turnball_kick: we're there, stopping the ball");    DebugKick(printf("  Stop ball kick\n"));    dir = 0;    dist = 0;        PlayerMovementCorrection(&dir, &dist);    *pCom = dokick(dir, dist, 1.0);    pCom->turn_neck = FALSE;    return KT_Success;  }  if (rotate == TURN_AVOID) {    rotate = RotToAvoidOpponent(target_dir + Mem->MyBodyAng());  }    if (rotate == TURN_CLOSEST) {    rotate = RotClosest(target_dir + Mem->MyBodyAng());  }      if (is_straight_kick(target_dir, EndDist, closeMarg)) {    float pow;          btraj = Polar2Vector(EndDist, target_dir) - Mem->BallRelativeToBodyPosition();    dir = btraj.dir();    dist = btraj.mod();        /* now we're goign to do some distance twiddling to get the ball to       get to the right angle and stop */    pow = dist / Mem->BallKickRate();    pow = Min(pow, Mem->CP_max_turn_kick_pow);    dist = pow * Mem->BallKickRate();          Mem->LogAction4(90, "turnball_kick: striaght kick: dist %f at %f", dist, dir);    DebugKick(printf("  Straight kick# dir: %f dist: %f\n", dir, dist));    PlayerMovementCorrection(&dir, &dist);    *pCom = dokick(dir, dist, 1.0);    pCom->turn_neck = FALSE;  } else if (Mem->BallDistance() < closeMarg) {    /* ball is too close to do a tangent kick, so do a kick at 90 degrees */    dir = ((int)rotate)*(-90) + Mem->BallAngleFromBody();    dist = 2.0*sqrt(Sqr(Mem->CP_opt_ctrl_dist) - Sqr(Mem->BallDistance()));    Mem->LogAction2(90, "turnball_kick: 90 deg kick");    DebugKick(printf("  Close kick# dir: %f dist: %f\n", dir, dist));    PlayerMovementCorrection(&dir, &dist);    *pCom = dokick(dir, dist, kickFac);    pCom->turn_neck = FALSE;  } else {    /* do a turning kick */    /* we make a circle around the player of radius closeMarg       and calculate the trajectory that goes in the right direction and is       tangent to the circle */    dir = 180 + Mem->BallAngleFromBody() + ((int)rotate)*ASin(closeMarg / Mem->BallDistance());    DebugKick(printf(" ball dist: %f\tclosest_margin: %f\n",	   Mem->BallDistance(), closeMarg));    dist = sqrt(Sqr(Mem->BallDistance()) - Sqr(closeMarg));    dist +=      sqrt(Sqr(Mem->CP_opt_ctrl_dist) - Sqr(closeMarg));    DebugKick(printf("  Turning ball# dir: %f dist: %f\n", dir, dist));    Mem->LogAction2(90, "turnball_kick: turning kick");    PlayerMovementCorrection(&dir, &dist);    *pCom = dokick(dir, dist, kickFac);    pCom->turn_neck = FALSE;  }  return KT_DidKick;}KickToRes TurnballTo(AngleDeg rel_dir, TurnDir rotate){  TurnKickCommand com;  KickToRes res = turnball_kick(rel_dir, rotate, TRUE, &com);  if (res == KT_Success || res == KT_DidKick || res == KT_TurnedToBall)    DoTurnKickCommand(com);  return res;}/*******************************************************************************************//* if we kick the ball as hard as possible in the right direction, will it   be a collision with the player? *//* all we have to do is look at the predicted ball position with that kick and   see if it is within the player's radius of the player */int is_hard_kick_coll(float abs_dir, TurnKickCommand* pcom,		      Vector* pvPredBall, float targ_vel, Bool* pCanKickToTraj = NULL){  DebugKick2(cout << "Is hard_kick_coll here" << endl);  *pcom = dokick(abs_dir - Mem->MyBodyAng(), targ_vel, 1.0, pCanKickToTraj);  *pvPredBall = Mem->BallPredictedPosition(1, pcom->power, pcom->angle);  DebugKick(cout << "IsColl: PredBall: " << *pvPredBall	    << "\tMyPos: " << Mem->MyPredictedPosition() << endl);  DebugKick(cout << "diff: " << (*pvPredBall - Mem->MyPredictedPosition()).mod()	    << "\tmarg: " << Mem->CP_hard_kick_margin << endl);  return (*pvPredBall - Mem->MyPredictedPosition()).mod() <=    Mem->SP_player_size + Mem->CP_hard_kick_dist_buffer;}/* Used when we decide a kick in the right direction woudl be a collision,   so we need to turnball to kick the ball more to the side of us,   OR, when in KM_HardestKick, moving the ball back for a kick */TurnKickCommand hard_kick_turnball(float abs_dir, TurnDir rot, Bool StopBall = FALSE){  TurnKickCommand com;  //TurnDir rot = RotToAvoidOpponent(abs_dir);  /* SMURF - should this have a larger dokick_factor? */  Mem->LogAction3(70, "hard_kick_turnball: %f", abs_dir);  DebugKick2(cout << "Doing a hard_kick_turnball" << endl);  KickToRes res = turnball_kick(abs_dir - Mem->MyBodyAng(), rot, StopBall,				&com, Mem->CP_hardest_kick_ball_dist,				Mem->CP_closest_margin, Mem->CP_dokick_factor);  if (res == KT_DidNothing || res == KT_LostBall)    my_error("	hard_kick_turnball: Something weird happened: %d", res);  return com;}TurnKickCommand kick_hard_moderate(AngleDeg abs_dir, float targ_vel, TurnDir rot){  /* Here's the idea:       See if one strong kick will be a collision (if it is, turnball)       o.w. manipulate our kick so that we get another one next cycle       (but make sure the final velocity is higher)       it that makes it a collision, just do the strong kick */    /* there is no reasning about turning the ball backward to get max power.       See KM_HardestKick for that */  Mem->LogAction2(60, "kick_hard_moderate");  DebugKick2(cout << endl << "Time: " << Mem->CurrentTime.t	     << "\tkick_hard_moderate called" << endl);  TurnKickCommand kickCom;  TurnKickCommand HKCommand;  Vector vPredBall;  Bool CanKickToTraj;  if (is_hard_kick_coll(abs_dir, &kickCom, &vPredBall, targ_vel, &CanKickToTraj)) {    DebugKick2(cout << " Moderate: collision, still thinking about it" << endl);    if (Mem->BallDistance() < Mem->SP_player_size + Mem->CP_hard_kick_dist_buffer) {      Mem->LogAction2(70, "kick_hard_moderate: ball is too close to plan for collision ");      return hard_kick_turnball(abs_dir, rot);          } else {      if (CanKickToTraj) {	Mem->LogAction2(70, "kick_hard_moderate: planning for collision ");	/* figure out what kick it would be to just go to the edge of the player */	Vector sol1, sol2;	int numSol =	  RayCircleIntersect(Ray(Mem->BallAbsolutePosition(),				 vPredBall - Mem->BallAbsolutePosition()),			     Mem->SP_player_size + Mem->CP_hard_kick_dist_buffer,			     Mem->MyPredictedPosition(),			     &sol1, &sol2);	if (numSol == 0) {	  my_error("Woah! No solution to kicking a ball less hard!");	  return hard_kick_turnball(abs_dir, rot);      	}	Vector vBallTarg = sol1;	/* SMURF: is there any case where we wouldn't want to do this kick? */	DebugKick2(cout << "Trying to kick to edge of player" << endl);	Vector vBallPath = vBallTarg.Global2Relative(Mem->BallAbsolutePosition(), Mem->MyBodyAng());	HKCommand = dokick(vBallPath.dir(), vBallPath.mod());	return HKCommand;      } else {	/* can't kick to trajectory and it's a collision.	   That's fine! Just do the kick */	Mem->LogAction2(70, "kick_hard_moderate: collision, but that's okay!");	return kickCom;      }    }  } else {    DebugKick(cout << " MyPos: " << Mem->MyPos() << endl);    DebugKick(cout << " vPredBall: " << vPredBall << endl);    Vector vPredBallRel =      vPredBall.Global2Relative(Mem->MyPredictedPosition(), Mem->MyBodyAng());    DebugKick(cout << " vPredBallRel: " << vPredBallRel << endl);    if (vPredBallRel.mod() < Mem->SP_kickable_area-Mem->CP_hard_kick_dist_buffer ||	!Mem->BallWillBeKickable()) {      /* we're going to get another kick next time or this will be our last	   kick anyway - do the strong one! */      Mem->LogAction2(70, "kick_hard_moderate: strong with another or last kick");      DebugKick2(cout << " Moderate: strong with another kick or last kick!" << endl);      return kickCom;    }    DebugKick2(cout << " Moderate: deciding whether to do one or two kicks" << endl);    /* we're goign to set vBall to be the relative vector (to new pos)	 to the position that will give us another kick next time */    float oneKickVel = Min(Mem->SP_ball_speed_max,			   (vPredBall - Mem->BallAbsolutePosition()).mod());    float twoKickVel = 0.0;    Vector sol1, sol2;    int numSol;    Vector vBallTarg;    numSol =      RayCircleIntersect(Ray(Mem->BallAbsolutePosition(),