worldmodelhighlevel.cpp

2003年RoboCup仿真组世界冠军源代码 足球机器人 仿真组 的源代码

  {
    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 returns whether the position 'pos' is inside the playfield. */
bool WorldModel::isInField( VecPosition pos, double dMargin )
{
  return Rect( 
             VecPosition( + PITCH_LENGTH/2.0 - dMargin, 
                          - PITCH_WIDTH/2.0  + dMargin ),
             VecPosition( - PITCH_LENGTH/2.0 + dMargin, 
                          + PITCH_WIDTH/2.0  - dMargin ) 
             ).isInside( pos );
}

/*! This method returns whether the position 'pos' is before the opp goal. */
bool WorldModel::isBeforeGoal( VecPosition pos )
{
  return Rect(
             VecPosition( + PENALTY_X - 2,    - ( SS->getGoalWidth()/2.0 + 1)),
             VecPosition( + PITCH_LENGTH/2.0, + ( SS->getGoalWidth()/2.0 + 1))
             ).isInside( pos );
}

/*! This method determine the strategic position for the specified object. 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 obj for which the strategic position should be calculated.
    \param ft formation for which to calculate the strategic position
    \return VecPosition strategic position for player 'iPlayer' */
VecPosition WorldModel::getStrategicPosition( ObjectT obj, FormationT ft )
{
  return getStrategicPosition( SoccerTypes::getIndex( obj ), ft );
}
  
/*! 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.
    \param ft formation for which to calculate the strategic position
    \return VecPosition strategic position for player 'iPlayer' */
VecPosition WorldModel::getStrategicPosition( int iPlayer, FormationT ft )
{
  if( iPlayer > MAX_TEAMMATES )
    cerr << "WM:getStrategicPosition with player nr " << iPlayer << endl;

  VecPosition pos, posBall = getBallPos();
  bool bOwnBall = isBallInOurPossesion();

  // -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 = max( -0.5, getOffsideX() - 1.5 );

  if( bOwnBall && 
      getGlobalPosition(
        SoccerTypes::getTeammateObjectFromIndex(iPlayer)).getX() 
      < posBall.getX() )
    dMaxX = max( dMaxX, posBall.getX()  );

  // 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( isGoalKickThem() )
    dMaxX = min( dMaxX, PENALTY_X - 1.0 );
  else if( isBeforeKickOff() )
    dMaxX = min( dMaxX, -2.0 );
  else if ( isOffsideUs() )
    dMaxX = posBall.getX() - 0.5;

  // change the ball position on which strategic position is based
  // depending on the different deadball situation and thus the
  // expected movement of the ball
  if( isBeforeKickOff() )
    posBall.setVecPosition( 0, 0 );
  else if( isGoalKickUs() ||
      getTimeSinceLastCatch(  ) < PS->getCyclesCatchWait() + 5  ||
      ( isFreeKickUs() && posBall.getX() < - PENALTY_X ) )
    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 );
  else if( isFreeKickThem() )
    posBall.setX( posBall.getX() - 5.0 );
  else if( isBallInOurPossesion() && 
           !( isDeadBallUs() || isDeadBallThem() ) )
    posBall.setX( posBall.getX() + 5.0 );
  else if( posBall.getX() < - PENALTY_X + 5.0 )
    posBall = predictPosAfterNrCycles( OBJECT_BALL, 3 );

  // get the strategic position
  pos = formations->getStrategicPosition( iPlayer, posBall, dMaxX,
                          bOwnBall, PS->getMaxYPercentage(),
					  ft );
  return pos;
}

/*! This method returns a global position on the field which denotes
    the position to mark position 'pos'. It receives three arguments:
    a position pos (usually an opponent) that the agent wants to mark,
    a distance 'dDist' representing the desired distance between o and
    the marking position and a type indicator that denotes the type of
    marking that is required. We distinguish three types of marking: -
    MARK BALL: marking the opponent by standing at a distance 'dDist'
    away from him on the line between him and the ball. This type of
    marking will make it di眂ult for the opponent to receive a pass.
    - MARK GOAL: marking the opponent by standing at a distance
    'dDist' away from him on the line between him and the center point
    of the goal he attacks. This type of marking will make it
    difficult for the opponent to score a goal.  - MARK BISECTOR:
    marking the opponent by standing at a distance 'dDist' away from
    him on the bisector of the ball-opponent-goal angle. This type of
    marking enables the agent to intercept both a direct and a leading
    pass to the opponent.  \param pos position that has to be marked
    \param dDist distance marking position is located from object
    position \param mark marking technique that should be used \return
    position that is the marking position. */
VecPosition WorldModel::getMarkingPosition( VecPosition pos, double dDist, 
					    MarkT mark)
{
  VecPosition posBall  = getBallPos();
    //edictPosAfterNrCycles( OBJECT_BALL, 3 );
  VecPosition posGoal  = getPosOwnGoal( );
  if( posBall.getX() < - PITCH_LENGTH/2.0 + 10.0 )
    posGoal.setX( posBall.getX() + 1  );
  else if( posBall.getX() > -PITCH_LENGTH/3.0 )
  {
    posGoal.setX( -PITCH_LENGTH/2.0 );
    double dY  = posBall.getY();
    if( fabs( dY ) > 12 )
      dY += ( sign( dY ) > 0   ) -5 ? : 5 ;
    posGoal.setY( dY );
  }
      
  VecPosition posAgent = getAgentGlobalPosition();
  VecPosition posMark;
  AngDeg      ang, angToGoal, angToBall;

  if( mark == MARK_GOAL )                       // position in direction goal
  {   
    angToGoal = (posGoal-pos).getDirection( );    
    Line line = Line::makeLineFromTwoPoints( pos, posGoal );

    // we want to know when distance from ball to point p equals distance
    // from opp to point p :
    // d1 + d3 = sqrt(d2^2 + d3^2) > (d1+d3)^2 = d2^2 + d3^2 =>
    // d1^2 + 2*d1*d3 = d2^2 -> d3 = (d2^2 - d1^2 ) / 2*d1
    double dCalcDist;
    VecPosition posIntersect = line.getPointOnLineClosestTo( posAgent );
    double dDistAgent = posIntersect.getDistanceTo( posAgent );
    double dDistOpp = posIntersect.getDistanceTo( pos );
    dCalcDist = (dDistAgent*dDistAgent-dDistOpp*dDistOpp)/(2*dDistOpp);
    double dExtra = 2.0;
    //    if( posBall.getX() <  PENALTY_X + 5 )
    if( pos.getDistanceTo(posAgent) < 5 )
      dExtra = 0.0;
    dCalcDist += dDistOpp + dExtra;
    Log.log( 513, "dDistOpp %f dDistAgent %f calc %f min %f",
	     dDistOpp, dDistAgent, dCalcDist, 0.75*pos.getDistanceTo(posGoal));
    dCalcDist = min( dCalcDist, 0.75*pos.getDistanceTo( posGoal ) );
    double x = -PITCH_LENGTH/2 + 4;
    double y = line.getYGivenX( x);
    posMark = pos + VecPosition( dCalcDist, angToGoal, POLAR );
    if( posMark.getX() < x )
      {
	Log.log( 513, "change posmark to (%f,%f)", x, y );
	posMark.setVecPosition( x, y );
      }
    // if interception point iss outside range or very close to marking
    // point, but far away from opp (is this possible?) move closer.
    if( ! line.isInBetween( posMark, pos, posGoal ) ||
	( posMark.getDistanceTo( posAgent ) < 1.5 &&
	  posMark.getDistanceTo( pos ) > 2*dDist ) )
    {
      Log.log( 513, "set marking position at dDist %f", min(dDistAgent,7.0) );
      posMark   = pos + VecPosition( min( dDistAgent, 7.0 ), angToGoal, POLAR );
    }
    Log.log( 513, "marking position calc (%f,%f) pos(%f,%f) calcdist %f", 
	     posMark.getX(), posMark.getY(), pos.getX(), pos.getY(), 
	     dCalcDist );
  }
  else if( mark == MARK_BALL )                  // position in direction ball
  {
    angToBall = (posBall-pos).getDirection( );
    posMark   = pos + VecPosition( dDist, angToBall, POLAR );
  }
  else if( mark == MARK_BISECTOR )             // pos between ball and goal
  {
    angToBall = (posBall - pos).getDirection( );
    angToGoal = (posGoal - pos).getDirection( );
    ang       = getBisectorTwoAngles( angToBall, angToGoal );
    posMark   = pos + VecPosition( dDist, ang ,POLAR );
  }
  if( fabs( posMark.getX() ) > PITCH_LENGTH/2.0 - 2.0 )
    posMark.setX( sign(posMark.getX())*(PITCH_LENGTH/2.0 - 2.0) );
  return posMark;

}


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

  /