position.c

足球机器人仿真组CMU97的源码

    else if ( global_y < PA_Y/2 )             return THEIR_PC_FLAG;
    else if ( global_y < PA_Y + (Y0-PA_Y)/2 ) return THEIR_PL_FLAG;
    else if ( Mem->PlayMode != BEFORE_KICK_OFF) printf("%d:%d Flag Confusion 6\n",MyNumber,CurrentTime);
  }
  else{
    if ( global_y < -PA_Y - (Y0-PA_Y)/2 )     return RF_FLAG;
    else if ( global_y > PA_Y + (Y0-PA_Y)/2 ) return LF_FLAG;
    else if ( Mem->PlayMode != BEFORE_KICK_OFF) printf("%d:%d Flag Confusion 7\n",MyNumber,CurrentTime);
  }
  return FALSE;
}

int PositionInfo::ClosestUnseenPlayer(char *side, float *global_x, float *global_y){
  float minDistance = 1000;
  float Distance;
  PositionMobile *closestPlayer;
  float player_x,player_y;
  
  if ( *side != TheirSide ){ /* MySide or unknown */
    for (int i=1; i<TEAM_SIZE+1; i++){
      if ( !MyTeam[i]->gospel && MyNumber != i && 
	   CurrentTime - MyTeam[i]->get_global_set_time() < 100 ){    /* set recently */
	MyTeam[i]->get_globalxy(&player_x,&player_y); /* Stored, not computed values */
	Distance = GetDistance(&player_x,&player_y,global_x,global_y);
	if ( Distance < minDistance ){
	  minDistance = Distance;
	  closestPlayer = MyTeam[i];
	}
      }
    }
  }
  if ( *side != MySide ){ /* MySide or unknown */
    for (int i=1; i<TEAM_SIZE+1; i++){
      if ( !TheirTeam[i]->gospel ){
	GetPlayerGlobalXY(TheirSide,i,&player_x,&player_y);
	Distance = GetDistance(&player_x,&player_y,global_x,global_y);
	if ( Distance < minDistance ){
	  minDistance = Distance;
	  closestPlayer = TheirTeam[i];
	}
      }
    }
  }
  
  if ( minDistance > 10 )
    return 0;         /* No player is close enough to guess */

#if 0
  printf("Min distance from (%.1f, %.1f) = %.1f \n",*global_x,*global_y,minDistance);
  printf("Chose player %d\n",closestPlayer->num);
#endif

  *side = closestPlayer->side;
  return closestPlayer->num;
}

int   PositionInfo::MyVelValid(){
  if ( my_vel_dir == NODIR )
    return FALSE;
  else 
    return TRUE;
}

float PositionInfo::GetMyVelMagnitude(){ 
  return my_vel_mag;
}

float PositionInfo::GetMyVelDirection(){
  return my_vel_dir;
}

float PositionInfo::GetPlayerDistance(char side, int number)
{
  if (side == MySide)
    return MyTeam[number]->get_r();
  else
    return TheirTeam[number]->get_r();
}

float PositionInfo::GetPlayerAngle(char side, int number)
{
  if (side == MySide)
    return MyTeam[number]->get_tDEG();
  else
    return TheirTeam[number]->get_tDEG();
}

float PositionInfo::GetPlayerAngleRad(char side, int number)
{
  if (side == MySide)
    return MyTeam[number]->get_t();
  else
    return TheirTeam[number]->get_t();
}

float PositionInfo::GetPlayerVelDirection(char side, int number)
{
  float dir;  /* was in global coordinates, want relative to me */

  if (side == MySide)
    dir = MyTeam[number]->get_vel_dir();
  else if (side == TheirSide)
    dir = TheirTeam[number]->get_vel_dir();
  else 
    my_error("Which side?");

  if ( dir == NODIR )
    return NODIR;    
  dir = dir - rad_to_deg(global_angle);

  CleanAngle(&dir);
  return dir;
}

float PositionInfo::GetPlayerVelMagnitude(char side, int number)
{
  if (side == MySide)
    return MyTeam[number]->get_vel_mag();
  if (side == TheirSide)
    return TheirTeam[number]->get_vel_mag();
}

float PositionInfo::PlayerDistanceValid(char side, int number)
{
  if (side == MySide)
    return MyTeam[number]->get_rconf();
  else
    return TheirTeam[number]->get_rconf();
}
  
float PositionInfo::PlayerAngleValid(char side, int number)
{
  if (side == MySide)
    return MyTeam[number]->get_tconf();
  else
    return TheirTeam[number]->get_tconf();
}

float PositionInfo::PlayerValid(char side, int number)
{
  float conf = MIN(PlayerDistanceValid(side, number),PlayerAngleValid(side, number)); 
  
  if ( conf > 0 ){
    float x,y;
    GetPlayerGlobalXY(side,number,&x,&y);
    if ( !InPitch(x,y) ) conf = 0;
  }

  return conf;
}

int   PositionInfo::PlayerVelValid(char side, int number)
{
  if ( side == MySide )
    return ( (MyTeam[number]->get_vel_dir() == NODIR) ? FALSE : TRUE); 
  else 
    return ( (TheirTeam[number]->get_vel_dir() == NODIR) ? FALSE : TRUE); 
}

float PositionInfo::GetBallDistance() { return Ball->get_r(); }

float PositionInfo::GetBallAngle() { return Ball->get_tDEG(); }

float PositionInfo::GetBallAngleRad() { return Ball->get_t(); }

float PositionInfo::GetBallVelDirection() { 
  float dir =  Ball->get_vel_dir();

  if ( dir == NODIR )
    return NODIR;    
  dir = dir - rad_to_deg(global_angle);

  CleanAngle(&dir);   /* was in global coordinates, want relative to me */
  return dir;
}

float PositionInfo::GetBallVelMagnitude() { return Ball->get_vel_mag(); }

float PositionInfo::GetBallRelVelDirection() { return Ball->get_rel_vel_tDEG(); }

float PositionInfo::GetBallRelVelMagnitude() { return Ball->get_rel_vel_r(CurrentTime-LastTime);}

float PositionInfo::GetBallTrajectoryAbsoluteDirection() { return Ball->get_traj_dir(); }

float PositionInfo::GetBallTrajectoryRelativeDirection() { 
  float dir =  GetBallTrajectoryAbsoluteDirection();
  dir -= global_angle;  /* was in global coordinates, want relative to me */
  CleanAngleRad(&dir);   

  dir = rad_to_deg(dir);
  return dir;
}

float PositionInfo::GetBallTrajectoryMagnitude() { return Ball->get_traj_mag(); }

float PositionInfo::BallTrajectoryValid() { 
  return Ball->get_traj_conf(CurrentTime,MIN(TRAJ_VALID_TIME,TimePerSight()*3)); 
}

float PositionInfo::BallDistanceValid() { return (float) Ball->get_rconf(); }

float PositionInfo::BallAngleValid() { return (float) Ball->get_tconf(); }

float PositionInfo::BallValid()
{
  return MIN(BallDistanceValid(),BallAngleValid()); 
}

float PositionInfo::BallRelVelocityValid() { return Ball->get_old_conf(); }

int   PositionInfo::BallVelValid() { 
  return ( (Ball->get_vel_dir() == NODIR) ? FALSE : TRUE); 
}

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

void PositionInfo::SeeLine(int l, float dist, float ang, int tm)
{
  line = l;
  line_r = dist; line_t = fabs(deg_to_rad(ang));

  switch (line) {
  case LINE_T : global_angle = (ang < 0) ? 
    normalize(M_PI - line_t) : normalize(line_t); break;
  case LINE_B : global_angle = (ang < 0) ? 
    normalize(-line_t) : normalize(M_PI + line_t); break;
  case LINE_R : global_angle = (ang < 0) ? 
    normalize(M_PI_2 - line_t) : normalize(-M_PI_2 + line_t); break;
  case LINE_L : global_angle = (ang < 0) ? 
    normalize(-M_PI_2 - line_t) : normalize(M_PI_2 + line_t); break;
  }
}

void PositionInfo::SeeLine(int l, float ang, int tm)
{
  /*******  Can't use -1 if later on dists to lines are < 0 when off field ******/
  float dist = NO_LINE; /* Indicates that we no longer know how far the line is */
                        /* Be careful how it's used now (triangulation)         */
  /*******  Can't use -1 if later on dists to lines are < 0 when off field ******/

  SeeLine(l,dist,ang,tm);
}

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

void PositionInfo::SeeMarker(int marker, float dist, float ang, int tm)
{
  int m = ConvertMarker(marker);
  Markers[m]->set_polar(dist, deg_to_rad(ang), 1.0);
}

void PositionInfo::SeeMarker(int marker, float ang, int tm)
{
  my_error("Shouldn't process markers when using low quality -- no info");
  int m = ConvertMarker(marker);
  /* For purpose of triangulation, it's not really visble */
  Markers[m]->set_t_gospel(deg_to_rad(ang), 1.0);
}

void PositionInfo::SeeMarker(int marker, float dist, float ang, 
			     float distChng, float dirChng, int tm){

  if (dirChng == NODIR)
    my_error("Shouldn't be here without a valid dirChng");

  float rad_ang = deg_to_rad(ang);
  my_vel_x = -(distChng*cos(rad_ang) - deg_to_rad(dirChng)*dist*sin(rad_ang));
  my_vel_y = -(distChng*sin(rad_ang) + deg_to_rad(dirChng)*dist*cos(rad_ang));

  SeeMarker(marker,dist,ang,tm);
}

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

void PositionInfo::SeeBall(float ang, int tm)
{
  Ball->set_t_gospel(deg_to_rad(ang), 1.0);
}

void PositionInfo::SeeBall(float dist, float ang, int tm)
{
  Ball->set_polar(dist, deg_to_rad(ang), 1.0);
}

void PositionInfo::SeeBall(float dist, float ang, float distChng, float dirChng, int tm)
{
  if ( (my_vel_x!=NODIR && my_vel_y!=NODIR) ||
       !dashing ){ /* If I didn't just move, assume no motion */

    if (!dashing)
      my_vel_x = my_vel_y = 0;

    float vel_mag, vel_dir;

    float rad_ang = deg_to_rad(ang);
    float Wx = distChng*cos(rad_ang) - deg_to_rad(dirChng)*dist*sin(rad_ang);
    float Wy = distChng*sin(rad_ang) + deg_to_rad(dirChng)*dist*cos(rad_ang);

    float Ux = Wx + my_vel_x;
    float Uy = Wy + my_vel_y;

    vel_mag = sqrt(Ux*Ux + Uy*Uy);
    vel_dir = rad_to_deg(my_atan2(Uy,Ux));

    /* Don't know if this should be in here or not */
/*
    if (Wx < 0) { 
      vel_dir += 180;
      CleanAngle(&vel_dir);
    }
*/  
    Ball->set_vel(vel_dir,vel_mag);  /* This is now in global coordinates               */
                                     /* changes to relative angle in GetBallVelDirection*/
#if 0
    printf("*****Global vel (dir,mag):  (%.1f, %.1f)\n",vel_dir,vel_mag);
#endif
  }

  /* Use for NN training? */

  SeeBall(dist,ang,tm);
}

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

void PositionInfo::SeePlayer(char side, int num, float ang, int time)
{
  if (side == MySide)
    MyTeam[num]->set_t_gospel(deg_to_rad(ang), 1.0);
  else
    TheirTeam[num]->set_t_gospel(deg_to_rad(ang), 1.0);
}

void PositionInfo::SeePlayer(char side, int num, float dist, float ang, int time)
{
  if (side == MySide)
    MyTeam[num]->set_polar(dist, deg_to_rad(ang), 1.0);
  else
    TheirTeam[num]->set_polar(dist, deg_to_rad(ang), 1.0);
}

void PositionInfo::SeePlayer(char side, int num, 
			     float dist, float ang, float distChng, float dirChng, int time)
{
  PositionMobile *Player;

  if (side == MySide)
    Player = MyTeam[num];
  if (side == TheirSide)
    Player = TheirTeam[num];

  if ( (my_vel_x!=NODIR && my_vel_y!=NODIR) ||
       !dashing ){ /* If I didn't just move, assume no motion */

    if (!dashing)
      my_vel_x = my_vel_y = 0;

    float vel_mag, vel_dir;

    float rad_ang = deg_to_rad(ang);
    float Wx = distChng*cos(rad_ang) - deg_to_rad(dirChng)*dist*sin(rad_ang);
    float Wy = distChng*sin(rad_ang) + deg_to_rad(dirChng)*dist*cos(rad_ang);

    float Ux = Wx + my_vel_x;
    float Uy = Wy + my_vel_y;

    vel_mag = sqrt(Ux*Ux + Uy*Uy);
    vel_dir = rad_to_deg(my_atan2(Uy,Ux));
    if (Wx < 0) { 
      vel_dir += 180;
      CleanAngle(&vel_dir);
    }
  
    Player->set_vel(vel_dir,vel_mag);/* This is now in global coordinates               */
                                     /* changes to relative angle in GetBallVelDirection*/
  }

  SeePlayer(side,num,dist,ang,time);
}

void SeePlayer(float dist, float ang, float distChng, float dirChng, int time){
  my_error("shouldn't be getting here");
}

void PositionInfo::SeePlayer(char side, float dist, float ang, int time)
{
  int num=0;

  for (int i=1; i<TEAM_SIZE; i++){

    if ( PlayerAngleValid(side,i) /*> PlayerDistanceValid(side,i)*/
	 && fabs(GetPlayerAngle(side,i) - ang) < 1 ){
      if ( num ){ /* Already assigned */
	if ( fabs(GetPlayerDistance(side,i) - dist) < 2 ||
	     ( GetPlayerDistance(side,num) != dist &&
	       PlayerAngleValid(side,i) > PlayerAngleValid(side,num)) )
	  num = i;
	/*my_error("Two players at same angle!!!");*/
      }
      else       /* not assigned yet  */
	num = i;
    }
  }

  if ( num ){
#if PLAYER_ID_DEBUG
    printf("SeePlayer1 %d:  Player %d thinks Player %d is at %.1f, %.1f\n",
	   CurrentTime,MyNumber,num,dist,ang);
#endif
    SeePlayer(side, num, dist, ang, time);
  }
  else
    SeeExtraPlayer(side, dist, ang, time);
}

void PositionInfo::SeePlayer(char side, float ang, int time)
{
  int num=0;
  
  for (int i=1; i<TEAM_SIZE; i++){

    if ( PlayerAngleValid(side,i) /*> PlayerDistanceValid(side,i)*/
	 && fabs(GetPlayerAngle(side,i) - ang) < 1 ){
      if ( num ){ /* Already assigned */
	if ( PlayerAngleValid(side,i) > PlayerAngleValid(side,num) )
	  num = i;
	/*my_error("Two players at same angle!!!"); */
      }
      else       /* not assigned yet  */
	num = i;
    }
  }

  if ( num ){
#if PLAYER_ID_DEBUG
    printf("SeePlayer2 %d:  Player %d thinks Player %d is at ang %.1f\n",
	   CurrentTime,MyNumber,num,ang);
#endif
    SeePlayer(side, num, ang, time);
  }
}

void PositionInfo::SeePlayer(float dist, float ang, int time)
{
  int num=0;
  char side = '0';

  /* Close players that are "felt" come here.
    count number of close players
    if 1, you know who it is (right?)
    else disambiguate by angles?
    Or turn quickly to look?
  */

  for (int i=1; i<TEAM_SIZE; i++){
    if ( PlayerAngleValid(MySide,i) /*> PlayerDistanceValid(MySide,i) */
	 && fabs(GetPlayerAngle(MySide,i) - ang) < 1 ){
      if ( num ){ /* Already assigned */
	if ( fabs(GetPlayerDistance(MySide,i) - dist) < 2 ||
	     ( GetPlayerDistance(MySide,num) != dist &&
	       PlayerAngleValid(MySide,i) > PlayerAngleValid(side,num)) ){
	  side = MySide; 
	  num = i;
	}
	/* my_error("Two players at same angle!!!"); */
      }
      else{       /* not assigned yet  */
	side = MySide; 
	num = i;
      }
    }
    if ( PlayerAngleValid(TheirSide,i) /*> PlayerDistanceValid(TheirSide,i) */
	 && fabs(GetPlayerAngle(TheirSide,i) - ang) < 1 ){
      if ( num ){ /* Already assigned */
	if ( fabs(GetPlayerDistance(TheirSide,i) - dist) < 2 ||
	     ( GetPlayerDistance(TheirSide,num) != dist &&
	       PlayerAngleValid(TheirSide,i) > PlayerAngleValid(side,num)) ){
	  side = TheirSide; 
	  num = i;
	}
	/*my_error("Two players at same angle!!!");*/
      }
      else{       /* not assigned yet  */
	side = TheirSide; 
	num = i;
      }
    }
  }

  if (num){
#if PLAYER_ID_DEBUG 
    printf("SeePlayer3 %d:  Player %d thinks Player %d is at %.1f, %.1f\n",
	   CurrentTime,MyNumber,num,dist,ang);
#endif
    SeePlayer(side, num, dist, ang , time);
  }
  else
    SeeExtraPlayer('?', dist, ang, time);
}

void PositionInfo::SeePlayer(float ang, int time)
{

  if ( fabs(ang) > GetViewAngle()/2 ){
    SeePlayer((float)FEEL_DISTANCE,ang,time);   /* dist must be less than 3 */
    return;
  }

  int num=0;
  char side = '0';

  for (int i=1; i<TEAM_SIZE; i++){
    if ( PlayerAngleValid(MySide,i) /*> PlayerDistanceValid(MySide,i) */
	 && fabs(GetPlayerAngle(MySide,i) - ang) < 1 ){
      if ( num ){ /* Already assigned */
	if ( PlayerAngleValid(MySide,i) > PlayerAngleValid(side,num) ){
	  side = MySide; 
	  num = i;
	}
	/*my_error("Two players at same angle!!!"); */
      }
      else{       /* not assigned yet  */
	side = MySide; 
	num = i;
      }
    }
    if ( PlayerAngleValid(TheirSide,i) /*> PlayerDistanceValid(TheirSide,i) */
	 && fabs(GetPlayerAngle(TheirSide,i) - ang) < 1 ){
      if ( num ){ /* Already assigned */
	if ( PlayerAngleValid(TheirSide,i) > PlayerAngleValid(side,num) ){
	  side = TheirSide; 
	  num = i;
	}
	/*my_error("Two players at same angle!!!"); */
      }
      else{       /* not assigned yet  */