00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00046 #include <stdio.h>
00047 #include "WorldModel.h"
00048
00049
00050
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 )
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:
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
00149 double dEffort = ( sta != NULL ) ? sta->getEffort() : 1.0;
00150 double dAcc = dActualPower * SS->getDashPowerRate() * dEffort;
00151
00152
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
00160 if( vel->getMagnitude() > SS->getPlayerSpeedMax() )
00161 vel->setMagnitude( SS->getPlayerSpeedMax() );
00162
00163
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
00185 double dEffectiveAngle = getActualTurnAngle( dSendAngle, vel->getMagnitude());
00186 *angBody = VecPosition::normalizeAngle( *angBody + dEffectiveAngle );
00187 *angNeck = VecPosition::normalizeAngle( *angNeck + dEffectiveAngle );
00188
00189
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
00212
00213
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;
00222 Stamina stamina;
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
00308
00309
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
00340 sta -= ( dPower > 0.0 ) ? dPower : -2*dPower ;
00341 if( sta < 0 ) sta = 0;
00342
00343
00344 if( sta <= SS->getRecoverDecThr()*SS->getStaminaMax() &&
00345 rec > SS->getRecoverMin() )
00346 rec -= SS->getRecoverDec();
00347
00348
00349 if( sta <= SS->getEffortDecThr()*SS->getStaminaMax() &&
00350 eff > SS->getEffortMin() )
00351 eff -= SS->getEffortDec();
00352
00353
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
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