memposition.c

RoboCup 2D 仿真组冠军源代码之1998年冠军队——CMUnited98源代码

  PlayerObject *player = GetPlayer(side, n);
  if ( player == NULL ) 
    return 0;
  else 
    return player->face_valid();
}  



/********************************************************************************/
/********************************************************************************/
/********************************************************************************/

Vector PositionInfo::BallPredictedPosition(int steps, float kick_power, AngleDeg kick_angle)
{
  if ( !BallVelocityValid() ) my_error("Need to know ball velocity for prediction");

  Vector new_vel = Polar2Vector( BallKickRate()*kick_power, MyAng()+kick_angle );
  return GetBall()->estimate_future_pos(steps, new_vel);
}

/********************************************************************************/

Bool PositionInfo::BallWillBeKickable(int steps, float dash_power, float buffer)
{
  if ( !MyConf() || !BallPositionValid() ) my_error("Can't predict kickable");

  Vector ball_predicted_position = BallPredictedPosition(steps);
  Vector my_predicted_position   = MyPredictedPosition  (steps,dash_power);

  return my_predicted_position.dist(ball_predicted_position) <= SP_kickable_area-buffer ? TRUE : FALSE; 
}

/*****************************************************************************************/

Unum PositionInfo::PlayerWithBall(float buffer)
{
  if ( !BallPositionValid() ) return Unum_Unknown;
  Vector ball = BallAbsolutePosition();

  Unum teammate = ClosestTeammateToBall();
  Unum opponent = ClosestOpponentToBall();

  float teammate_distance = 400, opponent_distance = 400;
  if ( teammate != Unum_Unknown )
    teammate_distance = TeammateDistanceTo(teammate,ball);
  if ( opponent != Unum_Unknown )
    opponent_distance = OpponentDistanceTo(opponent,ball);

  if ( teammate_distance < opponent_distance && BallKickableForTeammate(teammate,buffer) )
    return teammate;
  else if ( opponent_distance < teammate_distance && BallKickableForOpponent(opponent,buffer) )
    return -opponent;
  else
    return Unum_Unknown;
}

/*****************************************************************************************/

Unum PositionInfo::TeammateWithBall(float buffer)
{
  Unum player = PlayerWithBall(buffer);
  if ( player > 0 ) return player;
  else return Unum_Unknown;
}

/*****************************************************************************************/

Unum PositionInfo::OpponentWithBall(float buffer)
{
  Unum player = PlayerWithBall(buffer);
  if ( player < 0 ) return -player;
  else return Unum_Unknown;
}

/*****************************************************************************************/

char PositionInfo::TeamWithBall(float buffer)
{
  Unum player = PlayerWithBall(buffer);
  if ( player == Unum_Unknown ) return '?';
  if ( player > 0 ) return MySide;
  if (player < 0) return TheirSide;
  my_error("player with ball needs to say something");
  return '*';
}



/********************************************************************************/
/********************************************************************************/
/********************************************************************************/

PlayerObject *PositionInfo::GetTeammate(Unum num)
{
  if ( num==Unum_Unknown ) my_error("Shouldn't get a teammate with unknown num");
  if ( num==MyNumber ) my_error("Shouldn't get self from MyPlayer");

  for (int i=0; i<num_my_players; i++)
    if ( MyPlayer[i]->unum == num ) return MyPlayer[i];

  return NULL;
}

/*********************************************************************************/

PlayerObject *PositionInfo::GetOpponent(Unum num)
{
  if ( num==Unum_Unknown ) my_error("Shouldn't get an opponent with unknown num");

  for (int i=0; i<num_their_players; i++)
    if ( TheirPlayer[i]->unum == num ) return TheirPlayer[i];

  return NULL;
}

/*********************************************************************************/

PlayerObject *PositionInfo::GetPlayer(char side, Unum num)
{
  if ( side == MySide )
    return GetTeammate(num);
  else if ( side == TheirSide ) 
    return GetOpponent(num);
  else 
    my_error("Can't get a player from an unknown side");

  return NULL;
}


/*********************************************************************************/

PlayerObject *PositionInfo::GetNewPlayer(char side, Unum num)
{
  while ( num_free_players <= 0 ||
	  (side == MySide && num_my_players >= SP_team_size-1) ||
	  (side == TheirSide && num_their_players >= SP_team_size) ){
    if ( !ForgetAPlayer(side) )  /* All the players on that side are valid */
      return NULL;
  }

  PlayerObject *player = FreePlayer[--num_free_players]; 
  
  if ( side == MySide )
    MyPlayer[num_my_players++] = player;
  else if ( side == TheirSide )
    TheirPlayer[num_their_players++] = player;
  else /* side == '?' */
    TeamlessPlayer[num_teamless_players++] = player;

  player->side = side;
  player->unum = num;
  
  return player;
}

/*********************************************************************************/

Bool PositionInfo::ForgetAPlayer(char side)
{
  /* char original_side = side; */

  /* If there aren't enough on the team, forget a teamless player */
  if ( side == MySide && num_my_players < SP_team_size-1 ) 
    side = '?';
  if ( side == TheirSide && num_their_players < SP_team_size ) 
    side = '?';

  /* get rid of the one with unknown numbers that have lowest conf */
  int least_conf_index = -1;
  float least_conf = CP_max_conf+1;  /* Must forget a player, even if all at max conf */
  for ( int i=0; i<num_players; i++ ){

    if ( side == 'f' ) continue;
    if ( side != '?' && side != Player[i].side ) continue;

    if ( (Player[i].unum == Unum_Unknown || Player[i].side == '?') && Player[i].pos_valid() < least_conf ){
      least_conf = Player[i].pos_valid();
      least_conf_index = i;
    }
  }

  if ( least_conf_index >= 0 ){
    side = Player[least_conf_index].side;   
    Player[least_conf_index].reset();
  }
  else if ( side == MySide && num_my_players > SP_team_size-1 ){
    if ( ResetMyDuplicatePlayers() == FALSE ){
      my_error("There should be a duplicate MyPlayer");
      return FALSE; /* else do the clean below */
    }
  }
  else if ( side == TheirSide && num_their_players > SP_team_size ){
    if ( ResetTheirDuplicatePlayers() == FALSE ){
      my_error("There should be a duplicate TheirPlayer");
      return FALSE; /* else do the clean below */
    }
  }
  else
    return FALSE;

  CleanAllPlayers();
  /* if      ( side == MySide    ) CleanMyPlayers();
  else if ( side == TheirSide ) CleanTheirPlayers();
  else if ( side == '?'       ) CleanTeamlessPlayers();
  else my_error("Which side?"); */

  return TRUE;
}

/*********************************************************************************/

void PositionInfo::CleanMyPlayers()
{
  int new_num_my_players = 0;

  for (int i=0; i<num_my_players; i++){
    if ( MyPlayer[i]->pos_valid() ) 
      MyPlayer[new_num_my_players++] = MyPlayer[i];
    else{
      MyPlayer[i]->side = 'f';
      MyPlayer[i]->unum = Unum_Unknown;
      FreePlayer[num_free_players++] = MyPlayer[i];
    }
  }

  num_my_players = new_num_my_players;

  while ( num_my_players > SP_team_size-1 ){
    /* my_stamp; printf("%d of my players\n",num_my_players);    */
    if ( ForgetAPlayer(MySide) == FALSE ){  /* recurses through CleanMyPlayers */
      my_error("Should be able to forget a teammate");
      printf("Number of players (%d %d %d %d)\n",
	     num_my_players,num_their_players,num_teamless_players,num_free_players);
      for (int i=0; i<num_players; i++)
	printf("%c %d   %f\n",Player[i].side,Player[i].unum,Player[i].pos_valid());
    }
  }

  if (num_my_players > SP_team_size-1){
    my_error("Too many of my players %d",num_my_players);
    /* my_stamp;
    for (int i=0; i<num_my_players; i++)
      printf("%d %.1f  ",MyPlayer[i]->unum,MyPlayer[i]->pos_valid() );
    printf("\n");
    dump_core("dump");*/
  }
}

/*********************************************************************************/

void PositionInfo::CleanTheirPlayers()
{
  int new_num_their_players = 0;

  for (int i=0; i<num_their_players; i++){
    if ( TheirPlayer[i]->pos_valid() ) 
      TheirPlayer[new_num_their_players++] = TheirPlayer[i];
    else{
      TheirPlayer[i]->side = 'f';
      TheirPlayer[i]->unum = Unum_Unknown;
      FreePlayer[num_free_players++] = TheirPlayer[i];
    }
  }
  num_their_players = new_num_their_players;

  while ( num_their_players > SP_team_size ){
    /* my_stamp; printf("%d of their players\n",num_their_players);    */
    if ( ForgetAPlayer(TheirSide) == FALSE ){  /* recurses through CleanTheirPlayers */
      my_error("Should be able to forget an opponent");
      printf("Number of players (%d %d %d %d)\n",
	     num_my_players,num_their_players,num_teamless_players,num_free_players);
      for (int i=0; i<num_players; i++)
	printf("%c %d   %f\n",Player[i].side,Player[i].unum,Player[i].pos_valid());
    }
  }

  if (num_their_players > SP_team_size){
    my_error("Too many of their players %d",num_their_players);
    /*my_stamp;
    for (int i=0; i<num_their_players; i++)
      printf("%d %.1f  ",TheirPlayer[i]->unum,TheirPlayer[i]->pos_valid() );
    printf("\n");
    dump_core("dump");*/
  }
}

/*********************************************************************************/

void PositionInfo::CleanTeamlessPlayers()
{
  int new_num_teamless_players = 0;

  for (int i=0; i<num_teamless_players; i++){
    if ( TeamlessPlayer[i]->pos_valid() ){
      /* player may have been identified as being on one team or another */
      if ( TeamlessPlayer[i]->side == MySide ){
	MyPlayer[num_my_players++] = TeamlessPlayer[i];
      }
      else if ( TeamlessPlayer[i]->side == TheirSide ){
	TheirPlayer[num_their_players++] = TeamlessPlayer[i];
      }
      else if ( TeamlessPlayer[i]->side == '?' )
	TeamlessPlayer[new_num_teamless_players++] = TeamlessPlayer[i];
      else
	my_error("Teamless players should have side '?'");
    }      
    else{
      TeamlessPlayer[i]->side = 'f';
      TeamlessPlayer[i]->unum = Unum_Unknown;
      FreePlayer[num_free_players++] = TeamlessPlayer[i];
    }
  }

  num_teamless_players = new_num_teamless_players;

  while ( num_teamless_players > num_players )
    if ( ForgetAPlayer('?') == FALSE ) /* recurses through CleanTeamlessPlayers */
      my_error("Should be able to forget a teamless player");

  if (num_teamless_players > num_players ){
    my_error("Too many of teamless players %d",num_teamless_players);
    my_stamp;
    for (int i=0; i<num_teamless_players; i++)
      printf("%d %.1f  ",TeamlessPlayer[i]->unum,TeamlessPlayer[i]->pos_valid() );
    printf("\n");
  }
}

/*********************************************************************************/

void PositionInfo::CleanAllPlayers()
{
  CleanTeamlessPlayers();  /* has to be first to move over reconciled players */
  CleanMyPlayers();
  CleanTheirPlayers();
}

/*********************************************************************************/

Bool PositionInfo::ResetMyDuplicatePlayers()
{
  if ( num_my_players <= SP_team_size-1 ) my_error("Shouldn't be duplicates");
  
  int MyPlayerIndex[MAX_PLAYERS]; /* Map uniform number to index in MyPlayer */
  
  for (int num=1; num<=SP_team_size; num++)
    MyPlayerIndex[num] = -1;

  Bool DuplicateFound = FALSE;
  Unum unum;
  for (int i=0; i<num_my_players; i++){
    unum = MyPlayer[i]->unum;
    if ( unum == Unum_Unknown ) my_error("Catch unknowns before duplicates (my)");

    if ( unum == MyNumber ){
      MyPlayer[i]->reset();
      DuplicateFound = TRUE;
      my_error("How did my number get in the mix?",unum);
    }
    else if ( MyPlayerIndex[unum] == -1 )
      MyPlayerIndex[unum] = i;
    else if ( MyPlayer[MyPlayerIndex[unum]]->pos_valid() >= MyPlayer[i]->pos_valid() ){
      MyPlayer[i]->reset();
      DuplicateFound = TRUE;
      my_error("There were 2 %d's on my side",unum);
    }
    else{
      MyPlayer[MyPlayerIndex[unum]]->reset();
      MyPlayerIndex[unum] = i;
      DuplicateFound = TRUE;
      my_error("There were 2 %d's on my side",unum);
    }
  }
      
  return Dupli