position.c

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

/* position.c
 * CMUnited-97 (soccer client for Robocup-97)
 * Peter Stone <[e-mail]>
 * Computer Science Department
 * Carnegie Mellon University
 * Copyright (C) 1997 Peter Stone
 *
 * CMUnited-97 was created by Peter Stone and Manuela Veloso
 *
 * You may copy and distribute this program freely as long as you retain this notice.
 * If you make any changes or have any comments we would appreciate a message.
 */

#include "global.h"

#define DEBUG                   0
#define PLAYER_ID_DEBUG         0

struct {
  float x, y;
} marker_global[] = { 
  {X0, 0},      /* GOAL_R   */
  {-X0, 0},     /* GOAL_L   */
  {X0, -Y0},    /* FLAG_RB  */
  {0, -Y0},     /* FLAG_B   */
  {-X0, -Y0},   /* FLAG_LB  */
  {-X0, Y0},    /* FLAG_LT  */
  {0, Y0},      /* FLAG_T   */
  {X0, Y0},     /* FLAG_RT  */
  {PA_X,PA_Y},  /* FLAG_PRT */
  {PA_X,0},     /* FLAG_PRC */
  {PA_X,-PA_Y}, /* FLAG_PRB */
  {-PA_X,PA_Y}, /* FLAG_PLT */
  {-PA_X,0},    /* FLAG_PLC */
  {-PA_X,-PA_Y} /* FLAG_PLB */
};

struct {
  int a, b;
} line_endpoints[] = {
  {2, 7},
  {5, 4},
  {-1, -1}, // Not really a line.
  {4, 2}, 
  {-1, -1}, 
  {-1, -1},
  {7, 5}
};

//
// Position
//

Position::Position()
{
  new_rconf = new_tconf 
    = cur_rconf = cur_tconf 
    = 0.0;
  gospel = 0;
}

float Position::estimate_degrade(float c){
  if (!gospel){
    new_rconf *= c;
    new_tconf *= c;
  }
}

float Position::translate(float dr, float dt, float c)
{
  float x, y;

  if (gospel) return get_new_conf();

  // Do rotation first.
  new_t += dt;

  // Now, movement along y-axis.

  /* Distance not valid, but need some magnitude for direction */
  if (!new_r) new_r = 100; 

  x = get_new_x(); y = get_new_y() + dr;
  relxy_to_polar(x, y, &new_r, &new_t);

  new_rconf *= c;
  new_tconf *= c;
}

float Position::set_polar(float r, float t, float c)
{
  new_r = r;
  new_t = normalize(t);
  new_rconf = c;
  new_tconf = c;
  gospel = 1;
  return get_new_conf();
}

float Position::set_r(float r, float c)
{
  new_r = r;
  new_rconf = c;
  return get_new_conf();
}

float Position::set_t(float t, float c)
{
  new_t = normalize(t);
  new_tconf = c;
  return get_new_conf();
}

float Position::set_t_gospel(float t, float c)
{
  gospel = 1;  /* Assume that you do set_t when you know the angle */
  return set_t(t, c);
}

float Position::set_xy(float x, float y, float c)
{
  relxy_to_polar(x, y, &new_r, &new_t);
  new_rconf = c;
  new_tconf = c;
  gospel = 1;
  return get_new_conf();
}

void Position::tick()
{
  cur_r = new_r;
  cur_t = new_t;
  cur_rconf = new_rconf;
  cur_tconf = new_tconf;

  gospel = 0;
}

void Position::reset()
{
  set_polar(0,0,0);
}

//
// PositionStationary
//

float PositionStationary::estimate_degrade()
{
  Position::estimate_degrade(DECAY_S_ESTIMATE);
}

void PositionStationary::get_translate(float *dr, float *dt, float *c)
{
  *dr = new_r - cur_r;
  *dt = new_t - cur_t;
  *c = get_new_conf();
}

float PositionStationary::translate(float dr, float dt, float c)
{
  Position::translate(dr, dt, c);
}

void PositionStationary::tick()
{
  Position::tick();
}

//
// PositionMobile
//

PositionMobile::PositionMobile()
{
  got_global_info = FALSE;
  old_global_set_time = global_set_time = 0;
  points = new PointQueue(5);
  global_x = global_y = NODIR;
  side = 'b'; /* if not otherwise set, ball */
  num = 0;
  Position::Position();
}

PositionMobile::~PositionMobile()
{
  delete points;
}

float PositionMobile::estimate_degrade()
{
  Position::estimate_degrade(DECAY_M_ESTIMATE);
}

float PositionMobile::translate(float dr, float dt, float c)
{
  Position::translate(dr, dt, c);
}

void PositionMobile::tick(int time_steps)
{
  old_r = cur_r;
  old_t = cur_t;
  old_rconf = cur_rconf;
  old_tconf = cur_tconf;

  if (!gospel) {
    for (int i=0; i<time_steps; i++){
      new_rconf *= DECAY_M_TICK;
      new_tconf *= DECAY_M_TICK;
    }
  }

  cur_vel_dir = new_vel_dir;
  new_vel_dir = NODIR;
  cur_vel_mag = new_vel_mag;
  new_vel_mag = 0;

  Position::tick();
}

void PositionMobile::reset()
{
  Position::reset();
  old_r = old_t = old_rconf = old_tconf = 0;
  old_global_set_time = global_set_time = 0;
}
  
float PositionMobile::get_rel_vel_t()
{
  float dx = get_x() - get_old_x();
  float dy = get_y() - get_old_y();

  return my_atan2(dx, dy);
}

float PositionMobile::get_rel_vel_tDEG()
{
  return rad_to_deg(get_rel_vel_t());
}

float PositionMobile::get_rel_vel_r(int dt)
{
  float dx = get_x() - get_old_x(); 
  float dy = get_y() - get_old_y();
  
  return sqrt(dx*dx + dy*dy)/dt;
}
 
#define TRAJ_VALID_TIME 15
float PositionMobile::get_traj_mag(){
  int dt = global_set_time - old_global_set_time;
  if ( dt > TRAJ_VALID_TIME )
    my_error("Some global info is out of date (traj mag)");

  return GetDistance(&global_x,&global_y,&old_global_x,&old_global_y)/dt;
}

float PositionMobile::get_traj_dir(){

  int dt;
  float dx,dy,dir;

#if 0
  dt = global_set_time - old_global_set_time;
  if ( dt > 15 )
    my_error("Some global info is out of date (traj dir) ");
#endif
  dx = global_x - old_global_x;
  dy = global_y - old_global_y;

  float short_dir = my_atan2(dy, dx);

  return short_dir;
/*
  printf(" **** now(%d) (%.1f,%.1f)   then(%d) (%.1f,%.1f)   dx/dy %.1f %.1f\n",
	 global_set_time,global_x,global_y,
	 old_global_set_time,old_global_x,old_global_y,dx,dy);
*/

  int index = points->OldestIndexWithin(15);
  if ( index==END_OF_QUEUE )
    my_error("Some global info is out of date (traj dir) ");

  if ( global_x != points->GetTailX() )
    my_error("Did I update queue correctly?? ");
  
  dx = global_x - points->GetIndexX(index);
  dy = global_y - points->GetIndexY(index);
  dir = my_atan2(dy, dx);

#if 1
  float diff = rad_to_deg(short_dir - dir);
  CleanAngle(&diff);
  if ( fabs(diff) > 10 )
    printf("%d:%d  going back to %d -- %.1f.  One step -- %.1f\n",
	   Mem->MyNumber,Mem->CurrentTime,index,rad_to_deg(dir),rad_to_deg(short_dir));
#endif

  return dir;
  /*return short_dir;*/
}

float PositionMobile::get_traj_conf(int time, int dt){
  /* Both current and old globals are recent */
  if ( time != global_set_time || global_set_time - old_global_set_time > dt )
    return FALSE;
  else
    return get_conf();
/*
  if ( time - global_set_time > 10 || time - old_global_set_time > 15 )
    return FALSE;

  if ( time == global_set_time )
    return get_conf();
  else 
    return pow(DECAY_M_TICK,time - old_global_set_time);
*/      
}
 
void PositionMobile::set_vel_dir(float d)
{
  new_vel_dir = d;
}

void PositionMobile::set_vel_mag(float m)
{
  new_vel_mag = m;
}

void PositionMobile::set_vel(float d, float m)
{
  set_vel_dir(d);
  set_vel_mag(m);
}

void PositionMobile::clear_globalxy(){
  global_set_time = old_global_set_time = 0;
}

void PositionMobile::set_globalxy(float x, float y, int time){
  if ( InPitch(x,y) ){  
    got_global_info = TRUE; 
    update_globalxy(x,y,time);
  }
}

void PositionMobile::update_globalxy(float x, float y, int time)
{
  old_global_x        = global_x;
  old_global_y        = global_y;
  old_global_set_time = global_set_time;

  global_x        = x; 
  global_y        = y;
  global_set_time = time;

  points->Add(x,y,time);
  /*points->Print();*/
}

//
// PositionInfo
//

PositionInfo::PositionInfo()
{
  int obj=0;
  int i;

  for(i=0; i<NUM_MARKERS; i++) {
    Markers[i] = new PositionStationary();
    objects[obj++] = Markers[i];
  }

  for(i=0; i<TEAM_SIZE+1; i++) {
    MyTeam[i] = new PositionMobile();
    objects[obj++] = MyTeam[i];
    TheirTeam[i] = new PositionMobile();
    objects[obj++] = TheirTeam[i];
  }

  for(i=0; i<2*TEAM_SIZE; i++) {
    ExtraPlayers[i] = new PositionMobile();
  }
  NumExtraPlayers = 0;

  Ball = new PositionMobile();
  objects[obj++] = Ball;

  Dashed = FALSE;

  view_width   = NORMAL_WIDTH;   
  view_quality = HIGH_QUALITY;

  CurrentTime = LastTime = 0;

  my_vel_x = my_vel_y = NODIR;
}

PositionInfo::Initialize(const char s, const int n, char *tn){
  MySide = s;
  TheirSide = (s == 'l') ? 'r' : 'l';
  MyTeamName = tn;
  MyTeamNameLen = strlen(MyTeamName);
  MyNumber = n;

  for(int i=0; i<TEAM_SIZE+1; i++) {
    MyTeam[i]->side = MySide;
    MyTeam[i]->num = i;
    TheirTeam[i]->side = TheirSide;
    TheirTeam[i]->num = i;
  }
}

PositionInfo::~PositionInfo()
{
  int i;

  for(i=0; i<NUM_MARKERS; i++)
    delete Markers[i];

  for (i=0; i<TEAM_SIZE+1; i++) {
    delete MyTeam[i];
    delete TheirTeam[i];
  }

  delete Ball;
}

int PositionInfo::ConvertMarker(int marker, float *x, float *y)
{
  if (marker < NUM_MARKERS)
    return marker;

  int ActualMarker;

  if (MySide == 'l') {
    switch(marker) {
    case MY_GOAL:       ActualMarker = GOAL_L;   break;
    case THEIR_GOAL:    ActualMarker = GOAL_R;   break;
    case LB_FLAG:       ActualMarker = FLAG_LT;  break;
    case LC_FLAG:       ActualMarker = FLAG_T;   break;
    case LF_FLAG:       ActualMarker = FLAG_RT;  break;
    case RB_FLAG:       ActualMarker = FLAG_LB;  break;
    case RC_FLAG:       ActualMarker = FLAG_B;   break;
    case RF_FLAG:       ActualMarker = FLAG_RB;  break;
    case MY_PL_FLAG:    ActualMarker = FLAG_PLT; break;
    case MY_PC_FLAG:    ActualMarker = FLAG_PLC; break;
    case MY_PR_FLAG:    ActualMarker = FLAG_PLB; break;
    case THEIR_PL_FLAG: ActualMarker = FLAG_PRT; break;
    case THEIR_PC_FLAG: ActualMarker = FLAG_PRC; break;
    case THEIR_PR_FLAG: ActualMarker = FLAG_PRB; break;
    }
  } else {
    switch(marker) {
    case MY_GOAL:       ActualMarker = GOAL_R;   break;
    case THEIR_GOAL:    ActualMarker = GOAL_L;   break;
    case LB_FLAG:       ActualMarker = FLAG_RB;  break;
    case LC_FLAG:       ActualMarker = FLAG_B;   break;
    case LF_FLAG:       ActualMarker = FLAG_LB;  break;
    case RB_FLAG:       ActualMarker = FLAG_RT;  break;
    case RC_FLAG:       ActualMarker = FLAG_T;   break;
    case RF_FLAG:       ActualMarker = FLAG_LT;  break;
    case MY_PL_FLAG:    ActualMarker = FLAG_PRB; break;
    case MY_PC_FLAG:    ActualMarker = FLAG_PRC; break;
    case MY_PR_FLAG:    ActualMarker = FLAG_PRT; break;
    case THEIR_PL_FLAG: ActualMarker = FLAG_PLB; break;
    case THEIR_PC_FLAG: ActualMarker = FLAG_PLC; break;
    case THEIR_PR_FLAG: ActualMarker = FLAG_PLT; break;
    }
  }
  *x = marker_global[ActualMarker].x;
  *y = marker_global[ActualMarker].y;

  return ActualMarker;
}

float PositionInfo::GetMarkerDistance(int marker)
{
  int m = ConvertMarker(marker);
  return Markers[m]->get_r();
}

float PositionInfo::GetMarkerAngle(int marker)
{
  int m = ConvertMarker(marker);
  return Markers[m]->get_tDEG();
}

float PositionInfo::GetMarkerGlobalX(int marker)
{
  int m = ConvertMarker(marker);
  return marker_global[m].x;
}

float PositionInfo::GetMarkerGlobalY(int marker)
{
  int m = ConvertMarker(marker);
  return marker_global[m].y;
}

float PositionInfo::MarkerValid (int marker)
{
  int m = ConvertMarker(marker);
  return Markers[m]->get_conf();
}

int PositionInfo::ClosestGoal()
{
  return (global_x < 0) ? MY_GOAL : THEIR_GOAL;
}

int PositionInfo::ClosestFlag()
{
  /* Chop the field into a 5 x 5 grid.  
     If there's no flag in the current section, it's an error */

  if ( global_x < -PA_X - (X0-PA_X)/2 ){
    if ( global_y < -PA_Y - (Y0-PA_Y)/2 )     return RB_FLAG;
    else if ( global_y > PA_Y + (Y0-PA_Y)/2 ) return LB_FLAG;
    else if ( Mem->PlayMode != BEFORE_KICK_OFF) printf("%d:%d Flag Confusion 1\n",MyNumber,CurrentTime);
  }
  else if ( global_x < -PA_X/2 ){
    if ( global_y < -PA_Y - (Y0-PA_Y)/2 ){ if ( Mem->PlayMode != BEFORE_KICK_OFF) printf("%d:%d Flag Confusion 2\n",MyNumber,CurrentTime);}
    else if ( global_y < -PA_Y/2 )            return MY_PR_FLAG; 
    else if ( global_y < PA_Y/2 )             return MY_PC_FLAG;
    else if ( global_y < PA_Y + (Y0-PA_Y)/2 ) return MY_PL_FLAG;
    else if ( Mem->PlayMode != BEFORE_KICK_OFF) printf("%d:%d Flag Confusion 3\n",MyNumber,CurrentTime);
  }
  else if ( global_x < PA_X/2 ){
    if ( global_y < -PA_Y - (Y0-PA_Y)/2 )     return RC_FLAG;
    else if ( global_y > PA_Y + (Y0-PA_Y)/2 ) return LC_FLAG;
    else if ( Mem->PlayMode != BEFORE_KICK_OFF) printf("%d:%d Flag Confusion 4\n",MyNumber,CurrentTime);
  }
  else if ( global_x < PA_X + (X0-PA_X)/2 ){
    if ( global_y < -PA_Y - (Y0-PA_Y)/2 ){  if ( Mem->PlayMode != BEFORE_KICK_OFF) printf("%d:%d Flag Confusion 5\n",MyNumber,CurrentTime);}
    else if ( global_y < -PA_Y/2 )            return THEIR_PR_FLAG;