kick.c

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

			     vPredBall - Mem->BallAbsolutePosition()),
			 Mem->SP_kickable_area - Mem->CP_hard_kick_dist_buffer,
			 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->SP_player_size + Mem->CP_hard_kick_dist_buffer) {
	/* we can do it without collision */
	/* now see if this is actually goign to be better */
	vBallTarg =
	  vBallTarg.Global2Relative(Mem->BallAbsolutePosition(), Mem->MyBodyAng());
	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->BallRelativeToBodyPosition()).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 */
      Mem->LogAction2(70, "kick_hard_moderate: doing one hard kick");
      DebugKick2(cout << " Moderate- Doing one hard kick" << endl);
      return kickCom;
    } else {
      /* do the weaker kick */
      Mem->LogAction2(70, "kick_hard_moderate: doing first of two kicks");
      DebugKick2(cout << " Moderate- doing first of two kicks" << endl);
      //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;
  HKCommand.turn_neck = FALSE;
  
  DebugKick(printf("\nsmart_kick_hard: Time: %d\n", Mem->CurrentTime.t));

  Mem->LogAction4(50, "smart_kick_hard_abs: angle = %.1f, mode = %d",abs_dir,mode);

  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 the velocity isn's valid, turn to face ball */

  if ( !Mem->BallVelocityValid() ) {
    float ball_ang_from_body = Mem->BallAngleFromBody(); /* for efficiency */
    Mem->LogAction2(60, "smart_kick_hard_abs: turning to face ball");
    DebugKick2(printf("smart_kcik_hard: turning to face ball\n"));
    if (Mem->CanSeeBallWithNeck()) {
      Mem->LogAction2(70, "smart_kick_hard_abs: just turniong neck");
      HKCommand.time = Mem->CurrentTime;
      HKCommand.type = CMD_kick;
      HKCommand.angle = ball_ang_from_body + 180;
      HKCommand.power = Mem->CP_stop_ball_power;

      HKCommand.turn_neck = TRUE;
      HKCommand.turn_neck_angle = Mem->LimitTurnNeckAngle(Mem->BallAngleFromNeck());
    } else {
      /* turn body to face ball, and turn neck to straight ahead */
      Mem->LogAction2(70, "smart_kick_hard_abs: turning neck and body");
      HKCommand.time = Mem->CurrentTime;
      HKCommand.type = CMD_turn;
	HKCommand.turn_neck = TRUE;
      if (fabs(ball_ang_from_body) > Mem->MaxEffectiveTurn()) {
	/* out body can't get to where we want to go */
	HKCommand.angle = 180; /* get our maximum effective turn */
	HKCommand.turn_neck_angle = ball_ang_from_body -
	  signf(ball_ang_from_body)*Mem->MaxEffectiveTurn();
      } else {
	HKCommand.angle = ball_ang_from_body;
	HKCommand.turn_neck_angle = -Mem->MyNeckRelAng();
      }
    }
    return DoTurnKickCommand(HKCommand);
  } 

  rot = KickRotationDirectionAbs(abs_dir, rot);

#ifdef NEVER
  /* With the new kick code, the ball goes through the player, 
     so we don't usually need to do a turn-ball as part of a shot */
  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);
    }
  }
#endif  
  
  
  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->MyBodyAng() + ((int)Mem->HKrot)*90;
      if (fabs(target_dir) > Mem->CP_max_hard_kick_angle_err) {
	Mem->LogAction2(70, "smart_kick_hard_abs: hardest turning for power");
	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;
	HKCommand.turn_neck = FALSE;
	break; //used to be: 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)*(Mem->CP_hardest_kick_ball_ang);
    NormalizeAngleDeg(&turn_target);
    if ( Mem->HKStep == 0 &&
	 fabs(GetNormalizeAngleDeg(turn_target-Mem->MyBodyAng()-Mem->BallAngleFromBody()))>
	 Mem->CP_KickTo_err) {
      /* on step 0, we turn ball to back of us */      
      Mem->LogAction2(70, "smart_kick_hard_abs: hardest turning ball backwards");
      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 */
      Mem->LogAction2(70, "smart_kick_hard_abs: calling kick_hard_moderate");
      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;
    break; //used to be: return DoTurnKickCommand(HKCommand);
  }
  
  case KM_Hard:
    /* SMURF */
    my_error("KM_Hard not implemented yet!");
    //dump_core("blah");
    break;
    
  case KM_Moderate:
    HKCommand = 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;
      Bool   CanKickToTraj;
      if (!is_hard_kick_coll(abs_dir, &kickCom, &vPredBall, targ_vel, &CanKickToTraj)) {
	/* there is no collsion! */
	if (mode == KM_Quickly &&
	    Mem->BallWillBeKickable() &&
	    fabs(Mem->BallAngleFromBody()) > Mem->CP_max_hard_kick_angle_err) {
	  /* In KM_Quickly mode, we can turn to ball and get it next time */
	  Mem->LogAction2(70, "smart_kick_hard_abs: km_quickly: turning for power");
	  HKCommand.time = Mem->CurrentTime;
	  HKCommand.type = CMD_turn;
	  HKCommand.angle = Mem->AngleToFromBody(Mem->BallPredictedPosition());
	} else
	  HKCommand = kickCom;
	break; //used to be:return DoTurnKickCommand(HKCommand);
      } else {
	/* do turnball */
	if (!CanKickToTraj) {
	  /* can't kick to trajectory, so just do the collision kick */
	  Mem->LogAction2(70, "smart_kick_hard_abs: km_quick: can't kick to traj, taking the collision");
	  HKCommand = kickCom;
	} else {
	  Mem->LogAction2(70, "smart_kick_hard_abs: km_quick: turnball");
	  HKCommand = hard_kick_turnball(abs_dir, rot);
	}
	break; //used to be:return DoTurnKickCommand(hard_kick_turnball(abs_dir, rot));
      }
    
    }
    my_error("How did I get to end of KM_Quickly/KM_Quickest_Release");
    break;
    
  default:
    my_error("Invalid/Unimplemented kick mode passed to smart_kick_hard");
    break;
  }

  /* now we'll add a turn_neck into this to look at where the ball will be
     If a turn_neck is already there, we won't do this */
  if (!HKCommand.turn_neck) {
    if (HKCommand.time != Mem->CurrentTime)
      my_error("Adding a turn_neck to an invalid HKCommand");
    Mem->LogAction2(70, "smart_kick_hard_abs: doing a turn_neck");
    Vector pred_ball_pos;
    if (HKCommand.type == CMD_kick)
      pred_ball_pos = Mem->BallPredictedPosition(1, HKCommand.power, HKCommand.angle);
    else
      pred_ball_pos = Mem->BallPredictedPosition(1);
    float pred_ball_dir = (pred_ball_pos - Mem->MyPredictedPosition()).dir();
    pred_ball_dir -= Mem->MyNeckGlobalAng();
    pred_ball_dir -= (HKCommand.type == CMD_turn ? HKCommand.angle : 0);

    HKCommand.turn_neck = TRUE;
    HKCommand.turn_neck_angle = Mem->LimitTurnNeckAngle(GetNormalizeAngleDeg(pred_ball_dir));
  }
  
  return DoTurnKickCommand(HKCommand);
}

/* 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)
{
  Mem->LogAction2(60, "smart_pass: doing a turn_neck");
  return smart_kick_hard(Mem->AngleToFromBody(pt), mode,
			 Mem->VelAtPt2VelAtFoot(pt, targ_vel_at_pt),
			 rot);
}


/* should be used when we have free kicks and stuff */
/* kick_ang: absolute angle for the direction that the ball will be kicked */
/* returns whether we are in the right place or not */
Bool go_to_static_ball(float kick_ang)
{
  Line l;

  Mem->LogAction5(50, "go_to_static_ball: ball at (%.1f, %.1f) for kick angle %.2f",
		  Mem->BallX(), Mem->BallY(), kick_ang);
  
  if (!Mem->BallPositionValid())
    my_error("go_to_static_ball: lost ball");

  /* we can be pretty tolerant of angle errors, but distance errors start to matter
     real quickly */

  Vector targ_pt = Mem->BallAbsolutePosition() +
    Polar2Vector(Mem->CP_hardest_kick_ball_dist, kick_ang);

  /* we want to try and face the ball at all times */
  turn_neck(Mem->LimitTurnNeckAngle(Mem->BallAngleFromNeck()));
  
  if (Mem->MySpeed() == 0 &&
      Mem->DistanceTo(targ_pt) <= Mem->CP_static_kick_dist_err) {
    return TRUE;
  }

  /* if we are real far from the ball, just use the regular go_to_point */
  if (Mem->BallDistance() > 2 * Mem->SP_kickable_area) {
    Mem->LogAction2(60, "go_to_static_ball: far away, using go_to_point");
    if (go_to_point(targ_pt, 0 /* no buffer */, Min(Mem->SP_stamina_inc,Mem->SP_max_power))
	!= AQ_ActionQueued)
      my_error("go_to_static_ball: far away, why didn't go_to_point do anything?");
    return FALSE;
  }

  /* make sure that we go around the ball */
  l.LineFromTwoPoints(Mem->MyPos(), targ_pt);
  Vector proj_ball_pt = l.ProjectPoint(Mem->BallAbsolutePosition());
  if (proj_ball_pt.dist(Mem->BallAbsolutePosition()) <=
      Mem->SP_player_size + Mem->SP_ball_size + Mem->CP_collision_buffer &&
      l.InBetween(proj_ball_pt, Mem->MyPos(), targ_pt)) {
    /* we'll do a 90 degree dodge -we always go right */
    Vector dodge_pt = Mem->MyPos() +
      Polar2Vector(Mem->SP_player_size, Mem->BallAngleFromBody() + Mem->MyBodyAng() + 90);
    Mem->LogAction2(60, "go_to_static_ball: dodging the ball");
    if (go_to_point(dodge_pt, 0 /* no buffer */, Min(Mem->SP_stamina_inc,Mem->SP_max_power))
	!= AQ_ActionQueued)
      my_error("go_to_static_ball: dodging, why didn't go_to_point do anything?");
    return FALSE;
  }

  /* now we need to get to the target_point */
  /* first see if we need to turn */
  l.LineFromRay(Mem->MyPos(), Mem->MyBodyAng());
  float ang = Mem->AngleToFromBody(targ_pt);
  if (fabs(ang) > 90 ||
      (l.dist(targ_pt) > Mem->CP_static_kick_dist_err &&
       ang > Mem->CP_static_kick_ang_err)) {
    Mem->LogAction2(60, "go_to_static_ball: turning to target_point");
    turn(Mem->AngleToFromBody(targ_pt));
    return FALSE;
  }
  
  /* now calculate the speed we should be going to land right on the point */
  float targ_speed =
    SolveForFirstTermInfGeomSeries(Mem->SP_player_decay, Mem->DistanceTo(targ_pt));
  float dash_pow =
    MinMax(-Mem->SP_stamina_inc / 2,
	   (targ_speed - Mem->MySpeed()) / Mem->SP_dash_power_rate,
	   Mem->SP_stamina_inc);
  Mem->LogAction5(60, "go_to_static_ball: targ_speed: %.2f\tMySpeed: %.2f\tdash_pow: %.2f",
	    targ_speed, Mem->MySpeed(), dash_pow);
  if (fabs(dash_pow) > 1) {
    dash(dash_pow);
    return FALSE;
  }

  return TRUE;
}