kick.c

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

    PlayerMovementCorrection(&dir, &dist);
    *pCom = dokick(dir, dist, kickFac);

  } 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->BallAngle() +
      ((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));
    PlayerMovementCorrection(&dir, &dist);
    *pCom = dokick(dir, dist, kickFac);

  }

  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)
{
  /* the Mem->SP_ball_speed_max is just to get as hard a kick as possible */
  *pcom = dokick(abs_dir - Mem->MyAng(), targ_vel);
  *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->CP_hard_kick_margin;
}

/* 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 */
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? */
  KickToRes res = turnball_kick(abs_dir - Mem->MyAng(), rot, StopBall,
				&com, Mem->CP_hard_kick_end_turn_dist,
			  Mem->CP_hard_kick_margin, Mem->CP_hard_kick_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 */
  TurnKickCommand kickCom;
  TurnKickCommand HKCommand;
  Vector vPredBall;
  if (is_hard_kick_coll(abs_dir, &kickCom, &vPredBall, targ_vel)) {
    DebugKick(printf(" Moderate: collision- doing turnball\n"));
    return hard_kick_turnball(abs_dir, rot);      
  } else {
    DebugKick(cout << " MyPos: " << Mem->MyPos() << endl);
    DebugKick(cout << " vPredBall: " << vPredBall << endl);
    Vector vPredBallRel =
      vPredBall.Global2Relative(Mem->MyPredictedPosition(), Mem->MyAng());
    DebugKick(cout << " vPredBallRel: " << vPredBallRel << endl);
    if (vPredBallRel.mod() < Mem->SP_kickable_area ||
	!Mem->BallWillBeKickable()) {
      /* we're going to get another kick next time or this will be our last
	   kick anyway - do the strong one! */
      DebugKick(printf(" Moderate: strong with another kick or last kick!\n"));
      return kickCom;
    }
    /* 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(Mem->BallAbsolutePosition(),
			 vPredBall - Mem->BallAbsolutePosition(),
			 Mem->SP_kickable_area - Mem->CP_hard_kick_err,
			 Mem->MyPredictedPosition(),
			 &sol1, &sol2);
    /* we want the solution that's furthest along the ray - that's sol2
	 if there are two solution */
    if (numSol != 0) {
	
      if (numSol == 2)
	vBallTarg = sol2;
      else
	vBallTarg = sol1;

      /* see if the first of the two kicks is a coll */
      if ((vBallTarg - Mem->MyPredictedPosition()).mod() >=
	  Mem->CP_hard_kick_margin) {
	/* we can do it without collision */
	/* now see if this is actually goign to be better */
	vBallTarg =
	  vBallTarg.Global2Relative(Mem->BallAbsolutePosition(), Mem->MyAng());
	float kickrate =
	  Mem->GetBall()->calc_kick_rate(vBallTarg.mod(), vBallTarg.dir());
	/* the first kick */
	//DebugKick( cout << "vBallTarg: " << vBallTarg << "\tBallAbsPos: " <<
	//	 Mem->BallAbsolutePosition() << endl );
	twoKickVel = (vBallTarg - Mem->BallRelativePosition()).mod() *
	  Mem->SP_ball_decay;
	DebugKick(printf("  twoKickVel: first kick: %f\n", twoKickVel));
	/* the second kick */
	twoKickVel = Min(Mem->SP_ball_speed_max,
			 twoKickVel + kickrate * Mem->SP_max_power);
	DebugKick(printf("  oneKickVel: %f\ttwoKickVel: %f\n", oneKickVel, twoKickVel));
      } else
	my_error("kick_hard_moderate- no ray intersection?");	
    }
      
    /* remember if twoKick is a collision, then it's velocity will be 0 */
    if (numSol == 0 || oneKickVel >= twoKickVel) {
      /* do the one hard kick */
      DebugKick(printf(" Moderate- Doing one hard kick\n"));
      return kickCom;
    } else {
      /* do the weaker kick */
      DebugKick(printf(" Moderate- doing first of two kicks\n"));
      //DebugKick(printf(" Predicted distance: %f\n",
      //		 (vBallTarg - Mem->MyPredictedPosition()).mod() ));
      //DebugKick(cout << " BallCurrPos: " << Mem->BallAbsolutePosition() << endl);
      HKCommand = dokick(vBallTarg.dir(), vBallTarg.mod());
      DebugKick(cout << " KickTraj: " << vBallTarg << endl);
      DebugKick(cout << " PredPos: " << Mem->MyPredictedPosition() << endl);
      return HKCommand;
    }
      
  }    
}


TurnDir KickRotationDirectionAbs(AngleDeg abs_ang, TurnDir rot)
{
  if (rot == TURN_AVOID) 
    rot = RotToAvoidOpponent(abs_ang);    
  if (rot == TURN_CLOSEST) 
    rot = RotClosest(abs_ang);
  if (rot != TURN_CW && rot != TURN_CCW)
    my_error("KickRotationDirection: bad TurnDir");

  return rot;
}


/* see above for description of KM_Moderate */
/* KM_HardestKick: Moves the ball to the side of us (relative to the kick
   direction) then uses KM_Moderate */
/* KM_Quickly, KM_QuickestRelease: get rid of the ball as fast as possible,
   will turnball if neccesary. KM_Quickly will turn to face the ball if it can
   so that we get a harder kick */
/* returns 1 if a kick actually done */
int smart_kick_hard_abs(float abs_dir, KickMode mode, float targ_vel,
			TurnDir rot)
{
  TurnKickCommand HKCommand;
  HKCommand.time = -1;
  
  DebugKick(printf("\nsmart_kick_hard: Time: %d\n", Mem->CurrentTime.t));

  if (!Mem->BallPositionValid()) {
    my_error("smart_kick_hard called with ball position not valid");
    return 0;
  }

  if (!Mem->BallKickable()) {
    my_error("smart_kick_hard called with ball not kickable!");
    return 0;
  }
  
  if (!Mem->BallVelocityValid()) {
    DebugKick(printf("smart_kick_hard: turning to face ball\n"));
    HKCommand.time = Mem->CurrentTime;
    HKCommand.type = CMD_turn;
    HKCommand.angle = Mem->BallAngle();
    return DoTurnKickCommand(HKCommand);
  }

  rot = KickRotationDirectionAbs(abs_dir, rot);
  
  if (mode <= KM_Moderate &&
      Mem->IsPointBehind(Mem->BallAbsolutePosition(), abs_dir)) {
    /* see if we need to rotate one way */
    DebugKick(cout << "smart_kick_hard: decign if rotating " << rot << endl);
    /* now decide if ball is on the wrong side	 */
    if (Mem->IsPointBehind(Mem->BallAbsolutePosition(), abs_dir+((int)rot)*90)) {
      /* start rotating right way */
      DebugKick(cout << "smart_kick_hard: special turnball to avoid opp" << endl);
      TurnKickCommand com;
      KickToRes res = turnball_kick( abs_dir - Mem->MyAng(),
				     //abs_dir + ((int)rot)*90 - Mem->MyAng(),
				     rot, FALSE, &com);
      if (res == KT_DidNothing || res == KT_LostBall)
	my_error("smart_kick_hard: special turnball; turnkick failed, res: %d", res);
      else
	return DoTurnKickCommand(com);
    }
  }
  
  
  
  switch (mode) {
  case KM_None:
    my_error("KM_None is not a valid kick mode for smart_kick_hard!");
    break;
    
  case KM_HardestKick: {      
    if (Mem->CurrentTime - 1 != Mem->HKTime) {
      DebugKick(printf("First in a chain of calls\n"));
      /* this is the first in a chain of calls */      
      Mem->HKStep = 0;
      /* decide which way to rotate ball */
      Mem->HKrot = rot;
      /*AngleDeg ang = Mem->BallAngle() + Mem->MyAng() - abs_dir;
      NormalizeAngleDeg(&ang);
      if (ang >= 0)
	Mem->HKrot = TURN_CW;
      else
	Mem->HKrot = TURN_CCW;*/
	  
      /* see if we need to turn */
      AngleDeg target_dir = abs_dir - Mem->MyAng() + ((int)Mem->HKrot)*90;
      if (fabs(target_dir) > Mem->CP_hard_kick_err) {
	DebugKick(printf("turning to get hard kick\n"));
	Mem->HKTime = Mem->CurrentTime;
	Mem->HKStepNext = 0;
	HKCommand.type = CMD_turn;
	HKCommand.angle = target_dir;
	HKCommand.time = Mem->CurrentTime;
	return DoTurnKickCommand(HKCommand);
      }
    } else
      Mem->HKStep = Mem->HKStepNext;

    /* AngleDeg turn_target = abs_dir + 180 +
      ((int)Mem->HKrot)*Mem->CP_hardest_kick_angle_disp; */
    AngleDeg turn_target = abs_dir + 
      ((int)Mem->HKrot)*(90 + Mem->CP_static_kick_ang);
    NormalizeAngleDeg(&turn_target);
    if ( Mem->HKStep == 0 &&
	 fabs(GetNormalizeAngleDeg(turn_target-Mem->MyAng()-Mem->BallAngle()))>
	 Mem->CP_KickTo_err) {
      /* on step 0, we turn ball to back of us */      
      HKCommand = hard_kick_turnball(turn_target, TURN_CLOSEST, TRUE);//TRUE to stop ball
      Mem->HKStepNext = 0;
    }
    if (Mem->HKStep == 1 ||
	(Mem->HKStep == 0 && HKCommand.time != Mem->CurrentTime)) {
      /* on step 1, we call the moderate code */
      /* or if step 0 had turnball problems, we'll go ahead and drop
	 to this code */
      HKCommand = kick_hard_moderate(abs_dir, targ_vel, rot);
      Mem->HKStepNext = 1;
    }
    if (Mem->HKStep != 0 && Mem->HKStep != 1)
      my_error("HKstep is not in a valid state: %d", Mem->HKStep);
    
    Mem->HKTime = Mem->CurrentTime;
    return DoTurnKickCommand(HKCommand);
    break;
  }
  
  case KM_Hard:
    my_error("KM_Hard not implemented!");
    break;
    
  case KM_Moderate:
    return DoTurnKickCommand(kick_hard_moderate(abs_dir, targ_vel, rot));
  break;
    
  case KM_Quickly:
  case KM_QuickestRelease: {
      Mem->HKStep = Mem->HKStepNext = -1;
      /* see if the hardest kick in the direction we want will be collision */
      /* use our dokick function to correct for vel */
      TurnKickCommand kickCom;
      Vector vPredBall;
      if (!is_hard_kick_coll(abs_dir, &kickCom, &vPredBall, targ_vel)) {
	/* there is no collsion! */
	if (mode == KM_Quickly &&
	    Mem->BallWillBeKickable() &&
	    fabs(Mem->BallAngle()) > Mem->CP_max_hard_kick_angle_err) {
	  /* In KM_Quickly mode, we can turn to ball and get it next time */
	  HKCommand.time = Mem->CurrentTime;
	  HKCommand.type = CMD_turn;
	  HKCommand.angle = Mem->BallAngle();
	} else
	  HKCommand = kickCom;
	return DoTurnKickCommand(HKCommand);
      } else {
	/* do turnball */
	return DoTurnKickCommand(hard_kick_turnball(abs_dir, rot));
      }
    
    }
    
    break;
    
  default:
    my_error("Invalid/Unimplemented kick mode passed to smart_kick_hard");
    break;
  }

  my_error("Got to end of smart_kick_hard?");
  return 0;
  
}

/* good passes require that the ball is not moving too quickly when it reaches
   the intended recipient, so this cover function helps acheive that */
int smart_pass(Vector pt, float targ_vel_at_pt, KickMode mode, TurnDir rot)
{
  return smart_kick_hard(Mem->AngleTo(pt), mode,
			 Mem->VelAtPt2VelAtFoot(pt, targ_vel_at_pt),
			 rot);
}