worldmodelhighlevel.c

机器足球2D比赛程序 对trlen_base_2002的改进

/*! This method determines the power with which the ball must be kicked in
  order to travel a given distance and still have a speed after that
  distance
  \param dDistance distance ball should travel
  \param dEndSpeed speed ball should have at target position
  \return power value for kick command */
double
WorldModel::getKickSpeedToTravel (double dDistance, double dEndSpeed)
{
  // if endspeed is zero we have an infinite series and return the first term
  // that corresponds to the distance that has to be travelled.
  if (dEndSpeed < 0.0001)
    return Geometry::getFirstInfGeomSeries (dDistance, SS->getBallDecay ());

  // use geometric series to calculate number of steps and with that the
  // velocity to give to the ball, we start at endspeed and therefore use
  // the inverse of the ball decay (r).
  // s = a + a*r + .. a*r^n since we calculated from endspeed (a) to
  // firstspeed, firstspeed equals a*r^n = endspeed*r^nr_steps
  double dNrSteps = Geometry::getLengthGeomSeries (dEndSpeed,
						   1.0 / SS->getBallDecay (),
						   dDistance);
  return getFirstSpeedFromEndSpeed (dEndSpeed, (int) rint (dNrSteps));
}

/*! This method returns the speed that has to be given to the ball when
  it should have an endspeed of 'dEndSpeed' after 'dCycles' number of cycles.
  This can be calculated using a geometric series.
  \param dEndSpeed desired end speed for the ball
  \param dCycles nr of cycles after which ball should have speed 'dEndSpeed'
  \return initial speed given to the ball to have speed 'dEndSpeed' after
  'dCycles' cycles. */
double
WorldModel::getFirstSpeedFromEndSpeed (double dEndSpeed, double dCycles)
{
  // geometric series: s = a + a*r^1 + .. + a*r^n, now given endspeed = a*r^n ->
  // endspeed = firstspeed * ratio ^ length ->
  // firstpeed = endspeed * ( 1 / ratio ) ^ length
  return dEndSpeed * pow (1 / SS->getBallDecay (), dCycles);
}

/*! This method returns the speed that has to be given to the ball when
  it should have travelled a distance 'dDist' after 'dCycles' number of
  cycles. This can be calculated using a geometric series.
  \param dDist distance the ball has to travel
  \param dCycles nr of cycles after which ball should have travelled 'dDist'
  \return initial speed for the ball to travel 'dDist' in 'dCycles' cycles */
double
WorldModel::getFirstSpeedFromDist (double dDist, double dCycles)
{
  return Geometry::getFirstGeomSeries (dDist, SS->getBallDecay (), dCycles);
}

/*! This method returns the speed the ball will have after 'dCycles' cycles
  when it is given an initial speed of 'dFirstSpeed'.
  This can be calculated using a geometric series.
  \param dFirstSpeed given speed to the ball
  \param dCycles nr of cycles after which ball speed should be determined
  \return speed of the ball after 'dCycles' server cycles */
double
WorldModel::getEndSpeedFromFirstSpeed (double dFirstSpeed, double dCycles)
{
  // geometric series: s = a + a*r^1 + .. + a*r^n, with firstspeed = a ->
  // endspeed = firstspeed * ratio ^ length ;
  return dFirstSpeed * pow (SS->getBallDecay (), dCycles);
}

/*! This method determines the angle that should be sent to the soccerserver when
  the player wants to turn angDesiredAngle. This value depends on the current
  velocity and the inertia moment of the player
  \param angDesiredAngle angle that player wants to turn
  \param dSpeed current speed of the player
  \return angle that can be sent with turn command */
AngDeg
WorldModel::getAngleForTurn (AngDeg angDesiredAngle, double dSpeed)
{
  AngDeg a = angDesiredAngle * (1.0 + SS->getInertiaMoment () * dSpeed);
  if (a > SS->getMaxMoment ())
    return SS->getMaxMoment ();
  else if (a < SS->getMinMoment ())
    return SS->getMinMoment ();
  else
    return a;
}

/*! This method determines the actual angle that is used when 'angTurn' is
  sent to the SoccerServer. This value depends on the current velocity and
  the inertia moment of the player
  \param angAngleForSend angle send with turn command
  \param dSpeed current speed of the player
  \return actual angle that player is turned */
AngDeg
WorldModel::getActualTurnAngle (AngDeg angTurn, double dSpeed)
{
  return angTurn / (1.0 + SS->getInertiaMoment () * dSpeed);
}

/*! This method determines the optimal dash power to mantain an optimal speed
  When the current speed is too high and the distance is very small, a
  negative dash is performed. Otherwise the difference with the maximal speed
  is determined and the dash power rate is set to compensate for this
  difference.
  \param posRelTo relative point to which we want to dash
  \param angBody body angle of the agent
  \param vel current velocity of the agent
  \param dEffort current effort of the player
  \return dash power that should be sent with dash command */
double
WorldModel::getPowerForDash (VecPosition posRelTo, AngDeg angBody,
			     VecPosition vel, double dEffort)
{
  // the acceleration desired is the x-direction to the relative position we
  // we want to move to. If point lies far away, we dash maximal. Furthermore
  // we subtract the x contribution of the velocity because it is not necessary
  // to dash maximal.
  double dAcc = posRelTo.rotate (-angBody).getX ();	// get distance in direction
  if (dAcc > SS->getPlayerSpeedMax ())	// if too far away
    dAcc = SS->getPlayerSpeedMax ();	// set maximum speed
  dAcc -= vel.rotate (-angBody).getX ();	// subtract current velocity

  // acceleration = dash_power * dash_power_rate * effort ->
  // dash_power = acceleration / (dash_power_rate * effort )
  double dDashPower = dAcc / (SS->getDashPowerRate () * dEffort);
  if (dDashPower > SS->getMaxPower ())
    return SS->getMaxPower ();
  else if (dDashPower < SS->getMinPower ())
    return SS->getMinPower ();
  else
    return dDashPower;
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//                                                  Orit                                                         //
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

/*! 判断是否可以将球传给队友。
 *  具体的判断过程：
 *    1.  首先找出传球范围内的队员t1, t2, ..., tn。
 *    2.  如果该名队友周围的人数很多，则其不在传球范围内。
 *    3.  对于每一个可行的队友，确定搜索范围（关于自己到队友连续对称的一个扇形），找出传球给每个队友的最佳路线。
 *    4.  比较所有队友的最佳路线，从而完成计算。
 *
 *    param \passT : 传球方式；
 *    param \posT : 接球者相对于自己的位置。
 *    param \tmt ： 记录可以接球的队友；
 *    param \posPass : 记录传球要到的位置，（该位置不一定就是接球队友的位置）； 
 * 
 */
bool
WorldModel::canSmartPassBallToTmt (PassT passT, ObjectT & tmt,
				   VecPosition & posPass)
{
  //return false;

  tmt = OBJECT_ILLEGAL;
  posPass = VecPosition (0, 0);

  VecPosition posAgent = getAgentGlobalPosition ();
  double myX = posAgent.getX ();
  double myY = posAgent.getY ();
  VecPosition posBall = getBallPos ();

  AngDeg angBody = getAgentGlobalBodyAngle ();
  AngDeg angEyeLeft = angBody - 75;
  AngDeg angEyeRight = angBody + 75;
  //AngDeg        angArea     = fabs(VecPosition::normalizeAngle(angEyeLeft-angEyeRight));        

  double speedStart = 0;	//球被踢出时的初始速度。
  int maxT = 20;		//搜索的最大周期。 
  int tSlow = 0;		//队友截球的延迟周期。

  //--确定总的搜索范围
  AngDeg angLeft = -180;	//传球路线左边最偏的方向
  AngDeg angRight = +180;	//传球路线右边最偏的方向
  AngDeg angInc = 5;		//传球路线搜索步长的度数

  if (myX <= 39 || fabs (myY) <= 15)
    {
      angLeft = -90;
      angRight = +90;
    }
  else if (myX > 39 && myY > 15)
    {
      angLeft = -180;
      angRight = -60;
    }
  else if (myX > 39 && myY < -15)
    {
      angLeft = 60;
      angRight = 180;
    }
  else
    {
      Log.logWithTime (777, "error!.........smart pass angle-left-right");
    }

  //--判断每一名队友。    
  int t;
  VecPosition posBallPre = posBall;	//球在t个周期后的位置。
  double dDistT = 0;		// t 周期后球运动的距离。
  double tmpDistToGoal = 0;	//球在t个周期后到球门的距离。
  double minDistToGoal = 1000;	//到球门的最短距离。
  double tmpDistToOpp = 0;	//球在t个周期后距对手最近的距离。
  double maxDistToOpp = -1000;	//到对手的最远距离。
  int timeBestPass = 0;		//最佳路线的截球周期。

  double dConfThr = PS->getPlayerConfThr ();	// for
  int iIndex;
  ObjectSetT setTmt = OBJECT_SET_TEAMMATES;
  for (ObjectT o = iterateObjectStart (iIndex, setTmt, dConfThr);
       o != OBJECT_ILLEGAL; o = iterateObjectNext (iIndex, setTmt, dConfThr))
    {

      if (getAgentObjectType () == o)	// don't include me
	continue;

      VecPosition posTmt = getGlobalPosition (o);
      AngDeg angTmt = (posTmt - posBall).getDirection ();	//角度范围外的不考虑
      if (angTmt < angLeft || angTmt > angRight)
	continue;

      /*      
         if (!isAngInInterval(angTmt, angLeft, angRight))
         continue;
       */
      /*
         if (!isAngInInterval(angTmt, angEyeLeft, angEyeRight))
         continue;
       */

      double ballToTmt = posBall.getDistanceTo (posTmt);
      if (passT == PASS_FAST)
	{
	  if (ballToTmt < 5)
	    {
	      speedStart = 2.0;
	      maxT = 3;
	      tSlow = 0;
	    }
	  else if (ballToTmt < 10)
	    {
	      speedStart = 2.2;
	      maxT = 6;
	      tSlow = 1;
	    }
	  else if (ballToTmt < 15)
	    {
	      speedStart = 2.5;
	      maxT = 8;
	      tSlow = 2;
	    }
	  else if (ballToTmt < 20)
	    {
	      speedStart = 2.7;
	      maxT = 12;
	      tSlow = 3;
	    }
	  else
	    continue;
	}
      else if (passT == PASS_NORMAL)
	{
	  if (ballToTmt < 5)
	    {
	      speedStart = 2.0;
	      maxT = 3;
	      tSlow = 0;
	    }
	  else if (ballToTmt < 10)
	    {
	      speedStart = 2.0;
	      maxT = 6;
	      tSlow = 1;
	    }
	  else if (ballToTmt < 15)
	    {
	      speedStart = 2.3;
	      maxT = 8;
	      tSlow = 2;
	    }
	  else if (ballToTmt < 20)
	    {
	      speedStart = 2.6;
	      maxT = 12;
	      tSlow = 3;
	    }
	  else if (ballToTmt < 25)
	    {
	      speedStart = 2.7;
	      maxT = 15;
	      tSlow = 3;
	    }
	  else
	    continue;
	}
      else
	Log.logWithTime (777, "error!..............can pass-pass type!");

      //确定队友tmt的搜索范围。
      AngDeg angL = angTmt - 5;
      AngDeg angR = angTmt + 5;

      //计算传给队友tmt的每一条传球路线的可能性。
      //--搜索每一条传球路线。 
      for (double angTry = angL; angTry <= angR; angTry += angInc)
	{
	  //判断ang是否在自己的观察范围内
	  AngDeg ang = VecPosition::normalizeAngle (angTry);

	  //if (!isAngInInterval(ang, angEyeLeft, angEyeRight))
	  //       continue;      

	  //ang在可观察的范围内
	  t = 1;
	  while (t < maxT)
	    {
	      dDistT =
		Geometry::getSumGeomSeries (speedStart, SS->getBallDecay (),
					    (double) t);
	      posBallPre = posBall + VecPosition (dDistT, ang, POLAR);

	      if (isPosOutOfCourt (posBallPre))
		break;		//球出界，失败路线
	      else if (getBallByOpp (posBallPre, t, tmpDistToOpp))
		break;		//先被对手拦截，失败路线
	      else
		{
		  double dTmtMove = SS->getPlayerSpeedMax () * (t - tSlow);
		  double dTmtKickable = dTmtMove + SS->getMaximalKickDist ();	//队友的可踢范围。
		  double dTmt2BallPre = posTmt.getDistanceTo (posBallPre);
		  if (dTmt2BallPre < dTmtKickable)
		    {		//可以被队友截住
		      tmpDistToGoal =
			posBallPre.getDistanceTo (VecPosition (52.5, 0));
		      if ((tmpDistToOpp > 5 && tmpDistToGoal < minDistToGoal)
			  || (tmpDistToOpp <= 5
			      && tmpDistToOpp > maxDistToOpp))
			{
			  //更新最佳路线。
			  timeBestPass = t;
			  tmt = o;
			  posPass = posBallPre;
			  minDistToGoal = tmpDistToGoal;
			  maxDistToOpp = tmpDistToOpp;
			}
		    }
		}
	      t++;
	    }			// while ( t < maxT )
	}			// for (angTry = angL;...)
    }
  iterateObjectDone (iIndex);

  //查看是否找到最优路线。
  if ((tmt != OBJECT_ILLEGAL) && (maxDistToOpp > 3))
    {
      Log.logWithTime (777, " smart pass to tmt(%d)",
		       SoccerTypes::getIndex (tmt) + 1);
      return true;
    }
  else
    return false;

}

/*! 判断球是否会被对手截住。
 *
 *     参数 posBallPre : 球运动t个周期后的位置
 *     参数 t : 球运动的周期数
 *     参数 minDistToOpp : 球到对手的最近距离
 *     
 */
bool
WorldModel::getBallByOpp (VecPosition posBallPre, int t, double &minDistToOpp)
{
  minDistToOpp = 1000;
  //////////////////////

  double dConfThr = PS->getPlayerConfThr ();	// for
  int iIndex;
  ObjectSetT setOpp = OBJECT_SET_OPPONENTS;

  bool catchedByOpp = false;
  ObjectT oppGoalie = getOppGoalieType ();	//守门员
  VecPosition posAgent = getAgentGlobalPosition ();

  for (ObjectT o = iterateObjectStart (iIndex, setOpp, dConfThr);
       o != OBJECT_ILLEGAL; o = iterateObjectNext (iIndex, setOpp, dConfThr))
    {