behavior.cpp.svn-base

一个巴西大学编写的2D仿真组足球机器人程序

  // when it is theoretical possible
  // try one dash and check whether ball is in catchable area
  dDist =SS->getBallSpeedMax()+SS->getPlayerSpeedMax()+SS->getCatchableAreaL();
  if( WM->getRelativeDistance( OBJECT_BALL ) < dDist )
  {
    posClosestToBall = lineGoalie.getPointOnLineClosestTo( posBall );
    dPower           = WM->getPowerForDash(
                          posClosestToBall-posPred,
                          angBody,
                          velMe,
                          sta.getEffort()           );
    posPred          = WM->predictAgentPos( 1, (int)dPower);
    if ( posPred.getDistanceTo( posBall ) < SS->getCatchableAreaL() )
      return SoccerCommand( CMD_DASH, dPower );
  }

  // when it is theoretical possible
  // try two dashes and check whether ball is in catchable area
  // otherwise try first two  dashes and check whether ball is in catchable
  // area, thereafter for turn and dash.
  dDist = SS->getBallSpeedMax()*(1.0+SS->getBallDecay())
           + 2*SS->getPlayerSpeedMax()
           + SS->getCatchableAreaL();
  if( WM->getRelativeDistance( OBJECT_BALL ) < dDist )
  {
    // try two dashes
    // first predict the position in the next cycle when dash with full power
    // is performed. Then calculate the dash power to reach the point where the
    // ball will be in two cycles and predict the global position of the agent
    // after a dash with this power. If the position is in the catchable area
    // return a dash with full power.
    posBall = WM->predictPosAfterNrCycles( OBJECT_BALL, 2 );
    soc     = dashToPoint( posBall );
    WM->predictAgentStateAfterCommand(soc,&posPred,&velMe,&angBody,&ang,&sta );
    dPower=WM->getPowerForDash(posBall-posPred,angBody,velMe,sta.getEffort());
    WM->predictStateAfterDash( dPower, &posPred, &velMe, &sta, angBody,
                               WM->getAgentObjectType() );
    if( posPred.getDistanceTo(posBall) < SS->getCatchableAreaL() )
      return soc;

    // try one turn and a dash
    posBall = WM->predictPosAfterNrCycles( OBJECT_BALL, 2 );
    posPred = WM->predictAgentPos( 2, 0 );
    ang     = (posBall - posPred).getDirection();
    ang     = VecPosition::normalizeAngle( ang - angBody );
    if (fabs( ang ) > PS->getPlayerWhenToTurnAngle() ) // if we want to turn
    {
      soc = turnBodyToPoint( posBall, 2 );             // perform turn
      WM->predictAgentStateAfterCommand(soc,&posPred,&velMe,&angBody,&ang,&sta
);
      dPower=WM->getPowerForDash(posBall-posPred, angBody, 
                                 velMe, sta.getEffort());
      WM->predictStateAfterDash( dPower, &posPred, &velMe, &sta, angBody);
      if( posPred.getDistanceTo(posBall) < SS->getCatchableAreaL() )
        return soc;
    }
  }

  // did not succeed
  return SoccerCommand( CMD_ILLEGAL );
}

/*! This skill enables an agent to kick the ball from its current
    position to a given position 'posTarget' in such a way that it has
    a remaining speed equal to 'dEndSpeed' when it reaches this
    position. However, it is possible that the ball cannot reach this
    velocity with a single kick either because the needed magnitude of
    the generated velocity vector exceeds the maximum speed of the
    ball or due to the fact that the current ball speed in combination
    with the position of the ball relative to the agent make it
    impossible to achieve the required acceleration. If the magnitude
    of needed velocity vector is larger than ball speed max it is
    certain that even in the optimal situation (i.e. if the ball lies
    directly in front of the agent and has zero velocity) the agent
    will not be able to kick the ball to the target position at the
    desired speed. In this case the expected ball movement is computed
    after executing a kick with maximum power into the direction of
    'posTarget'.  If the magnitude of the resulting movement vector is
    larger than a given percentage ('getPlayerWhenToKick' defined in
    the PlayerSettings) of the maximum ball speed then this kick is
    actually performed despite the fact that it cannot produce the
    wanted effect. Otherwise, the agent shoots the ball close to his
    body and directly in front of him using the kickBallCloseToBody
    skill. In this way he will be able to kick the ball with more
    power in the next cycle. However, if the magnitude of the desired
    velocity vector is smaller than ball speed max it is possible to
    reach the target point at the desired speed in the optimal
    situation.  If the power that must be supplied to the kick command
    to achieve this acceleration is less than or equal to the maximum
    power the accelerateBallToVelocity skill is used to perform the
    desired kick.  Otherwise, the agent uses the kickBallCloseToBody
    skill to put the ball in a better kicking position for the next
    cycle.

   \param posTarget target position where the ball should be shot to
   \param endSpeed desired speed ball should have in target position
   \return SoccerCommand that produces kick  */
SoccerCommand Behavior::kickTo( VecPosition posTarget, double dEndSpeed )
{
  VecPosition posBall  = WM->getBallPos();
  VecPosition velBall  = WM->getGlobalVelocity(OBJECT_BALL);
  VecPosition posTraj  = posTarget - posBall;
  VecPosition posAgent = WM->getAgentGlobalPosition();
  VecPosition velDes   = VecPosition(
               WM->getKickSpeedToTravel( posTraj.getMagnitude(), dEndSpeed ),
               posTraj.getDirection(),
               POLAR                                                       ); 
  double      dPower;
  AngDeg      angActual;

  if( WM->predictAgentPos(1, 0 ).getDistanceTo( posBall + velDes ) < 
      SS->getBallSize() + SS->getPlayerSize() )
  {
    Line line = Line::makeLineFromTwoPoints( posBall, posBall + velDes );
    VecPosition posBodyProj = line.getPointOnLineClosestTo( posAgent );
    double dDist = posBall.getDistanceTo( posBodyProj );
    if( velDes.getMagnitude() < dDist )
      dDist -=  SS->getBallSize() + SS->getPlayerSize();
    else
      dDist +=  SS->getBallSize() + SS->getPlayerSize();
    Log.log( 101, "kick results in collision, change velDes from (%f,%f)",
	     velDes.getX(), velDes.getY() );
    velDes.setVecPosition( dDist, velDes.getDirection(), POLAR );
  }

  Log.log( 101, "ball (%f,%f), agent (%f,%f), to (%f,%f) ang %f %f" ,
          WM->getBallPos().getX(), WM->getBallPos().getY(),
          WM->getAgentGlobalPosition().getX(),
          WM->getAgentGlobalPosition().getY(),
          posTarget.getX(), posTarget.getY(),
          WM->getAgentGlobalBodyAngle(),
          WM->getAgentGlobalNeckAngle() );
  Log.log( 101, "relpos body (%f,%f), vel. ball:(%f,%f) dist: %f (%f,%f,%f)" ,
          WM->getRelativeDistance( OBJECT_BALL ),
          WM->getRelativeAngle( OBJECT_BALL, true ),
           velBall.getX(), velBall.getY(),
           SS->getMaximalKickDist(), SS->getPlayerSize(), SS->getBallSize(),
             SS->getKickableMargin() );


  double   dDistOpp;
  ObjectT  objOpp     = WM->getClosestInSetTo( OBJECT_SET_OPPONENTS, 
                                                 OBJECT_BALL, &dDistOpp );

  if( velDes.getMagnitude() > SS->getBallSpeedMax() ) // can never reach point
  {
             dPower     = SS->getMaxPower();
    double   dSpeed     = WM->getActualKickPowerRate() * dPower;
    double   tmp        = velBall.rotate(-velDes.getDirection()).getY();
             angActual  = velDes.getDirection() - asinDeg( tmp / dSpeed );
    double   dSpeedPred = (WM->getGlobalVelocity(OBJECT_BALL)+
                           VecPosition(dSpeed,angActual, POLAR )
                          ).getMagnitude();
  
    // but ball acceleration in right direction is very high
    if( dSpeedPred > PS->getPlayerWhenToKick()*SS->getBallAccelMax() )
    {
      Log.log( 101, "pos (%f,%f) too far, but can acc ball good to %f k=%f,%f",
               velDes.getX(), velDes.getY(), dSpeedPred, dSpeed, tmp );
      return accelerateBallToVelocity( velDes );    // shoot nevertheless
    }
    else if( WM->getActualKickPowerRate() >
             PS->getPlayerWhenToKick() * SS->getKickPowerRate() )
    {
      Log.log( 101, "point too far, freeze ball" ); // ball well-positioned
      return freezeBall();                          // freeze ball
    }
    else
    {
      Log.log( 101, "point too far, reposition ball (k_r = %f)",
               WM->getActualKickPowerRate()/(SS->getKickPowerRate()) );
      return kickBallCloseToBody( 0 );            // else position ball better
    }
  }
  else                                            // can reach point
  {
    VecPosition accBallDes = velDes - velBall;
    dPower = WM->getKickPowerForSpeed(accBallDes.getMagnitude());
    if( dPower <= 1.05*SS->getMaxPower() || // with current ball speed
	(dDistOpp < 2.0 && dPower <= 1.30*SS->getMaxPower() ) )
    {                               // 1.05 since cannot get ball fully perfect
      Log.log( 101, "point good and can reach point %f", dPower );
      return accelerateBallToVelocity( velDes );  // perform shooting action
    }
    else
    {
      Log.log( 101, "point good, but reposition ball since need %f",dPower );
      SoccerCommand soc = kickBallCloseToBody( 0 );
      VecPosition   posPredBall;
      WM->predictBallInfoAfterCommand( soc, &posPredBall );
      dDistOpp = posPredBall.getDistanceTo( WM->getGlobalPosition( objOpp ) );
      return soc;            // position ball better
    }
  }
}


/*! This skill enables an agent to turn towards a global angle while
    keeping the ball in front of him. It is used, for example, when a
    defender has intercepted the ball in his defensive area and faces
    his own goal. In this situation the defender usually wants to pass
    the ball up the field into an area that is currently not visible
    to him and to this end he will first use this skill to turn with
    the ball towards the opponent's goal. Turning with the ball
    requires a sequence of commands to be performed. The ball first
    has to be kicked to a desired position relative to the agent, then
    it has to be stopped dead at that position and finally the agent
    must turn towards the ball again. Each time when this skill is
    called it has to be determined which part of the sequence still
    has to be executed. This is done as follows.  If the absolute
    difference between the desired angle and the global angle of the
    ball relative to the position of the agent is larger than the
    value 'angKickThr' then the kickBallCloseToBody skill is used to
    kick the ball to a position close to the agent and at the desired
    angle.  Otherwise, it is checked whether the ball still has speed
    from the previous action. If the remaining ball speed exceeds the
    given value 'dFreezeThr' then the ball is stopped dead at its
    current position using the freezeBall skill. In all other cases
    the agent turns his body towards the specified angle 'ang'.

    \param ang global direction in which ball and player should be faced
    \param angKickThr when ball angle difference is larger than this value
                      ball is repositioned
    \param dFreezeThr when ball lies correct, but has speed higher than this
                      value, ball is frozen.
    \return Soccercommand to turn with the ball to global angle 'ang'.
   */

/*
SoccerCommand Behavior::turnWithBallTo( AngDeg ang, AngDeg angKickThr,
                                           double dFreezeThr )
{
  // if opponent is close
  // if ball is located more than 'angKickThr' degrees from ang
  //  kick ball to point right in front of player in direction ang
  // else if ball has still speed higher than 'dFreezeThr'
  /// freezeBall
  // else
  //  turn to direction 'ang'
  VecPosition posAgent = WM->getAgentGlobalPosition();
  VecPosition posBall  = WM->getBallPos();
  AngDeg      angBody  = WM->getAgentGlobalBodyAngle();
  AngDeg      angDiff  = (posBall-posAgent).getDirection() - ang;
              angDiff  = VecPosition::normalizeAngle( angDiff );
  double      dDist;
  ObjectT     objOpp   = WM->getClosestInSetTo( OBJECT_SET_OPPONENTS,
                           WM->getAgentObjectType(), &dDist );
  VecPosition posOpp   = WM->getGlobalPosition( objOpp );

  if( objOpp != OBJECT_ILLEGAL && dDist < 2.5 )
  {
    if( posBall.getDistanceTo( posOpp ) < posBall.getDistanceTo( posAgent ) )
    {
      ang = (posOpp - posAgent).getDirection() + 180;
      return kickBallCloseToBody( VecPosition::normalizeAngle( ang-angBody ));
    }
  }
  else if( fabs( angDiff ) > angKickThr )
  {
    Log.log( 101, "turnWithBall: kick ball close %f", ang );
    return kickBallCloseToBody( VecPosition::normalizeAngle( ang - angBody ) );
  }

  // hier niet altijd freezen -> kan dan niet meer goed liggen omdat je zelf
  // doorsch