kick.c

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

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

/* kick.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.
 */

/* kick.C contains functions for kicking the ball
   Created by Patrick Riley
 */


#include <math.h>
#include "Memory.h"
#include "client.h"
#include "kick.h"
#include "test.h"
#include "behave.h"

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

int DoTurnKickCommand(TurnKickCommand com)
{
  if (com.time != Mem->CurrentTime) {
    my_error("DoTurnKickCommand- told to do command not set this cycle");
    return 0;
  }
    
  switch (com.type) {
  case CMD_dash:
    DebugKick(printf("DoTurnKickCommand: dash\n"));
    dash(com.power);
    return 1;
  case CMD_turn:
    DebugKick(printf("DoTurnKickCommand: turn\n"));
    turn(com.angle);
    return 1;
  case CMD_kick:
    DebugKick(printf("DoTurnKickCommand: kick\n"));
    kick(com.power, com.angle);
    return 1;

  default:
    my_error("PatInfo::DoTurnKickCommand- unimplemented type!");
    return 0;
  }
  
}


/* decides if we can kick staright to the decired point around the player
   without a collision.
   (EndDist, dir) is a relative polar vector for the ball's final position
   closeMarg is what the radius of the player is considered to be */
int is_straight_kick(float dir, float EndDist, float closeMarg)
{
  Vector btraj = Polar2Vector(EndDist, dir) -
    Mem->BallRelativePosition();
  float ang;
  float dist;
  int res;

  DebugKick(printf("    isStriaght ball abs pos mod: %f\t dir: %f\n",
	 Mem->BallAbsolutePosition().mod(), Mem->BallAbsolutePosition().dir()));
  DebugKick(printf("    isStriaght ball rel pos mod: %f\t dir: %f\n",
	 Mem->BallRelativePosition().mod(), Mem->BallRelativePosition().dir()));
  DebugKick(printf("    isStriaght btraj mod: %f\t dir: %f\n", btraj.mod(), btraj.dir()));
  /* Apply the law of cosines to the anle formed by the player's center(A),
     the ball's current position(B), and the ball target position(C).
     The angle calculated is ABC */
  ang = ACos( (Sqr(EndDist) - Sqr(btraj.mod()) -
	       Sqr(Mem->BallDistance()))/
	      (-2 * Mem->BallDistance() * btraj.mod()) );
  DebugKick(printf("   isStraight ang: %f\n", ang));
  if (fabs(ang) > 90) {
    DebugKick(printf("    isStraight: Obtuse!\n"));
    return 1; /* obtuse angle implies definately straight */
  }
  /* get the height of the triangle, ie how close to the player the ball will
     go */
  dist = Sin(ang) * Mem->BallDistance(); 
  DebugKick(printf("   isStraight dist: %f\n", dist));
  res = (fabs(dist) > closeMarg);
  return ( res );  
} 

/* picks a rotation (CW or CCW) to turnball based on the nearest opponent or
   teamless player */
TurnDir RotToAvoidOpponent(float abs_dir)
{
  TurnDir rot;
  float dist = HUGE;
  float ang;
  Unum opp = Mem->ClosestOpponent();
  if (opp != Unum_Unknown) {
    dist = Mem->OpponentDistance(opp);
    ang = Mem->OpponentAngle(opp);
  }
  if (Mem->NumTeamlessPlayers() > 0) {
    Vector pos = Mem->ClosestTeamlessPlayerPosition();
    float d = Mem->DistanceTo(pos);
    if (d < dist) {
      dist = d;
      ang = Mem->AngleTo(pos);
    }
  }
  if (dist < Mem->CP_turnball_opp_worry_dist) {
    /* there is an opponent near enough to worry about */
    DebugKick(printf("In RotToAvoidOpponent, avoiding opponent\n"));
    if (GetNormalizeAngleDeg(ang - (abs_dir - Mem->MyAng()))
	< 0)
      rot = TURN_CCW;
    else
      rot = TURN_CW;
  } else
    rot = TURN_CLOSEST;
  return rot;
}

/* Picks the rotation direction (CW or CCW) such that the ball has to travel
   the least distance */
TurnDir RotClosest(float abs_dir)
{
  AngleDeg cw_ang = (abs_dir - Mem->MyAng()) - Mem->BallAngle();
  AngleDeg ccw_ang = Mem->BallAngle() - (abs_dir - Mem->MyAng());
  DebugKick(printf("Test1: cw_ang: %f\tccw_ang: %f\n", cw_ang, ccw_ang));
  if (cw_ang < 0) cw_ang += 360;
  if (ccw_ang < 0) ccw_ang += 360;
  DebugKick(printf("Test2: cw_ang: %f\tccw_ang: %f\n", cw_ang, ccw_ang));
  if (cw_ang < ccw_ang)
    return TURN_CW;
  else
    return TURN_CCW;  
}

  

/* Kick with direction ddir and power such that the ball moves distance ddist
   If ddist is too big, kick it as hard as possible.
   corrects for ball velocity
   Returns a command object partially filled out
   The distance is multiplied by the distFactor */
/* returns KickCommand.time = -1 for error */
TurnKickCommand dokick(AngleDeg ddir, float ddist, float distFactor,
		       bool* pCanKickToTraj) 
{
  float v0;
  float power;
  float kick_dir = ddir;
  TurnKickCommand com;
  com.time = -1;
  com.type = CMD_kick;
  if (pCanKickToTraj)
    *pCanKickToTraj = TRUE;

  v0 = ddist * distFactor;

  NormalizeAngleDeg(&ddir);

  DebugKick(printf(" dokick: ddir: %f\tv0: %f\n", ddir, v0));
  DebugKick(printf(" kickrate: %f\tmyang: %f\n", Mem->BallKickRate(), Mem->MyAng() ));

  if (Mem->BallKickRate() == 0.0)
    my_error("dokick: Huh? BallKickRate is 0!");

  if (!Mem->BallVelocityValid())
    DebugKick(printf("In dokick with velocity not valid. Assuming it's 0."));
  
  if (Mem->BallVelocityValid() &&
      Mem->BallAbsoluteVelocity() != 0) { /* correct for ball velocity */
    Vector tmp = Polar2Vector(v0, ddir) - Mem->BallRelativeVelocity();
    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));
  } else
     power = v0  / Mem->BallKickRate();

  if (power > Mem->SP_max_power) {
    DebugKick(printf("Trying to kick over SP_max_power! Correcting...\n"));
    if (!Mem->BallVelocityValid() || Mem->BallAbsoluteVelocity() == 0) {
      power = Mem->SP_max_power;
    } 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(Mem->BallRelativePosition(),
			   Polar2Vector(1, ddir),
			   Mem->SP_max_power * Mem->BallKickRate(),
			   Mem->Global2RelativeToMe(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->BallRelativeHeading() + 180;
	if (pCanKickToTraj)
	  *pCanKickToTraj = FALSE;
      } else {
	if (numSol == 2)
	  vNewTraj = sol2;
	else
	  vNewTraj = sol1;

	Vector vNewKick = vNewTraj -
	  Mem->Global2RelativeToMe(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));
      }
    }    
  } 

  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 coordinates
   call this before calling dokick to correct for movement of the player */
void PlayerMovementCorrection(AngleDeg* pdir, float* pdist)
{
  DebugKick(printf("Before corr- dir: %f\t dist: %f\n", *pdir, *pdist));
  Vector vRelBallTarg = Mem->BallRelativePosition() +
    Polar2Vector(*pdist, *pdir);
  Vector vNewRelTarg = Mem->MyPredictedPosition() +
    vRelBallTarg - Mem->MyPos();
  Vector vNewRelTraj = vNewRelTarg - Mem->BallRelativePosition();
  *pdir  = vNewRelTraj.dir();
  *pdist = vNewRelTraj.mod();
  DebugKick(printf("After corr- dir: %f\t dist: %f\n", *pdir, *pdist));
}


/* 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;
  
  DebugKick(printf("\nat turnball_kick: target_dir: %f\n", target_dir));

  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() ) {
    return KT_LostBall;
  }

  /* if the velocity isn's valid, turn to face ball */
  if ( !Mem->BallVelocityValid() ) {
    DebugKick(printf("turning to face ball\n"));
    /* we should reason about whether we can see the ball before turning,
       or maybe we should try and stop the ball */
    pCom->time = Mem->CurrentTime;
    pCom->type = CMD_turn;
    pCom->angle = Mem->BallAngle();
    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->BallAngle())) < Mem->CP_KickTo_err) {
    /* Do something to indicate we are done */
    if (!StopBall || Mem->BallSpeed() < Mem->CP_max_ignore_vel)
      return KT_DidNothing;
    DebugKick(printf("  Stop ball kick\n"));
    dir = 0;
    dist = 0;    
    PlayerMovementCorrection(&dir, &dist);
    *pCom = dokick(dir, dist, 1.0);
    return KT_Success;
  }

  if (rotate == TURN_AVOID) {
    rotate = RotToAvoidOpponent(target_dir + Mem->MyAng());
  }
  
  if (rotate == TURN_CLOSEST) {
    rotate = RotClosest(target_dir + Mem->MyAng());
  }
  
  
  if (is_straight_kick(target_dir, EndDist, closeMarg)) {
    float pow;
      
    btraj = Polar2Vector(EndDist, target_dir) -
      Mem->BallRelativePosition();
    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();
      
    DebugKick(printf("  Straight kick# dir: %f dist: %f\n", dir, dist));
    PlayerMovementCorrection(&dir, &dist);
    *pCom = dokick(dir, dist, 1.0);

  } 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->BallAngle();
    dist = 2.0*sqrt(Sqr(Mem->CP_opt_ctrl_dist) - Sqr(Mem->BallDistance()));
    DebugKick(printf("  Close kick# dir: %f dist: %f\n", dir, dist));