worldmodelhighlevel.cpp

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

    Log.log( 553, "predicted pos %d cycles: (%f,%f)" ,
      iCycle, pos.getX(), pos.getY() );
    iCycle ++;
  }

  return ( iCycle == 20 ) ? false : true;
}

/*! This method returns whether the ball is in our possesion. This is defined
     by the fact if the fastest player to the ball is a teammate or not.
     \return bool indicating whether a teammate is the fastest player to the
             ball. */
bool WorldModel::isBallInOurPossesion( )
{
  int     iCyc;
  ObjectT o = getFastestInSetTo( OBJECT_SET_PLAYERS, OBJECT_BALL, &iCyc );

  if( o == OBJECT_ILLEGAL )
    return false;
  if( SoccerTypes::isTeammate( o ) )
    return true;
  else
    return false;
}

/*! This method returns whether the ball lies in the own penalty area.
    \return bool indicating whether ball lies in own penalty area.     */
bool WorldModel::isBallInOwnPenaltyArea( )
{
  return isInOwnPenaltyArea( getBallPos() );
}

/*! This method returns whether the specified position lies in the own penalty
    area.
    \param pos position which should be checked
    \return bool indicating whether 'pos' lies in own penalty area. */
bool WorldModel::isInOwnPenaltyArea( VecPosition pos )
{
  ObjectT     objFlag = ( getSide() == SIDE_LEFT  )
                              ?  OBJECT_FLAG_P_L_C
                              :  OBJECT_FLAG_P_R_C ;

  if( isPenaltyUs() || isPenaltyThem() )
    objFlag = ( getSidePenalty() == SIDE_LEFT ) ? OBJECT_FLAG_P_L_C 
                                                : OBJECT_FLAG_P_R_C ;
  VecPosition posFlag =SoccerTypes::getGlobalPositionFlag( objFlag, getSide());
  if( fabs(pos.getX())   > fabs(posFlag.getX()) &&
      fabs( pos.getY() ) < PENALTY_AREA_WIDTH/2.0 )
    return true;

  return false;
}

/*! This method returns whether the specified position lies in the opponent
    penalty area.
    \param pos position which should be checked
    \return boolean indicating whether 'pos' lies in opponent penalty area. */
bool WorldModel::isInTheirPenaltyArea( VecPosition pos )
{
  ObjectT     objFlag = ( getSide() == SIDE_LEFT )
                              ?  OBJECT_FLAG_P_R_C
                              :  OBJECT_FLAG_P_L_C ;
  VecPosition posFlag = SoccerTypes::getGlobalPositionFlag( objFlag,getSide());

  if ( pos.getX() > posFlag.getX() &&
       fabs(pos.getY()) < PENALTY_AREA_WIDTH/2.0 )
    return true;

  return false;
}

/*! This method determines whether the confidence for 'o' is good. The
    confidence of the object is compared to the player_conf_thr
    defined in PlayerSettings. When the confidence is higher than this
    value and the object does not equal the agent object type true is
    returned, otherwise false.

    \param o object of which confidence value should be returned
    \return bool indicating whether object information has good confidence. */
bool WorldModel::isConfidenceGood( ObjectT o )
{
  return getConfidence( o ) > PS->getPlayerConfThr() &&
         o != getAgentObjectType();
}

/*! This method determines whether the confidence for 'o' is very
    good. The confidence of the object is compared to the
    player_high_conf_thr defined in PlayerSettings. When the
    confidence is higher than this value and the object does not equal
    the agent object type true is returned, otherwise false.

    \param o object of which confidence value should be returned
    \return bool indicating whether object information has good confidence. */
bool WorldModel::isConfidenceVeryGood( ObjectT o )
{
  return getConfidence( o ) > PS->getPlayerHighConfThr() &&
         o != getAgentObjectType();
}

/*! This method checks whether the specified object stands onside. This is done
    by comparing the x coordinate of the object to the offside line.
    \return boolean indicating whether 'obj' stands onside. */
bool WorldModel::isOnside( ObjectT obj )
{
  return getGlobalPosition( obj ).getX() < getOffsideX() - 0.5 ;
}

/*! This method determines whether there stands an opponent in the global
    direction of the specified angle and in distance 'dDist'. An opponent is
    considered to stand in the global direction when the angle difference with
    the specified angle is smaller than 60 degrees.
    \param ang angle of the global direction in which to check opponents
    \param dDist distance in which opponents should be checked
    \return bool indicating wheter an opponent was found. */
bool WorldModel::isOpponentAtAngle( AngDeg ang , double dDist )
{
  VecPosition posAgent   = getAgentGlobalPosition();
  VecPosition posOpp;
  AngDeg      angOpp;
  int         iIndex;

  for( ObjectT o = iterateObjectStart( iIndex, OBJECT_SET_OPPONENTS );
       o != OBJECT_ILLEGAL;
       o = iterateObjectNext ( iIndex, OBJECT_SET_OPPONENTS ) )
  {
    posOpp    = getGlobalPosition( o );
    angOpp    = ( posOpp - posAgent ).getDirection() ;
    if( fabs( angOpp - ang ) < 60 &&
        posAgent.getDistanceTo( posOpp ) < dDist )
      return true;
    else if( fabs( angOpp - ang ) < 120 &&
             posAgent.getDistanceTo( posOpp ) < dDist/2.0 )
      return true;
  }
  iterateObjectDone( iIndex );
  return false;
}

/*! This method returns the inverse confidence, i.e. the time that belongs
    to the specified confidence. This can be used to determine
    the time the object was last seen when the confidence is given. Herefore
    the current time is used.
    \param dConf confidence
    \return server cycle the object was last seen. */
Time WorldModel::getTimeFromConfidence( double dConf )
{
  return getCurrentTime()-(int)((1.00-dConf)*100);
}

/*! This method returns the object type of the last opponent defender. This
    opponent resembles the offside line. 
    \param if non-null dX will be filled with the x position of this object
    \return object type of the last opponent defender */
ObjectT WorldModel::getLastOpponentDefender( double *dX )
{
  double  dHighestX = 0.0;
  double  dSecondX  = 0.0, x;

  ObjectT o, oLast = OBJECT_ILLEGAL, oSecondLast = OBJECT_ILLEGAL;
  for( int i = 0; i < MAX_OPPONENTS ; i ++ )
  {
    o = Opponents[i].getType();
    if( isConfidenceGood( o ) )
    {
      x = Opponents[i].getGlobalPosition().getX();
      if( x > dHighestX )         // if larger x than highest
      {
        dSecondX    = dHighestX;  // make second the previous highest
        dHighestX   = x;          // and this the new one
        oSecondLast = oLast;
        oLast       = o;
      }
      else if( x > dSecondX )     // if smaller than 1st  and larger than 2nd
      {
        dSecondX    = x;          // make it the second
        oSecondLast = o;
      }
    }
  }
  
  // if highest x is outside pen_area, it cannot be the goalie (unless playing
  // Portugal ;-) ), so assume goalie is just not seen
  if( dHighestX < PENALTY_X && getOppGoalieType() == OBJECT_ILLEGAL )
  {
    dSecondX    = dHighestX;
    oSecondLast = oLast;
  }
  if( dX != NULL )
    *dX = dSecondX ;
  return oSecondLast;
}

/*! This method returns the x coordinate of the offside line using the known
    information in the WorldModel. If a player moves beyond this line, he
    stands offside. First the opponent with the second highest x coordinate is
    located, then the maximum of this x coordinate and the ball x coordinate
    is returned.
    \param bIncludeComm boolean indicating whether communicated offside line
          should also be included.
    \return x coordinate of the offside line. */
double WorldModel::getOffsideX( bool bIncludeComm )
{
  double  x, dAgentX;

  getLastOpponentDefender( &dAgentX );
  x = getBallPos().getX();
  x = max( x, dAgentX );
  if( bIncludeComm == true && getCurrentTime() - m_timeCommOffsideX < 3 )
    x = max( x, m_dCommOffsideX );
  return x ;
}

/*! This method returns the outer position on the field given a position 'pos'
    and a global angle 'ang'. The outer position is defined as the point on
    the field where the line created from this position and angle crosses
    either a side line, goal line or penalty line. To be on the safe side a
    small value is specified, which denotes the distance from the side line
    that should be returned.
    \param pos position on the field from which outer position should be
    calculated
    \param ang global angle which denotes the global direction in pos
    \param dDist distance from line
    \param bWithPenalty boolean denoting whether penalty area should be taken
                        into account (if false only goal line and side line
                        are used.
    \return position denoting the outer position on the field */
VecPosition WorldModel::getOuterPositionInField( VecPosition pos, AngDeg ang,
                                  double dDist, bool bWithPenalty )
{
  VecPosition posShoot;

  // make shooting line using position and desired direction
  Line lineObj     = Line::makeLineFromPositionAndAngle( pos, ang );

  // get intersection point between the created line and goal line
  Line lineLength  = Line::makeLineFromPositionAndAngle(
                            VecPosition( PITCH_LENGTH/2.0 - dDist, 0.0 ), 90 );
  posShoot         = lineObj.getIntersection( lineLength );

  // check whether it first crosses the penalty line
  Line linePenalty = Line::makeLineFromPositionAndAngle(
                            VecPosition( PENALTY_X - dDist, 0.0 ), 90.0 );
  double dPenaltyY = lineObj.getIntersection(linePenalty).getY();

  if( bWithPenalty && fabs(dPenaltyY) < PENALTY_AREA_WIDTH/2.0 )
  {
    if( fabs(dPenaltyY) < PENALTY_AREA_WIDTH/2.0 - 5.0 ||   // crosses inside
        fabs(posShoot.getY()) <  PENALTY_AREA_WIDTH/2.0 )   // or ends inside
      posShoot = lineObj.getIntersection( linePenalty );
  }

  // check where it crosses the side line
  Line lineSide = ( ang < 0 )
     ? Line::makeLineFromPositionAndAngle(
                           VecPosition( 0.0, - PITCH_WIDTH/2.0 + dDist ),0.0 )
     : Line::makeLineFromPositionAndAngle(
                           VecPosition( 0.0, + PITCH_WIDTH/2.0 - dDist ),0.0 );

  if( fabs(posShoot.getY()) > PITCH_WIDTH/2.0 - dDist )
    posShoot = lineObj.getIntersection( lineSide );

  return posShoot;
}

/*! This method determines the (global) direction which has the largest
    angle between the opponents and is located in the interval angMin..
    angMax.
    \param origin of which the angles angMin and angMax are based on.
    \param angMin minimal global direction that should be returned
    \param angMax maximal global direction that should be returned
    \param angLargest will contain the size of the largest angle of the
           direction that is returned
    \param dDist only opponents with relative distance smaller than this value
           will be taken into account.
    \return global direction with the largest angle between opponents */
AngDeg WorldModel::getDirectionOfWidestAngle(VecPosition posOrg, AngDeg angMin,
                               AngDeg angMax, AngDeg *angLargest, double dDist)
{
  list<double> v;
  list<double> v2;
  double       temp;
  int          iIndex;
  double       dConf  = PS->getPlayerConfThr();

  // add all angles of all the opponents to the list v
  for( ObjectT o = iterateObjectStart( iIndex, OBJECT_SET_OPPONENTS, dConf );
       o != OBJECT_ILLEGAL;
       o = iterateObjectNext ( iIndex, OBJECT_SET_OPPONENTS, dConf ) )
  {
    if( getRelativeDistance( o ) < dDist )
      v.push_back( (getGlobalPosition(o)-posOrg).getDirection());
  }
  iterateObjectDone( iIndex );
  v.sort();

  // if goalkeeper is spotted and he is located within the range that we want
  // to shoot at, make sure the angle with the goalkeeper is large enough, since
  // he has better intercepting capabilities than the normal players

  ObjectT     objGoalie = getOppGoalieType();
  VecPosition posGoalie = getGlobalPosition( objGoalie );
  AngDeg      angGoalie;

  if( objGoalie != OBJECT_ILLEGAL && posOrg.getX() > PITCH_LENGTH/4.0 &&
      posOrg.getDistanceTo( posGoalie ) < dDist )
  {
    angGoalie = ( posGoalie - posOrg ).getDirection();
    Log.log( 560, "direction_widest_angle: min %f max %f angGoalie %f",
                                                  angMin, angMax, angGoalie );

    if( posOrg.getY() > 0 ) // right side of the field
    {
      angGoalie = VecPosition::normalizeAngle( angGoalie - 33 );
      angMax    = max( angMin, min( angGoalie, angMax ) );
    }
    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 )