memposition.c

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

  float f = get_abs_neck_ang() - Mem->MyNeckGlobalAng();
  NormalizeAngleDeg(&f);
  return f;
}

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

AngleDeg PlayerObject::get_neck_rel_ang()
{
  float f = get_rel_to_neck_neck_ang() - get_rel_to_neck_body_ang();
  NormalizeAngleDeg(&f);
  return f;
}

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

void PlayerObject::set_body_ang_from_neck      (AngleDeg body_ang, Time time)
{
  rnbang  = body_ang;
  rnbatime = time;
  seen_body_ang = TRUE;
}

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

void PlayerObject::set_neck_ang_from_neck      (AngleDeg neck_ang, Time time)
{
  rnnang  = neck_ang;
  rnnatime = time;
  seen_neck_ang = TRUE;
}

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

void PlayerObject::set_heard_info_w_angs(float x, float y, float pconf, float dx, float dy, float vconf, 
				  AngleDeg bang, float bconf, AngleDeg nang, float nconf, 
			          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 I have better face info, I have better neck info   */
  /* IMPORTANT -- above assumes body and neck angles are only heard together 
     and always with position info */
  if ( heard_body_ang && 
       (bconf < hbaconf || (bconf == hbaconf && dist > hbadist)) )
    return;

  MobileObject::set_heard_info(x,y,pconf,dx,dy,vconf,dist,time);
  hbang  = bang;
  hbaconf = bconf;
  hbadist = dist;
  hbatime = time;
  heard_body_ang = TRUE;

  hnang  = nang;
  hnaconf = nconf;
  hnadist = dist;
  hnatime = time;
  heard_neck_ang = TRUE;
}

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

void  PlayerObject::update(Time time)
{
  /* IMPORTANT -- assuming that body and neck angle are only seen together
     Otherwise, might have to reason about whether the neck angle has changed */

  if ( Mem->NewSight && seen_body_ang ){
    if ( !seen_neck_ang ) my_error("Should see body and neck angle together");
    update_seen_body_and_neck_angs(Mem->LastSightTime);
  }

  if ( gbatime < time ){
    if ( gnatime >= time ) my_error("Should see body and neck angle together");
    update_estimate(time);
  }

  if ( heard_body_ang ){
    if ( !heard_neck_ang ) my_error("Should hear body and neck angle together");
    update_heard(time);
  }

  MobileObject::update(time);
  seen_body_ang = heard_body_ang = seen_neck_ang = heard_neck_ang = FALSE;
}

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

void  PlayerObject::update_seen_body_and_neck_angs(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(2)? %d (%d %d) %d %d %c %d",
	       seen_time.t, seen_body_ang, seen_neck_ang,
	       rnbatime.t, rnnatime.t, side, unum);
    return;
  }

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

  gbang   = GetNormalizeAngleDeg(rnbang + Mem->MyNeckGlobalAng());
  gbaconf = max_conf;
  gbatime = time;
  gnang   = GetNormalizeAngleDeg(rnnang + Mem->MyNeckGlobalAng()); 
          /**** OR gbang + rnnang ****/
  gnaconf = max_conf;
  gnatime = time;
}

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

void  PlayerObject::update_estimate(Time time)
{
  if ( !body_ang_valid() || !neck_ang_valid() ) return;
  
  while ( gbatime < time ){
    gbaconf *= conf_decay;
    ++gbatime;
  }
  while ( gnatime < time ){
    gnaconf *= conf_decay;
    ++gnatime;
  }
}

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

void  PlayerObject::update_heard(Time time)
{
  if ( !heard_body_ang || !heard_neck_ang ) /* Should only be together */
    my_error("Should only be here if I heard face info");
  if ( time < hbatime || time < hnatime ) my_error("How did I fall behind?");
  if ( body_ang_valid() && gbatime < time )  
    my_error("Should estimate before processing sounds (p1)");
  if ( neck_ang_valid() && gnatime < time )  
    my_error("Should estimate before processing sounds (p2)");
    
  float decayed_hbaconf = hbaconf * Exp(conf_decay,(time-hbatime));
  if ( decayed_hbaconf > gbaconf || !pos_valid() ||
       (decayed_hbaconf == gbaconf && get_dist() > hbadist) ){
    /* Update gpos from hpos     */
    gbang   = hbang;
    gbatime = hbatime;
    gbaconf = hbaconf;
  }

  while ( body_ang_valid() && gbatime < time )
    update_estimate(gbatime+1);

  float decayed_hnaconf = hnaconf * Exp(conf_decay,(time-hnatime));
  if ( decayed_hnaconf > gnaconf || !pos_valid() ||
       (decayed_hnaconf == gnaconf && get_dist() > hnadist) ){
    /* Update gpos from hpos     */
    gnang   = hnang;
    gnatime = hnatime;
    gnaconf = hnaconf;
  }

  while ( neck_ang_valid() && gnatime < time )
    update_estimate(gnatime+1);
}

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

void  PlayerObject::reset()
{
  MobileObject::reset();
  hbaconf = gbaconf = hnaconf = gnaconf = 0;
  rnbatime = gbatime = hbatime = rnnatime = gnatime = hnatime = 0;
  hbadist = hnadist = 0;
  seen_body_ang = heard_body_ang = seen_neck_ang = heard_neck_ang = FALSE;
}

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

void  PlayerObject::clear_seen()
{
  MobileObject::clear_seen();
  seen_body_ang = seen_neck_ang = FALSE;
}

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

void  PlayerObject::forget()
{
  MobileObject::forget();
  gbaconf = gnaconf = 0;
#ifndef NO_ACTION_LOG
  float slide_dist = 5 / Sin(Mem->MyViewAngle(Mem->CurrentTime));
  Vector pos = get_rel_to_neck_pos();
  Mem->LogAction7(175,"Forgetting player %d: (%.1f, %.1f) (%.1f, %.1f)",
		  (side == Mem->MySide) ? unum : -unum, 
		  pos.x, pos.y,
		  (pos - Vector(slide_dist, 0)).x, (pos - Vector(slide_dist, 0)).y);
  Mem->LogAction5(175,"Forgetting player %d (still): old va: %.0f  new va: %.0f",
		  (side == Mem->MySide) ? unum : -unum, 
		  Mem->MyViewAngle(Mem->LastSightTime),
		  Mem->MyViewAngle(Mem->CurrentTime));
#endif                                                                                 
} 

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

void PlayerObject::sanitize_times()
{
  MobileObject::sanitize_times();
  Mem->sanitize_time(rnbatime);
  Mem->sanitize_time(hbatime);
  Mem->sanitize_time(rnnatime);
  Mem->sanitize_time(hnatime);
}  

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

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

  if ( MobileObject::in_view_range(view_ang, angle_buffer, 0) &&
       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 */