worldmodelpredict.c

uva trilearn的robocup源程序

/*
Copyright (c) 2000-2002, 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.C
<pre>
<b>File:</b>          WorldModelPredict.C
<b>Project:</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 <math.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, Stamina *sta )
{
  switch( com.commandType ) // based on kind of command, choose action
  {
    case CMD_DASH:
      if( playMode != PM_BEFORE_KICK_OFF )
        predictStateAfterDash( com.dPower, pos, vel, sta, *angGlobalBody );
      break;
    case CMD_TURN:
      predictStateAfterTurn(com.dAngle,pos,vel,angGlobalBody,angGlobalNeck,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:
      predictStateAfterDash( 0.0, pos, vel, sta, *angGlobalBody );
      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 );
      break;
    default:
      return false;
      break;
  }
  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, sta );

  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 )
{
  // get acceleration associated with actualpower
  double dEffort = ( sta != NULL ) ? sta->getEffort() : 1.0;
  double dAcc    = dActualPower * SS->getDashPowerRate() * 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 *= SS->getPlayerDecay();
  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, Stamina *sta )
{
  // calculate effective angle that is turned and turn body and neck accordingly
  double dEffectiveAngle = getActualTurnAngle( dSendAngle, vel->getMagnitude());
  *angBody = VecPosition::normalizeAngle( *angBody + dEffectiveAngle );
  *angNeck = VecPosition::normalizeAngle( *angNeck + dEffectiveAngle );

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

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