kick.c

卡内基梅隆大学99年机器人足球世界杯2D仿真组源代码。卡内基梅隆大学在人工智能界的巨牛

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

/* kick.C
 * CMUnited99 (soccer client for Robocup99)
 * Peter Stone <[e-mail]>
 * 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 <math.h>
#include "Memory.h"
#include "client.h"
#include "kick.h"
#include "test.h"
#include "behave.h"

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

/* these PatsTest_* functions are just that.
   They are temporary function that can be used to see some specific kicking
   behavior.
   What they do isn't documented anywhere but the source code below */

void PatsTest_static()
{
  DebugKick(printf("\nTime: %d\n", Mem->CurrentTime.t));  
  if (!Mem->MyConf())
    scan_field_with_body();
  else if (!Mem->BallPositionValid())
    face_neck_to_ball();
  else {
    if (Mem->CurrentTime.t % 100 < 10 && Mem->BallKickable())
      smart_kick_hard_abs(45, KM_Moderate);
    else {
      Bool res = go_to_static_ball(45);
      cout << "result: " << res << endl;
    }    
  }  
}


void PatsTest_conv(void)
{
  Vector g, r, g2;
  for (float i=-10.0; i<= 10.0; i+= 1.0) {
    for (float j=-10.0; j<= 10.0; j+= 1.0) {
      g = Vector(i,j);
      r = g.Global2Relative(-3.0, 37);
      g2 = r.Relative2Global(-3.0, 37);
      if (fabs(g.x - g2.x) > FLOAT_EPS ||
	  fabs(g.y - g2.y) > FLOAT_EPS)
	printf("Error i:%f\tj: %f\n", i, j);
    }
  }
  exit(1); 
}


void PatsTest_turn(void)
{
  KickToRes res;
  static int ang=180;
  static int wait_count = 0;

  /* SMURF - there is something wrong with Time class!
  if (Mem->CurrentTime - Mem->LastActionTime < Mem->CP_kick_time_space)
    return;
    */
  //TurnKickCommand com;
  if (Mem->CurrentTime.t % 100 < 20)
    return;
  
  res = KT_None;
  if (Mem->BallKickable()) {
    
    if (wait_count <= 0) {      
      DebugKick(printf("About to call turnball_kick\n"));
      res = TurnballTo((AngleDeg)ang - Mem->MyBodyAng(), TURN_CLOSEST);
    } else {
      DebugKick(printf("waiting...\n"));
      wait_count--;
    } 
    
  } else {
  } 

    
  if (res == KT_LostBall) 
    return;
  else if (res == KT_Success) {
    DebugKick(printf("SUCESS-play_with_ball!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n"));
    DebugKick(printf("ang: %d", ang));
    ang = (90 + ang) % 360;
    DebugKick(printf("new ang: %d\n", ang));
  } else if (res == KT_DidNothing)
    return;
  return;

} 

void PatsTest_kick()
{
  if (!strcmp(Mem->MyTeamName, "CMUnited")) {
    //float targ_vel;
    Vector pass_targ;
  
    fflush(stdout);

    change_view(VW_Narrow);
    
    if (!Mem->BallKickable()) {
      //scan_field();
      return;
    } 

    /*    if((int)(Mem->CurrentTime.t) % 50 < 5 ) {
      return; 
    }*/

  DebugKick(printf("\nTime: %d\n", Mem->CurrentTime.t));  

  /*
  targ_vel = 1.0 + (Mem->CurrentTime.t / 50)*.1;
  printf("Target vel is: %f\n", targ_vel);*/
    pass_targ = Vector(-20, -20 + (Mem->CurrentTime.t / 50)*5);
    //cout << "Pass Target: " << pass_targ << endl;
  
  //DebugKick(printf("the (simulated) ball velocity is: %g\n",) 
	   //	 Mem->BallAbsoluteVelocity().mod());
  /*
  DebugKick(printf("My angle: %f\n", Mem->MyAng()));
  if (fabs(Mem->MyAng() - 90) > 5) {
    DebugKick(printf("Turning to 90\n"));
    //turn(90-Mem->MyAng());
    turn(Mem->MarkerAngle(Mem->RM_RC_Flag));
    return;
  }
  */
    if (Mem->BallKickable()) {
      /*if (step == 4)
	if (step == 3) {
	step = 0; 
	turn(90 - Mem->MyAng());
	} 	*/
      smart_kick_hard_abs(180, KM_Moderate);
      //smart_kick_hard_abs(180, KM_HardestKick);
      //smart_kick_hard_abs(180, KM_Moderate, targ_vel);
      //smart_pass(pass_targ);
    } 
  } else {
    static int FirstTime = TRUE;
    const int ydist = 2;
    //    if (Mem->MyNumber == 1)
    //      test_go_to_point(Vector(-.2, -1), .5, 50);
    if (!FirstTime)
      return;
    switch (Mem->MyNumber) {
    case 1: move(-.2, -1); break;
    case 2: move(-20,  ydist); break;
    case 3: move(-30, -ydist); break;
    case 4: move(-40,  ydist); break;
    case 5: move(-50, -ydist); break;
    }
    FirstTime = FALSE;
  }
  
  return;
}

void PatsTest_kick2()
{
  fflush(stdout);

  if (Mem->ViewWidth != VW_Wide)
    change_view(VW_Wide);
	  
  static int WasBallKickable;
  DebugKick(printf("Time: %d\n", Mem->CurrentTime.t));
  //DebugKick(printf(" Ball Distance: %f\n", Mem->BallDistance() ));
  //DebugKick(cout << " MyPos: " << Mem->MyPos() << endl));
  if (!Mem->BallPositionValid() || !Mem->BallKickable()) {
    WasBallKickable = FALSE;
    
    DebugKick(printf("chasing ball\n"));
    if (Mem->BallPositionValid()) {
      if (fabs(Mem->BallAngleFromBody()) > Mem->CP_KickTo_err) {
	DebugKick(printf("turning to face ball\n"));
	DebugKick(printf("the angle is: %f\n", Mem->BallAngle()));
	turn(Mem->BallAngleFromBody());
      } else {
	DebugKick(printf("dashing\n"));
	float power=Mem->BallDistance()*40;
	if (power>40) power = 40;
	dash(power);
      } 
    } else {
      DebugKick(printf("turning randomly\n"));
      turn(60);
    } 
    return;
  } 


  //DebugKick(printf("the (simulated) ball velocity is: %g\n",) 
	   //	 Mem->BallAbsoluteVelocity().mod());

  if (Mem->BallKickable()) {
    if (!smart_kick_hard_abs(0, KM_HardestKick))
      printf("test_kick_hard: UhOh, something bad happened\n");
    return;
  } 

  return;
}


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);
    break;
  case CMD_turn:
    DebugKick(printf("DoTurnKickCommand: turn\n"));
    turn(com.angle);
    break;
  case CMD_kick:
    DebugKick(printf("DoTurnKickCommand: kick\n"));
    kick(com.power, com.angle);
    break;

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

  if (com.turn_neck) {
    turn_neck(com.turn_neck_angle);
  }
  return 1;  
}


/* 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->BallRelativeToBodyPosition();
  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"));
    Mem->LogAction2(120, "is_straight_kick: obtuse angle");
    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));
  Mem->LogAction3(120, "is_straight_kick: %f", 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;
  AngleDeg opp_ang;
  Unum opp = Mem->ClosestOpponent();
  AngleDeg ball_ang = Mem->BallAngleFromBody();
  if (opp != Unum_Unknown) {
    dist = Mem->OpponentDistance(opp);
    opp_ang = Mem->OpponentAngleFromBody(opp);
  }
  if (Mem->NumTeamlessPlayers() > 0) {
    Vector pos = Mem->ClosestTeamlessPlayerPosition();
    float d = Mem->DistanceTo(pos);
    if (d < dist) {
      dist = d;
      opp_ang = Mem->AngleToFromBody(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"));
    AngleDeg ball_to_targ = GetNormalizeAngleDeg((abs_dir - Mem->MyBodyAng()) - ball_ang);
    AngleDeg opp_to_targ = GetNormalizeAngleDeg((abs_dir - Mem->MyBodyAng()) - opp_ang);

    if ( ball_to_targ * opp_to_targ < 0 ){ /* they're on opposite sides of the target */
      Mem->LogAction2(120, "RotToAvoidOpponent: CLOSEST");
      rot = TURN_CLOSEST;
    }
    else if ( ball_to_targ < opp_to_targ ){
      Mem->LogAction2(120, "RotToAvoidOpponent: CW");
      rot = TURN_CW;
    }
    else{
      Mem->LogAction2(120, "RotToAvoidOpponent: CCW");
      rot = TURN_CCW;
    }
  } else {
    Mem->LogAction2(120, "RotToAvoidOpponent: no opponents close enough");
    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->MyBodyAng()) - Mem->BallAngleFromBody();
  AngleDeg ccw_ang = Mem->BallAngleFromBody() - (abs_dir - Mem->MyBodyAng());
  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;
  com.turn_neck = FALSE;
  if (pCanKickToTraj)
    *pCanKickToTraj = TRUE;

  Mem->LogAction6(110, "dokick: %.2f (%.2f) at %.2f, rate %.5f",
		  ddist, distFactor, ddir, Mem->BallKickRate());
  
  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!");