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,2001, 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 */
00043 #include <stdio.h>
00044 #include "WorldModel.h"
00045 
00046 /*****************************************************************************/
00047 /************************** PREDICTIONS **************************************/
00048 /*****************************************************************************/
00049 
00062 bool WorldModel::predictStateAfterCommand( SoccerCommand com,
00063     VecPosition *pos, VecPosition *vel, AngDeg *angGlobalBody,
00064     AngDeg *angGlobalNeck, Stamina *sta )
00065 {
00066   switch( com.commandType ) // based on kind of command, choose action
00067   {
00068     case CMD_DASH:
00069       if( playMode != PM_BEFORE_KICK_OFF )
00070         predictStateAfterDash( com.dPower, pos, vel, sta, *angGlobalBody );
00071       break;
00072     case CMD_TURN:
00073       predictStateAfterTurn(com.dAngle,pos,vel,angGlobalBody,angGlobalNeck,sta);
00074       break;
00075     case CMD_TURNNECK: // note that position and velocity are not updated
00076       *angGlobalNeck = VecPosition::normalizeAngle(*angGlobalNeck + com.dAngle);
00077       break;
00078     case CMD_KICK:
00079     case CMD_CATCH:
00080       predictStateAfterDash( 0.0, pos, vel, sta, *angGlobalBody );
00081       break;
00082     case CMD_MOVE:
00083       pos->setVecPosition( com.dX, com.dY );
00084       vel->setMagnitude( 0.0 );
00085       break;
00086     case CMD_ILLEGAL:
00087       predictStateAfterDash( 0.01, pos, vel, sta, *angGlobalBody );
00088       break;
00089     default:
00090       return false;
00091       break;
00092   }
00093   return true;
00094 }
00095 
00106 bool WorldModel::predictAgentStateAfterCommand( SoccerCommand com,
00107     VecPosition *pos, VecPosition *vel, AngDeg *angGlobalBody,
00108     AngDeg *angGlobalNeck, Stamina *sta )
00109 {
00110   *pos           = getAgentGlobalPosition();
00111   *vel           = getAgentGlobalVelocity();
00112   *angGlobalBody = getAgentGlobalBodyAngle();
00113   *angGlobalNeck = getAgentGlobalNeckAngle();
00114   *sta           = getAgentStamina();
00115   predictStateAfterCommand( com, pos, vel, angGlobalBody, angGlobalNeck, sta );
00116 
00117   return true;
00118 }
00119 
00125 VecPosition WorldModel::predictAgentPosAfterCommand( SoccerCommand com )
00126 {
00127   VecPosition p1, p2;
00128   AngDeg      a1, a2;
00129   Stamina     sta;
00130   predictAgentStateAfterCommand( com, &p1, &p2, &a1, &a2, &sta );
00131   return p1;
00132 }
00133 
00142 void WorldModel::predictStateAfterDash( double dActualPower, VecPosition *pos,
00143              VecPosition *vel, Stamina *sta, double dDirection )
00144 {
00145   // get acceleration associated with actualpower
00146   double dEffort = ( sta != NULL ) ? sta->getEffort() : 1.0;
00147   double dAcc    = dActualPower * SS->getDashPowerRate() * dEffort;
00148 
00149   // add it to the velocity; negative acceleration in backward direction
00150   if( dAcc > 0 )
00151     *vel += VecPosition::getVecPositionFromPolar( dAcc, dDirection );
00152   else
00153     *vel += VecPosition::getVecPositionFromPolar( fabs(dAcc),
00154         VecPosition::normalizeAngle(dDirection+180));
00155 
00156   // check if velocity doesn't exceed maximum speed
00157   if( vel->getMagnitude() > SS->getPlayerSpeedMax() )
00158     vel->setMagnitude( SS->getPlayerSpeedMax() );
00159 
00160   // add velocity to current global position and decrease velocity
00161   *pos += *vel;
00162   *vel *= SS->getPlayerDecay();
00163   if( sta != NULL )
00164     predictStaminaAfterDash( dActualPower, sta );
00165 }
00166 
00178 void WorldModel::predictStateAfterTurn( AngDeg dSendAngle, VecPosition *pos,
00179           VecPosition *vel, AngDeg *angBody, AngDeg *angNeck, Stamina *sta )
00180 {
00181   // calculate effective angle that is turned and turn body and neck accordingly
00182   double dEffectiveAngle = getActualTurnAngle( dSendAngle, vel->getMagnitude());
00183   *angBody = VecPosition::normalizeAngle( *angBody + dEffectiveAngle );
00184   *angNeck = VecPosition::normalizeAngle( *angNeck + dEffectiveAngle );
00185 
00186   // update as if dashed with no power
00187   predictStateAfterDash( 0.0, pos, vel, sta, *angBody );
00188   return;
00189 }
00190 
00200 VecPosition WorldModel::predictPosAfterNrCycles( ObjectT o, int iCycles,
00201                                        int iDashPower, VecPosition *velocity )
00202 {
00203   VecPosition vel = getGlobalVelocity( o );
00204   VecPosition pos = getGlobalPosition( o );
00205 
00206   if( o == OBJECT_BALL )
00207   {
00208     // get the speed and the distance it travels in iCycle's.
00209     // use this distance and direction it travels in, to calculate new pos
00210     // geom series is serie s=a+ar+..+ar^n...decay=r,iCycles=n,dSpeed=a
00211     double dDist = Geometry::getSumGeomSeries( vel.getMagnitude(),
00212                                                SS->getBallDecay(),
00213                                                (double)iCycles);
00214     pos          += VecPosition( dDist, vel.getDirection(), POLAR );
00215   }
00216   else if( SoccerTypes::isKnownPlayer( o ) )
00217   {
00218     double      dDirection = 0.0; // used when no info about global body
00219     Stamina     stamina;          // used when object is agent
00220 
00221     if( getAgentObjectType() == o )
00222     {
00223       dDirection = getAgentGlobalBodyAngle();
00224       stamina    = getAgentStamina();
00225     }
00226     else if( getTimeGlobalAngles(o) > getCurrentTime() - 2 )
00227       dDirection = getGlobalBodyAngle(o);
00228 
00229     for( int i = 0; i < iCycles ; i ++ )
00230       predictStateAfterDash( iDashPower, &pos, &vel, &stamina, dDirection );
00231   }
00232   if( velocity != NULL )
00233     *velocity = vel;
00234   return pos;
00235 }
00236 
00243 VecPosition WorldModel::predictAgentPos( int iCycles, int iDashPower )
00244 {
00245   return predictPosAfterNrCycles( getAgentObjectType(), iCycles, iDashPower);
00246 }
00247 
00256 int WorldModel::predictNrCyclesToPoint( ObjectT o, VecPosition posTo,
00257                                         AngDeg angToTurn )
00258 {
00259   VecPosition posGlobal        = getGlobalPosition( o );
00260   AngDeg      angBody;
00261   AngDeg      ang;
00262 
00263   if( getAgentObjectType() == o )
00264     angBody = getAgentGlobalBodyAngle();
00265   else if( fabs(getTimeGlobalAngles(o)-getCurrentTime()) < 2 )
00266     angBody = getGlobalBodyAngle( o );
00267   else
00268     angBody = 180;
00269 
00270   int iExtraCycles;
00271   ang = fabs(angBody-(posTo-posGlobal).getDirection());
00272   if( ang > 20 && ang < 100 )
00273     iExtraCycles = 1;
00274   else if( ang > 100 )
00275     iExtraCycles = 2;
00276   else
00277     iExtraCycles = 0;
00278 
00279   if( posGlobal.getDistanceTo( posTo ) < SS->getMaximalKickDist() )
00280     return 0;
00281   else
00282     return iExtraCycles + int((posGlobal.getDistanceTo( posTo )
00283            /SS->getPlayerSpeedMax()));
00284 }
00285 
00291 int WorldModel::predictNrCyclesToObject( ObjectT objFrom, ObjectT objTo )
00292 {
00293   if( objFrom == OBJECT_ILLEGAL || objTo == OBJECT_ILLEGAL ||
00294       getGlobalPosition( objFrom ).getDistanceTo( getGlobalPosition( objTo )
00295       ) > 50 )
00296     return 101;
00297 
00298   int         iCycles      = 0;
00299   int         iCyclesToObj = 100;
00300   VecPosition posObj;
00301 
00302   while( iCycles <= iCyclesToObj && iCycles < 100)
00303   {
00304     iCycles      = iCycles + 1  ;
00305     posObj       = predictPosAfterNrCycles( objTo, iCycles );
00306     iCyclesToObj = predictNrCyclesToPoint( objFrom, posObj,
00307                                            PS->getPlayerWhenToTurnAngle() );
00308   }
00309   if( iCycles == 100 )
00310     printf( "(WorldModel::predictNrCyclesToObject) More than 100 cycles??\n" );
00311 
00312   return iCyclesToObj;
00313 }
00314 
00315 
00316 
00323 void WorldModel::predictStaminaAfterDash( double dPower, Stamina *stamina )
00324 {
00325   double sta = stamina->getStamina();
00326   double eff = stamina->getEffort();
00327   double rec = stamina->getRecovery();
00328 
00329   // double negative value when dashed backwards
00330   sta -= ( dPower > 0.0 ) ? dPower : -2*dPower ;
00331   if( sta < 0 ) sta = 0;
00332 
00333   // stamina below recovery threshold, lower recovery
00334   if( sta <= SS->getRecoverDecThr()*SS->getStaminaMax() &&
00335                          rec > SS->getRecoverMin() )
00336       rec -= SS->getRecoverDec();
00337 
00338   // stamina below effort decrease threshold, lower effort
00339   if( sta <= SS->getEffortDecThr()*SS->getStaminaMax() &&
00340                    eff > SS->getEffortMin() )
00341       eff -= SS->getEffortDec();
00342 
00343   // stamina higher than effort incr threshold, raise effort and check maximum
00344   if( sta >= SS->getEffortIncThr() * SS->getStaminaMax() &&
00345       eff < 1.0)
00346   {
00347     eff += SS->getEffortInc();
00348     if ( eff > 1.0 )
00349        eff = 1.0;
00350   }
00351 
00352   // increase stamina with (new) recovery value and check for maximum
00353   sta += rec*SS->getStaminaIncMax();
00354   if ( sta > SS->getStaminaMax() )
00355     sta = SS->getStaminaMax();
00356 
00357   stamina->setStamina ( sta );
00358   stamina->setEffort  ( eff );
00359   stamina->setRecovery( rec );
00360 }
00361 
00372 bool WorldModel::isCollisionAfterDash( SoccerCommand soc )
00373 {
00374   if( soc.commandType != CMD_DASH )
00375     return false;
00376 
00377   VecPosition posPred = predictAgentPosAfterCommand( soc );
00378   VecPosition pos;
00379 
00380   for( int i = 0 ; i < MAX_TEAMMATES; i ++ )
00381   {
00382     if( isConfidenceGood( Teammates[i].getType() ) )
00383       pos = predictPosAfterNrCycles( Teammates[i].getType(), 1, 100 );
00384     if( pos.getDistanceTo( posPred ) < SS->getPlayerSize() )
00385       return true;
00386   }
00387 
00388   for( int i = 0 ; i < MAX_OPPONENTS; i ++ )
00389   {
00390     if( isConfidenceGood( Opponents[i].getType() ) )
00391       pos = predictPosAfterNrCycles( Opponents[i].getType(), 1, 100 );
00392     if( pos.getDistanceTo( posPred ) < SS->getPlayerSize() )
00393       return true;
00394   }
00395   return false;
00396 }
00397 

Generated on Thu Mar 7 00:37:44 2002 for UvA Trilearn 2001 by doxygen1.2.12 written by Dimitri van Heesch, © 1997-2001