memposition.c

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

  return get_dist() <= Mem->SP_catch_area_l ? TRUE : FALSE; 
}

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

float BallObject::get_kick_rate(Time time)
{ 
  if ( ektime != time ){
    if (!kickable()){
      my_error("not kickable %d %d  %d %d",ektime.t,ektime.s, time.t,time.s);
      my_error("not kickable (ball at %.1f %.1f)",gpos.x,gpos.y);
    }

    if (time != Mem->MyUpdateTime())
      my_error("Calculating a kick_rate for a time not equal to update time (%d.%d) (%d.%d)",
	       time.t, time.s, Mem->MyUpdateTime().t, Mem->MyUpdateTime().s);
    
    effective_kick_rate = calc_kick_rate();
    ektime = time;
  }
  return effective_kick_rate;
}

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

void  BallObject::update_kick(Time time)
{
  if ( !(Mem->LastActionValid(time)) ) my_error("No action at that time (kick)");
  if ( Mem->LastActionType() != CMD_kick ) my_error("Last action wasn't a kick");
  if ( !pos_valid() || !vel_valid() ) my_error("Can't update ball if not known before");
  if ( !Mem->MyConf() ) my_error("Can't update ball if not localized");

  Mem->LogAction7(180, "Updating kick: time %d.%d, kick %.2f at %.2f, rate %.5f",
		  time.t, time.s, Mem->LastActionPower(), Mem->LastActionAngle(),
		  get_kick_rate(time));
  
  Vector kick_effect =
    Polar2Vector(get_kick_rate(time) * Mem->LastActionPower(), 
		 Mem->MyBodyAng() + Mem->LastActionAngle());
  /* store this kick for later estimation */
  /* this is now done explicitly by send_action in client.C 
  past_kicks[past_kick_idx].time = time;
  past_kicks[past_kick_idx].kick_effect = kick_effect;
  past_kick_idx = past_kick_inc(past_kick_idx);
  */
   
  gvel += kick_effect;
  if ( gvel.mod() > max_speed )
     gvel *= ( max_speed/gvel.mod() );
}

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


float BallObject::calc_kick_rate(float dist, float ang)
{
  return  Mem->SP_kick_power_rate *
      (1 - .25 * fabs( ang ) / 180.0 -
       .25 * (dist - Mem->SP_ball_size - Mem->SP_player_size) / Mem->SP_kickable_margin);
}

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

void BallObject::set_past_kick(float pow, AngleDeg ang, Time t)
{
  Vector kick_effect =
    Polar2Vector(get_kick_rate(t) * pow, Mem->MyBodyAng() + ang);
  past_kicks[past_kick_idx].time = t;
  past_kicks[past_kick_idx].kick_effect = kick_effect;
  past_kick_idx = past_kick_inc(past_kick_idx);
}


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

void BallObject::forget_past_kick(Time t)
{
  for(int i=0; i < num_past_kicks; i++) {
    if (past_kicks[i].time == t)
      past_kicks[i].kick_effect = Vector(0,0); /* make the kick effectless */
  }
  
}

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

void BallObject::update(Time time)
{

  MobileObject::update(time);

  if ( kickable() ){
    effective_kick_rate = calc_kick_rate();
    ektime = time;
  }
  
  if ( vel_valid() && get_speed() > max_speed )
    gvel *= max_speed/get_speed();
}

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

void BallObject::update_seen(Time time)
{
  Time   prev_seen_time = last_seen_time;
  Vector prev_seen_pos  = last_seen_pos; 

  Vector epos, evel;
  float estimate_valid = pos_valid();
  if ( estimate_valid ){
    /* if LastSightTime == CurrentTime-1, current is an estimate for the appropriate time */
    epos = gpos;
    evel = gvel;
    if ( Mem->LastSightTime == Mem->CurrentTime ){
      if (time != Mem->CurrentTime)
	my_error("BallObject::update seen: times seen strange: %d.%d %d.%d",
		 time.t, time.s, Mem->CurrentTime.t, Mem->CurrentTime.s);
      /* see if there is a kick we need to update for */
      if (Mem->LastActionValid(Mem->CurrentTime-1) &&
	  Mem->LastActionType() == CMD_kick &&
	  Mem->LastActionTime() == Mem->CurrentTime - 1) {
	/*Mem->LogAction5(180, "ball velocity inval: updating for kick %.2f at %.2f, rate %.5f",
			Mem->LastActionPower(), Mem->LastActionAngle(), get_kick_rate(Mem->CurrentTime-1)); */
	evel += Polar2Vector(get_kick_rate(Mem->CurrentTime-1) * Mem->LastActionPower(), 
			     Mem->MyBodyAng() + Mem->LastActionAngle());
      }
      epos += evel;     /* estimate is the old position + velocity */
      evel *= Mem->SP_ball_decay;
      estimate_valid = vel_valid();
    }
  }

  MobileObject::update_seen(time);

  /*****************************************************/
  /* THIS IS DISTANCE BASED BALL VELOCITY INVALIDATION */
  /*****************************************************/
  /* Only if I see the object, but not its velocity and I have a valid estimate */
  if ( seen && !seen_moving && estimate_valid
       && Mem->sight_position_correction_time == time){ 
    /* first we need to see if we estiamted a collision, in which case we should
       NOT invalidate the velocity, but multiply it by -1.0 instead */
    if ((epos + Mem->sight_position_correction).dist(Mem->MyPos()) <
	Mem->SP_player_size + Mem->SP_ball_size) {
      Mem->LogAction3(175, "ball: update seen predicts a collision %.2f",
		      (epos + Mem->sight_position_correction).dist(Mem->MyPos()));
      gvel = evel * -.1;
      gvconf = Mem->CP_min_valid_conf / Mem->CP_ball_conf_decay; //only valid for 2 cycles
      gvtime = time;
      /* now, don't use postion based velocity estimation */
      use_pos_based_vel_estimate = FALSE;
      pos_based_vel_time = time;
    } else {
      /* first we need to figure out the maximum error that the server would give
       total_dist_err is supposed to the the total dist across the error margin of
       the reporting of the ball's position. The angle is +/- .5 degrees and the
       distance is quantize as follows
       Quantize(exp(Quantize(log(vi.distance + EPS),vi.qstep)),0.1) ;

       After that, we have to add in the possible noise from the server.
       We're just going to add in the noise once for every cycle between our
       sight times
    */
      float d = get_dist();
      float just_dist_err = d * Mem->quantize_err_const / 2;
      float inner_perp_err = Mem->Tan_of_half_deg * (d - just_dist_err);
      float outer_perp_err = Mem->Tan_of_half_deg * (d + just_dist_err);
      float total_dist_err =
	sqrt(Sqr(inner_perp_err) + Sqr(just_dist_err)) +
	sqrt(Sqr(outer_perp_err) + Sqr(just_dist_err));

      total_dist_err += Mem->LastSightInterval *
	sqrt(2.0 * Sqr(Mem->SP_ball_rand * gvel.mod()));
    
      Vector diff = gpos - (epos + Mem->sight_position_correction);
      if (diff.mod() > total_dist_err * Mem->CP_ball_vel_invalidation_factor) {
	Mem->LogAction6(175, "Invalidating ball velocity: %.2f > %.2f, thought vel was (%.2f, %.2f)",
			diff.mod(), total_dist_err * Mem->CP_ball_vel_invalidation_factor,
			evel.x, evel.y);
	Mem->LogAction6(175,"Invalidating ball vel(still): gpos (%.1f %.1f), sight_position_correction (%.1f %.1f)",
			gpos.x, gpos.y,
			Mem->sight_position_correction.x, Mem->sight_position_correction.y);
	gvconf = 0;
	if (!(pos_based_vel_time == time && !use_pos_based_vel_estimate)) {
	  // if it's already been set, don't touch it
	  use_pos_based_vel_estimate = (time-prev_seen_time == 1) ? TRUE : FALSE;
	  pos_based_vel_time = time;
	} 
      }
    }
  }


  /**********************************************/
  /* THIS IS POSITION BASED VELOCITY ESTIMATION */
  /**********************************************/
  if ( Mem->MyConf() && pos_valid() &&
       gvconf <= Exp(Mem->CP_ball_conf_decay, 2) &&
       get_dist() <= Mem->SP_feel_distance &&
       prev_seen_time >= Mem->PreviousSightTime() &&
       !(use_pos_based_vel_estimate == FALSE &&
	 pos_based_vel_time == time)  ){
    /* Don't estimate velocity if the ball's far -- too much noise */

    /* Can estimate based on the last seen position of the ball */
    //cout << "Time: " << time.t << "  Using position based vel estimate" << endl;
    Vector total_kick_eff(0,0);
    /* we're goign to look through our past kicks to find ones that occured in
       the time cycle that we're looking at */
    int pkidx;
    for(pkidx = past_kick_dec(past_kick_idx);
	past_kicks[pkidx].time >= prev_seen_time;
	pkidx = past_kick_dec(pkidx)) {
      /* this kick falls in our range */
      if (past_kicks[pkidx].time > time)
	my_error("Postion Based Vel Estimate: Already have a future kick???");
      total_kick_eff += past_kicks[pkidx].kick_effect *
	SumGeomSeries(1, motion_decay, time - past_kicks[pkidx].time);     
    }
    /* POSITION BASED VELOCITY ESTIMATION
       A problem that arises using position based velocity estimes is that
       we store the global position of the ball as calculated relative to the
       player (whose global position we identify by flags). Becuase of the
       error in seeing the flags, our global position ossilates around the true
       value, adding additional error to the position based velocity
       calculation. The solution is to take the difference between where we
       expected to be and where we observe we are at each new sight and store
       that. We then correct the global ball position for that. This
       essentially removes the error in calculating our global position from
       this calculation, leaving only the ball observation error, giving us a
       better value */
    if (Mem->CP_use_new_position_based_vel && Mem->MyConf() &&
	Mem->sight_position_correction_time == time) {
      //cout << "time: " << time.t << "\tcorr: " << Mem->sight_position_correction.mod() << endl;
      gvel = (gpos - (prev_seen_pos+Mem->sight_position_correction) - total_kick_eff)
	/ SumGeomSeries(1,motion_decay,time-prev_seen_time);      
      Mem->LogAction8(175,"Position based velocity estimating: gpos (%.1f %.1f), prev_seen_pos (%.1f %.1f), sight_position_correction (%.1f %.1f)",
		      gpos.x,gpos.y,prev_seen_pos.x,prev_seen_pos.y,
		      Mem->sight_position_correction.x,Mem->sight_position_correction.y);
    } else{ 
      Mem->LogAction6(175,"Old position based velocity estimating: gpos (%.1f %.1f), prev_seen_pos (%.1f %.1f)",
		      gpos.x,gpos.y,prev_seen_pos.x,prev_seen_pos.y);
      gvel = (gpos - prev_seen_pos - total_kick_eff)
	/ SumGeomSeries(1,motion_decay,time-prev_seen_time);
    /* gvel is now the velocty at prev_seen_time */
    }

    Time t;
    pkidx = past_kick_inc(pkidx);
    for(t = prev_seen_time; t < time; ++t) {
      if (pkidx != past_kick_idx && past_kicks[pkidx].time == t) {	
	gvel += past_kicks[pkidx].kick_effect;
	pkidx = past_kick_inc(pkidx);
      }      
      gvel *= motion_decay;
    }
    
    gvconf = max_conf; /* so it overrides heard info */
    gvtime = 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_to_body_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;
      Mem->LogAction2(160, "Ball collision");
      /* turn off position based velocity estimation for this cycle */
      use_pos_based_vel_estimate = FALSE;
      pos_based_vel_time = time; 
    }
    /* my_stamp; printf("COLLISION!  --   check computation\n"); */
  }
}

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

Bool BallObject::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_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_to_body_body_ang()
{
  float f = get_abs_body_ang() - Mem->MyBodyAng();
  NormalizeAngleDeg(&f);
  return f;
}

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

AngleDeg PlayerObject::get_rel_to_body_neck_ang()
{
  float f = get_abs_neck_ang() - Mem->MyBodyAng();
  NormalizeAngleDeg(&f);
  return f;
}

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

AngleDeg PlayerObject::get_rel_to_neck_body_ang()
{
  float f = get_abs_body_ang() - Mem->MyNeckGlobalAng();
  NormalizeAngleDeg(&f);
  return f;
}

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

AngleDeg PlayerObject::get_rel_to_neck_neck_ang()
{