memposition.c

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

      estimate_valid = vel_valid();
    }
  }

  MobileObject::update_seen(time);

  if ( seen && !seen_moving && estimate_valid ){ /* Only if I see the object, but not its velocity and I have a valid estimate */
    Vector diff = gpos - epos;
    if ( get_dist() < Mem->SP_feel_distance &&
	 diff.mod() > Mem->CP_ball_invalidation_dist_err ) {
      /* It seems like a good idea to invalidate the ball's velocity if the
	 ball's position is way off from where we expect (identify collisions?)
	 but we had trouble when we did this so it's commented out */
      /* gvconf = 0;
      use_pos_based_vel_estimate = FALSE;
      pos_based_vel_time = time; */
    }
  }
  
  if ( seen_time > last_seen_time ){
    last_seen_time = time; /* == last_seen_time except when clock was stopped */
    last_seen_pos  = gpos;
  }
}

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

void BallObject::estimate_pos(Time time)
{
  MobileObject::estimate_pos(time);
  if ( !Mem->MyConf() ) return; 
  if ( !pos_valid() )   return; /* Can't check for collisions */  

  /* Only worry about collisions for the ball */
  if ( get_dist() < Mem->SP_player_size + Mem->SP_ball_size) {
    if ( vel_valid() ){
      float r = Mem->SP_ball_size + Mem->SP_player_size;
      float d = get_dist();
      float th = fabs(GetNormalizeAngleDeg(get_rel_heading() - 180));
      float l1 = d * Cos(th);
      float h = d * Sin(th);
      float cosp = h / r;
      float sinp = sqrt(1.0 - Sqr(cosp));
      float l2 = r * sinp;
      gpos += gvel*motion_decay*(-(l1+l2)/Max(get_speed()*motion_decay, 1.0e-10));
      gvel *= -0.1;
    }
    /* my_stamp; printf("COLLISION!  --   check computation\n"); */
  }
}

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

Bool BallObject::in_view_range(Time time, float angle_buffer, float distance_buffer)
{
  if ( !pos_valid() || !Mem->MyConf() ) return FALSE;

  if (MobileObject::in_view_range(time, angle_buffer) || 
      get_dist() < Mem->SP_feel_distance - distance_buffer ) 
    return TRUE;
  return FALSE;
}



/********************************************************************************/
/********************************************************************************/
/********************************************************************************/
/*                        PlayerObject Class                                    */
/********************************************************************************/
/********************************************************************************/
/********************************************************************************/

void PlayerObject::Initialize(float max, float min_valid, float decay, float motion, float max_speed)
{
  MobileObject::Initialize(OBJ_Player,max,min_valid,decay,motion,max_speed);
  side = 'f'; /* free */
  unum = Unum_Unknown;
  reset();
}

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

AngleDeg PlayerObject::get_rel_face()
{
  float f = get_abs_face() - Mem->MyAng();
  NormalizeAngleDeg(&f);
  return f;
}

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

void PlayerObject::set_face      (AngleDeg face , Time time)
{
  rface  = face;
  rftime = time;
  seen_face = TRUE;
}

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

void PlayerObject::set_heard_info_w_face(float x, float y, float pconf, float dx, float dy, float vconf, 
				  AngleDeg face, float fconf, float dist, Time time)
{
  /* Don't overwrite better infor from the same cycle      */
  /* If I have better face info, I have better pos. info   */
  if ( heard_face && 
       (fconf < hfconf || (fconf == hfconf && dist > hfdist)) )
    return;

  MobileObject::set_heard_info(x,y,pconf,dx,dy,vconf,dist,time);
  hface  = face;
  hfconf = fconf;
  hfdist = dist;
  hftime = time;
  heard_face = TRUE;
}

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

void  PlayerObject::update(Time time)
{
  if ( Mem->NewSight && seen_face )
    update_seen_face(Mem->LastSightTime);

  if ( gftime < time )
    update_estimate(time);

  if ( heard_face )
    update_heard(time);

  MobileObject::update(time);
  seen_face = heard_face = FALSE;
}

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

void  PlayerObject::update_seen_face(Time time)
{
  if ( !Mem->MyConf() ) return;

  sanitize_times();

  if ( seen_time != Mem->LastSightTime ){
    if ( seen_time != Mem->LastSightTime-1 ) my_error("Why the sight delay?");
    return;
  }

  if ( Mem->MyUpdateTime() != time ) my_error("Must have missed a cycle (player)"); 

  gface  = rface + Mem->MyAng();
  gfconf = max_conf;
  gftime = time;
}

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

void  PlayerObject::update_estimate(Time time)
{
  if ( !face_valid() ) return;
  
  while ( gftime < time ){
    gfconf *= conf_decay;
    ++gftime;
  }
}

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

void  PlayerObject::update_heard(Time time)
{
  if ( !heard_face ) my_error("Should only be here if I heard face info");
  if ( time < hftime ) my_error("How did I fall behind?");
  if ( face_valid() && gftime < time )  my_error("Should estimate before processing sounds (p)");
    
  float decayed_hfconf = hfconf * Exp(conf_decay,(time-hftime));
  if ( decayed_hfconf > gfconf || 
       (decayed_hfconf == gfconf && get_dist() < hfdist) ){
    /* Update gpos from hpos     */
    gface  = hface;
    gftime = hftime;
    gfconf = hfconf;
  }

  while ( face_valid() && gftime < time )
    update_estimate(gftime+1);
}

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

void  PlayerObject::reset()
{
  MobileObject::reset();
  hfconf = gfconf = 0;
  rftime = gftime = hftime = 0;
  hfdist = 0;
  seen_face = heard_face = FALSE;
}

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

void  PlayerObject::forget()
{
  MobileObject::forget();
  gfconf = 0;
} 

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

void PlayerObject::sanitize_times()
{
  MobileObject::sanitize_times();
  Mem->sanitize_time(rftime);
  Mem->sanitize_time(hftime);
}  

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

Bool PlayerObject::in_view_range(Time time, float angle_buffer, float distance_buffer)
{
  if ( !pos_valid() || !Mem->MyConf() ) return FALSE;

  if ( MobileObject::in_view_range(time, angle_buffer) && 
       get_dist() < Mem->SP_unum_far_length - distance_buffer ) 
    return TRUE;
  return FALSE;
}


/********************************************************************************/
/********************************************************************************/
/********************************************************************************/
/*                        PositionInfo Class                                    */
/********************************************************************************/
/********************************************************************************/
/********************************************************************************/

void PositionInfo::Initialize()
{
  /* printf("Calling Position Initialize\n"); */

  /* if true, multiply all coords by -1 inside initialize */
  Bool rotate = (Mem->MySide == 'l') ? FALSE : TRUE;  
  
  int i=0;

  Marker = new StationaryObject[SP_num_markers];
  Marker[i].Initialize((MarkerType) i, Vector( -SP_pitch_length/2.0, 0.0 ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Goal_L */
  Marker[i].Initialize((MarkerType) i, Vector( SP_pitch_length/2.0, 0.0 ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Goal_R */

  Marker[i].Initialize((MarkerType) i, Vector( 0.0, 0.0 ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_C */
  Marker[i].Initialize((MarkerType) i, Vector( 0.0, -SP_pitch_width/2.0 ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_CT */
  Marker[i].Initialize((MarkerType) i, Vector( 0.0, SP_pitch_width/2.0 ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_CB */

  Marker[i].Initialize((MarkerType) i, Vector( -SP_pitch_length/2.0, -SP_pitch_width/2.0 ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_LT */
  Marker[i].Initialize((MarkerType) i, Vector( -SP_pitch_length/2.0, SP_pitch_width/2.0 ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_LB */
  Marker[i].Initialize((MarkerType) i, Vector( SP_pitch_length/2.0, -SP_pitch_width/2.0 ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_RT */
  Marker[i].Initialize((MarkerType) i, Vector( SP_pitch_length/2.0, SP_pitch_width/2.0 ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_RB */

  Marker[i].Initialize((MarkerType) i, Vector( -SP_pitch_length/2.0+SP_penalty_area_length,
				-SP_penalty_area_width/2.0 ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_PLT */
  Marker[i].Initialize((MarkerType) i, Vector( -SP_pitch_length/2.0+SP_penalty_area_length, 0 ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_PLC */
  Marker[i].Initialize((MarkerType) i, Vector( -SP_pitch_length/2.0+SP_penalty_area_length,
				SP_penalty_area_width/2.0 ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_PLB */
  Marker[i].Initialize((MarkerType) i, Vector( SP_pitch_length/2.0-SP_penalty_area_length,
				-SP_penalty_area_width/2.0 ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_PRT */
  Marker[i].Initialize((MarkerType) i, Vector( SP_pitch_length/2.0-SP_penalty_area_length, 0 ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_PRC */
  Marker[i].Initialize((MarkerType) i, Vector( SP_pitch_length/2.0-SP_penalty_area_length,
				SP_penalty_area_width/2.0 ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_PRB */

  Marker[i].Initialize((MarkerType) i, Vector( -SP_pitch_length/2.0, -SP_goal_width/2.0 ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_GLT */
  Marker[i].Initialize((MarkerType) i, Vector( -SP_pitch_length/2.0, SP_goal_width/2.0 ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_GLB */
  Marker[i].Initialize((MarkerType) i, Vector( SP_pitch_length/2.0, -SP_goal_width/2.0 ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_GRT */
  Marker[i].Initialize((MarkerType) i, Vector( SP_pitch_length/2.0, SP_goal_width/2.0 ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_GRB */

  Marker[i].Initialize((MarkerType) i, Vector( -50.0, -SP_pitch_width/2.0-SP_pitch_margin ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_TL50 */
  Marker[i].Initialize((MarkerType) i, Vector( -40.0, -SP_pitch_width/2.0-SP_pitch_margin ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_TL40 */
  Marker[i].Initialize((MarkerType) i, Vector( -30.0, -SP_pitch_width/2.0-SP_pitch_margin ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_TL30 */
  Marker[i].Initialize((MarkerType) i, Vector( -20.0, -SP_pitch_width/2.0-SP_pitch_margin ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_TL20 */
  Marker[i].Initialize((MarkerType) i, Vector( -10.0, -SP_pitch_width/2.0-SP_pitch_margin ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_TL10 */
  Marker[i].Initialize((MarkerType) i, Vector( 0.0, -SP_pitch_width/2.0-SP_pitch_margin ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_T0 */
  Marker[i].Initialize((MarkerType) i, Vector( 10.0, -SP_pitch_width/2.0-SP_pitch_margin ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_TR10 */
  Marker[i].Initialize((MarkerType) i, Vector( 20.0, -SP_pitch_width/2.0-SP_pitch_margin ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_TR20 */
  Marker[i].Initialize((MarkerType) i, Vector( 30.0, -SP_pitch_width/2.0-SP_pitch_margin ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_TR30 */
  Marker[i].Initialize((MarkerType) i, Vector( 40.0, -SP_pitch_width/2.0-SP_pitch_margin ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_TR40 */
  Marker[i].Initialize((MarkerType) i, Vector( 50.0, -SP_pitch_width/2.0-SP_pitch_margin ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_TR50 */

  Marker[i].Initialize((MarkerType) i, Vector( -50.0, SP_pitch_width/2.0+SP_pitch_margin ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_BL50 */
  Marker[i].Initialize((MarkerType) i, Vector( -40.0, SP_pitch_width/2.0+SP_pitch_margin ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_BL40 */
  Marker[i].Initialize((MarkerType) i, Vector( -30.0, SP_pitch_width/2.0+SP_pitch_margin ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_BL30 */
  Marker[i].Initialize((MarkerType) i, Vector( -20.0, SP_pitch_width/2.0+SP_pitch_margin ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_BL20 */
  Marker[i].Initialize((MarkerType) i, Vector( -10.0, SP_pitch_width/2.0+SP_pitch_margin ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_BL10 */
  Marker[i].Initialize((MarkerType) i, Vector( 0.0, SP_pitch_width/2.0+SP_pitch_margin ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_B0 */
  Marker[i].Initialize((MarkerType) i, Vector( 10.0, SP_pitch_width/2.0+SP_pitch_margin ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_BR10 */
  Marker[i].Initialize((MarkerType) i, Vector( 20.0, SP_pitch_width/2.0+SP_pitch_margin ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_BR20 */
  Marker[i].Initialize((MarkerType) i, Vector( 30.0, SP_pitch_width/2.0+SP_pitch_margin ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_BR30 */
  Marker[i].Initialize((MarkerType) i, Vector( 40.0, SP_pitch_width/2.0+SP_pitch_margin ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_BR40 */
  Marker[i].Initialize((MarkerType) i, Vector( 50.0, SP_pitch_width/2.0+SP_pitch_margin ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_BR50 */

  Marker[i].Initialize((MarkerType) i, Vector( -SP_pitch_length/2.0-SP_pitch_margin, -30 ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_LT30 */
  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_LT20 */
  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_LT10 */
  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_L0 */
  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_LB10 */
  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_LB20 */
  Marker[i].Initialize((MarkerType) i, Vector( -SP_pitch_length/2.0-SP_pitch_margin, 30 ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_LB30 */

  Marker[i].Initialize((MarkerType) i, Vector( SP_pitch_length/2.0+SP_pitch_margin, -30 ),
			   CP_max_conf, CP_min_valid_conf, rotate); i++; /* Flag_RT30 */
  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 */