/* -*- Mode: C++ -*- *//* kick.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 <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!");