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

BasicPlayer.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 */
00047 #include "BasicPlayer.h"
00048 #include "Parse.h"        // parseFirstInt
00049 
00050 /********************** LOW-LEVEL SKILLS *************************************/
00051 
00056 SoccerCommand BasicPlayer::alignNeckWithBody( )
00057 {
00058   return SoccerCommand( CMD_TURNNECK, WM->getAgentBodyAngleRelToNeck( ) );
00059 }
00060 
00079 SoccerCommand BasicPlayer::turnBodyToPoint( VecPosition pos, int iCycles = 1 )
00080 {
00081   VecPosition posGlobal = WM->predictAgentPos(iCycles, 0);
00082   AngDeg angTurn        = (pos - posGlobal).getDirection();
00083   angTurn              -= WM->getAgentGlobalBodyAngle();
00084   angTurn               = VecPosition::normalizeAngle( angTurn );
00085   angTurn               = WM->getAngleForTurn( angTurn, WM->getAgentSpeed() );
00086 
00087   return SoccerCommand( CMD_TURN, angTurn );
00088 }
00089 
00102 SoccerCommand BasicPlayer::turnBackToPoint( VecPosition pos, int iCycles = 1 )
00103 {
00104   VecPosition posGlobal = WM->predictAgentPos(iCycles, 0);
00105   AngDeg angTurn        = (pos - posGlobal).getDirection();
00106   angTurn              -= (WM->getAgentGlobalBodyAngle() + 180);
00107   angTurn               = VecPosition::normalizeAngle( angTurn );
00108   angTurn               = WM->getAngleForTurn( angTurn, WM->getAgentSpeed() );
00109 
00110   return SoccerCommand( CMD_TURN, angTurn );
00111 }
00112 
00134 SoccerCommand BasicPlayer::turnNeckToPoint(VecPosition pos, SoccerCommand soc)
00135 {
00136   VecPosition posMe,   velMe;
00137   AngDeg      angBody, angNeck, angActual;
00138   Stamina     sta;
00139 
00140   // predict agent information after command 'soc' is performed
00141   // calculate the desired global angle of the neck
00142   // calculate the desired angle of the neck relative to the body
00143   WM->predictAgentStateAfterCommand(soc,&posMe,&velMe,&angBody,&angNeck,&sta);
00144   AngDeg angDesGlobNeck   = (pos - posMe).getDirection();
00145   AngDeg angNeckRelToBody = VecPosition::normalizeAngle(angDesGlobNeck-angBody);
00146 
00147   // calculate the current angle of the body relative to the neck
00148   // check if the desired neck angle relative to the body is possible:
00149   // if angle is smaller than the minimum or larger than the maximum neck angle
00150   //  turn neck to the minimum or maximum neck angle + the current neck angle
00151   // else calculate the desired angle relative to the body
00152   AngDeg angBodyRelToNeck = VecPosition::normalizeAngle(angBody-angNeck);
00153   if( angNeckRelToBody < SS->getMinNeckAng() )
00154     angActual = SS->getMinNeckAng() + angBodyRelToNeck;
00155   else if( angNeckRelToBody > SS->getMaxNeckAng() )
00156     angActual = SS->getMaxNeckAng() + angBodyRelToNeck;
00157   else
00158     angActual = angNeckRelToBody + angBodyRelToNeck;
00159   return SoccerCommand( CMD_TURNNECK, angActual );
00160 }
00161 
00173 SoccerCommand BasicPlayer::searchBall()
00174 {
00175   static Time timeLastSearch;
00176   static int  iSign       = 1;
00177 
00178   // calculate maximum turn angle
00179   AngDeg ang = 2*SoccerTypes::getHalfViewAngleValue( WM->getAgentViewAngle() );
00180   ang        = WM->getAngleForTurn( ang - ang/10.0, WM->getAgentSpeed() );
00181 
00182   // calculate whether expected ball position is too the left or right of body
00183   // do this by making angle in directon body and back of body and check
00184   // whether estimated ball prediction lies between these two angles
00185   // only change sign when not searched in previous cycles, otherwise could
00186   // turn left, right, left, right and never find ball
00187   AngDeg angBody = WM->getAgentGlobalBodyAngle();
00188   AngDeg angMax  = VecPosition::normalizeAngle( angBody + 180 );
00189   AngDeg angBall = (WM->getBallPos()-
00190                                   WM->getAgentGlobalPosition()).getDirection();
00191 
00192   if( WM->getCurrentTime() - timeLastSearch  > 3 )
00193     iSign = isAngInInterval( angBall, angBody, angMax ) ? 1: -1;
00194   timeLastSearch = WM->getCurrentTime();
00195 
00196   if( WM->getCurrentTime() == WM->getTimeLastSeeMessage() )
00197     return SoccerCommand( CMD_TURN, iSign * ang );
00198   return SoccerCommand( CMD_TURN, 1 );
00199 }
00200 
00201 
00214 SoccerCommand BasicPlayer::dashToPoint( VecPosition pos )
00215 {
00216   double dDashPower = WM->getPowerForDash(
00217                                  pos - WM->getAgentGlobalPosition(),
00218                                  WM->getAgentGlobalBodyAngle(),
00219                                  WM->getAgentGlobalVelocity(),
00220                                  WM->getAgentEffort()                );
00221   return SoccerCommand( CMD_DASH, dDashPower );
00222 
00223 }
00224 
00237 SoccerCommand BasicPlayer::freezeBall( )
00238 {
00239   // determine power needed to kick the ball to compensate for current speed
00240   // get opposite direction (current direction + 180) relative to body
00241   // and make the kick command.
00242 
00243   double dPower = WM->getKickPowerForSpeed( WM->getBallSpeed() );
00244   if( dPower > SS->getMaxPower() )
00245   {
00246     Log.log( 552, "%d: freeze ball has to much power", WM->getCurrentCycle() );
00247     dPower = (double)SS->getMaxPower();
00248   }
00249   double dAngle = WM->getBallDirection() + 180 - WM->getAgentGlobalBodyAngle();
00250   dAngle        = VecPosition::normalizeAngle( dAngle );
00251   return SoccerCommand( CMD_KICK, dPower, dAngle );
00252 }
00253 
00273 SoccerCommand BasicPlayer::kickBallCloseToBody( AngDeg ang )
00274 {
00275   AngDeg      angBody    = WM->getAgentGlobalBodyAngle();
00276   VecPosition posAgent   = WM->predictAgentPos( 1, 0 );
00277   double      dDist      = SS->getPlayerSize() +
00278                            SS->getBallSize()   +
00279                            SS->getKickableMargin()/6.0;
00280   AngDeg      angGlobal  = VecPosition::normalizeAngle( angBody + ang );
00281   VecPosition posDesBall = posAgent   + VecPosition( dDist, angGlobal, POLAR );
00282   VecPosition vecDesired = posDesBall - WM->getBallPos();
00283   VecPosition vecShoot   = vecDesired - WM->getGlobalVelocity( OBJECT_BALL );
00284   double      dPower     = WM->getKickPowerForSpeed( vecShoot.getMagnitude() );
00285   AngDeg      angActual  = vecShoot.getDirection() - angBody;
00286               angActual  = VecPosition::normalizeAngle( angActual );
00287 
00288   if( dPower > SS->getMaxPower() && WM->getBallSpeed() > 0.1 )
00289   {
00290     Log.log( 500, "kickBallCloseToBody: cannot compensate for ball speed, freeze");
00291     Log.log( 101, "kickBallCloseToBody: cannot compensate for ball speed, freeze");
00292     return freezeBall();
00293   }
00294   else if( dPower > SS->getMaxPower() )
00295   {
00296     Log.log( 101, "kickBallCloseToBody: ball has no speed, but far away" );
00297     dPower = 100;
00298   }
00299   else
00300     Log.log( 101, "(kick %f %f), vecDesired (%f,%f)", dPower, angActual,
00301     vecDesired.getX(), vecDesired.getY() );
00302   return SoccerCommand( CMD_KICK, dPower, angActual );
00303 }
00304 
00323 SoccerCommand BasicPlayer::accelerateBallToVelocity( VecPosition velDes )
00324 {
00325   AngDeg      angBody  = WM->getAgentGlobalBodyAngle();
00326   VecPosition velBall  = WM->getGlobalVelocity( OBJECT_BALL );
00327   VecPosition accDes   = velDes - velBall;
00328   double      dPower;
00329   double      angActual;
00330 
00331   // if acceleration can be reached, create shooting vector
00332   if( accDes.getMagnitude() < SS->getBallAccelMax() )
00333   {
00334     dPower    = WM->getKickPowerForSpeed   ( accDes.getMagnitude() );
00335     angActual = VecPosition::normalizeAngle( accDes.getDirection() - angBody );
00336     if( dPower <= SS->getMaxPower() )
00337       return SoccerCommand( CMD_KICK, dPower, angActual );
00338   }
00339 
00340   // else determine vector that is in direction 'velDes' (magnitude is lower)
00341          dPower    = SS->getMaxPower();
00342   double dSpeed    = WM->getActualKickPowerRate() * dPower;
00343   double tmp       = velBall.rotate(-velDes.getDirection()).getY();
00344          angActual = velDes.getDirection() - asinDeg( tmp / dSpeed );
00345          angActual = VecPosition::normalizeAngle( angActual - angBody );
00346 
00347   return SoccerCommand( CMD_KICK, dPower, angActual );
00348 }
00349 
00358 SoccerCommand BasicPlayer::catchBall()
00359 {
00360   // true means returned angle is relative to body instead of neck
00361   return SoccerCommand( CMD_CATCH, WM->getRelativeAngle( OBJECT_BALL, true ));
00362 }
00363 
00364 
00370 SoccerCommand BasicPlayer::communicate( char *str )
00371 {
00372   return SoccerCommand( CMD_SAY, str );
00373 }
00374 
00379 SoccerCommand BasicPlayer::teleportToPos( VecPosition pos )
00380 {
00381   return SoccerCommand( CMD_MOVE, pos.getX(), pos.getY() );
00382 
00383 }
00384 
00385 /********************** INTERMEDIATE LEVEL SKILLS ****************************/
00386 
00395 SoccerCommand BasicPlayer::turnBodyToObject( ObjectT o )
00396 {
00397   return turnBodyToPoint( WM->predictPosAfterNrCycles(o, 1) );
00398 }
00399 
00415 SoccerCommand BasicPlayer::turnNeckToObject( ObjectT o, SoccerCommand soc )
00416 {
00417   return turnNeckToPoint( WM->predictPosAfterNrCycles(o, 1), soc );
00418 }
00419 
00451 SoccerCommand BasicPlayer::moveToPos( VecPosition posTo, AngDeg angWhenToTurn,
00452                                       double dDistBack, bool bMoveBack )
00453 {
00454   VecPosition posPred   = WM->predictAgentPos( 1, 0 );
00455   AngDeg      angBody   = WM->getAgentGlobalBodyAngle();
00456   VecPosition posAgent  = WM->getAgentGlobalPosition();
00457   AngDeg      angTo     = ( posTo - posPred ).getDirection();
00458               angTo     = VecPosition::normalizeAngle( angTo - angBody );
00459   AngDeg      angBackTo = VecPosition::normalizeAngle( angTo + 180 );
00460   double      dDist     = posAgent.getDistanceTo( posTo );
00461 
00462   if( bMoveBack )
00463   {
00464     if( fabs( angBackTo ) < angWhenToTurn )
00465       return dashToPoint( posTo );
00466     else
00467       return turnBackToPoint( posTo );
00468   }
00469   else if(  fabs( angTo     ) < angWhenToTurn ||
00470            (fabs( angBackTo ) < angWhenToTurn && dDist < dDistBack ) )
00471     return dashToPoint( posTo );
00472   else
00473     return turnBodyToPoint( posTo );
00474 }
00475 
00540 SoccerCommand BasicPlayer::interceptClose( )
00541 {
00542   SoccerCommand soc;
00543   double        dPower, dDist;
00544   AngDeg        ang,    ang2;
00545   VecPosition   s1,     s2;
00546 
00547   // first determine whether the distance to the ball is not too large
00548   dDist = 2*SS->getPlayerSpeedMax()
00549           + (1.0 + SS->getBallDecay())*SS->getBallSpeedMax()
00550           + SS->getMaximalKickDist();
00551   if( WM->getRelativeDistance( OBJECT_BALL ) > dDist )
00552     return SoccerCommand( CMD_ILLEGAL );
00553 
00554   // initialize all variables with information from the worldmodel.
00555   VecPosition   posAgent = WM->getAgentGlobalPosition( );
00556   VecPosition   posPred  = WM->predictAgentPos( 1, 0 );
00557   VecPosition   posBall  = WM->predictPosAfterNrCycles( OBJECT_BALL, 1 );
00558   VecPosition   velMe    = WM->getAgentGlobalVelocity( );
00559   Stamina       sta      = WM->getAgentStamina( );
00560   AngDeg        angBody  = WM->getAgentGlobalBodyAngle( );
00561 
00562   // make a line from center of body in next cycle with direction of body
00563   // use next cycle since current velocity is always propogated to position in
00564   // next cycle.  Make a circle around the ball with a radius equal to the
00565   // sum of your own body, the ball size and a small buffer. Then calculate
00566   // the intersection between the line and this circle. These are the (two)
00567   // points that denote the possible agent locations close to the ball
00568   // From these two points we take the point where the body direction of the
00569   // agent makes the smallest angle with the ball (with backward
00570   // dashing we sometime have to dash "over" the ball to face it up front)
00571   Line line = Line::makeLineFromPositionAndAngle(posPred,angBody);
00572   dDist     = SS->getPlayerSize()+SS->getBallSize()+SS->getKickableMargin()/6;
00573   int  iSol = line.getCircleIntersectionPoints( Circle(posBall,dDist), &s1, &s2);
00574   if (iSol > 0)                                          // if a solution
00575   {
00576     if (iSol == 2)                                       // take the best one
00577     {
00578      ang  = VecPosition::normalizeAngle((posBall - s1).getDirection() -angBody);
00579      ang2 = VecPosition::normalizeAngle((posBall - s2).getDirection() -angBody);
00580      if ( fabs(ang2) < 90)
00581        s1 = s2;                                          // and put it in s1
00582     }
00583 
00584     // try one dash
00585     // now we have the interception point we try to reach in one cycle. We
00586     // calculate the needed dash power from the current position to this point,
00587     // predict were we will stand if we execute this command and check whether
00588     // we are in the kickable distance
00589     dPower  = WM->getPowerForDash(s1-posAgent, angBody, velMe,sta.getEffort() );
00590     posPred = WM->predictAgentPos( 1, (int)dPower);
00591     if ( posPred.getDistanceTo( posBall ) < SS->getMaximalKickDist() )
00592       return SoccerCommand( CMD_DASH, dPower );
00593   }
00594 
00595   // try one dash by getting close to ball
00596   // this handles situation where ball cannot be reached within distance
00597   // SS->getKickableMargin()/6
00598   soc = dashToPoint( posBall );
00599   WM->predictAgentStateAfterCommand(soc,&posPred,&velMe,&angBody,&ang,&sta );
00600   if ( posPred.getDistanceTo( posBall ) < SS->getMaximalKickDist() )
00601     return soc;
00602   
00603   // try turn and then dash
00604   // first predict the angle between the agent and the ball after two cycles,
00605   // is this larger than the angle when to turn, turn to position of the ball
00606   // after two cycles. Then perform a dash based on the position of the ball
00607   // after two cycles and the position of the agent after the turn. If after
00608   // these two command the agent is in the kickable distance, return turn
00609   // command.
00610   posBall = WM->predictPosAfterNrCycles( OBJECT_BALL, 2 );
00611   posPred = WM->predictAgentPos( 2, 0 );
00612   ang     = (posBall - posPred).getDirection();
00613   ang     = VecPosition::normalizeAngle( ang - angBody );
00614   if (fabs( ang ) > PS->getPlayerWhenToTurnAngle() ) // if we want to turn
00615   {
00616     soc = turnBodyToPoint( posBall, 2 );             // perform turn
00617     WM->predictAgentStateAfterCommand(soc,&posPred,&velMe,&angBody,&ang,&sta );
00618     dPower = WM->getPowerForDash(posBall-posPred,angBody,velMe,sta.getEffort());
00619     WM->predictStateAfterDash( dPower, &posPred, &velMe, &sta, angBody);
00620     if (posPred.getDistanceTo(posBall) < SS->getMaximalKickDist())
00621       return soc;
00622   }
00623 
00624   // try two dashes
00625   // first predict the position in the next cycle when dash with full power
00626   // is performed. Then calculate the dash power to reach the point where the
00627   // ball will be in two cycles and predict the global position of the agent
00628   // after a dash with this power. If the position is in the kickable distance
00629   // return a dash with full power.
00630   soc = SoccerCommand( CMD_DASH, SS->getMaxPower() );
00631   WM->predictAgentStateAfterCommand(soc,&posPred,&velMe,&angBody,&ang,&sta );
00632   dPower=WM->getPowerForDash(posBall-posPred,angBody,velMe,sta.getEffort());
00633   WM->predictStateAfterDash( dPower, &posPred, &velMe, &sta, angBody );
00634   if (posPred.getDistanceTo(posBall) < SS->getMaximalKickDist())
00635     return soc;
00636 
00637   // try three dashes
00638   // same as with two dashes, but now try two maximum dashes and
00639   posBall = WM->predictPosAfterNrCycles( OBJECT_BALL, 3 );
00640   soc = SoccerCommand( CMD_DASH, SS->getMaxPower() );
00641   WM->predictAgentStateAfterCommand(soc,&posPred,&velMe,&angBody,&ang,&sta );
00642   WM->predictStateAfterCommand( soc, &posPred, &velMe, &angBody, &ang,&sta );
00643   dPower=WM->getPowerForDash(posBall-posPred,angBody,velMe,sta.getEffort());
00644   WM->predictStateAfterDash( dPower, &posPred, &velMe, &sta, angBody );
00645   if (posPred.getDistanceTo(posBall) < SS->getMaximalKickDist())
00646     return soc;
00647 
00648   // cannot intercept ball in two cycles
00649   return SoccerCommand( CMD_ILLEGAL );
00650 }
00651 
00688 SoccerCommand BasicPlayer::interceptCloseGoalie( )
00689 {
00690   SoccerCommand soc;
00691   double        dPower, dDist;
00692   AngDeg        ang;
00693   VecPosition   posClosestToBall;
00694 
00695   // initialize all variables with information from worldmodel.
00696   VecPosition   posPred   = WM->predictAgentPos( 1, 0 );
00697   VecPosition   posBall   = WM->predictPosAfterNrCycles( OBJECT_BALL, 1 );
00698   VecPosition   velMe     = WM->getAgentGlobalVelocity( );
00699   Stamina       sta       = WM->getAgentStamina( );
00700   AngDeg        angBody   = WM->getAgentGlobalBodyAngle( );
00701   Line          lineGoalie=Line::makeLineFromPositionAndAngle(posPred,angBody);
00702 
00703   // when it is theoretical possible
00704   // try one dash and check whether ball is in catchable area
00705   dDist = SS->getBallSpeedMax()+SS->getPlayerSpeedMax()+SS->getCatchableAreaL();
00706   if( WM->getRelativeDistance( OBJECT_BALL ) < dDist )
00707   {
00708     posClosestToBall = lineGoalie.getPointOnLineClosestTo( posBall );
00709     dPower           = WM->getPowerForDash(
00710                           posClosestToBall-posPred,
00711                           angBody,
00712                           velMe,
00713                           sta.getEffort()           );
00714     posPred          = WM->predictAgentPos( 1, (int)dPower);
00715     if ( posPred.getDistanceTo( posBall ) < SS->getCatchableAreaL() )
00716       return SoccerCommand( CMD_DASH, dPower );
00717   }
00718 
00719   // when it is theoretical possible
00720   // try two dashes and check whether ball is in catchable area
00721   // otherwise try first two  dashes and check whether ball is in catchable
00722   // area, thereafter for turn and dash.
00723   dDist = SS->getBallSpeedMax()*(1.0+SS->getBallDecay())
00724            + 2*SS->getPlayerSpeedMax()
00725            + SS->getCatchableAreaL();
00726   if( WM->getRelativeDistance( OBJECT_BALL ) < dDist )
00727   {
00728     // try two dashes
00729     // first predict the position in the next cycle when dash with full power
00730     // is performed. Then calculate the dash power to reach the point where the
00731     // ball will be in two cycles and predict the global position of the agent
00732     // after a dash with this power. If the position is in the catchable area
00733     // return a dash with full power.
00734     posBall = WM->predictPosAfterNrCycles( OBJECT_BALL, 2 );
00735     soc     = dashToPoint( posBall );
00736     WM->predictAgentStateAfterCommand(soc,&posPred,&velMe,&angBody,&ang,&sta );
00737     dPower=WM->getPowerForDash(posBall-posPred,angBody,velMe,sta.getEffort());
00738     WM->predictStateAfterDash( dPower, &posPred, &velMe, &sta, angBody );
00739     if( posPred.getDistanceTo(posBall) < SS->getCatchableAreaL() )
00740       return soc;
00741 
00742     // try one turn and a dash
00743     posBall = WM->predictPosAfterNrCycles( OBJECT_BALL, 2 );
00744     posPred = WM->predictAgentPos( 2, 0 );
00745     ang     = (posBall - posPred).getDirection();
00746     ang     = VecPosition::normalizeAngle( ang - angBody );
00747     if (fabs( ang ) > PS->getPlayerWhenToTurnAngle() ) // if we want to turn
00748     {
00749       soc = turnBodyToPoint( posBall, 2 );             // perform turn
00750       WM->predictAgentStateAfterCommand(soc,&posPred,&velMe,&angBody,&ang,&sta );
00751       dPower=WM->getPowerForDash(posBall-posPred,angBody,velMe,sta.getEffort());
00752       WM->predictStateAfterDash( dPower, &posPred, &velMe, &sta, angBody);
00753       if( posPred.getDistanceTo(posBall) < SS->getCatchableAreaL() )
00754         return soc;
00755     }
00756   }
00757 
00758   // did not succeed
00759   return SoccerCommand( CMD_ILLEGAL );
00760 }
00761 
00792 SoccerCommand BasicPlayer::kickTo( VecPosition posTarget, double dEndSpeed )
00793 {
00794   VecPosition posBall = WM->getBallPos();
00795   VecPosition velBall = WM->getGlobalVelocity(OBJECT_BALL);
00796   VecPosition posTraj = posTarget - posBall;
00797   VecPosition velDes  = VecPosition(
00798                WM->getKickSpeedToTravel( posTraj.getMagnitude(), dEndSpeed ),
00799                posTraj.getDirection(),
00800                POLAR                                                        );
00801   double      dPower;
00802   AngDeg      angActual;
00803 
00804   Log.log( 101, "ball (%f,%f), agent (%f,%f), to (%f,%f) ang %f %f" ,
00805           WM->getBallPos().getX(), WM->getBallPos().getY(),
00806           WM->getAgentGlobalPosition().getX(),
00807           WM->getAgentGlobalPosition().getY(),
00808           posTarget.getX(), posTarget.getY(),
00809           WM->getAgentGlobalBodyAngle(),
00810           WM->getAgentGlobalNeckAngle() );
00811   Log.log( 101, "rel pos body (%f,%f), velocity ball: (%f,%f) kick_dist: %f  (%f,%f,%f)" ,
00812           WM->getRelativeDistance( OBJECT_BALL ),
00813           WM->getRelativeAngle( OBJECT_BALL, true ),
00814            velBall.getX(), velBall.getY(),
00815            SS->getMaximalKickDist(), SS->getPlayerSize(), SS->getBallSize(), 
00816              SS->getKickableMargin() );
00817 
00818   if( WM->isKickInUs() || WM->isFreeKickUs())
00819   {
00820     Log.log( 101, "kick in or free kick, accelerate ball" );
00821     return accelerateBallToVelocity( velDes );  
00822   }
00823 
00824   if( velDes.getMagnitude() > SS->getBallSpeedMax() ) // can never reach point
00825   {
00826            dPower     = SS->getMaxPower();
00827     double dSpeed     = WM->getActualKickPowerRate() * dPower;
00828     double tmp        = velBall.rotate(-velDes.getDirection()).getY();
00829            angActual  = velDes.getDirection() - asinDeg( tmp / dSpeed );
00830     double dSpeedPred = ( WM->getGlobalVelocity(OBJECT_BALL)+
00831                          VecPosition(dSpeed,angActual, POLAR )).getMagnitude();
00832 
00833     // but ball acceleration in right direction is very high
00834     if( dSpeedPred > PS->getPlayerWhenToKick()*SS->getBallAccelMax() )
00835     {
00836       Log.log( 101, "point (%f,%f) too far, but can acccelerate ball good to %f k=%f,%f",
00837                      velDes.getX(), velDes.getY(), dSpeedPred, dSpeed, tmp );
00838       return accelerateBallToVelocity( velDes );    // shoot nevertheless
00839     }
00840     else if( WM->getActualKickPowerRate() >
00841         PS->getPlayerWhenToKick() * SS->getKickPowerRate() )
00842     {
00843       Log.log( 101, "point too far, freeze ball" ); // ball well-positioned
00844       return freezeBall();                          // freeze ball
00845     }
00846     else
00847     {
00848       Log.log( 101, "point too far, reposition ball (k_r = %f)",
00849                       WM->getActualKickPowerRate()/(SS->getKickPowerRate()) );
00850       return kickBallCloseToBody( 0 );            // else position ball better
00851     }
00852   }
00853   else                                            // can reach point
00854   {
00855     VecPosition accBallDes = velDes - velBall;
00856     dPower = WM->getKickPowerForSpeed(accBallDes.getMagnitude());
00857     if( dPower <= 1.05*SS->getMaxPower() )        // with current ball speed
00858     {                               // 1.05 since cannot get ball fully perfect
00859       Log.log( 101, "point good and can reach point" );
00860       return accelerateBallToVelocity( velDes );  // perform shooting action
00861     }
00862     else
00863     {
00864       Log.log( 101, "point good, but reposition ball since need %f",dPower );
00865       return kickBallCloseToBody( 0 );            // else position ball better
00866     }
00867   }
00868 }
00869 
00898 /*
00899 SoccerCommand BasicPlayer::turnWithBallTo( AngDeg ang, AngDeg angKickThr,
00900                                            double dFreezeThr )
00901 {
00902   // if opponent is close
00903   // if ball is located more than 'angKickThr' degrees from ang
00904   //  kick ball to point right in front of player in direction ang
00905   // else if ball has still speed higher than 'dFreezeThr'
00907   // else
00908   //  turn to direction 'ang'
00909   VecPosition posAgent = WM->getAgentGlobalPosition();
00910   VecPosition posBall  = WM->getBallPos();
00911   AngDeg      angBody  = WM->getAgentGlobalBodyAngle();
00912   AngDeg      angDiff  = (posBall-posAgent).getDirection() - ang;
00913               angDiff  = VecPosition::normalizeAngle( angDiff );
00914   double      dDist;
00915   ObjectT     objOpp   = WM->getClosestInSetTo( OBJECT_SET_OPPONENTS,
00916                            WM->getAgentObjectType(), &dDist );
00917   VecPosition posOpp   = WM->getGlobalPosition( objOpp );
00918 
00919   if( objOpp != OBJECT_ILLEGAL && dDist < 2.5 )
00920   {
00921     if( posBall.getDistanceTo( posOpp ) < posBall.getDistanceTo( posAgent ) )
00922     {
00923       ang = (posOpp - posAgent).getDirection() + 180;
00924       return kickBallCloseToBody( VecPosition::normalizeAngle( ang - angBody ));
00925     }
00926   }
00927   else if( fabs( angDiff ) > angKickThr )
00928   {
00929     Log.log( 101, "turnWithBall: kick ball close %f", ang );
00930     return kickBallCloseToBody( VecPosition::normalizeAngle( ang - angBody ) );
00931   }
00932 
00933   // hier niet altijd freezen -> kan dan niet meer goed liggen omdat je zelf
00934   // doorschiet.
00935   if( WM->getBallSpeed() > dFreezeThr )
00936   {
00937     Log.log( 101, "turnWithBall: freeze ball" );
00938     return freezeBall();
00939   }
00940 
00941   ACT->putCommandInQueue( alignNeckWithBody() );
00942   return turnBodyToPoint( posAgent + VecPosition(1.0, ang, POLAR ) );
00943 }
00944 */
00945 SoccerCommand BasicPlayer::turnWithBallTo( AngDeg ang, AngDeg angKickThr,
00946                                            double dFreezeThr )
00947 {
00948   // if opponent is closer to the ball than I am
00949   //  kick ball away from his direction
00950   // if the ball will be in my kick_range in the next cycle
00951   //  turn to direction ang
00952   // if ball will be in kick range if frozen
00953   //   freeze_ball
00954   // else
00955   //   kickBallCloseToBody(ang)
00956   VecPosition   posAgent= WM->getAgentGlobalPosition();
00957   VecPosition   posBall = WM->getBallPos();
00958   AngDeg        angBody = WM->getAgentGlobalBodyAngle();
00959   double        dDist;
00960   ObjectT       objOpp  = WM->getClosestInSetTo( OBJECT_SET_OPPONENTS,
00961                           WM->getAgentObjectType(), &dDist );
00962   VecPosition   posOpp  = WM->getGlobalPosition( objOpp );
00963   SoccerCommand soc     = turnBodyToPoint(posAgent+VecPosition(1.0,ang,POLAR));
00964 
00965   if( objOpp != OBJECT_ILLEGAL && dDist < 2.5 )
00966   {
00967     AngDeg angBall = (posBall-posAgent).getDirection();
00968     AngDeg angOpp  = (posOpp -posAgent).getDirection();
00969     if( fabs( VecPosition::normalizeAngle( angBall - angOpp ) ) < 90  )
00970     {
00971       ang = angOpp + 180;
00972       Log.log( 101, "turnWithBall: kick ball away from opp at ang %f", angOpp );
00973       return kickBallCloseToBody( VecPosition::normalizeAngle( ang - angBody ));
00974     }
00975   }
00976 
00977   VecPosition posAgentPred = WM->predictAgentPosAfterCommand( soc );
00978   VecPosition posBallPred  = WM->predictPosAfterNrCycles( OBJECT_BALL, 1 );
00979   if( posAgentPred.getDistanceTo( posBallPred ) < 0.8*SS->getMaximalKickDist()
00980    || 
00981    ( posAgentPred.getDistanceTo(posBallPred) < 0.9*SS->getMaximalKickDist() 
00982      && WM->getBallSpeed() < 0.1 ))
00983   {
00984     Log.log( 101, "turnWithBall: turn since ball will be kickable in t+1" );
00985     return soc;
00986   }
00987 
00988   posAgentPred = WM->predictPosAfterNrCycles( WM->getAgentObjectType(), 1, 0 );
00989 
00990   // if ball will not be in kickable distance in next turn, we have to freeze
00991   // the ball. Do this only when current ball position (same as frozen ball
00992   // position) lies within kickable distance of predicted agent position in
00993   // next cycle. When ball lies at edge of the kickable distance, do not
00994   // freeze, since when vel.is not completely correct can move outside area
00995   if( posAgentPred.getDistanceTo( posBall ) < 0.8*SS->getMaximalKickDist() && 
00996       WM->getBallSpeed() > 0.1  )
00997   {
00998     Log.log( 101, "turnWithBall: freeze ball" );
00999     return freezeBall();
01000   }
01001   else
01002   {
01003     Log.log( 101, "turnWithBall: kick ball close to desired turn angle" );
01004     return kickBallCloseToBody( VecPosition::normalizeAngle( ang - angBody ) );
01005   }
01006 }
01007 
01008 
01009 
01010 
01060 SoccerCommand BasicPlayer::moveToPosAlongLine( VecPosition pos, AngDeg ang,
01061     double dDistThr, int iSign, AngDeg angThr, AngDeg angCorr )
01062 {
01063   Line        l        = Line::makeLineFromPositionAndAngle( pos, ang );
01064   VecPosition posBall  = WM->getBallPos();
01065   VecPosition posAgent = WM->getAgentGlobalPosition();
01066   AngDeg      angBody  = WM->getAgentGlobalBodyAngle();
01067   VecPosition posProj  = l.getPointOnLineClosestTo( posAgent );
01068   double      dDist    = posAgent.getDistanceTo( posProj );
01069   double      dDiff    = pos.getDistanceTo     ( posProj );
01070 
01071   // if deviated too much from line, compensate
01072   if( dDist > dDistThr )
01073   {
01074     // check on which side of line agent is located
01075     VecPosition posOrg(0,0);
01076     Line        m            = Line::makeLineFromTwoPoints( posOrg, posAgent );
01077     VecPosition posIntersect = l.getIntersection( m );
01078     int         iSide;
01079     if( posAgent.getDistanceTo(posOrg) < posIntersect.getDistanceTo( posOrg ) )
01080       iSide = +1;
01081     else
01082       iSide = -1;
01083 
01084     // adjust desired turning angle to move back to line in coming cycles
01085     ang = ang + iSign * iSide * angCorr;
01086   }
01087 
01088   Log.log( 553, "y difference to defend point %f", dDiff );
01089   // if current body angle differs much from desired turning angle, turn body
01090   if( fabs( VecPosition::normalizeAngle( ang - angBody ) ) > angThr )
01091   {
01092     Log.log( 553, "angle differs too much body = %f, des = %f", angBody, ang );
01093     return turnBodyToPoint( posAgent + VecPosition( 1.0, ang, POLAR ) );
01094   }
01095   else if( ( posBall.getX() > -PITCH_LENGTH/4.0 && dDiff > 0.6 ) )
01096     return dashToPoint( pos );  // move later when ball is far from pen. area.
01097   else if( ( posBall.getX() < -PITCH_LENGTH/4.0 && dDiff > 0.3 ) )
01098     return dashToPoint( pos );  // move earlier when is ball near pen. area. 
01099   else
01100     return SoccerCommand( CMD_ILLEGAL );
01101 }
01102 
01103 
01104 /********************** HIGH LEVEL SKILLS ************************************/
01105 
01115 SoccerCommand BasicPlayer::intercept( bool isGoalie )
01116 {
01117   SoccerCommand soc = (isGoalie) ? interceptCloseGoalie() : interceptClose();
01118   if( soc.commandType != CMD_ILLEGAL )
01119   {
01120     Log.log( 502, "intercept in two cycles" );
01121     return soc;
01122   }
01123 
01124   // else find the best interception point
01125   int         iSol;
01126   VecPosition posBall = getInterceptionPointBall( &iSol, isGoalie  );
01127   Log.log( 502, "intercept ball in %d cycles", iSol );
01128   return moveToPos( posBall, PS->getPlayerWhenToTurnAngle(), 3.0 );
01129 }
01130 
01169 SoccerCommand BasicPlayer::dribble( AngDeg ang, DribbleT dribbleT )
01170 {
01171   double        dLength;
01172   AngDeg        angBody  = WM->getAgentGlobalBodyAngle();
01173   VecPosition   posAgent = WM->getAgentGlobalPosition();
01174   SoccerCommand soc;
01175 
01176   // if not turned into correct direction, turn with the ball to that angle
01177   AngDeg angDiff = VecPosition::normalizeAngle( ang - angBody );
01178   if( fabs( angDiff ) > PS->getDribbleAngThr() )
01179     return turnWithBallTo( ang, PS->getTurnWithBallAngThr(),
01180                                 PS->getTurnWithBallFreezeThr() );
01181 
01182   switch( dribbleT )
01183   {
01184     case DRIBBLE_WITHBALL:
01185       dLength = 4.0;
01186       break;
01187     case DRIBBLE_SLOW:
01188       dLength = 5.0;
01189       break;
01190     case DRIBBLE_FAST:
01191       dLength = 10.0;
01192       break;
01193     default:
01194       dLength = 0.0;
01195       break;
01196   }
01197 
01198   // determine shooting point, relative to agent since moving in that dir.
01199   VecPosition posDribble = posAgent + VecPosition( dLength, angBody, POLAR );
01200 
01201   // adjust when point lies outside side of field
01202   // to a point that lies at distance 2.0 from the side of the field
01203   if( fabs( posDribble.getY() ) > PITCH_WIDTH/2.0  - 3.0 ) 
01204     posDribble.setY( (PITCH_WIDTH/2.0  - 3.0)*sign(posDribble.getY()) );
01205   if( fabs( posDribble.getX() ) > PITCH_LENGTH/2.0 - 3.0 )
01206     posDribble.setX( (PITCH_LENGTH/2.0  - 3.0)*sign(posDribble.getX()) );  
01207 
01208   soc = kickTo( posDribble, 0.5 );
01209 
01210   // if ball is kickable but already heading in the right direction, so only
01211   // small correction term is necessary, start intercepting
01212   SoccerCommand soc2      = intercept( false );
01213   if( soc.dPower < 7  && WM->isCollisionAfterDash( soc2 ) == false )
01214   {
01215     Log.log( 560, "wanted to dribble, but only small kick %f", soc.dPower );
01216     return soc2;
01217   }
01218   return soc;
01219 }
01220 
01230 SoccerCommand BasicPlayer::directPass( VecPosition pos, PassT passType)
01231 {
01232   if( passType == PASS_NORMAL )
01233     return kickTo( pos, PS->getPassEndSpeed() );
01234   else if( passType == PASS_FAST )
01235     return kickTo( pos, PS->getFastPassEndSpeed() );
01236   else
01237     return SoccerCommand( CMD_ILLEGAL );
01238 }
01239 
01256 SoccerCommand BasicPlayer::leadingPass( ObjectT o, double dDist )
01257 {
01258   VecPosition posShoot = WM->getGlobalPosition( o )
01259                          + VecPosition( dDist, 0.0, POLAR );
01260   return kickTo( posShoot, PS->getPassEndSpeed() );
01261 }
01262 
01289 SoccerCommand BasicPlayer::throughPass( ObjectT o, VecPosition posEnd,
01290                                         AngDeg *angMax )
01291 {
01292   VecPosition posShoot = getThroughPassShootingPoint( o, posEnd, angMax );
01293   double      dEnd     = getEndSpeedForPass( o, posShoot );
01294 
01295   return kickTo( posShoot, dEnd );
01296 }
01297 
01379 SoccerCommand BasicPlayer::outplayOpponent( ObjectT o, VecPosition pos,
01380                                             VecPosition *posTo )
01381 {
01382   // future: take more than one opponent into account
01383 
01384   VecPosition posAgent   = WM->getAgentGlobalPosition();
01385   AngDeg      angBody    = WM->getAgentGlobalBodyAngle();
01386 
01387   AngDeg      ang        = (pos - posAgent).getDirection();
01388   // if not heading in the desired direction, first turn with the ball
01389   AngDeg angTmp = VecPosition::normalizeAngle( ang - angBody ) ;  
01390   if( fabs( angTmp ) > PS->getDribbleAngThr() )
01391     return turnWithBallTo( ang, PS->getTurnWithBallAngThr(),
01392                                 PS->getTurnWithBallFreezeThr() );
01393 // NEW
01394 //  ang = WM->getAgentGlobalBodyAngle();
01395   Line        l          = Line::makeLineFromPositionAndAngle(posAgent,ang);
01396   VecPosition posObj     = WM->getGlobalPosition( o );
01397   VecPosition posProj    = l.getPointOnLineClosestTo( posObj );
01398   double      dDistOpp   = posProj.getDistanceTo( posObj );
01399   double      dDistAgent = posProj.getDistanceTo( posAgent );
01400   VecPosition posShoot;
01401 
01402   // we want to know when distance from ball to point p equals distance
01403   // from opp to point p :
01404   // d1 + d3 = sqrt(d2^2 + d3^2) > (d1+d3)^2 = d2^2 + d3^2 =>
01405   // d1^2 + 2*d1*d3 = d2^2 -> d3 = (d2^2 - d1^2 ) / 2*d1
01406   double dCalcDist;
01407   if( o != OBJECT_ILLEGAL )
01408   {
01409     dCalcDist = (dDistOpp*dDistOpp-dDistAgent*dDistAgent)/(2*dDistAgent);
01410     dCalcDist += dDistAgent;
01411   }
01412   else
01413     dCalcDist = 20;
01414 
01415 
01416   Log.log( 552, "outplay opponent %d, calc: %f, opp: %f, agent:  %f",
01417      SoccerTypes::getIndex( o ) + 1, dCalcDist, dDistOpp, dDistAgent );
01418   Log.log( 560, "outplay opponent %d, calc: %f, opp: %f, agent:  %f",
01419      SoccerTypes::getIndex( o ) + 1, dCalcDist, dDistOpp, dDistAgent );
01420 
01421   if( dCalcDist > 7.0 ) // if point far away, kick there
01422   {
01423     dCalcDist = min( posAgent.getDistanceTo( pos ), dCalcDist - 2.5 );
01424     dCalcDist = min( 20, dCalcDist );
01425     posShoot  = posAgent + VecPosition( dCalcDist, ang, POLAR );
01426   }
01427   else if( dDistAgent < dDistOpp - 0.3 ) // point close and well-positioned
01428   {                                      // shoot far away and outplay opp
01429     dCalcDist = min( posAgent.getDistanceTo( pos ), 20);
01430     posShoot = posAgent + VecPosition( dCalcDist, ang, POLAR );
01431   }
01432   else                                   // opponent stands in line
01433     return SoccerCommand( CMD_ILLEGAL );
01434 
01435   if( posShoot.getDistanceTo( WM->getAgentGlobalPosition() ) < 2.5 )
01436   {
01437     Log.log( 552, "calculated point too close" );
01438     Log.log( 560, "calculated point too close (%f,%f)", posShoot.getX(),
01439      posShoot.getY() );
01440     return SoccerCommand( CMD_ILLEGAL );
01441   }
01442   else if( WM->getNrInSetInCone( OBJECT_SET_OPPONENTS,PS->getConeWidth(),
01443     posAgent, posShoot ) != 0 )
01444   {
01445     Log.log( 552, "outplay: is opponent in cone" );
01446     Log.log( 560, "outplay: is opponent in cone (%f,%f)", posShoot.getX(),
01447      posShoot.getY() );
01448     return SoccerCommand( CMD_ILLEGAL );
01449   }
01450 
01451   if( posTo != NULL )
01452     *posTo = posShoot;
01453   return kickTo( posShoot, 0.5 );
01454 }
01455 
01485 SoccerCommand BasicPlayer::clearBall( ClearBallT type, SideT s, AngDeg *angMax )
01486 {
01487   VecPosition posBall = WM->getBallPos();
01488   VecPosition posLeft, posRight;
01489   double      clearDist = PS->getClearBallDist();
01490 
01491   double      dPitchY = PITCH_WIDTH / 2.0;
01492   if( type == CLEAR_BALL_DEFENSIVE )
01493   {
01494     posLeft.setVecPosition ( 0, - dPitchY + clearDist );
01495     posRight.setVecPosition( 0, + dPitchY - clearDist );
01496   }
01497   else if( type == CLEAR_BALL_OFFENSIVE )
01498   {
01499     posLeft.setVecPosition ( PENALTY_X - clearDist, - dPitchY + clearDist );
01500     posRight.setVecPosition( PENALTY_X - clearDist, + dPitchY - clearDist );
01501   }
01502   else if( type == CLEAR_BALL_OFFENSIVE_SIDE )
01503   {
01504     posLeft.setVecPosition ( PENALTY_X - clearDist, - dPitchY + 8 );
01505     posRight.setVecPosition( PENALTY_X - clearDist, + dPitchY - 8 );
01506   }
01507   else if( type == CLEAR_BALL_GOAL && posBall.getY() > 0 )
01508   {
01509     posLeft.setVecPosition ( max(PENALTY_X - 2.0, posBall.getX()-10),  0.0 );
01510     posRight.setVecPosition( PITCH_LENGTH/2.0 - 5.0, 0.0 );
01511   }
01512   else if( type == CLEAR_BALL_GOAL && posBall.getY() < 0 )
01513   {
01514     posLeft.setVecPosition ( PITCH_LENGTH/2.0 - 5.0, 0.0 );
01515     posRight.setVecPosition( max(PENALTY_X - 2.0, posBall.getX()-10),  0.0 );
01516   }
01517   else
01518     return SoccerCommand( CMD_ILLEGAL );
01519 
01520   if( type != CLEAR_BALL_GOAL && s == SIDE_RIGHT ) // take only right part of
01521   {
01522     if( type == CLEAR_BALL_OFFENSIVE_SIDE )
01523       posLeft.setY( 15.0 );
01524     else
01525       posLeft.setY ( 0.0 );                          // field into account
01526     
01527   }
01528   else if( type != CLEAR_BALL_GOAL && s == SIDE_LEFT )
01529   {
01530     if( type == CLEAR_BALL_OFFENSIVE_SIDE )
01531       posRight.setY( -15.0 );
01532     else
01533       posRight.setY( 0.0 );
01534   }
01535 
01536   // get angle of ball with left and right points
01537   // get the largest angle between these two angles
01538   AngDeg angLeft  = (posLeft  - posBall).getDirection();
01539   AngDeg angRight = (posRight - posBall).getDirection();
01540   double dDist;
01541   if( type != CLEAR_BALL_GOAL )
01542     dDist = PS->getClearBallOppMaxDist();
01543   else
01544     dDist = max(posBall.getDistanceTo(posLeft),posBall.getDistanceTo(posRight));
01545   AngDeg ang      = WM->getDirectionOfWidestAngle( posBall, angLeft, angRight,
01546                                         angMax, dDist );
01547 
01548   Line l1 = Line::makeLineFromPositionAndAngle( posBall, ang );
01549   Line l2 = Line::makeLineFromTwoPoints( posLeft, posRight );
01550   VecPosition posShoot = l1.getIntersection( l2 );
01551   Log.log( 560, "angLeft %f, right %f, best %f point (%f,%f)",
01552   angLeft, angRight, ang, posShoot.getX(), posShoot.getY() );
01553   if( type == CLEAR_BALL_GOAL  )
01554     return kickTo( posShoot, SS->getBallSpeedMax() );
01555   else if( type == CLEAR_BALL_OFFENSIVE )
01556     return kickTo( posShoot, 0.25 );
01557   else if( type == CLEAR_BALL_OFFENSIVE_SIDE )
01558   {
01559     return kickTo( posShoot, (posBall.getX()>20) ? 1.2 : 2.7);
01560   }
01561   else
01562     return kickTo( posShoot, 0.5 );
01563 }
01564 
01608 SoccerCommand BasicPlayer::mark( ObjectT o, double dDist, MarkT mark )
01609 {
01610   VecPosition posMark  = getMarkingPosition( o, dDist, mark );
01611   VecPosition posAgent = WM->getAgentGlobalPosition();
01612   VecPosition posBall  = WM->getGlobalPosition( OBJECT_BALL );
01613   AngDeg      angBody  = WM->getAgentGlobalBodyAngle();
01614 
01615   if( o == OBJECT_BALL )
01616   {
01617     if( posMark.getDistanceTo( posAgent ) < 1.5 )
01618       return turnBodyToObject( OBJECT_BALL );
01619     else
01620      return moveToPos( posMark, 30.0, 3.0, false );
01621   }
01622 
01623   // if marking position is outside pen. area and marking position is a small
01624   // distance further to the center, do not move but wait for opponent.
01625   if( posMark.getX() > -PENALTY_X && posAgent.getX() < posMark.getX()  )
01626   {
01627     if( posMark.getX() - posAgent.getX() < 4.0 &&
01628         posMark.getDistanceTo( posAgent ) < 4.0 )
01629     {
01630       Log.log( 455, "%do not move upwards on field to opponent, but wait" );
01631       return turnBodyToPoint( posAgent - VecPosition ( 10, 0 ));
01632     }
01633     Log.log( 455, "opponent upwards on field, so change x" );
01634     posMark.setX( posAgent.getX() );
01635   }
01636   else
01637   {
01638     posAgent = WM->predictAgentPos( 1, 0 );
01639     Line lineMe = Line::makeLineFromPositionAndAngle( posAgent, angBody );
01640     VecPosition posOpp = WM->getGlobalPosition( o );
01641     posOpp.setX( posOpp.getX() - 1 );
01642     Line lineBall = Line::makeLineFromTwoPoints( posBall, posOpp );
01643     VecPosition posIntersect = lineMe.getIntersection( lineBall );
01644     if( posIntersect.getDistanceTo( posMark ) < 4.0 &&
01645         posIntersect.getDistanceTo( posAgent ) > 0.5 )
01646     {
01647       Log.log( 455, "move forward to approximated good marking point" );
01648       return moveToPos( posIntersect, 90, 3.0 );
01649     }
01650   }
01651 
01652   double x = WM->getOffsideX();
01653   if( posMark.getX() >  x )
01654     posMark.setX( x - 2.0 );
01655 
01656   if( WM->isOffsideUs( ) && posMark.getX() > posBall.getX() )
01657     posMark.setX( posBall.getX() - 1.0 );
01658 
01659   if( posMark.getDistanceTo( posAgent )  < 2.0 )
01660   {
01661     Log.log( 455, "turn body to -180" );
01662     return turnBodyToPoint( posAgent - VecPosition ( 10, 0 ));
01663   }
01664   Log.log( 455, "%move to mark position (%f %f)",posMark.getX(),posMark.getY());
01665 
01666   return moveToPos( posMark, PS->getPlayerWhenToTurnAngle(), 3.0, false );
01667 }
01668 
01669 
01708 SoccerCommand BasicPlayer::defendGoalLine( double dDist )
01709 {
01710   // determine defending point as intersection keeper line and line ball-goal
01711   VecPosition posBall    = WM->getBallPos(); 
01712   VecPosition posAgent   = WM->getAgentGlobalPosition();
01713   Line        lineGoal   = Line::makeLineFromPositionAndAngle(
01714                             VecPosition( - PITCH_LENGTH/2.0 + dDist, 0 ), 90 );
01715 
01716   VecPosition posGoalLeft ( -PITCH_LENGTH/2.0, -SS->getGoalWidth()/2.0 );
01717   VecPosition posGoalRight( -PITCH_LENGTH/2.0,  SS->getGoalWidth()/2.0 );
01718   Line left    = Line::makeLineFromTwoPoints( posBall, posGoalLeft  );
01719   Line right   = Line::makeLineFromTwoPoints( posBall, posGoalRight );
01720   posGoalLeft  = left.getIntersection ( lineGoal );
01721   posGoalRight = right.getIntersection( lineGoal );
01722   double dDistLeft  = posGoalLeft.getDistanceTo( posBall );
01723   double dDistRight = posGoalRight.getDistanceTo( posBall );
01724   double dDistLine  = posGoalLeft.getDistanceTo( posGoalRight );
01725   VecPosition posDefend  = posGoalLeft+
01726       VecPosition( 0, (dDistLeft/(dDistLeft+dDistRight))*dDistLine);
01727 
01728   bool        bBallInPen = WM->isInOwnPenaltyArea( posBall );
01729 
01730   // do not stand further to side than goalpost
01731   if( fabs( posDefend.getY() ) > SS->getGoalWidth()/2.0 )
01732     posDefend.setY( sign(posDefend.getY())*SS->getGoalWidth()/2.0);
01733 
01734   // if too far away from line, move directly towards it
01735   double dDiff = ( bBallInPen == true ) ? 1.5 : 0.5;
01736   if( posDefend.getX() + dDiff < posAgent.getX()  )
01737   {
01738     Log.log( 553, "move backwards to guard point" );
01739     return moveToPos( posDefend, 30, -1.0, true ); // always backwards
01740   }
01741   else if( posDefend.getX() - dDiff > posAgent.getX() )
01742   {
01743     Log.log( 553, "move forward to guard point" );
01744     return moveToPos( posDefend, 30, -1.0 );       // always forward
01745   }
01746 
01747   // desired body angle is in direction of the ball
01748   // predicted movement direction in subsequent cycles is in moving dir. ball
01749   AngDeg  angDes;
01750   if( fabs( posBall.getY() - posDefend.getY() ) > 0.5 )
01751     angDes = sign( posBall.getY() - posDefend.getY() )*90.0;
01752   else
01753     angDes = sign( WM->getAgentGlobalBodyAngle() )*90.0;
01754   int     iSign     = sign( WM->getGlobalVelocity( OBJECT_BALL ).getY() );
01755 
01756   // move to position along line: when ball in penalty area, never adjust body
01757   // angle (value 3.0) and change trajectory when angle difference is > 7
01758   // when ball is outside pen. area, adjust angle to move to line when more
01759   // than 0.5 from desired line, adjust when angle diff > 2 with 12 degrees
01760   if( bBallInPen )
01761   {
01762     Log.log( 553, "move along line, with ball in penalty area" );
01763     return moveToPosAlongLine( posDefend, angDes, 3.0, iSign, 7.0, 12.0 );
01764   }
01765   else
01766   {
01767     Log.log( 553, "move along line, with ball outside penalty area (%f,%f) (%f,%f) %f",
01768       WM->getAgentGlobalPosition().getX(),
01769       WM->getAgentGlobalPosition().getY(),
01770       posBall.getX(), posBall.getY(), WM->getConfidence( OBJECT_BALL) );
01771     Log.log( 553, "%s", WM->strLastSeeMessage );
01772     return moveToPosAlongLine( posDefend, angDes, 0.5, iSign, 2.0, 12.0 );
01773   }
01774 }
01775 
01781 SoccerCommand BasicPlayer::interceptScoringAttempt( )
01782 {
01783   SoccerCommand soc;
01784   VecPosition   posAgent = WM->getAgentGlobalPosition();
01785   VecPosition   posBall  = WM->getBallPos();
01786   Line          lineBall = Line::makeLineFromPositionAndAngle( posBall,
01787                                     WM->getBallDirection() );
01788   Line          lineKeeper = Line::makeLineFromPositionAndAngle( posAgent, 90 );
01789   bool          bInterceptAtSide = false;
01790 
01791   // first create intersection point between ball trajectory and rectangle
01792   // that is located just in front of the goal mouth
01793   VecPosition   posIntersect = lineBall.getIntersection( lineKeeper );
01794   if( fabs( posIntersect.getY() ) > SS->getGoalWidth()/2.0  )
01795   {
01796      VecPosition posGoalPost( -PITCH_LENGTH/2.0,
01797                          sign(posBall.getY())*(SS->getGoalWidth()/2.0 + 0.5));
01798      Line l = Line::makeLineFromPositionAndAngle( posGoalPost, 0 );
01799      posIntersect = lineBall.getIntersection( l );
01800 
01801      // if intersection point does not cross rectangle between the agent and
01802      // the goalpost, we just move to edge of the rectangle
01803      if( posIntersect.getX() < posGoalPost.getX()  ||
01804          posIntersect.getX() > posAgent.getX() )
01805        posIntersect.setVecPosition( posAgent.getX(), posGoalPost.getY() );
01806      else
01807        bInterceptAtSide = true; // interception point is at side of rectangle
01808   }
01809 
01810   // first try close intercept
01811   soc = interceptCloseGoalie();
01812   if( ! soc.isIllegal() )
01813   {
01814     Log.log( 553, "close intercept" );
01815 // new na GO 2002
01816     return soc;
01817 // end new na GO 2002    
01818   }
01819 
01820   // if far away from goal, just intercept ball
01821   if( fabs( -PITCH_LENGTH/2.0 - posAgent.getX() ) > 8.0 )
01822   {
01823     Log.log( 553, "I am far away from keeper line" );
01824     return intercept( true );
01825   }
01826   else if( posBall.getX() < posAgent.getX() &&
01827            fabs( posBall.getY() ) < SS->getGoalWidth()/2.0 + 2.0 )
01828   {
01829     Log.log( 553, "ball heading and ball behind me" );
01830     return intercept( true );
01831   }
01832 
01833   // move to interception point
01834   if( posIntersect.getDistanceTo( posAgent ) < 0.5 )
01835   {
01836      Log.log( 553, "close to intersection point keeperline" );
01837      soc = turnBodyToObject( OBJECT_BALL );
01838   }
01839   else if( sign( (posIntersect - posAgent ).getDirection() ) ==
01840            sign( WM->getAgentGlobalBodyAngle()             ) ||
01841        bInterceptAtSide == true)
01842   {
01843     Log.log( 553, "move forward to intersection point keeperline" );
01844     soc = moveToPos( posIntersect, 20, SS->getGoalWidth() );
01845   }
01846   else
01847   {
01848     Log.log( 553, "move backward to intersection point keeperline" );
01849     soc = moveToPos( posIntersect, 20, SS->getGoalWidth(), true );
01850   }
01851 
01852   return soc;
01853 }
01854 
01855 /********************** UTILITY METHODS **************************************/
01856 
01868 VecPosition BasicPlayer::getThroughPassShootingPoint( ObjectT objTeam, VecPosition  
01869                                                posEnd, AngDeg  *angMax  )
01870 {
01871   VecPosition posTeam = WM->getGlobalPosition( objTeam );
01872   return getShootPositionOnLine( posTeam, posEnd, angMax );
01873 }
01874 
01888 VecPosition BasicPlayer::getInterceptionPointBall( int *iCyclesBall,
01889                                                    bool isGoalie )
01890 {
01891   VecPosition posPred  = WM->getAgentGlobalPosition();
01892   VecPosition velMe    = WM->getAgentGlobalVelocity();
01893   double      dSpeed, dDistExtra;
01894   VecPosition posMe, posBall;
01895   AngDeg      ang, angBody, angNeck;
01896   Stamina     sta;
01897   double      dMaxDist;
01898 
01899   if( isGoalie && 
01900       !WM->isInOwnPenaltyArea(WM->predictPosAfterNrCycles( OBJECT_BALL, 45)))
01901   {
01902     *iCyclesBall = -1;
01903     return posBall;
01904   }
01905 
01906   dMaxDist = (isGoalie) ? SS->getCatchableAreaL() : SS->getMaximalKickDist();
01907 
01908   // predict the position of the agent when current velocity is propogated
01909   dSpeed     = WM->getAgentSpeed();
01910   dDistExtra = Geometry::getSumInfGeomSeries( dSpeed, SS->getPlayerDecay() );
01911   posPred   += VecPosition( dDistExtra, velMe.getDirection(), POLAR );
01912 
01913   // for each loop, check whether agent can reach ball in less cycles
01914   for ( int i = 0; i <= PS->getPlayerWhenToIntercept(); i++ )
01915   {
01916     // (re-)initialize all the needed variables
01917     // set ball prediction one further to get right in front of ball line
01918     velMe   = WM->getAgentGlobalVelocity();
01919     angBody = WM->getAgentGlobalBodyAngle();
01920     angNeck = WM->getAgentGlobalNeckAngle();
01921     posBall = WM->predictPosAfterNrCycles( OBJECT_BALL, i + 1 );
01922     posMe   = WM->getAgentGlobalPosition();
01923     ang     = (posBall - posPred).getDirection();
01924     ang     = VecPosition::normalizeAngle( ang - angBody );
01925     sta     = WM->getAgentStamina();
01926     int turn = 0;
01927 
01928     // as long as not correctly headed for point, simulate a turn command
01929     while (fabs(ang) > PS->getPlayerWhenToTurnAngle() && turn<5)
01930     {
01931       turn++;
01932       WM->predictStateAfterTurn( WM->getAngleForTurn( ang,velMe.getMagnitude() ),
01933                    &posMe, &velMe, &angBody, &angNeck, &sta             );
01934       ang      = (posBall - posPred).getDirection();
01935       ang      = VecPosition::normalizeAngle( ang - angBody );
01936     }
01937 
01938     if( turn > 1 )
01939     {
01940       Log.log( 502, "nr of turns needed: %d", turn );
01941     }
01942 
01943     // for cycles that are left over after turn(s), execute dash with full power
01944     for( ; turn < i; turn++ )
01945       WM->predictStateAfterDash(SS->getMaxPower(),&posMe,&velMe,&sta,angBody);
01946 
01947     // if in kickable distance or passed ball, we can reach the ball!
01948     if (posMe.getDistanceTo( posBall ) < dMaxDist  ||
01949        (posMe.getDistanceTo( posPred ) > posBall.getDistanceTo( posPred ) +
01950                                          dMaxDist) )
01951     {
01952       *iCyclesBall = i;
01953       return posBall;
01954     }
01955   }
01956   *iCyclesBall = -1;
01957   return posBall;
01958 }
01959 
01971 VecPosition BasicPlayer::getShootPositionOnLine( VecPosition p1,
01972                                       VecPosition  p2, AngDeg *angLargest )
01973 {
01974   VecPosition posBall  = WM->getBallPos();
01975   Line   line          = Line::makeLineFromTwoPoints( p1, p2 );
01976   double dRadius       = min( PS->getClearBallOppMaxDist(),
01977                               posBall.getDistanceTo( p2 )  );
01978   AngDeg angMin        = (p1 - posBall ).getDirection();
01979   AngDeg angMax        = (p2 - posBall ).getDirection();
01980   // not correct when line crosses -180 boundary, but will never happenk
01981   AngDeg angShoot      = WM->getDirectionOfWidestAngle(
01982                              posBall, min(angMin, angMax),
01983                                  max(angMin,angMax), angLargest, dRadius );
01984   Line   line2         = Line::makeLineFromPositionAndAngle( posBall,
01985                                                              angShoot );
01986   return line.getIntersection( line2 );
01987 }
01988 
02002 double BasicPlayer::getEndSpeedForPass( ObjectT o, VecPosition posPass )
02003 {
02004   // we want that the ball arrives at that point after length nr of cycles
02005   // where length is the nr of cycles it takes the player to get there.
02006   VecPosition posBall = WM->getBallPos();
02007   double      dDist   = posBall.getDistanceTo( posPass );
02008   double      dLength = WM->predictNrCyclesToPoint( o, posPass,
02009                                         PS->getPlayerWhenToTurnAngle() );
02010   double      dFirst  = WM->getFirstSpeedFromDist( dDist, dLength );
02011   if( dFirst > SS->getBallSpeedMax() )
02012     dFirst = SS->getBallSpeedMax();
02013   double dEnd         = WM->getEndSpeedFromFirstSpeed( dFirst, dLength );
02014   if( dEnd > PS->getPassEndSpeed() )
02015     dEnd = PS->getPassEndSpeed();
02016   else if( dEnd < 0.6 )
02017     dEnd = 0.6;
02018   else if( dLength > 10.0 )
02019     dEnd = 0.6;
02020 
02021   return dEnd;
02022 }
02023 
02046 VecPosition BasicPlayer::getMarkingPosition( ObjectT o, double dDist, MarkT mark)
02047 {
02048   VecPosition posBall  = WM->predictPosAfterNrCycles( OBJECT_BALL, 3 );
02049   VecPosition posGoal  = WM->getPosOwnGoal( );
02050   VecPosition pos      = WM->getGlobalPosition( o ) - VecPosition( 1.0, 0.0 );
02051   VecPosition posMark;
02052   AngDeg      ang, angToGoal, angToBall;
02053 
02054   if( mark == MARK_GOAL )                       // position in direction goal
02055   {
02056     angToGoal = (posGoal-pos).getDirection( );
02057     posMark   = pos + VecPosition( dDist, angToGoal, POLAR );
02058   }
02059   else if( mark == MARK_BALL )                  // position in direction ball
02060   {
02061     angToBall = (posBall-pos).getDirection( );
02062     posMark   = pos + VecPosition( dDist, angToBall, POLAR );
02063   }
02064   else if( mark == MARK_BISECTOR )             // position between ball and goal
02065   {
02066     angToBall = (posBall - pos).getDirection( );
02067     angToGoal = (posGoal - pos).getDirection( );
02068     ang       = getBisectorTwoAngles( angToBall, angToGoal );
02069     posMark   = pos + VecPosition( dDist, ang ,POLAR );
02070   }
02071   return posMark;
02072 }

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