www.pudn.com > Product_Submit2004.rar > Sleek.cc, change:2004-10-12,size:35268b


#include "Sleek.h" 
#include "../Common/Common.h" 
#include "../TOOLS/BallTricks.h" 
#include "../TOOLS/BasicTricks.h" 
#include "../TOOLS/HeadTricks.h" 
#include "../TOOLS/ComplexTricks.h" 
#include "../TOOLS/MovementTricks.h" 
#include "../TOOLS/KeeperTricks.h" 
 
// Sleek is elite. 2004 high level behaviour code. 
void Sleek::CalculateHomePosition() { 
  actualHomeX = homeX; 
  actualHomeY = homeY; 
 
  if (myPosition == BL_GOALKEEPER) { 
    double theta = (atan2(wo_ball_->y_-homeY, homeX-wo_ball_->x_) - (PI / 2.0)); 
    while (theta > PI) theta -= 2*PI; 
    while (theta < -PI) theta += 2*PI; 
    actualHomeX = homeX+homeRadius*sin(theta)*2.0; 
    actualHomeY = homeY+homeRadius*cos(theta)*0.30; 
    if (actualHomeY < -210) actualHomeY = -210; 
    if (actualHomeY > -200) actualHomeY = -200; 
 
    // if the ball is in the corners bring the circle focus back 
    // so the ball wont sneak down the walls 
    if (wo_ball_->y_ < - 160 && wo_ball_->x_ > 65) { 
      actualHomeY = -210; 
      actualHomeX = 35;             
    } 
    else if (wo_ball_->y_ < - 160 && wo_ball_->x_ < -65 ) { 
      actualHomeY = -210; 
      actualHomeX = -35; 
    } else { // else its far away or in the middle - use normal code  
      double theta = (atan2(wo_ball_->y_-homeY, homeX-wo_ball_->x_) - (PI / 2.0)); 
      while (theta > PI) theta -= 2*PI; 
      while (theta < -PI) theta += 2*PI; 
      actualHomeX = homeX+homeRadius*sin(theta)*2.0; 
      actualHomeY = homeY+homeRadius*cos(theta)*0.30; 
      if (actualHomeY < -210) actualHomeY = -210; 
    } 
  } else { // not keeper ! 
    if (myPosition == BL_CENTREDEFENDER /*|| myPosition == BL_CENTRESTRIKER (MQ 5/7)*/) { 
      actualHomeX = (wo_ball_->x_ + homeX)/2.0; 
    } else if (myPosition == BL_LEFTSTRIKER || myPosition == BL_RIGHTSTRIKER) { 
      actualHomeY = (wo_ball_->y_ + homeY)/2.0; 
      if (actualHomeY<(homeY+10)) actualHomeY = homeY; 
    }  
  } 
 
  // extra bounds on position if not the keeper .. 
  if (myPosition != BL_GOALKEEPER) { 
    if (actualHomeY > chaseUpperY-boundOffset) actualHomeY = chaseUpperY-boundOffset; 
    if (actualHomeY < chaseLowerY+boundOffset) actualHomeY = chaseLowerY+boundOffset; 
    if (actualHomeY < -150) actualHomeY = -150; 
 
    if (actualHomeX > chaseUpperX-boundOffset) actualHomeX = chaseUpperX-boundOffset; 
    if (actualHomeX < chaseLowerX+boundOffset) actualHomeX = chaseLowerX+boundOffset; 
  } 
} 
 
 
void Sleek::NewPlayingModel() { 
  CheckWireless(); 
  CalculateHomePosition(); 
 
  if (!inPlaying) { 
    if (myPosition != BL_GOALKEEPER && oneInchPunch && (frame_ - lastReadyFrame_) < 30 && robotState_.GetKickOff() == RobotState::KO_OPPONENT) { 
      lcq_.Clear(); 
      ChangeTrick(currentTrick, new BasicTricks::MultiTrick(4, new BasicTricks::Step(LocomotionCommand::TP_SINGLEWALK,180,200,0,0,1.82), new BasicTricks::Step(LocomotionCommand::TP_SINGLEWALK,180,200,0,0,1.82), new BasicTricks::Step(LocomotionCommand::TP_SINGLEWALK,180,200,0,0,1.82), new BallTricks::WaitForKick())); 
      ChangeTrick(headTrick, new HeadTricks::LookAt(-10.0,0.0,0.0,35)); 
   //   ChangeTrick(currentTrick, new ComplexTricks::AdvancedChaseWithKick()); 
   //   ChangeTrick(headTrick, new BasicTricks::NullHead()); 
    } else { 
      ChangeTrick(currentTrick, new BasicTricks::NullBody()); 
      ChangeTrick(headTrick, new BasicTricks::NullHead()); 
    } 
  } 
  inPlaying=true; 
  inGoHome=false; 
 
  int hC = headTrick->Continue(); 
  if (hC < 1) { 
    if (currentTrick->IsUsingHead()) { 
      ChangeTrick(headTrick, new BasicTricks::NullHead()); 
    } else { 
      ChangeTrick(headTrick, ConstructMoveToPointHead()); 
    } 
  } 
  // watching the ball and moving home. count! 
  // flick look code 
 
  watchingBallCounter++; // counter since we last did headpan ! 
  if (strcmp(headTrick->GetName(),"FollowBallWithHeadSticky")==0 && strcmp(currentTrick->GetName(),"MoveToPoint")==0) { 
    if (vo_ball_ == NULL || ABS(vo_ball_->distance_) > 40.0) { 
      if (watchingBallCounter > 120 && (myPosition == BL_GOALKEEPER && wo_self_->uncertX_+wo_self_->uncertY_ > 60)) { 
        ChangeTrick(headTrick, new HeadTricks::QuickPan(30,!lastLookedLeft)); 
        lastLookedLeft = !lastLookedLeft; 
        watchingBallCounter = 0; 
      } 
    } 
  }  
   
  if (!IsBallVisibleTeamNotSelf() && strcmp(headTrick->GetName(),"QuickPan")==0 && strcmp(currentTrick->GetName(),"MoveToPoint")==0) { 
    if (vo_ball_ == NULL || ABS(vo_ball_->distance_) > 40.0) { 
      if (wo_self_->uncertX_+wo_self_->uncertY_ < 20) { // pretty certain of possible. get out ! 
        ChangeTrick(headTrick, ConstructMoveToPointHead()); 
      } 
    } 
  } 
 
  currentTrickResult = currentTrick->Continue(); 
  double ballDistance = CalculateBallDistance(); 
 
  //executing normally. we may wish to interrupt. 
  if (currentTrickResult >= 1) { 
    // we're searching but somebody else has seen the ball .. maybe we don't need to search anymore 
    if (SearchForBall()) { 
       // If somebody can see it and its not close to me then i should just look where they say it is 
       if (IsBallVisibleTeam() && (pow(wo_self_->x_ - wo_ball_->x_, 2)+pow(wo_self_->y_ - wo_ball_->y_, 2) > 30*30) && !nonStopSearch) { 
         ChangeTrick(currentTrick, new MovementTricks::MoveToPoint(actualHomeX,actualHomeY,0.0,MovementTricks::MoveToPoint::MTP_DYNAMIC_FACE_BALL)); 
         if (myPosition == BL_GOALKEEPER) ((MovementTricks::MoveToPoint*)(currentTrick))->SetKeeper(true); 
       } 
    // we're chasing, but we're allowed to interrupt. Also don't interrupt if we are close to the ball. 
    // if we're doing postchasesearchforball w/i advancedchasewithkick, we cannot force it to exit out here. 
    } else if (InterruptibleChase() && !PostChaseSearchForBall()) { 
      // NB - don't necessarily need to be able to see the ball to be allowed to continue chasing (!) 
      // therefore CANNOT use AmIAllowedToChase() here !! 
      if (IsBallWithinEnlargedChaseBounds() == false || ClosestToBallNotSelf() < ballDistance) { 
        if (debugOutput) 
          cout << "Sleek: Aborted AdvancedChaseWithKick (chase bound exceeded)." << endl << flush; 
        // change to move home and look around accordingly 
        ChangeTrick(currentTrick, new MovementTricks::MoveToPoint(actualHomeX,actualHomeY,0.0,MovementTricks::MoveToPoint::MTP_DYNAMIC_FACE_BALL)); 
        ChangeTrick(headTrick, ConstructMoveToPointHead()); 
        if (myPosition == BL_GOALKEEPER) ((MovementTricks::MoveToPoint*)(currentTrick))->SetKeeper(true); 
      } 
    // currently moving home or searching. 
    } else if (MoveToPoint()) { 
      // ball is within our chase area ! 
      if (AmIAllowedToChase(ballDistance)) { 
        if (debugOutput) 
          cout << "Sleek: Ball within chase area. Aborting MoveToPoint and chasing." << endl << flush; 
     
        ChangeTrick(currentTrick, new ComplexTricks::AdvancedChaseWithKick()); 
        ChangeTrick(headTrick, new BasicTricks::NullHead()); 
 
      } else if (ballPrediction_.ShouldIDive()) { 
        // if we're the goalie, or we're facing basically towards opposition goal ... 
        if (myPosition == BL_GOALKEEPER || ABS(RAD_TO_DEG(wo_self_->heading_)) < 80) { 
            /* int direction = ballPrediction_.DiveDirection(); 
             if (lcq_.Front().motionType < 4) {   
               if (direction == BallPrediction::BP_LEFT) utils.Kick(LocomotionCommand::TP_GOALIE_LEFT,false); 
               else if (direction == BallPrediction::BP_RIGHT) utils.Kick(LocomotionCommand::TP_GOALIE_RIGHT,false); 
               else utils.Kick(LocomotionCommand::TP_GOALIE_BLOCK, false); 
             } */ 
            if (!strutSystem_.isKicking && lcq_.IsWalkOnly()) { 
              utils.Kick(LocomotionCommand::TP_GOALIE_BLOCK, false); 
              ballPrediction_.Clear(); 
            } 
        } 
      } else if (!IsBallVisible() && (pow(wo_self_->x_ - wo_ball_->x_, 2)+pow(wo_self_->y_ - wo_ball_->y_, 2) < 30*30) && strcmp(headTrick->GetName(),"FollowBallWithHeadSticky")!=0) { 
        nonStopSearch = true; 
//        if (pow(wo_self_->x_ - wo_ball_->x_, 2)+pow(wo_self_->y_ - wo_ball_->y_, 2) < 40*40) nonStopSearch = true; 
        ChangeTrick(currentTrick, new MovementTricks::SearchForBall(180.0)); 
        ChangeTrick(headTrick, new BasicTricks::NullHead()); 
      } else { 
        // not chasing, not blocking. reposition instead. 
        ((MovementTricks::MoveToPoint*)(currentTrick))->ResetHome((int)actualHomeX,(int)actualHomeY);     
 
      } 
    } 
    // shit finished. decide what to do ! 
  } else if (currentTrickResult < 1) { 
    if (AmIAllowedToChase(ballDistance)) { 
      if (debugOutput) 
        cout << "Sleek: Ball within chase area, previous trick has ended. Chasing." << endl << flush; 
 
      ChangeTrick(currentTrick, new ComplexTricks::AdvancedChaseWithKick()); 
      ChangeTrick(headTrick, new BasicTricks::NullHead()); 
    } else { 
      // if self is within chase bounds, and we did NOT just search, and the ball is not visible .. spin around ! 
      // note - don't do this if in sticky search. 
      if (!SearchForBall() && (strcmp(currentTrick->GetName(), "PostChaseSearchForBall")!=0) && (IsBallVisible() == false) && (IsSelfWithinChaseBounds()) && strcmp(headTrick->GetName(),"FollowBallWithHeadSticky")!=0 && strcmp(headTrick->GetName(),"QuickPan")!=0) { 
        if (debugOutput) 
          cout << "Sleek: Searching for ball." << endl << flush; 
 
        nonStopSearch = false; 
        if (pow(wo_self_->x_ - wo_ball_->x_, 2)+pow(wo_self_->y_ - wo_ball_->y_, 2) < 40*40) nonStopSearch = true; 
 
        ChangeTrick(currentTrick, new MovementTricks::SearchForBall(360.0)); 
        ChangeTrick(headTrick, new BasicTricks::NullHead()); 
 
      } else { 
        // if either a) we've just searched, b) the ball is visible but out of bounds or c) WE are out of bounds, then go home! 
        ChangeTrick(currentTrick, new MovementTricks::MoveToPoint(actualHomeX,actualHomeY,0.0,MovementTricks::MoveToPoint::MTP_DYNAMIC_FACE_BALL)); 
        ChangeTrick(headTrick, ConstructMoveToPointHead());   
        if (myPosition == BL_GOALKEEPER) ((MovementTricks::MoveToPoint*)(currentTrick))->SetKeeper(true);     
      } 
    } 
  } 
 
  if (isCaptain) { 
    RunCaptain(); 
  } 
 
  // Send wireless behaviour info (1 if doing a chase otherwise 0)  
  bool isChasing = false; 
  if (Chase() && !PostChaseSearchForBall()) { 
    isChasing = true; 
  } 
 
  if (isCaptain) { 
    CaptainMessage sndMsg; 
    sndMsg.chasing = isChasing; 
    sndMsg.distance = ballDistance; 
 
    othersNormal[BOTID_-1].distance = ballDistance; 
 
    // load up sendmsg with desired positions for everyone 
    for (int i = 0; i < 4; i++) { 
      int position = robotPosition[i]; 
      sndMsg.upperX[i] = robotRegions[position].upperX; 
      sndMsg.upperY[i] = robotRegions[position].upperY; 
      sndMsg.lowerX[i] = robotRegions[position].lowerX; 
      sndMsg.lowerY[i] = robotRegions[position].lowerY; 
      sndMsg.homeX[i] = robotRegions[position].homeX; 
      sndMsg.homeY[i] = robotRegions[position].homeY; 
      sndMsg.position[i] = position; 
 
      // captain needs to interpret his own position here (since he won't receive messages from himself) 
      if (i == BOTID_-1) { 
        chaseUpperX = robotRegions[position].upperX; 
        chaseUpperY = robotRegions[position].upperY; 
        chaseLowerX = robotRegions[position].lowerX; 
        chaseLowerY = robotRegions[position].lowerY; 
        homeX = robotRegions[position].homeX; 
        homeY = robotRegions[position].homeY; 
        myPosition = position; 
      } 
    } 
    outMessageSize_ = sizeof(CaptainMessage); 
    memcpy(outMessage_,&sndMsg,outMessageSize_);  
  } else { 
    NormalMessage sndMsg; 
    sndMsg.chasing = isChasing; 
    sndMsg.distance = ballDistance; 
    outMessageSize_ = sizeof(NormalMessage); 
    memcpy(outMessage_,&sndMsg,outMessageSize_);  
  } 
 
  //ledOn_[0] = false; 
  //ledOn_[2] = false; 
  //ledOn_[3] = false; 
  //ledOn_[5] = false; 
  if (myPosition == BL_GOALKEEPER) { 
 
  } else if (myPosition == BL_CENTREDEFENDER) { 
    //ledOn_[2] = true; // back right 
    //ledOn_[5] = true; // back left 
  } else if (myPosition == BL_LEFTDEFENDER) { 
    //ledOn_[2] = true; // back left 
  } else if (myPosition == BL_RIGHTDEFENDER) { 
    //ledOn_[5] = true; // back right 
  } else if (myPosition == BL_CENTRESTRIKER) { 
    //ledOn_[0] = true; // front right 
    //ledOn_[3] = true; // front left 
  } else if (myPosition == BL_LEFTSTRIKER) { 
    //ledOn_[0] = true; // front left 
  } else if (myPosition == BL_RIGHTSTRIKER) { 
    //ledOn_[3] = true; // front right 
  } 
 
  // trick has changed very rapidly. output ! check for oscillatory crap happening.. 
/*if (strcmp(currentTrick->GetName(),previousTrick)!=0) { 
    if (frame_-previousTrickFrame == 1) { 
      cout << "Switched from " << previousTrick << " to " << currentTrick->GetName() << endl << flush; 
    } 
    previousTrick = currentTrick->GetName(); 
    previousTrickFrame = frame_; 
  }*/ 
 
  if (emergencyTurnAround_ && lcq_.IsWalkOnly()) { 
    LocomotionCommand lc; 
    lc.Set(LocomotionCommand::TP_SINGLEWALK, 0.0, 0.0, 75.0, 0.0, 1.5); 
    utils.RawCommand(lc); 
    cout << "Emergency turn around triggered by vision! Shit.\n" << flush; 
  } 
} 
Trick* Sleek::ConstructMoveToPointHead() { 
  if (IsBallVisible()) { 
    return new HeadTricks::FollowBallWithHeadSticky(10); 
  } 
  return new HeadTricks::HeadPanForBall(); 
} 
 
bool Sleek::IsSelfWithinChaseBounds() { 
  if ((wo_self_->x_ <= chaseUpperX) && (wo_self_->x_ >= chaseLowerX) && (wo_self_->y_ <= chaseUpperY) && (wo_self_->y_ >= chaseLowerY)) { 
 
// change by CM - defenders roam free now :) 
// Make sure the defenders don't go into the keeper area 
//  if ((myPosition != BL_GOALKEEPER) && (wo_self_->x_ <= 50) && (wo_self_->x_ >= -50) && (wo_self_->y_ <= -160) && (wo_self_->y_ >= -300)) 
//     return false; 
    return true; 
  } 
  return false; 
} 
 
bool Sleek::IsBallWithinChaseBounds() { 
  if ((wo_ball_->x_ <= chaseUpperX) && (wo_ball_->x_ >= chaseLowerX) && (wo_ball_->y_ <= chaseUpperY) && (wo_ball_->y_ >= chaseLowerY)) { 
// change by CM - defenders roam free now :) 
// Make sure the defenders don't go into the keeper area 
//  if ((myPosition != BL_GOALKEEPER) && (wo_ball_->x_ <= 50) && (wo_ball_->x_ >= -50) && (wo_ball_->y_ <= -160) && (wo_ball_->y_ >= -300)) 
//    return false; 
    return true; 
  } 
  return false; 
} 
 
bool Sleek::IsBallWithinEnlargedChaseBounds() { 
  int chaseBonus = 0; 
  if (myPosition == BL_GOALKEEPER) chaseBonus = 20; 
 
  if ((wo_ball_->x_ <= chaseUpperX+chaseBonus) && (wo_ball_->x_ >= chaseLowerX-chaseBonus) && (wo_ball_->y_ <= chaseUpperY+chaseBonus) && (wo_ball_->y_ >= chaseLowerY-chaseBonus)) { 
// change by CM - defenders roam free now :) 
// Make sure the defenders don't go into the keeper area 
//  if ((myPosition != BL_GOALKEEPER) && (wo_ball_->x_ <= 50) && (wo_ball_->x_ >= -50) && (wo_ball_->y_ <= -160) && (wo_ball_->y_ >= -300)) 
//    return false; 
    return true; 
  } 
  return false; 
} 
 
bool Sleek::IsBallVisible() { 
  return (vo_ball_ != NULL); 
} 
bool Sleek::IsBallVisibleTeam() { 
  return (vo_ball_ != NULL || strcmp(headTrick->GetName(),"QuickPan")==0 || strcmp(headTrick->GetName(),"FollowBallWithHeadSticky")==0 || ClosestToBallNotSelf() < 525.0); 
} 
bool Sleek::IsBallVisibleTeamNotSelf() { 
  double smallestDistance = 10000.0; 
  for (int i=0; i<4; i++) { 
    if (othersNormal[i].distance < smallestDistance && i!=BOTID_-1) return true; 
  } 
  return false; 
} 
bool Sleek::AmIAllowedToChase(double distanceToBall) { 
  return IsBallVisible() && IsBallWithinChaseBounds() && (distanceToBall < ClosestToBallNotSelf()); 
} 
double Sleek::ClosestToBallNotSelf() { 
  double smallestDistance = 20000.0; 
  for (int i=0; i<4; i++) { 
    if (othersNormal[i].distance < smallestDistance && i!=BOTID_-1) smallestDistance = othersNormal[i].distance; 
  } 
  return smallestDistance; 
} 
int Sleek::WhoIsClosestToBall() { 
  double smallestDistance = 10000.0; 
  int closest = -2; 
  for (int i=0; i<4; i++) { 
    if (othersNormal[i].distance < smallestDistance) { 
      smallestDistance = othersNormal[i].distance; 
      closest = i; 
    } 
  } 
  return closest+1; 
} 
 
 
void Sleek::CheckWireless() { 
  for (int i = 0; i < BVR_BUFFERSIZE; i++) { 
    ToBVRMessage* msg = &(toBVRMessages[i]); 
    // do we have BVR data ? 
    if (msg->freeSlot==false) { 
      memcpy(&othersNormal[(msg->sourceBotID)-1],msg->data,sizeof(NormalMessage)); 
      lastReceivedTime[(msg->sourceBotID)-1] = frame_; 
      if (othersNormal[(msg->sourceBotID)-1].chasing) { 
        timeOfLastChase = frame_; 
      } 
      // actually have a captain message here. 
      if (msg->size == sizeof(CaptainMessage)) { 
        memcpy(&othersCaptain, msg->data, sizeof(CaptainMessage)); 
        chaseUpperX = othersCaptain.upperX[BOTID_-1]; 
        chaseUpperY = othersCaptain.upperY[BOTID_-1]; 
        chaseLowerX = othersCaptain.lowerX[BOTID_-1]; 
        chaseLowerY = othersCaptain.lowerY[BOTID_-1]; 
        homeX = othersCaptain.homeX[BOTID_-1]; 
        homeY = othersCaptain.homeY[BOTID_-1]; 
 
        myPosition = othersCaptain.position[BOTID_-1]; 
        // homeX,homeY etc all get checked in the positioning code next time NewPlayingModel gets called. 
        // ie, no need to do anything here 
      } 
      // message interpreted. we can free the slot now ! 
      msg->freeSlot=true; 
    } 
  } 
 
  //after ~4 seconds of no wireless from a robot make sure they don't stop us chasing 
  for (int i = 0; i < 4; i++) { 
    if (i != BOTID_-1) { 
      if (frame_-lastReceivedTime[i] > 100) { 
        othersNormal[i].chasing = false; 
        othersNormal[i].distance = 20000.0; 
      } 
    } 
  } 
} 
 
void Sleek::MovePlayer(int id, int newPosition) { 
  robotPosition[id-1] = newPosition; 
} 
 
void Sleek::AllocatePositions(int notThisId, int position1, int position2) { 
  int id1 = -1; int id2 = -1; 
  for (int i = 1; i < 5; i++) { 
    if (i == notThisId || robotPosition[i-1] == BL_GOALKEEPER) continue; 
    if (id1 == -1) id1 = i; 
    else if (id2 == -1) id2 = i; 
  } 
  if (id1 == -1 || id2 == -1) { 
    cout << "Sleek: AllocatePositions is screwed" << endl << flush; 
  } 
 
  double x1 = worldObjectsBVR_[id1-1].x_; 
  double y1 = worldObjectsBVR_[id1-1].y_; 
  double x2 = worldObjectsBVR_[id2-1].x_; 
  double y2 = worldObjectsBVR_[id2-1].y_; 
 
  double distance1 = (pow(x1-robotRegions[position1].homeX, 2) + pow(y1-robotRegions[position1].homeY,2))+(pow(x2-robotRegions[position2].homeX, 2) + pow(y2-robotRegions[position2].homeY,2)); 
  double distance2 = (pow(x2-robotRegions[position1].homeX, 2) + pow(y2-robotRegions[position1].homeY,2))+(pow(x1-robotRegions[position2].homeX, 2) + pow(y1-robotRegions[position2].homeY,2)); 
  if (distance1 < distance2) { 
    MovePlayer(id1, position1); 
    MovePlayer(id2, position2); 
  } else { 
    MovePlayer(id1, position2); 
    MovePlayer(id2, position1); 
  } 
} 
 
void Sleek::RunCaptain() { 
  // can't change positions more than once a second. Unsure of this bit, hence commented out .. 
//if (/*(frame_ - lastSwitchTime) > 0*/true) { 
 
    lastSwitchTime = frame_; 
    int closestId = WhoIsClosestToBall(); // ids are 1 through 4 inclusive 
    if (closestId < 1 || closestId > 4) { 
      if (closestId > 4 || closestId < -1 || closestId == 0) cout << "ClosestId Failed! "<< closestId << endl << flush; 
      return; 
    } 
    if (robotPosition[closestId-1] == BL_GOALKEEPER) { 
      // goalkeeper is closest robot... don't do any swapping. 
      return; 
    } 
 
    int bestPosition = -1; 
    double bestDistance = 1000000.0; 
    double x = worldObjectsBVR_[closestId-1].x_; 
    double y = worldObjectsBVR_[closestId-1].y_; 
    for (int i = 1; i < 5; i++) { 
      double d = pow(x-robotRegions[i].homeX, 2) + pow(y-robotRegions[i].homeY,2); 
      if (d < bestDistance) { 
        bestDistance = d; 
        bestPosition = i; 
      } 
    } 
    int oldPosition = robotPosition[closestId-1]; 
    if (oldPosition == bestPosition) return; 
//  cout << "Chose bestPosition: " << bestPosition << " for " << closestId << endl << flush; 
    MovePlayer(closestId,bestPosition); 
     
    if (bestPosition == BL_LEFTSTRIKER) { 
      AllocatePositions(closestId, BL_RIGHTSTRIKER, BL_CENTREDEFENDER); 
 
    } else if (bestPosition == BL_RIGHTSTRIKER) { 
      AllocatePositions(closestId, BL_LEFTSTRIKER, BL_CENTREDEFENDER); 
 
    } else if (bestPosition == BL_LEFTDEFENDER) { 
      AllocatePositions(closestId, BL_RIGHTDEFENDER, BL_CENTRESTRIKER); 
 
    } else if (bestPosition == BL_RIGHTDEFENDER) { 
      AllocatePositions(closestId, BL_LEFTDEFENDER, BL_CENTRESTRIKER); 
    } 
//} 
} 
 
 
Sleek::Sleek() { 
  configuration_.ParseFile("/ms/open-r/mw/data/captain.cfg"); 
  timeOfLastChase = frame_; 
  debugOutput = false; 
  homeRadius = configuration_.GetAsInt("HomeRadius"); 
  homeErrorDistance = configuration_.GetAsInt("HomeErrorDistance"); 
  currentTrick = new BasicTricks::NullBody(); 
  headTrick = new BasicTricks::NullHead(); 
  inPlaying=false; 
  inGoHome=false; 
 
  lastBallDistance = 20000; 
 
  keeperRoaming=false; 
 
  myPosition = -1; 
  for (int i=0; i<4; i++) { 
    lastReceivedTime[i] = frame_; 
 
    othersNormal[i].chasing = false; 
    othersNormal[i].distance = 20000.0; 
    othersCaptain.chasing=false; 
    othersCaptain.distance = 20000.0; 
    othersCaptain.position[i] = -1; 
    othersCaptain.upperX[i] = 0; 
    othersCaptain.upperY[i] = 0; 
    othersCaptain.lowerX[i] = 0; 
    othersCaptain.lowerY[i] = 0; 
    othersCaptain.homeX[i] = 0; 
    othersCaptain.homeY[i] = 0; 
  } 
  for (int i = 0; i < NUM_POSITIONS; i++) { 
    robotRegions[i].upperX = 0; 
    robotRegions[i].upperY = 0; 
    robotRegions[i].lowerX = 0; 
    robotRegions[i].lowerY = 0; 
    robotRegions[i].homeX = 0; 
    robotRegions[i].homeY = 0; 
  } 
 
  lastSwitchTime = 0; 
  if (BOTID_ == configuration_.GetAsInt("CaptainID")) isCaptain=true; else isCaptain=false; 
 
  if (isCaptain) cout << "CAPTAIN !" << endl << flush; 
 
  if (configuration_.GetAsInt("WP_GOALKEEPER") != -1) { 
    robotPosition[configuration_.GetAsInt("WP_GOALKEEPER")-1] = BL_GOALKEEPER; 
  } 
  if (configuration_.GetAsInt("WP_CENTREDEFENDER") != -1) { 
    robotPosition[configuration_.GetAsInt("WP_CENTREDEFENDER")-1] = BL_CENTREDEFENDER; 
  } 
  if (configuration_.GetAsInt("WP_LEFTDEFENDER") != -1) { 
    robotPosition[configuration_.GetAsInt("WP_LEFTDEFENDER")-1] = BL_LEFTDEFENDER; 
  } 
  if (configuration_.GetAsInt("WP_RIGHTDEFENDER") != -1) { 
    robotPosition[configuration_.GetAsInt("WP_RIGHTDEFENDER")-1] = BL_RIGHTDEFENDER; 
  } 
  if (configuration_.GetAsInt("WP_CENTRESTRIKER") != -1) { 
    robotPosition[configuration_.GetAsInt("WP_CENTRESTRIKER")-1] = BL_CENTRESTRIKER; 
  } 
  if (configuration_.GetAsInt("WP_LEFTSTRIKER") != -1) { 
    robotPosition[configuration_.GetAsInt("WP_LEFTSTRIKER")-1] = BL_LEFTSTRIKER; 
  } 
  if (configuration_.GetAsInt("WP_RIGHTSTRIKER") != -1) { 
    robotPosition[configuration_.GetAsInt("WP_RIGHTSTRIKER")-1] = BL_RIGHTSTRIKER; 
  } 
 
  // this blows chunks. 
  robotRegions[BL_GOALKEEPER].upperX = configuration_.GetAsInt("GoalKeeper_UpperX"); 
  robotRegions[BL_GOALKEEPER].upperY = configuration_.GetAsInt("GoalKeeper_UpperY"); 
  robotRegions[BL_GOALKEEPER].lowerX = configuration_.GetAsInt("GoalKeeper_LowerX"); 
  robotRegions[BL_GOALKEEPER].lowerY = configuration_.GetAsInt("GoalKeeper_LowerY"); 
  robotRegions[BL_GOALKEEPER].homeX = configuration_.GetAsInt("GoalKeeper_HomeX"); 
  robotRegions[BL_GOALKEEPER].homeY = configuration_.GetAsInt("GoalKeeper_HomeY"); 
 
  robotRegions[BL_LEFTSTRIKER].upperX = configuration_.GetAsInt("LeftStriker_UpperX"); 
  robotRegions[BL_LEFTSTRIKER].upperY = configuration_.GetAsInt("LeftStriker_UpperY"); 
  robotRegions[BL_LEFTSTRIKER].lowerX = configuration_.GetAsInt("LeftStriker_LowerX"); 
  robotRegions[BL_LEFTSTRIKER].lowerY = configuration_.GetAsInt("LeftStriker_LowerY"); 
  robotRegions[BL_LEFTSTRIKER].homeX = configuration_.GetAsInt("LeftStriker_HomeX"); 
  robotRegions[BL_LEFTSTRIKER].homeY = configuration_.GetAsInt("LeftStriker_HomeY"); 
 
  robotRegions[BL_RIGHTSTRIKER].upperX = configuration_.GetAsInt("RightStriker_UpperX"); 
  robotRegions[BL_RIGHTSTRIKER].upperY = configuration_.GetAsInt("RightStriker_UpperY"); 
  robotRegions[BL_RIGHTSTRIKER].lowerX = configuration_.GetAsInt("RightStriker_LowerX"); 
  robotRegions[BL_RIGHTSTRIKER].lowerY = configuration_.GetAsInt("RightStriker_LowerY"); 
  robotRegions[BL_RIGHTSTRIKER].homeX = configuration_.GetAsInt("RightStriker_HomeX"); 
  robotRegions[BL_RIGHTSTRIKER].homeY = configuration_.GetAsInt("RightStriker_HomeY"); 
 
  robotRegions[BL_CENTRESTRIKER].upperX = configuration_.GetAsInt("CentreStriker_UpperX"); 
  robotRegions[BL_CENTRESTRIKER].upperY = configuration_.GetAsInt("CentreStriker_UpperY"); 
  robotRegions[BL_CENTRESTRIKER].lowerX = configuration_.GetAsInt("CentreStriker_LowerX"); 
  robotRegions[BL_CENTRESTRIKER].lowerY = configuration_.GetAsInt("CentreStriker_LowerY"); 
  robotRegions[BL_CENTRESTRIKER].homeX = configuration_.GetAsInt("CentreStriker_HomeX"); 
  robotRegions[BL_CENTRESTRIKER].homeY = configuration_.GetAsInt("CentreStriker_HomeY"); 
 
  robotRegions[BL_LEFTDEFENDER].upperX = configuration_.GetAsInt("LeftDefender_UpperX"); 
  robotRegions[BL_LEFTDEFENDER].upperY = configuration_.GetAsInt("LeftDefender_UpperY"); 
  robotRegions[BL_LEFTDEFENDER].lowerX = configuration_.GetAsInt("LeftDefender_LowerX"); 
  robotRegions[BL_LEFTDEFENDER].lowerY = configuration_.GetAsInt("LeftDefender_LowerY"); 
  robotRegions[BL_LEFTDEFENDER].homeX = configuration_.GetAsInt("LeftDefender_HomeX"); 
  robotRegions[BL_LEFTDEFENDER].homeY = configuration_.GetAsInt("LeftDefender_HomeY"); 
 
  robotRegions[BL_RIGHTDEFENDER].upperX = configuration_.GetAsInt("RightDefender_UpperX"); 
  robotRegions[BL_RIGHTDEFENDER].upperY = configuration_.GetAsInt("RightDefender_UpperY"); 
  robotRegions[BL_RIGHTDEFENDER].lowerX = configuration_.GetAsInt("RightDefender_LowerX"); 
  robotRegions[BL_RIGHTDEFENDER].lowerY = configuration_.GetAsInt("RightDefender_LowerY"); 
  robotRegions[BL_RIGHTDEFENDER].homeX = configuration_.GetAsInt("RightDefender_HomeX"); 
  robotRegions[BL_RIGHTDEFENDER].homeY = configuration_.GetAsInt("RightDefender_HomeY"); 
 
  robotRegions[BL_CENTREDEFENDER].upperX = configuration_.GetAsInt("CentreDefender_UpperX"); 
  robotRegions[BL_CENTREDEFENDER].upperY = configuration_.GetAsInt("CentreDefender_UpperY"); 
  robotRegions[BL_CENTREDEFENDER].lowerX = configuration_.GetAsInt("CentreDefender_LowerX"); 
  robotRegions[BL_CENTREDEFENDER].lowerY = configuration_.GetAsInt("CentreDefender_LowerY"); 
  robotRegions[BL_CENTREDEFENDER].homeX = configuration_.GetAsInt("CentreDefender_HomeX"); 
  robotRegions[BL_CENTREDEFENDER].homeY = configuration_.GetAsInt("CentreDefender_HomeY"); 
 
 
  boundOffset = configuration_.GetAsInt("BoundOffset"); 
  oneInchPunch = configuration_.GetAsBool("OneInchPunch"); 
 
  myPosition = robotPosition[BOTID_-1]; 
 
  chaseUpperX = robotRegions[myPosition].upperX; 
  chaseUpperY = robotRegions[myPosition].upperY; 
  chaseLowerX = robotRegions[myPosition].lowerX; 
  chaseLowerY = robotRegions[myPosition].lowerY; 
  homeX = robotRegions[myPosition].homeX; 
  homeY = robotRegions[myPosition].homeY; 
 
  actualHomeX = homeX; 
  actualHomeY = homeY; 
 
  previousTrick = "nothing"; 
  previousTrickFrame = frame_; 
 
 
  watchingBallCounter=0; 
  lastLookedLeft=true; // no matter really. 
 
  // going home 
  xPosition = 0.0; 
  yPosition = 0.0;   
  headingDeg = 0.0; 
  kickLeft = true; 
 
  nonStopSearch = false; 
} 
 
Sleek::~Sleek() { 
  delete currentTrick; 
  delete headTrick; 
} 
 
 
void Sleek::NewGoHomeModel() { 
 
  int moveType = MovementTricks::MoveToPoint::MTP_FORWARDS;  
 
  if (!inGoHome) { 
    delete headTrick; 
    headTrick = new HeadTricks::HeadPan(93,-93,3,20);  
    headTrick->Start(); 
    if (currentTrick != NULL) delete currentTrick;                         //x,y,h,type 
    currentTrick = new MovementTricks::CheckLocalisation(270.0,30); 
    currentTrick->Start(); 
    
    xPosition = 0.0; 
    yPosition = 0.0;   
    headingDeg = 0.0; 
  }  
 
  if (robotState_.GetKickOff() == RobotState::KO_OWN && strcmp(currentTrick->GetName(),"CheckLocalisation")!=0) { 
    if (BOTID_ == 1) { 
      xPosition = 0.0; 
      yPosition = -210.0; 
      headingDeg = 0.0; 
    } else if (BOTID_ == 2) {  // Walk well away from the ball then approach it 
      if (headingDeg != 30.0) { 
        yPosition = -75.0; 
        if (kickLeft) xPosition = 60.0; else xPosition = -60.0;  
        headingDeg = 0.0; 
      }  
      if (wo_self_->y_ < -40 && headingDeg < 0.1) {  
        headingDeg = 30.0; 
      } 
      if (headingDeg > 0.1) { 
        if (vo_ball_ == NULL) { 
          if (!MoveToPoint()) { 
            ChangeTrick(currentTrick, new MovementTricks::MoveToPoint(xPosition,yPosition,headingDeg,moveType)); 
            ChangeTrick(headTrick, ConstructMoveToPointHead()); 
          } 
        } else {  
          //if (vo_ball_->heading_ < DEG_TO_RAD(20)) { 
            if (ABS(vo_ball_->distance_) > 32 && strcmp(currentTrick->GetName(), "AdvancedChaseWithKick") != 0) { 
              ChangeTrick(currentTrick, new ComplexTricks::AdvancedChaseWithKick()); 
              ChangeTrick(headTrick, new BasicTricks::NullHead()); 
            } else if (ABS(vo_ball_->distance_) <= 32) { 
              ChangeTrick(currentTrick, new BasicTricks::NullBody()); 
              ChangeTrick(headTrick, new HeadTricks::FollowBallWithHeadSticky(15)); 
            }  
          /*} else { 
            bool dir = true; 
            if (vo_ball_->heading_ > 0) dir = false; 
            ChangeTrick(currentTrick, new MovementTricks::FaceBeacon(&vo_ball_,dir)); 
            ChangeTrick(headTrick, ConstructMoveToPointHead()); 
          }*/ 
        } 
      } 
    } else if (BOTID_ == 3) { 
      xPosition = -100.0; 
      yPosition = -20.0; 
      headingDeg = 0.0; 
    } else if (BOTID_ == 4) { 
      xPosition = 60.0; 
      yPosition = -105.0; 
      headingDeg = 0.0; 
    } 
 
  } else if (robotState_.GetKickOff() == RobotState::KO_OPPONENT && strcmp(currentTrick->GetName(),"CheckLocalisation")!=0) { 
    if (BOTID_ == 1) { 
      xPosition = 10.0; 
      yPosition = -210.0; 
      headingDeg = 0.0; 
    } else if (BOTID_ == 2) { 
      xPosition = 60.0; 
      yPosition = -107.0; 
      headingDeg = 0.0; 
    } else if (BOTID_ == 3) { 
      xPosition = -10.0; 
      yPosition = -107.0; 
      headingDeg = 0.0; 
    } else if (BOTID_ == 4) { 
      xPosition = -60.0; 
      yPosition = -107.0; 
      headingDeg = 0.0; 
    } 
  } 
   
  if (MoveToPoint()) { 
    ((MovementTricks::MoveToPoint*)(currentTrick))->ResetHome(xPosition,yPosition); 
    double currx = wo_self_->x_; 
    double curry = wo_self_->y_; 
    double d = sqrt(((xPosition-currx)*(xPosition-currx)+(yPosition-curry)*(yPosition-curry))); 
    if (inGoHome && d < 20.0 && moveType == MovementTricks::MoveToPoint::MTP_FORWARDS) { 
      ((MovementTricks::MoveToPoint*)(currentTrick))->SetType(MovementTricks::MoveToPoint::MTP_DYNAMIC); 
    }  
  } 
   
  inGoHome=true; 
  int cC = currentTrick->Continue(); 
 
  if (cC < 1 && strcmp(currentTrick->GetName(),"CheckLocalisation")==0) { 
     delete currentTrick;                         //x,y,h,type 
    currentTrick = new MovementTricks::MoveToPoint(xPosition,yPosition,headingDeg,moveType); 
    currentTrick->Start(); 
    if (wo_self_->x_ < 0) kickLeft = false; else kickLeft = true; 
  } 
 
  int hC = 1; 
  if (!currentTrick->IsUsingHead()) hC=headTrick->Continue(); 
  if (hC < 1) { 
    if (currentTrick->IsUsingHead()) { 
      ChangeTrick(headTrick, new BasicTricks::NullHead()); 
    } else { 
      if (BOTID_ == 2 && headingDeg > 0.1) ChangeTrick(headTrick,ConstructMoveToPointHead()); 
      else ChangeTrick(headTrick, new HeadTricks::HeadPan(93,-93,3,30)); 
    } 
  } 
} 
 
 
void Sleek::NewReadyModel() { 
  CheckWireless(); 
  if (inPlaying) { 
    delete headTrick; 
    headTrick = new HeadTricks::HeadPanForBall(); 
    headTrick->Start(); 
    delete currentTrick; 
    currentTrick = new BasicTricks::NullBody(); 
    currentTrick->Start(); 
  } 
 
  if (vo_ball_ != NULL && strcmp(headTrick->GetName(),"FollowBallWithHeadSticky")!=0) { 
    delete headTrick; 
    headTrick = new HeadTricks::FollowBallWithHeadSticky(10); 
    headTrick->Start(); 
  } else { 
    if (strcmp(headTrick->GetName(),"HeadPanForBall")!=0) { 
      delete headTrick; 
      headTrick = new HeadTricks::HeadPanForBall(); 
      headTrick->Start(); 
    } 
  }  
  headTrick->Continue(); 
  
 
  inPlaying = false; 
  inGoHome = false; 
  timeOfLastChase = frame_; 
} 
 
double Sleek::CalculateBallDistance() { 
  keeperRoaming = false; 
  double ballDistance = 20000.0; 
  if (vo_ball_ != NULL) { 
    ballDistance = ABS(vo_ball_->distance_); 
    if (ballDistance > 520.0) ballDistance = 520.0; 
 
    double goalHeading = utils.GetHeadingToGoal(); 
    ballDistance+=(ABS(goalHeading)/180.0*ballDistance); 
 
    if ((MICRO_TO_DEG(sensorValues_[S_HEAD_TILT1])+MICRO_TO_DEG(sensorValues_[S_HEAD_TILT2]) < -25.0) && (ABS(MICRO_TO_DEG(sensorValues_[S_HEAD_PAN])) < 20.0)) { 
      ballDistance = 1.25; 
    } 
 
    // keeper cheats in bruce lee distance negotiation stuff... if the ball is within his bounds, 
    // he will always chase. however, keeper bounds depend on exactly what he is doing. 
    // this calculation takes this all into account. 
 
    // what bounds should we be using ? 
    bool useEnlargedBounds = (InterruptibleChase() && !PostChaseSearchForBall()); 
    bool withinBounds = false; 
    // use bounds that we want 
    if (useEnlargedBounds) { 
      withinBounds = IsBallWithinEnlargedChaseBounds(); 
    } else { 
      withinBounds = IsBallWithinChaseBounds(); 
    } 
 
    if (myPosition == BL_GOALKEEPER && withinBounds) { 
      ballDistance = 1.5; 
    } else if (myPosition == BL_GOALKEEPER && !withinBounds) { 
      // if no one else has chased for over 4 seconds and we're close to the ball, enable keeperRoaming ! 
      // this essentially just enlarges the keeper's chase bounds. 
 
      if (frame_-timeOfLastChase > 100 && ballDistance < 120.0) { 
        keeperRoaming = true; 
        ballDistance = 524.0; 
      } else { 
        ballDistance = 524.0; 
      } 
    } 
  } 
  //doing a chase and not interruptible AND we're too fucking close (head tilt measure), FUCK OFF AWAY FROM US!! 
  if (NonInterruptibleChase()) { 
    ballDistance = 1.0; 
  } else if ((Chase() && !PostChaseSearchForBall() && !IsBallVisible()) || (MoveToPoint() && strcmp(headTrick->GetName(),"FollowBallWithHeadSticky")==0 && vo_ball_ == NULL)) { 
    // chase or MTP has momentarily lost the ball. that's ok. don't fuck him up !!                                                                                  
    ballDistance = lastBallDistance; 
  } 
  
  lastBallDistance = ballDistance; 
//  cout << ballDistance << " " << endl; 
  return ballDistance; 
}