worldmodelhighlevel.c

uva trilearn的robocup源程序

    else
    {
      angGoalie = VecPosition::normalizeAngle( angGoalie + 33 );
      angMin    = min( angMax, max( angMin, angGoalie ) );      
    }
    Log.log( 560, "direction_widest_angle after: %f %f", angMin, angMax );
  }
       

  // Create new list with only opponents from interval [angMin..angMax].
  // Note that opponents outside angMin and angMax can have an influence
  // on the largest angle between the opponents, so they should be accounted
  // for. To this end, a projection is defined in both angMin and angMax.
  // The opponent with the smallest global angle difference a to angMin
  // (either inside or outside the interval [angMin..angMax]) is determined
  // and an extra angle angMin - a is added to the list. The situation for
  // angMax is analogous.

  double absMin     = 1000;
  double absMax     = 1000;
  double angProjMin = angMin;
  double angProjMax = angMax;
  double array[MAX_OPPONENTS+2];

  while( v.size() > 0 )
  {
    if( fabs( v.front() - angMin ) < absMin )      // opp near angMin
    {
      absMin     = fabs( v.front() - angMin ) ;    // make angMin wider
      angProjMin = angMin - absMin;                // to take him into account
    }
    if( fabs( v.front() - angMax ) < absMax )      // opp near angMax
    {
      absMax     = fabs( v.front() - angMax ) ;    // make angMax wider
      angProjMax = angMax + absMax;                // to take him into account
    }
    if( v.front() > angMin && v.front() < angMax ) // opp in range
      v2.push_back( v.front() );                   // add him
    v.pop_front();
  }

  // make all angles relative to angProjMin which has angle 0 and set them in
  // the range 0..360, where the range -180..0 is moved to 180..360. Do this by
  // adding 360 and then subtracting 360 if value is larger than 360.
  v.push_back( 0 );
  while( v2.size() > 0 )  // for all the opponents
  {
    temp = VecPosition::normalizeAngle(v2.front()-angProjMin)+360.0;
    if( temp > 360 )
      temp -= 360;
    v.push_back( temp );
    v2.pop_front();
  }
  // add max projection.
  temp = VecPosition::normalizeAngle(angProjMax-angProjMin)+360.0;
  if( temp > 360 )
    temp -= 360;

  v.push_back( temp );

  // sort the list
  v.sort();

  // put all the values in an array
  int i = 0;
  while( v.size() > 0 )
  {
    array[i++] = v.front();
    v.pop_front();
  }

  // find the largest angle and determine the associated midpoint direction
  double dLargest = -1000;
  double d;
  double ang      = UnknownAngleValue;
  for( int j = 0; j < i - 1 ; j ++ )
  {
    d = VecPosition::normalizeAngle(( array[j+1] - array[j] )/2.0);
    if( d > dLargest )
    {
      ang = angProjMin + array[j] + d;
      ang = VecPosition::normalizeAngle( ang );
      dLargest = d;
    }
  }

  if( ang == UnknownAngleValue ) // no angle found -> get angle in between
  {
    ang = getBisectorTwoAngles( angMin, angMax );
    if( angLargest != NULL )
      *angLargest = 360;
  }
  else if( angLargest != NULL )
    *angLargest = dLargest;

  return ang;
}


/*! This method determine the strategic position for the specified player. This 
    is done using the Formations class. In this class all information
    about the current formation, player number in formation and otheic
    values are stored. The strategic position is based on the position of
    the ball. If the confidence in the position of the ball is lower than the
    threshold defined in PlayerSettings, it is assumed that the ball is at
    position (0,0).
    \param iPlayer role in formation for which strategic position should be 
      returnd. With default value is -1 it is assumed that the strategic
      position of the agent itself should be returned. 
    \return VecPosition strategic position for player 'iPlayer' */
VecPosition WorldModel::getStrategicPosition( int iPlayer )
{
  VecPosition pos;
  
  // -1 is default -> get player number in formation
  if( iPlayer == -1 )
    iPlayer = formations->getPlayerInFormation();

  // get maximal allowed x coordinate, this is offside x coordinate
  double dMaxX = getOffsideX() - 3.0;

  // after standing offside we are not allowed to move for ball
  // with a goal kick of them we are not allowed to move into penalty area
  if( isOffsideUs( ) )
    dMaxX = min( dMaxX, getBallPos().getX() - 1.0 );
  if( isGoalKickThem() )
    dMaxX = min( dMaxX, PENALTY_X - 1.0 );
  if( isBeforeKickOff() )
    dMaxX = min( dMaxX, -2.0 );

  // change the ball position on which strategic position is based depending on
  // the different deadball situation and thus the expected movement of the ball
  VecPosition posBall = getBallPos();    
  if( isGoalKickUs() || 
      getTimeSinceLastCatch(  ) < PS->getCyclesCatchWait() + 5  )    
    posBall.setX( -PITCH_LENGTH/4 - 5.0 );
  else if( getConfidence( OBJECT_BALL ) < PS->getBallConfThr() )
    posBall.setVecPosition( 0.0, 0.0 );
  else if( isGoalKickThem() || 
           ( isFreeKickThem() && posBall.getX() > PENALTY_X ) )
    posBall.setX( PENALTY_X - 10.0 );

  // get the strategic position
  pos = formations->getStrategicPosition( iPlayer, posBall, dMaxX );

  return pos;
}


/*! The actual power with which the ball is kicked depends on the
    relative location of the ball to the player. The kick is more
    powerful when the ball is very close to and in front of the player.
    The actual kickpowerrate with which the power of the kick command
    is multiplied is equal to<BR>
    KickPowerRate*(1 - 0.25*DirDiff / 180 -
    0.25*(DistBall-PlayerSize-BallSize)/KickableMargin)<BR>
    with DirDiff = global angle of the ball rel to the body dir agent<BR>
         DistBall = the distance from the center of the player to the ball<BR>
    See soccermanual
    \return the actual kick power rate with which power is multiplied  */
double WorldModel::getActualKickPowerRate( )
{
 // true indicates that relative angle to body should be returned
 double dir_diff      = fabs( getRelativeAngle( OBJECT_BALL, true ) );
 double dist          = getRelativeDistance( OBJECT_BALL ) -
                        SS->getPlayerSize( ) - SS->getBallSize( );
 return SS->getKickPowerRate() *
           ( 1 - 0.25 * dir_diff/180.0 - 0.25 * dist / SS->getKickableMargin());
}

/*! The actual power with which the ball must be kicked depends on the
    relative location of the ball to the player. The kick is more
    powerful when the ball is very close to and in front of the player.
    The actual power with which the ball must be kicked is equal to<BR>
    Speed / KickPowerRate*(1 - 0.25*DirDiff / 180 -
    0.25*(DistBall-PlayerSize-BallSize)/KickableMargin)<BR>
    with DirDiff = global angle of the ball rel to the body dir agent<BR>
         DistBall = the distance from the center of the player to the ball<BR>
    See soccermanual for further information.
    This method receives a speed vector which the ball should have after the
    kick command and calculates the power for the kick command to reach this.
    This value can be higher than is possible to shoot!
    \param dDesiredSpeed the desired speed after the kick command
    \return the actual power for kick command to get dDesiredSpeed */
double WorldModel::getKickPowerForSpeed( double dDesiredSpeed )
{
  // acceleration after kick is calculated by power * eff_kick_power_rate
  // so actual kick power is acceleration / eff_kick_power_rate
  return dDesiredSpeed / getActualKickPowerRate( );
}


/*! 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;
}