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
00043 #include <stdio.h>
00044 #include "WorldModel.h"
00045
00046
00047
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 )
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:
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
00146 double dEffort = ( sta != NULL ) ? sta->getEffort() : 1.0;
00147 double dAcc = dActualPower * SS->getDashPowerRate() * dEffort;
00148
00149
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
00157 if( vel->getMagnitude() > SS->getPlayerSpeedMax() )
00158 vel->setMagnitude( SS->getPlayerSpeedMax() );
00159
00160
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
00182 double dEffectiveAngle = getActualTurnAngle( dSendAngle, vel->getMagnitude());
00183 *angBody = VecPosition::normalizeAngle( *angBody + dEffectiveAngle );
00184 *angNeck = VecPosition::normalizeAngle( *angNeck + dEffectiveAngle );
00185
00186
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
00209
00210
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;
00219 Stamina stamina;
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
00330 sta -= ( dPower > 0.0 ) ? dPower : -2*dPower ;
00331 if( sta < 0 ) sta = 0;
00332
00333
00334 if( sta <= SS->getRecoverDecThr()*SS->getStaminaMax() &&
00335 rec > SS->getRecoverMin() )
00336 rec -= SS->getRecoverDec();
00337
00338
00339 if( sta <= SS->getEffortDecThr()*SS->getStaminaMax() &&
00340 eff > SS->getEffortMin() )
00341 eff -= SS->getEffortDec();
00342
00343
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
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