worldmodelpredict.cpp

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

/*
Copyright (c) 2000-2003, Jelle Kok, University of Amsterdam
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

1. Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.

2. Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.

3. Neither the name of the University of Amsterdam nor the names of its
contributors may be used to endorse or promote products derived from this
software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

/*! \file WorldModelPredict.cpp
<pre>
<b>File:</b>          WorldModelPredict.cpp
<b>ProWoct:</b>       Robocup Soccer Simulation Team: UvA Trilearn
<b>Authors:</b>       Jelle Kok
<b>Created:</b>       12/02/2001
<b>Last Revision:</b> $ID$
<b>Contents:</b>      class definitions of WorldModel. This class contains
               methods that give predictions about future states of the world.
<hr size=2>
<h2><b>Changes</b></h2>
<b>Date</b>             <b>Author</b>          <b>Comment</b>
12/02/2001       Jelle Kok       Initial version created
</pre>
*/

#include <stdio.h>
#include "WorldModel.h"

/*****************************************************************************/
/************************** PREDICTIONS **************************************/
/*****************************************************************************/

/*! This method predicts the state of an player after it performs a specific
    SoccerCommand. The current state of the agent is passed by giving the
    position, velocity and body and neck direction. These arguments are
    updated and after return of this method will contain the new values
    as if the command was performed.
    \param com SoccerCommand that will be performed
    \param pos current position of the object.
    \param vel current velocity of the object
    \param angGlobalBody global body angle
    \param angGlobalBody global neck angle
    \param current stamina information.
    \return boolean which indicates whether values were updated */
bool WorldModel::predictStateAfterCommand( SoccerCommand com,
    VecPosition *pos, VecPosition *vel, AngDeg *angGlobalBody,
    AngDeg *angGlobalNeck, ObjectT obj, Stamina *sta )
{
  switch( com.commandType ) // based on kind of command, choose action
  {
    case CMD_DASH:
      predictStateAfterDash( com.dPower, pos, vel, sta, *angGlobalBody, obj );
      break;
    case CMD_TURN:
      predictStateAfterTurn( com.dAngle, pos, vel,
                             angGlobalBody, angGlobalNeck, obj, sta);
      break;
    case CMD_TURNNECK: // note that position and velocity are not updated
      *angGlobalNeck = VecPosition::normalizeAngle(*angGlobalNeck+com.dAngle);
      break;
    case CMD_KICK:
    case CMD_CATCH:
    case CMD_TACKLE:
      predictStateAfterDash( 0.0, pos, vel, sta, *angGlobalBody, obj );
      break;
    case CMD_MOVE:
      pos->setVecPosition( com.dX, com.dY );
      vel->setMagnitude( 0.0 );
      break;
    case CMD_ILLEGAL:
      predictStateAfterDash( 0.01, pos, vel, sta, *angGlobalBody, obj );
      break;
    default:
      return false;
  }
  return true;
}

/*! This method predicts the state of the agent after it performs a specific
    SoccerCommand. This method makes use of the method predictInfoAfterCommand.
    All arguments are initialized with the current values of the agent.
    \param com SoccerCommand that will be performed
    \param pos will be filled with updated position of agent
    \param vel will be filled with updated velocity of agent
    \param angGlobalBody will be filled with global body angle of agent
    \param angGlobalBody will be filled with global neck angle of agent
    \param current will be filled with stamina information of agent
    \return boolean which indicates whether values were updated */
bool WorldModel::predictAgentStateAfterCommand( SoccerCommand com,
    VecPosition *pos, VecPosition *vel, AngDeg *angGlobalBody,
    AngDeg *angGlobalNeck, Stamina *sta )
{
  *pos           = getAgentGlobalPosition();
  *vel           = getAgentGlobalVelocity();
  *angGlobalBody = getAgentGlobalBodyAngle();
  *angGlobalNeck = getAgentGlobalNeckAngle();
  *sta           = getAgentStamina();
  predictStateAfterCommand( com, pos, vel, angGlobalBody, angGlobalNeck, 
                            getAgentObjectType(), sta );

  return true;
}

/*! This method predicts the state of the agent after it performs a specific
    SoccerCommand. This method makes use of the method predictInfoAfterCommand.
    All arguments are initialized with the current values of the agent.
    \param com SoccerCommand that will be performed
    \param pos will be filled with updated position of agent
    \param vel will be filled with updated velocity of agent
    \param angGlobalBody will be filled with global body angle of agent
    \param angGlobalBody will be filled with global neck angle of agent
    \param current will be filled with stamina information of agent
    \return boolean which indicates whether values were updated */
bool WorldModel::predictObjectStateAfterCommand( ObjectT obj,SoccerCommand com,
    VecPosition *pos, VecPosition *vel, AngDeg *angGlobalBody,
    AngDeg *angGlobalNeck, Stamina *sta )
{
  if( obj == getAgentObjectType() )
    return predictAgentStateAfterCommand(
               com, pos, vel, angGlobalBody, angGlobalNeck, sta );
  *pos           = getGlobalPosition( obj );
  *vel           = getGlobalVelocity( obj );
  *angGlobalBody = getGlobalBodyAngle( obj  );
  *angGlobalNeck = getGlobalNeckAngle(obj );
  predictStateAfterCommand( com, pos, vel, angGlobalBody, angGlobalNeck, obj );

  return true;
}

/*! This method returns the global position of the agent after the specified
    command is performed. This method makes use of the method
    'predictAgentInfoAfterCommand'
    \param com SoccerCommand that will be performed.
    \return VecPosition new global position of the agent. */
VecPosition WorldModel::predictAgentPosAfterCommand( SoccerCommand com )
{
  VecPosition p1, p2;
  AngDeg      a1, a2;
  Stamina     sta;
  predictAgentStateAfterCommand( com, &p1, &p2, &a1, &a2, &sta );
  return p1;
}

/*! This method determines the state of a player after a dash command is
    performed. The current state of the player is specified by the passed
    arguments. After this method returns, all arguments are updated.
    \param pos initial position, will be changed to the predicted position
    \param vel intital velocity, will be changed to the predicted velocity
    \param dActualPower actual power that is send with dash command
    \param sta pointer to stamina, when not NULL, effort is used and updated
    \param dDirection direction of dash */
void WorldModel::predictStateAfterDash( double dActualPower, VecPosition *pos,
             VecPosition *vel, Stamina *sta, double dDirection, ObjectT obj )
{
  // get acceleration associated with actualpower
  double dEffort = ( sta != NULL ) ? sta->getEffort() : getEffortMax( obj );
  double dAcc    = dActualPower * getDashPowerRate( obj ) * dEffort;

  // add it to the velocity; negative acceleration in backward direction
  if( dAcc > 0 )
    *vel += VecPosition::getVecPositionFromPolar( dAcc, dDirection );
  else
    *vel += VecPosition::getVecPositionFromPolar( fabs(dAcc),
        VecPosition::normalizeAngle(dDirection+180));

  // check if velocity doesn't exceed maximum speed
  if( vel->getMagnitude() > SS->getPlayerSpeedMax() )
    vel->setMagnitude( SS->getPlayerSpeedMax() );

  // add velocity to current global position and decrease velocity
  *pos += *vel;
  *vel *= getPlayerDecay(obj);
  if( sta != NULL )
    predictStaminaAfterDash( dActualPower, sta );
}

/*! This method determines the state of a player after a turn command is
    performed. The global position is updated with the velocity and the
    velocity is updated. Then the actual turn angle is calculated taken the
    inertia into account. This actual turn angle is used to update both
    the global body and global neck direction.
    \param dSendAngle actual angle given in command
    \param pos initial position, will be changed to the predicted position
    \param vel intital velocity, will be changed to the predicted velocity
    \param angBody global body direction
    \param angNeck global neck direction
    \param sta Stamina of player can be NULL */
void WorldModel::predictStateAfterTurn( AngDeg dSendAngle, VecPosition *pos,
          VecPosition *vel, AngDeg *angBody, AngDeg *angNeck, ObjectT obj,
          Stamina *sta )
{
  // calculate effective turning angle and neck accordingly
  double dEffectiveAngle;
  dEffectiveAngle = getActualTurnAngle( dSendAngle, vel->getMagnitude(), obj );
  *angBody = VecPosition::normalizeAngle( *angBody + dEffectiveAngle );
  *angNeck = VecPosition::normalizeAngle( *angNeck + dEffectiveAngle );

  // update as if dashed with no power
  predictStateAfterDash( 0.0, pos, vel, sta, *angBody, obj );
  return;
}

void WorldModel::predictBallInfoAfterCommand( SoccerCommand soc,
                  VecPosition *pos, VecPosition *vel )
{
  VecPosition posBall = getGlobalPosition( OBJECT_BALL ) ;
  VecPosition velBall = getGlobalVelocity( OBJECT_BALL );

  if( soc.commandType == CMD_KICK )
  {
    int iAng   = (int)soc.dAngle;
    int iPower = (int)soc.dPower;

    // make angle relative to body
    // calculate added acceleration and add it to current velocity
    AngDeg ang = VecPosition::normalizeAngle(iAng+getAgentGlobalBodyAngle());
    velBall += VecPosition( getActualKickPowerRate()*iPower, ang, POLAR ) ;
    if( velBall.getMagnitude() > SS->getBallSpeedMax() )
      velBall.setMagnitude( SS->getBallSpeedMax() );
    Log.log( 600, "ang: %f kick_rate %f", ang, getActualKickPowerRate() );
    Log.log( 600, "update for kick: %f %f", soc.dPower, soc.dAngle );
  }  

  posBall += velBall;
  velBall *= SS->getBallDecay();

  if( pos != NULL )
    *pos = posBall;
  if( vel != NULL )
    *vel = velBall;
}

/*! This method determines the global position of the object o after iCycles
    When the object is the ball, only the decay of the ball is taken into
    account. When the object is a player it
    is assumed that the player dashes with 'iDashPower' every cycle.
    \param o objectT of which global position will be predicted
    \param iCycles pos is predicted after this number of cycles
    \param iDashPower dash power that is used every cycle in dash (default 100)
    \param vel will be filled with predicted global velocity after iCycles
    \return predicted global position after iCycles. */
VecPosition WorldModel::predictPosAfterNrCycles( ObjectT o, double dCycles,
         int iDashPower, VecPosition *posIn, VecPosition *velIn, bool bUpdate )
{
  VecPosition vel = ( velIn == NULL ) ? getGlobalVelocity( o ) : *velIn ;
  VecPosition pos = ( posIn == NULL ) ? getGlobalPosition( o ) : *posIn ;

  if( o == OBJECT_BALL )
  {
    // get the speed and the distance it travels in iCycle's.
    // use this distance and direction it travels in, to calculate new pos
    // geom series is serie s=a+ar+..+ar^n...decay=r,iCycles=n,dSpeed=a
    double dDist = Geometry::getSumGeomSeries( vel.getMagnitude(),
                                               SS->getBallDecay(),
                                               dCycles);
    pos          += VecPosition( dDist, vel.getDirection(), POLAR );
    vel          *= pow( SS->getBallDecay(), dCycles );
  }
  else if( SoccerTypes::isKnownPlayer( o )  )
  {
    double      dDirection = 0.0; // used when no info about global body
    Stamina     stamina;          // used when object is agent

    if( getAgentObjectType() == o )
    {
      dDirection = getAgentGlobalBodyAngle();
      stamina    = getAgentStamina();
    }
    else if( getTimeGlobalAngles(o) > getCurrentTime() - 2 )
      dDirection = getGlobalBodyAngle(o);

    for( int i = 0; i < (int)dCycles ; i ++ )
      predictStateAfterDash( iDashPower, &pos, &vel, &stamina, dDirection, o );
  }

  if( posIn != NULL && bUpdate )
    *posIn = pos;
  if( velIn != NULL && bUpdate )
    *velIn = vel;

  return pos;
}

/*! This method predicts the position of the agent after 'iCycles' when every
    cycle is dashed with 'iDashPower'. The method
    'predictGlobalPosAfterNrCycles' is used to calculate this position.
    \param iCycles number of cycles
    \param iDashPower dash power that is passed
    \return VecPosition indicating predicted global position of agent. */
VecPosition WorldModel::predictAgentPos( int iCycles, int iDashPower )
{
  return predictPosAfterNrCycles( getAgentObjectType(), iCycles, iDashPower);
}

/*! This method predicts the final position of the agent when no commands
    are issued, but the agent just rolls out.
    \return VecPosition indicating predicted final global position of agent. */