/*Copyright (c) 2000-2002, Jelle Kok, University of AmsterdamAll 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 "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