memposition.c

1999年卡耐基梅陇大学的2D仿真源代码

  Marker[i].Initialize((MarkerType) i, Vector( SP_pitch_length/2.0+SP_pitch_margin, -20 ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_RT20 */
  Marker[i].Initialize((MarkerType) i, Vector( SP_pitch_length/2.0+SP_pitch_margin, -10 ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_RT10 */
  Marker[i].Initialize((MarkerType) i, Vector( SP_pitch_length/2.0+SP_pitch_margin, 0 ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_R0 */
  Marker[i].Initialize((MarkerType) i, Vector( SP_pitch_length/2.0+SP_pitch_margin, 10 ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_RB10 */
  Marker[i].Initialize((MarkerType) i, Vector( SP_pitch_length/2.0+SP_pitch_margin, 20 ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_RB20 */
  Marker[i].Initialize((MarkerType) i, Vector( SP_pitch_length/2.0+SP_pitch_margin, 30 ),
			   CP_max_conf, CP_min_valid_conf, rotate); /* Flag_RB30 */

  if ( MySide == 'l' ){
    RM_My_Goal = Goal_L;
    RM_Their_Goal = Goal_R;
    RM_LB_Flag = Flag_LT;
    RM_LC_Flag = Flag_CT;
    RM_LF_Flag = Flag_RT;
    RM_RB_Flag = Flag_LB;
    RM_RC_Flag = Flag_CB;
    RM_RF_Flag = Flag_RB;
    RM_My_PC_Flag = Flag_PLC;    /* Center of my penalty area */
    RM_Their_PC_Flag = Flag_PRC; /* Center of theirs          */
  }
  else{ /* MySide == 'r' */
    RM_My_Goal = Goal_R;
    RM_Their_Goal = Goal_L;
    RM_LB_Flag = Flag_RB;
    RM_LC_Flag = Flag_CB;
    RM_LF_Flag = Flag_LB;
    RM_RB_Flag = Flag_RT;
    RM_RC_Flag = Flag_CT;
    RM_RF_Flag = Flag_LT;
    RM_My_PC_Flag = Flag_PRC;    /* Center of my penalty area */
    RM_Their_PC_Flag = Flag_PLC; /* Center of theirs          */
  }    

  i=0;

  Fieldline = new StationaryObject[SP_num_lines];
  Fieldline[i].Initialize((SideLine) i, Vector( -SP_pitch_length/2.0, 0.0 ), 
			 CP_max_conf, CP_min_valid_conf, rotate); i++; /* SL_Left */
  Fieldline[i].Initialize((SideLine) i, Vector( SP_pitch_length/2.0, 0.0 ), 
			 CP_max_conf, CP_min_valid_conf, rotate); i++; /* SL_Right */
  Fieldline[i].Initialize((SideLine) i, Vector( 0.0, -SP_pitch_width/2.0 ), 
			 CP_max_conf, CP_min_valid_conf, rotate); i++; /* SL_Top */
  Fieldline[i].Initialize((SideLine) i, Vector( 0.0, SP_pitch_width/2.0 ), 
			 CP_max_conf, CP_min_valid_conf, rotate); /* SL_Bottom */
  
  Ball.Initialize(CP_max_conf, CP_min_valid_conf, CP_ball_conf_decay, SP_ball_decay, SP_ball_speed_max);

  num_players          = SP_team_size*2-1;

  Player = new PlayerObject[num_players];     /* allow for all players but me          */
  for (i=0; i<num_players; i++)
    Player[i].Initialize(CP_max_conf, CP_min_valid_conf, CP_player_conf_decay, 
			 SP_player_decay, SP_player_speed_max );

  for (i=0; i<num_players; i++)
    FreePlayer[i] = &(Player[(num_players-1) - i]);   /* Player array backwards: take from end */

  UnknownPlayer = new TempPlayerObject[num_players];

  num_seen_markers     = 0;
  num_my_players       = 0;
  num_their_players    = 0;
  num_teamless_players = 0;
  num_free_players     = num_players;
  num_unknown_players  = 0;

  ClosestMarker = ClosestMotionMarker = No_Marker;
  SeenLine = SL_No_Line;

  for (i=1; i<=SP_team_size; i++)
    TiredTimes[i] = -CP_say_tired_interval;

  OwnPenaltyArea = Rectangle(-SP_pitch_length/2,                          //left
			     -SP_pitch_length/2 + SP_penalty_area_length, //right
			     -SP_penalty_area_width/2,                    //top
			     SP_penalty_area_width/2);                    //bottom
  OwnGoalieArea = Rectangle(-SP_pitch_length/2,                       //left
			    -SP_pitch_length/2 + SP_goal_area_length, //right
			    -SP_goal_area_width/2,                    //top	
			    SP_goal_area_width/2);                    //bottom

  TheirPenaltyArea = Rectangle(SP_pitch_length/2 - SP_penalty_area_length,//left
			      SP_pitch_length/2,                          //right
			     -SP_penalty_area_width/2,                    //top
			     SP_penalty_area_width/2);                    //bottom
  TheirGoalieArea = Rectangle(SP_pitch_length/2 - SP_goal_area_length,//left
			      SP_pitch_length/2,                          //right
			    -SP_goal_area_width/2,                    //top	
			    SP_goal_area_width/2);                    //bottom

  FieldRectangle = Rectangle(-SP_pitch_length/2,
			      SP_pitch_length/2,
			     -SP_pitch_width/2,
			      SP_pitch_width/2);

  MyLeftGoalKickSpot = OwnGoalieArea.TopRightCorner();
  MyRightGoalKickSpot = OwnGoalieArea.BottomRightCorner();
  TheirLeftGoalKickSpot = TheirGoalieArea.TopLeftCorner();
  TheirRightGoalKickSpot = TheirGoalieArea.BottomLeftCorner();

  my_offside_line = 0;

  quantize_err_const = exp(SP_dist_qstep / 2) - exp(-SP_dist_qstep / 2);
  Tan_of_half_deg = Tan(1.0/2.0);
}

PositionInfo::~PositionInfo()
{
  delete [] Fieldline;
  delete [] Marker;
  delete [] Player;
  delete [] UnknownPlayer;
}

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

void PositionInfo::SeeLine(SideLine l, float dist, float ang, Time tm)
{
  Fieldline[l].set_polar_from_neck(dist,ang,tm);
  SeenLine = l;
}

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

void PositionInfo::SeeLine(SideLine l, float ang, Time tm)
{
  Fieldline[l].set_angle_from_neck(ang,tm);
  SeenLine = l;
}




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

void PositionInfo::SeeMarker(MarkerType marker, float dist, float ang, Time tm)
{
  Marker[marker].set_polar_from_neck(dist,ang,tm);
  SeenMarker[num_seen_markers++] = marker;
}

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

void PositionInfo::SeeMarker(MarkerType marker, float ang, Time tm)
{
  Marker[marker].set_angle_from_neck(ang,tm);
  SeenMarker[num_seen_markers++] = marker;
  my_error("Shouldn't process markers when using low quality -- no info");
}

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

void PositionInfo::SeeMarker(MarkerType marker, float dist, float ang, 
			     float distChng, float dirChng, Time tm){
  Marker[marker].set_chinfo(distChng, dirChng, tm);
  Marker[marker].set_polar_from_neck(dist,ang,tm);
  SeenMarker[num_seen_markers++] = marker;
}



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

void PositionInfo::SeeBall(float ang, Time tm)
{
  Ball.set_angle_from_neck(ang,tm);
}

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

void PositionInfo::SeeBall(float dist, float ang, Time tm)
{
  Ball.set_polar_from_neck(dist,ang,tm);
}

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

void PositionInfo::SeeBall(float dist, float ang, float distChng, float dirChng, Time tm)
{
  Ball.set_chinfo(distChng, dirChng, tm);
  SeeBall(dist,ang,tm);
}




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

void PositionInfo::SeePlayer(char side, Unum num, float ang, Time time)
{
  PlayerObject *player;
  if ( (player = GetPlayer(side, num)) == NULL )
    player = GetNewPlayer(side, num);

  if ( player != NULL ) 
    player->set_angle_from_neck(ang,time);
  else 
    my_error("Can't get a player to see (1) %c %d (%d teamless)",side,num,num_teamless_players);
}

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

void PositionInfo::SeePlayer(char side, Unum num, float dist, float ang, Time time)
{
  PlayerObject *player;
  if ( (player = GetPlayer(side, num)) == NULL )
    player = GetNewPlayer(side, num);

  if ( player != NULL ) 
    player->set_polar_from_neck(dist,ang,time);
  else 
    my_error("Can't get a player to see (2) %c %d (%d teamless)",side,num,num_teamless_players);
}

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

void PositionInfo::SeePlayer(char side, Unum num, float dist, float ang, 
			     float distChng, float dirChng, float bodydir, float neckdir, Time time)
{
  PlayerObject *player;
  if ( (player = GetPlayer(side, num)) == NULL )
    player = GetNewPlayer(side, num);

  if ( player != NULL ){
    player->set_chinfo(distChng,dirChng,time);
    player->set_body_and_neck_ang_from_neck(bodydir,neckdir,time);
    player->set_polar_from_neck(dist,ang,time);
  }
  else 
    my_error("Can't get a player to see (3) %c %d (%d teamless)",side,num,num_teamless_players);
}

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

void PositionInfo::SeePlayer(char side, float dist, float ang, Time time)
{
  if ( num_unknown_players < num_players ){
    UnknownPlayer[num_unknown_players].set(side,dist,ang,time);
    num_unknown_players++;
  } 
  else
    my_error("Too many unknown players (1) %d %d %d %d",Mem->LastSightTime.t,Mem->LastSightTime.s,
	     time.t,time.s);
}

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

void PositionInfo::SeePlayer(char side, float ang, Time time)
{
  my_error("What can I do that's useful with just the player angle?");
  return;

  if ( num_unknown_players < num_players ){
    UnknownPlayer[num_unknown_players].set(side,0,ang,time);
    num_unknown_players++;
  } 
  else
    my_error("Too many unknown players (2)");
}

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

void PositionInfo::SeePlayer(float dist, float ang, Time time)
{
  if ( num_unknown_players < num_players ){
    UnknownPlayer[num_unknown_players].set('?',dist,ang,time);
    num_unknown_players++;
  } 
  else
    my_error("Too many unknown players (3) %d %d %d %d",Mem->LastSightTime.t,Mem->LastSightTime.s,
	     time.t,time.s);
}

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

void PositionInfo::SeePlayer(float ang, Time time)
{
  my_error("What can I do that's useful with just the player angle?");
  return;

  if ( num_unknown_players < num_players ){
    UnknownPlayer[num_unknown_players].set('?',0,ang,time);  
    num_unknown_players++;
  } 
  else
    my_error("Too many unknown players (4)");
}

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

void PositionInfo::HearBall(float x, float y, float conf, float dist, Time time)
{
  Ball.set_heard_info(x,y,conf,dist,time);
}

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

void PositionInfo::HearBall(float x, float y, float pconf, float dx, float dy, float vconf, 
			    float dist, Time time)
{
  Ball.set_heard_info(x,y,pconf,dx,dy,vconf,dist,time);
}

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


void PositionInfo::HearPlayer(char side, Unum num, float x, float y, float conf, float dist, Time time)
{
  /* When hearing a player location, remember not to call just getplayer, but also
     closestPlayerto(side,vector) -- might be one of the unknowns 
     can just update the side---update_players takes care of the rest */

  PlayerObject *player;
  if ( (player = GetPlayer(side, num)) == NULL ){
    Vector pos = Vector(x,y);
    if ( (player = ClosestPlayerObjectTo(side, pos)) != NULL ){
      if ( player->side == '?' )
	player->side = side; 
      player->unum = num;
    }
    else 
      player = GetNewPlayer(side, num);
  }

  if ( player != NULL ) 
    player->set_heard_info(x,y,conf,dist,time);
  else {
    /* Be more lenient in ClosestPlayerObjectTo??? */
    Unum teammate = ClosestTeammateTo(Vector(x,y));
    Unum opponent = ClosestOpponentTo(Vector(x,y));
    my_error("Can't get a player to hear (1) %c %d (%.1f %.1f)",side,num,
	     teammate == Unum_Unknown ? -1.0 : TeammateDistanceTo(teammate,Vector(x,y)),
	     opponent == Unum_Unknown ? -1.0 : OpponentDistanceTo(opponent,Vector(x,y)));
  }
}

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

void PositionInfo::HearPlayer(char side, Unum num, float x, float y, float pconf, 
			      float dx, float dy, float vconf, float dist, Time time)
{
  /* When hearing a play