Main Page   Class Hierarchy   Compound List   File List   Compound Members   File Members  

WorldModelPredict.C

Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2000-2002, Jelle Kok, University of Amsterdam
00003 All rights reserved.
00004 
00005 Redistribution and use in source and binary forms, with or without 
00006 modification, are permitted provided that the following conditions are met:
00007 
00008 1. Redistributions of source code must retain the above copyright notice, this 
00009 list of conditions and the following disclaimer. 
00010 
00011 2. Redistributions in binary form must reproduce the above copyright notice, 
00012 this list of conditions and the following disclaimer in the documentation 
00013 and/or other materials provided with the distribution. 
00014 
00015 3. Neither the name of the University of Amsterdam nor the names of its 
00016 contributors may be used to endorse or promote products derived from this 
00017 software without specific prior written permission. 
00018 
00019 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
00020 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
00021 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 
00022 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE 
00023 FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 
00024 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 
00025 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 
00026 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 
00027 OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 
00028 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00029 */
00046 #include <stdio.h>
00047 #include "WorldModel.h"
00048 
00049 /*****************************************************************************/
00050 /************************** PREDICTIONS **************************************/
00051 /*****************************************************************************/
00052 
00065 bool WorldModel::predictStateAfterCommand( SoccerCommand com,
00066     VecPosition *pos, VecPosition *vel, AngDeg *angGlobalBody,
00067     AngDeg *angGlobalNeck, Stamina *sta )
00068 {
00069   switch( com.commandType ) // based on kind of command, choose action
00070   {
00071     case CMD_DASH:
00072       if( playMode != PM_BEFORE_KICK_OFF )
00073         predictStateAfterDash( com.dPower, pos, vel, sta, *angGlobalBody );
00074       break;
00075     case CMD_TURN:
00076       predictStateAfterTurn(com.dAngle,pos,vel,angGlobalBody,angGlobalNeck,sta);
00077       break;
00078     case CMD_TURNNECK: // note that position and velocity are not updated
00079       *angGlobalNeck = VecPosition::normalizeAngle(*angGlobalNeck + com.dAngle);
00080       break;
00081     case CMD_KICK:
00082     case CMD_CATCH:
00083       predictStateAfterDash( 0.0, pos, vel, sta, *angGlobalBody );
00084       break;
00085     case CMD_MOVE:
00086       pos->setVecPosition( com.dX, com.dY );
00087       vel->setMagnitude( 0.0 );
00088       break;
00089     case CMD_ILLEGAL:
00090       predictStateAfterDash( 0.01, pos, vel, sta, *angGlobalBody );
00091       break;
00092     default:
00093       return false;
00094       break;
00095   }
00096   return true;
00097 }
00098 
00109 bool WorldModel::predictAgentStateAfterCommand( SoccerCommand com,
00110     VecPosition *pos, VecPosition *vel, AngDeg *angGlobalBody,
00111     AngDeg *angGlobalNeck, Stamina *sta )
00112 {
00113   *pos           = getAgentGlobalPosition();
00114   *vel           = getAgentGlobalVelocity();
00115   *angGlobalBody = getAgentGlobalBodyAngle();
00116   *angGlobalNeck = getAgentGlobalNeckAngle();
00117   *sta           = getAgentStamina();
00118   predictStateAfterCommand( com, pos, vel, angGlobalBody, angGlobalNeck, sta );
00119 
00120   return true;
00121 }
00122 
00128 VecPosition WorldModel::predictAgentPosAfterCommand( SoccerCommand com )
00129 {
00130   VecPosition p1, p2;
00131   AngDeg      a1, a2;
00132   Stamina     sta;
00133   predictAgentStateAfterCommand( com, &p1, &p2, &a1, &a2, &sta );
00134   return p1;
00135 }
00136 
00145 void WorldModel::predictStateAfterDash( double dActualPower, VecPosition *pos,
00146              VecPosition *vel, Stamina *sta, double dDirection )
00147 {
00148   // get acceleration associated with actualpower
00149   double dEffort = ( sta != NULL ) ? sta->getEffort() : 1.0;
00150   double dAcc    = dActualPower * SS->getDashPowerRate() * dEffort;
00151 
00152   // add it to the velocity; negative acceleration in backward direction
00153   if( dAcc > 0 )
00154     *vel += VecPosition::getVecPositionFromPolar( dAcc, dDirection );
00155   else
00156     *vel += VecPosition::getVecPositionFromPolar( fabs(dAcc),
00157         VecPosition::normalizeAngle(dDirection+180));
00158 
00159   // check if velocity doesn't exceed maximum speed
00160   if( vel->getMagnitude() > SS->getPlayerSpeedMax() )
00161     vel->setMagnitude( SS->getPlayerSpeedMax() );
00162 
00163   // add velocity to current global position and decrease velocity
00164   *pos += *vel;
00165   *vel *= SS->getPlayerDecay();
00166   if( sta != NULL )
00167     predictStaminaAfterDash( dActualPower, sta );
00168 }
00169 
00181 void WorldModel::predictStateAfterTurn( AngDeg dSendAngle, VecPosition *pos,
00182           VecPosition *vel, AngDeg *angBody, AngDeg *angNeck, Stamina *sta )
00183 {
00184   // calculate effective angle that is turned and turn body and neck accordingly
00185   double dEffectiveAngle = getActualTurnAngle( dSendAngle, vel->getMagnitude());
00186   *angBody = VecPosition::normalizeAngle( *angBody + dEffectiveAngle );
00187   *angNeck = VecPosition::normalizeAngle( *angNeck + dEffectiveAngle );
00188 
00189   // update as if dashed with no power
00190   predictStateAfterDash( 0.0, pos, vel, sta, *angBody );
00191   return;
00192 }
00193 
00203 VecPosition WorldModel::predictPosAfterNrCycles( ObjectT o, int iCycles,
00204                                        int iDashPower, VecPosition *velocity )
00205 {
00206   VecPosition vel = getGlobalVelocity( o );
00207   VecPosition pos = getGlobalPosition( o );
00208 
00209   if( o == OBJECT_BALL )
00210   {
00211     // get the speed and the distance it travels in iCycle's.
00212     // use this distance and direction it travels in, to calculate new pos
00213     // geom series is serie s=a+ar+..+ar^n...decay=r,iCycles=n,dSpeed=a
00214     double dDist = Geometry::getSumGeomSeries( vel.getMagnitude(),
00215                                                SS->getBallDecay(),
00216                                                (double)iCycles);
00217     pos          += VecPosition( dDist, vel.getDirection(), POLAR );
00218   }
00219   else if( SoccerTypes::isKnownPlayer( o )  )
00220   {
00221     double      dDirection = 0.0; // used when no info about global body
00222     Stamina     stamina;          // used when object is agent
00223 
00224     if( getAgentObjectType() == o )
00225     {
00226       dDirection = getAgentGlobalBodyAngle();
00227       stamina    = getAgentStamina();
00228     }
00229     else if( getTimeGlobalAngles(o) > getCurrentTime() - 2 )
00230       dDirection = getGlobalBodyAngle(o);
00231 
00232     for( int i = 0; i < iCycles ; i ++ )
00233       predictStateAfterDash( iDashPower, &pos, &vel, &stamina, dDirection );
00234   }
00235   if( velocity != NULL )
00236     *velocity = vel;
00237   return pos;
00238 }
00239 
00246 VecPosition WorldModel::predictAgentPos( int iCycles, int iDashPower )
00247 {
00248   return predictPosAfterNrCycles( getAgentObjectType(), iCycles, iDashPower);
00249 }
00250 
00259 int WorldModel::predictNrCyclesToPoint( ObjectT o, VecPosition posTo,
00260                                         AngDeg angToTurn )
00261 {
00262   VecPosition posGlobal        = getGlobalPosition( o );
00263   AngDeg      angBody;
00264   AngDeg      ang;
00265 
00266   if( getAgentObjectType() == o )
00267     angBody = getAgentGlobalBodyAngle();
00268   else if( fabs(getTimeGlobalAngles(o)-getCurrentTime()) < 2 )
00269     angBody = getGlobalBodyAngle( o );
00270   else
00271     angBody = 180;
00272 
00273   int iExtraCycles;
00274   ang = fabs(angBody-(posTo-posGlobal).getDirection());
00275   if( ang > 20 && ang < 100 )
00276     iExtraCycles = 1;
00277   else if( ang > 100 )
00278     iExtraCycles = 2;
00279   else
00280     iExtraCycles = 0;
00281 
00282   if( posGlobal.getDistanceTo( posTo ) < SS->getMaximalKickDist() )
00283     return 0;
00284   else
00285     return iExtraCycles + int((posGlobal.getDistanceTo( posTo )
00286            /min(1.0, SS->getPlayerSpeedMax())));
00287 }
00288 
00294 int WorldModel::predictNrCyclesToObject( ObjectT objFrom, ObjectT objTo )
00295 {
00296   VecPosition posPrev(UnknownDoubleValue,UnknownDoubleValue);
00297   
00298   if( objFrom == OBJECT_ILLEGAL || objTo == OBJECT_ILLEGAL ||
00299       getGlobalPosition( objFrom ).getDistanceTo( getGlobalPosition( objTo )
00300       ) > 40 )
00301     return 101;
00302 
00303   int         iCycles      = 0;
00304   int         iCyclesToObj = 100;
00305   VecPosition posObj(0,0);
00306  
00307   // continue calculating number of cycles to position until or we can get
00308   // earlier at object position, are past maximum allowed number of cycles or
00309   // the object does not move anymore.
00310   while( iCycles <= iCyclesToObj && iCycles < PS->getPlayerWhenToIntercept() &&  
00311          posObj.getDistanceTo( posPrev ) < EPSILON )
00312   {
00313     iCycles      = iCycles + 1  ;
00314     posPrev      = posObj;    
00315     posObj       = predictPosAfterNrCycles( objTo,   iCycles );
00316     iCyclesToObj = predictNrCyclesToPoint ( objFrom, posObj,
00317                                            PS->getPlayerWhenToTurnAngle() );
00318   }
00319   if( iCycles == PS->getPlayerWhenToIntercept() )
00320     printf( "(WorldModel::predictNrCyclesToObject) Too much cycles??\n" );
00321 
00322   return iCyclesToObj;
00323 }
00324 
00325 
00326 
00333 void WorldModel::predictStaminaAfterDash( double dPower, Stamina *stamina )
00334 {
00335   double sta = stamina->getStamina();
00336   double eff = stamina->getEffort();
00337   double rec = stamina->getRecovery();
00338 
00339   // double negative value when dashed backwards
00340   sta -= ( dPower > 0.0 ) ? dPower : -2*dPower ;
00341   if( sta < 0 ) sta = 0;
00342 
00343   // stamina below recovery threshold, lower recovery
00344   if( sta <= SS->getRecoverDecThr()*SS->getStaminaMax() &&
00345                          rec > SS->getRecoverMin() )
00346       rec -= SS->getRecoverDec();
00347 
00348   // stamina below effort decrease threshold, lower effort
00349   if( sta <= SS->getEffortDecThr()*SS->getStaminaMax() &&
00350                    eff > SS->getEffortMin() )
00351       eff -= SS->getEffortDec();
00352 
00353   // stamina higher than effort incr threshold, raise effort and check maximum
00354   if( sta >= SS->getEffortIncThr() * SS->getStaminaMax() &&
00355       eff < 1.0)
00356   {
00357     eff += SS->getEffortInc();
00358     if ( eff > 1.0 )
00359        eff = 1.0;
00360   }
00361 
00362   // increase stamina with (new) recovery value and check for maximum
00363   sta += rec*SS->getStaminaIncMax();
00364   if ( sta > SS->getStaminaMax() )
00365     sta = SS->getStaminaMax();
00366 
00367   stamina->setStamina ( sta );
00368   stamina->setEffort  ( eff );
00369   stamina->setRecovery( rec );
00370 }
00371 
00382 bool WorldModel::isCollisionAfterDash( SoccerCommand soc )
00383 {
00384   if( soc.commandType != CMD_DASH )
00385     return false;
00386 
00387   VecPosition posPred = predictAgentPosAfterCommand( soc );
00388   VecPosition posBall = predictPosAfterNrCycles( OBJECT_BALL, 1 );
00389   if( posPred.getDistanceTo( posBall ) < 
00390               SS->getPlayerSize() + SS->getBallSize()  )
00391    return true;
00392    
00393   return false;
00394 }
00395 

Generated on Tue Jul 2 10:18:55 2002 for UvA Trilearn 2002 by doxygen1.2.12 written by Dimitri van Heesch, © 1997-2001