position.c

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

	side = TheirSide; 
	num = i;
      }
    }
  }

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

void PositionInfo::SeeExtraPlayer(char side, float dist, float ang, int time){

  ExtraPlayers[NumExtraPlayers]->side = side;
  ExtraPlayers[NumExtraPlayers]->set_polar(dist, deg_to_rad(ang), 1);

  NumExtraPlayers++;
}

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

void PositionInfo::HearPlayer(char side, int num, 
			      float ang, char *msg, int time)
{
  /* Assume you're not hearing your own message (filtered in parse.c) */
  /* Don't do anything for opponent messages.                         */
  if ( side == MySide ){
    MyTeam[num]->set_t_gospel(deg_to_rad(ang), 1.0);
  }
}

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

void PositionInfo::ClearBallGlobalPosition()
{
  Ball->clear_globalxy();
}

void PositionInfo::UpdateBallGlobalPosition()
{
  if ( BallValid()==1 && (CurrentTime > Ball->get_global_set_time()) ){  
    /* don't do it if the ball's not valid or if the info's not new */
    float x,y;
    GetBallGlobalXY(&x, &y);
    Ball->update_globalxy(x,y,CurrentTime); 
  }
}

void PositionInfo::UpdatePlayerGlobalPosition(char side, int num)
{
  PositionMobile *Player = ((side == MySide) ? MyTeam[num] : TheirTeam[num]);
  if ( PlayerValid(side,num) && (CurrentTime > Player->get_global_set_time()) ){  
    /* don't do it if the player's not valid  or if the info's not new */
    float x,y;
    GetPlayerGlobalXY(side, num, &x, &y);
    Player->update_globalxy(x,y,CurrentTime); 
  }
}

void PositionInfo::Tick(int marker, int time)
{
  /* rely on marker == -1 if none were visible */
  int i;

  LastTime    = CurrentTime;
  CurrentTime = time;

  if (dashing)  /* I moved since the last time step */
    Dashed = TRUE;
  else          /* I didn't                         */
    Dashed = FALSE;

  /* Calculate change in direction. */
  if ( line==NO_LINE ){ /* If I didn't see a line, assume turn worked */
    global_angle -= deg_to_rad(LastTurnAngle);
    CleanAngleRad(&global_angle);
  }
  float t_change = global_angle - old_global_angle;
  float r_change;

  /* If we see a marker, triangulate to find others.  Otherwise
     estimate movement from visible line. */
  if (marker>=0) {
    r_change = triangulate(marker);
#if DEBUG
    printf("Triangulating...\n");
#endif
  } else /* if (dashing || turning) */ {
    /* We are only here if there are no visible markers. */
#if DEBUG
    printf("Estimating movement...\n");
#endif

    /* Calculate change in distance (r_change) using the endpoints
       of the visible line. */
    {
      float alpha,aep;

      if ( line != NO_LINE ){
	float ag = global_angle;
	/* aep is angle to end point of the line */
	aep = Markers[line_endpoints[line].a]->get_t() + t_change;
	CleanAngleRad(&aep); /* So it's not over PI */
	
	/* Do rotations, so calculations can be done as if the line
        was right.  Pretend line we're facing is right line. */
	switch(line) {
	case LINE_T: ag = normalize(ag - M_PI_2); break;
	case LINE_L: ag = normalize(ag - M_PI); break;
	case LINE_B: ag = normalize(ag + M_PI_2); break;
	case LINE_R: break;
	}
	
	/* Do calculation of old distance, by the law of sines. */
	if (aep > 0) alpha = M_PI_2 + ag - aep;
	else alpha = 3*M_PI_2 - ag + aep;
#if DEBUG
	printf("ag = %.1f, aep = %.1f, alpha = %.1f, oldr = %.1f\n",
	       rad_to_deg(ag),rad_to_deg(aep),rad_to_deg(alpha),
	       Markers[line_endpoints[line].a]->get_r());
#endif
      }

      if (line_r == NO_LINE || line == NO_LINE)    
	                 /* Low quality vision, so don't know line distance */
	r_change = 0;    /* Therefore, can't tell translational change      */
      else               /* High quality vision                             */
	r_change = (Markers[line_endpoints[line].a]->get_r() *
		    sin(alpha) / sin(alpha + aep)) - line_r;
#if DEBUG
      printf("Estimated movement, dr = %f, dt = %f\n", r_change, t_change);
#endif
    }

    /* Update markers based on estimation of movement. */
    for(i=0; i<NUM_EDGE_MARKERS; i++)
      Markers[i]->translate(-r_change, t_change, 1);
    /* Peter's addition */  /* This is if you didn't triangulate */
    if ( MySide == 'r' ){  /* x & y were flipped before, and will be flipped again */
      global_x *= -1;      /* here they need to be in team-independent format      */
      global_y *= -1;
    }
    global_x += r_change * cos(global_angle);
    global_y += r_change * sin(global_angle);
    /* Peter's addition */

    for (i=0; i<NUM_OBJECTS; i++){
      if ( i<NUM_MARKERS )
	((PositionStationary *)objects[i])->estimate_degrade();
      else
	((PositionMobile *)objects[i])->estimate_degrade();
    }
  }

  /* New: 12/16/96 */
  /* Update the positions of the mobile objects based on my move too */
  for(i=1; i<=TEAM_SIZE; i++){
    if (!MyTeam[i]->gospel && i != MyNumber){ /* If I saw it, don't update it */
      if ( TeammateValid(i) )
	MyTeam[i]->translate(-r_change, t_change, 1);
    }
    if (!TheirTeam[i]->gospel){ /* If I saw it, don't update it */
      if ( OpponentValid(i) )
	TheirTeam[i]->translate(-r_change, t_change, 1);
    }
  }
  if (!Ball->gospel){ /* If I saw it, don't update it */
    Ball->translate(-r_change, t_change, 1);
  }
  /* New: 12/16/96 */
	
#define ANG_DIV_FACT 3 

  if ( view_quality == HIGH_QUALITY ) 
    for (i=0; i<NUM_EDGE_MARKERS; i++)
      if (objects[i]->get_conf() && /* if it wasn't in view (know angle from talk)
				       then let it be                             */
	  fabs(objects[i]->get_new_tDEG()) < GetViewAngle()/ANG_DIV_FACT && 
	  !objects[i]->gospel){     /* actual width must be different from view_width */
	/*printf("%d:%d Adjusting view width from %d due to object %d\n",
	  MyNumber,CurrentTime,view_width,i); */
      if ( view_width == NARROW_WIDTH )  
	; /* Couldn't see because I'm confused about my angle */
      else if ( fabs(objects[i]->get_new_tDEG()) < GetViewAngle(NORMAL_WIDTH)/ANG_DIV_FACT )
	view_width = NARROW_WIDTH;
      else
	view_width = NORMAL_WIDTH; /* was wide_width */
      }

#if 1
  if ( view_quality == HIGH_QUALITY ) /* high */
    /* Make sure that objects which are supposed to be in view are in view. */
      for(i=NUM_MARKERS; i<NUM_OBJECTS; i++)
	i = NUM_OBJECTS-1;  /* only do this for the ball for now */
	if (objects[i]->get_conf() && /* if it wasn't in view (know angle from talk)
					 then let it be                             */
	    ( fabs(objects[i]->get_new_tDEG()) < GetViewAngle()/ANG_DIV_FACT || 
	      objects[i]->get_new_r() < FEEL_DISTANCE ) &&
	    !objects[i]->gospel){
	  objects[i]->set_r(0,0);  /* don't make it gospel */
	  objects[i]->set_t(0,0); 
	  //printf("%d:%d forgetting ball\n",Mem->MyNumber,Mem->CurrentTime);
	}
#endif

  old_global_angle = global_angle;
  if (MySide == 'r'){  /* Used the same orientation for both sides above*/
    global_x *= -1;    /* Now convert positions back to team-relative   */
    global_y *= -1;    /* Before using the communicated global info     */
    global_angle += M_PI;
  }
  CleanAngleRad(&global_angle);

  /* Process updates--first stationary objects (to get own motion) */
  for(i=0; i<NUM_MARKERS; i++){
    objects[i]->tick();
  }

  /* Now update mobile objects */
  UpdateMobileObjectPositions(TRUE);

  if ( BallValid() == 1 )  /* For trajectory calculations */
    UpdateBallGlobalPosition();

  /* Compute the magnitude and direction of my velocity */
  if ( my_vel_x != NODIR && my_vel_y != NODIR ){
    my_vel_mag = sqrt(my_vel_x*my_vel_x + my_vel_y*my_vel_y);
    my_vel_dir = rad_to_deg(my_atan2(my_vel_y,my_vel_x));
    if (my_vel_x < 0) { 
      my_vel_dir += 180;
      CleanAngle(&my_vel_dir);
    }
  }
  else if ( !Dashed ){
    my_vel_mag = 0;
    my_vel_dir = 0; /* Assume no motion if I didn't dash ??? */
  }
  else {
    my_vel_mag = 0;
    my_vel_dir = NODIR;
  }

  /* Clean up for next tick */
  my_vel_x = my_vel_y = NODIR; /* No longer needed--conversions already done */
  NumExtraPlayers = 0;
  line = NO_LINE;

  if ( !InPitch(global_x,global_y) && MarkerValid(THEIR_GOAL) ){
    /* printf("%d:%d  Thought I was at (%.1f, %.1f) -- conf %.1f.  Must be confused.  Resetting\n",Mem->MyNumber,Mem->CurrentTime,global_x,global_y,MarkerValid(THEIR_GOAL));*/
    PositionInfo::Reset();
  }

#if DEBUG
  Print();
#endif
}

void PositionInfo::UpdateMobileObjectPositions(int erase_data){
  PositionMobile *MobileObject;
  for(int i=NUM_MARKERS; i<NUM_OBJECTS; i++){

    /* Process the heard globals                     */
    /* Must be after my global position has been set */
    MobileObject = (PositionMobile *) objects[i]; /* Just for type purposes */
    if ( MobileObject->got_global_info ){   
      float x,y,r,t;

      /* If I didn't just see the object */
      if ( !MobileObject->gospel || MobileObject->get_new_conf() < 1 ){ 
	MobileObject->get_globalxy(&x,&y);     /* retrieve communicated values     */
	globalxy_to_polar(x,y,&r,&t);          /* convert them to polar            */
	MobileObject->set_polar(r,t,HEARSAY_CONFIDENCE); /* It's hearsay, so not sure */
      }
      if (erase_data || (!turning && !dashing)) /* want to save it to reuse
						  after next sight */
	MobileObject->got_global_info = FALSE;
    }
    
    if ( erase_data ){                            /* coming from Tick() */
      int store_globals = FALSE;
      if ( MobileObject->gospel && MobileObject->side != 'b' ) /* not for the ball */
	store_globals = TRUE;

      MobileObject->tick(CurrentTime - LastTime); /* equivalent to objects[i]->tick();*/

      if (store_globals)
	UpdatePlayerGlobalPosition(MobileObject->side,MobileObject->num); 
    }
    else
      MobileObject->tick(0);                      /* don't degrade confidence */
  }
  
  DisambiguateExtraPlayers(); /* includes doing another tick */
}

void PositionInfo::DisambiguateExtraPlayers(){
  float x,y;
  int num=0;

  for (int i=0; i<NumExtraPlayers; i++){
    ExtraPlayers[i]->tick(0); /* Move the new values to cur values */
    GetMobileGlobalXY(ExtraPlayers[i],&x,&y);
#if 0
    printf("extra player %d.  R,d (%.1f,%.1f),  x,y (%.1f,%.1f)\n",
	   i,ExtraPlayers[i]->get_r(),ExtraPlayers[i]->get_tDEG(),x,y);
#endif
    num = ClosestUnseenPlayer(&(ExtraPlayers[i]->side),&x,&y);

    if (num){
      SeePlayer(ExtraPlayers[i]->side,num,
		ExtraPlayers[i]->get_r(),ExtraPlayers[i]->get_tDEG(),CurrentTime);

      if (ExtraPlayers[i]->side == MySide){
	UpdatePlayerGlobalPosition(MySide,num); 
	MyTeam[num]->tick(0);
      }
      else if (ExtraPlayers[i]->side == TheirSide){
	UpdatePlayerGlobalPosition(TheirSide,num); 
	TheirTeam[num]->tick(0); 
      }
      else 
	my_error("Should know the team at this point");
    }
  }
  NumExtraPlayers = 0;
}

void PositionInfo::ClearAction()
{
  turning = 0; dashing = 0;
  LastTurnAngle = 0;
}

void PositionInfo::Reset()
{
  int i;

  for(i=NUM_MARKERS; i<NUM_OBJECTS; i++)
    objects[i]->reset();  /* only reset mobile objects */
}

void PositionInfo::Turning(float angle)
{
  turning = 1;
  LastTurnAngle = angle;

#if 0
  /*  This is no longer needed--it happens in Tick based on the ACTUAL
      change of position instead of an estimate:  12/16/96           */
  /*  turning_angle = deg_to_rad(angle);*/
  for (int i=0; i<TEAM_SIZE; i++){
    MyTeam[i]->translate(0,-deg_to_rad(angle),1);    /* The degradation of confidence */
    TheirTeam[i]->translate(0,-deg_to_rad(angle),1); /* happens in Tick               */
  }
  Ball->translate(0,-deg_to_rad(angle),1);
#endif
}

void PositionInfo::Dashing(float power)
{
  dashing=1;
}

void PositionInfo::Kicking(float power, float angle)
{
  /* ball's angle is probably where you kicked it */
  Ball->set_t(deg_to_rad(angle), 0.8*Ball->get_tconf());
  Ball->set_r(power/6, 0.6*Ball->get_rconf());
}

void PositionInfo::Acting(int code) {
#if 0
  switch(code){
  case FORG: ForgetAll(); break;
  case FORGBALL: Ball->Forget(); break;
  case FORGMYGOAL: MyGoal->Forget(); break;
  case FORGTHEIRGOAL: TheirGoal->Forget(); break;
  default: my_error("don't know that acting code");
  }
#endif
}

void PositionInfo::Print(){
  int i;
  printf("  +--------------------------------------------------+\n");
  printf("  + Time = %d\n", CurrentTime); 
  printf("  + Global Angle = %f\n", rad_to_deg(global_angle));
  printf("  + Global Position= (%.1f,%.1f)\n",global_x,global_y);
  printf("  + Velocity = (%.1f,%.1f)\n",my_vel_dir,my_vel_mag);
  printf("  + Stamina Lower bound = %d\n",Mem->GetMyStamina());
  float x,y;

  for(i=0; i<NUM_MARKERS; i++) { 
    printf("  + Marker[%d] : Dist = %f, Theta = %f, Conf = %f\n",
	   i, Markers[i]->get_r(), Markers[i]->get_tDEG(), 
	   Markers[i]->get_conf());
    polar_to_globalxy(Markers[i]->get_r(), Markers[i]->get_t(), &x, &y);
    printf("  +              x = %.1f, y = %.1f\n",x,y);
    if ( i < 8 && 
	 (fabs(fabs(x)-fabs(marker_global[i].x)) > .1 || 
	  fabs(fabs(y)-fabs(marker_global[i].y)) > .1)){
      printf("  +              *****ALERT*****\n");
    }
  }

  printf("  + Ball : Dist = %f, Theta = %f, Conf = %f\n",
	 Ball->get_r(), Ball->get_tDEG(), Ball->get_conf());
  polar_to_globalxy(Ball->get_r(), Ball->get_t(), &x, &y);
  printf("  +              x = %.1f, y = %.1f, VelDir = %.1f, VelMag = %.1f\n",
	 x,y,GetBallVelDirection(),GetBallVelMagnitude());
/*
  for(i=1; i<3; i++) {
    printf("  + Teammate[%d] : Dist = %.2f, Theta = %.2f, Conf = %.2f, AngleConf = %.2f\n",
	   i, MyTeam[i]->get_r(), MyTeam[i]->get_tDEG(), 
	   MyTeam[i]->get_conf(), MyTeam[i]->get_tconf());
    polar_to_globalxy(MyTeam[i]->get_r(), MyTeam[i]->get_t(), &x, &y);
    printf("  +              x = %.1f, y = %.1f, VelDir = %.1f, VelMag = %.1f\n",
	   x,y,GetPlayerVelDirection(MySide,i),GetPlayerVelMagnitude(MySide,i));
  }
*/
/*
  for(i=1; i<12; i++) {
    printf("  + Opponent[%d] : Dist = %.2f, Theta = %.2f, Conf = %.2f, AngleConf = %.2f\n",
	   i, TheirTeam[i]->get_r(), TheirTeam[i]->get_tDEG(), 
	   TheirTeam[i]->get_conf(), TheirTeam[i]->get_tconf());
    polar_to_globalxy(TheirTeam[i]->get_r(), TheirTeam[i]->get_t(), &x, &y);
    printf("  +              x = %.1f, y = %.1f, VelDir = %.1f, VelMag = %.1f\n",
	   x,y,GetPlayerVelDirection(TheirSide,i),GetPlayerVelMagnitude(TheirSide,i));
  }
*/
  printf("  +--------------------------------------------------+\n\n");
}

float PositionInfo::triangulate(int m)
{
  float mx = Markers[m]->get_new_x();
  float my = Markers[m]->get_new_y();
  float gx, gy;
  int i;

  // Calculate global position
  polar_to_xy(Markers[m]->get_new_r(), 
	      global_angle - Markers[m]->get_new_t(), 
	      &gx, &gy);

  gx = marker_global[m].x - gx; gy = marker_global[m].y - gy;

  /* Peter's addition */
  if ( MySide == 'r' ){  /* x & y were flipped before, and will be overwritten soon */
    global_x *= -1;      /* here they need to be in team-independent format         */
    global_y *= -1;
  }
  float r_change = GetDistance(&gx,&gy,&global_x,&global_y);
  global_x = gx;
  global_y = gy;
  /* Peter's addition */

  // Set the positions of the markers
  for(i=0; i<NUM_EDGE_MARKERS; i++) { /* Do all markers if I want to move to P_FLAGs */
    float r, t;
    xy_to_polar(marker_global[i].x - gx, marker_global[i].y - gy,
		&r, &t);
    Markers[i]->set_polar(r, global_angle - t, 1.0);
  }

  return r_change;
}

float PositionInfo::overall_conf()
{
  int i;
  float min=1;

  for (i=0; i<NUM_MARKERS; i++) {
    float c = Markers[i]->get_conf();
    if (c < min) min = c;
  }

  return min;
}

float PositionInfo::globalxy_to_polar_r(float x, float y){
  return GetDistance(&x,&y,&global_x,&global_y);
}

float PositionInfo::globalxy_to_polar_t(float x, float y){
  float dx = x - global_x;
  float dy = y - global_y;
  float angle = my_atan2(dy,dx);
  return global_angle - angle;
}

int PositionInfo::GetViewAngle(int width){

  int ang = DEFAULT_VIEW_ANGLE;

  if ( width == NARROW_WIDTH )       /* "narrow" */
    ang /= 2;
  else if ( width == WIDE_WIDTH )  /* "wide"   */
    ang *= 2;

  return ang;
}

int PositionInfo::GetViewAngle(){
  return GetViewAngle(view_width);
}

int PositionInfo::TimePerSight(){

  int time = SEND_STEP;

  if ( view_width == NARROW_WIDTH )     
    time /= 2;
  else if ( view_width == WIDE_WIDTH )  
    time*= 2;
  if ( view_quality == LOW_QUALITY )   
    time /= 2;

  return time/SIMULATOR_STEP;  /* gives a lower bound on the time that's passed */
}

float PositionInfo::AngleToPoint(float x, float y){
  return rad_to_deg(globalxy_to_polar_t(x,y));
}

float PositionInfo::DistanceToPoint(float x, float y){
  float myX,myY;
  GetGlobalXY(&myX,&myY);
  return GetDistance(&myX,&myY,&x,&y);
}