memposition.c

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

  /* moved to BallObject::update_seen */
#ifdef NEVER  
  /* Compute the estimated position */
  Vector epos;
  float estimate_valid = pos_valid();
  if ( estimate_valid ){
    epos = gpos; /* if LastSightTime == CurrentTime-1, current is an estimate */
    if ( Mem->LastSightTime == Mem->CurrentTime ){
      epos += gvel;     /* estimate is the old position + velocity */
      estimate_valid = vel_valid();
    }
  }
#endif  

  /****** Position ********/

  if ( seen ){
    /* Update gpos from dist,dir */
    if ( patime != time || pdtime !=time ){
      my_error("Need to do something here--got just ang %d %d  %d %d",
	       pdtime.t,pdtime.s,time.t,time.s); /*skip update if no dist */
    }
    rpos = Polar2Vector(dist,ang);
    rtime = time;
    gpos = Mem->MyPos() + rpos.rotate(Mem->MyAng());
    gtime = time;
    gconf = max_conf;
  }

  /****** Velocity ********/

  if ( seen_moving ){
    /* Update velocity from distch,dirch */
    /* This isn't the same rvel returned by get_rel_vel(), so don't store it */
    Vector temp_rvel = Vector(distch, dist*Tan(dirch)).rotate(ang);
    /* printf("rvel:  (%f %f)\n",rvel.mod(),rvel.dir()); */
    gvel = Mem->MyVel() + temp_rvel.rotate(Mem->MyAng());
    gvtime = time;
    gvconf = max_conf;
  }
  /* this has been moved to BallObject::update_seen */
#ifdef NEVER  
  else if ( estimate_valid ){ /* Only if I see the object, but not its velocity and I have a valid estimate */
    Vector diff = gpos - epos;
    if ( diff.mod() > .3 ) /* Make this a param... */
      /* only do this if the object is within SP_feel_distance??? */
      ; //printf("NEED TO RESET VELOCITY %d %f\n",type,diff.mod());  /* gvconf = 0; */
  }
#endif	
}

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

void  MobileObject::update_heard(Time time)
{
  if ( heard ){
    if ( time < hptime ) my_error("How did I fall behind?");
    if ( pos_valid() && gtime < time )  my_error("Should estimate before processing sounds (m1)");
    
    float decayed_hpconf = hpconf * Exp(conf_decay,(time-hptime));
    if ( decayed_hpconf > gconf || 
	 (decayed_hpconf == gconf && get_dist() < hpdist) ){
      /* Update gpos from hpos     */
      gpos  = hpos;
      gtime = hptime;
      gconf = hpconf;
    }
  }

  if ( heard_moving ){
    if ( time < hvtime ) my_error("How did I fall behind?");
    if ( vel_valid() && gvtime < time ) my_error("Should estimate before processing sounds (m2) %d %d %d %d",gvtime.t,gvtime.s,time.t,time.s);

    float decayed_hvconf = hvconf * Exp(conf_decay,(time-hptime));
    if ( decayed_hvconf > gvconf || 
	 (decayed_hvconf == gvconf && get_dist() < hvdist) ){
      /* Update gvel from hvel */
      gvel  = hvel;
      gvtime = hvtime;
      gvconf = hvconf;
    }
  }

  /* keep updating hptime, hvtime until at time */
  while ( vel_valid() && gvtime < gtime ){
    estimate_vel(gvtime+1);
    ++gvtime;
    gvconf *= conf_decay;
  }

  while ( pos_valid() && gtime < time )
    update_estimate( gtime+1 );
}

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

void  MobileObject::update_estimate(Time time)
{
  if ( !pos_valid() ) return;

  if ( gtime == time && vel_valid() ){ /* just vel */
    if ( gvtime == time ) my_error("pos and vel already updated");
    if ( Mem->NewAction && Mem->LastActionValid(gvtime) && 
	 Mem->LastActionType() == CMD_kick && type == OBJ_Ball )
      update_kick(gvtime);

    while ( vel_valid() && gvtime < gtime ){
      estimate_vel(time);
      ++gvtime;
      gvconf *= conf_decay;   
    }
    return;
  }

  while ( gtime < time ){ /* both pos and vel */

    if ( Mem->NewAction && Mem->LastActionValid(gtime) && 
	 Mem->LastActionType() == CMD_kick && type == OBJ_Ball && vel_valid() )
      update_kick(gtime);

    estimate_pos(time);
    ++gtime;
    gconf  *= conf_decay;   
    
    while ( vel_valid() && gvtime < gtime ){
      estimate_vel(time);
      ++gvtime;
      gvconf *= conf_decay;   
    }
  }
}

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

Vector MobileObject::estimate_future_pos(int steps, Vector extra_vel, Vector extra_vel_per)
{
  if ( !pos_valid() ) my_error("Can't estimate future if don't know present");

  Vector position = gpos;  
  Vector velocity;
  if (vel_valid()) velocity = gvel + extra_vel;
  else velocity = 0;
  
  for (int i=0; i<steps; i++){
    velocity += extra_vel_per;
    if ( velocity.mod() > max_speed )
      velocity *= ( max_speed/velocity.mod() );
    position += velocity;
    velocity *= motion_decay;
  }

  return position;
}

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

void  MobileObject::reset()
{
  Object::reset();
  gconf = 0;
  hpconf = hvconf = gvconf = 0;
  gvtime = hptime = hvtime = 0;
  hpdist = hvdist = 0;
  heard = seen_moving = heard_moving = FALSE;
} 

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

void  MobileObject::forget()
{
  gconf = gvconf = 0;
  //if (type == OBJ_Ball) printf("%d forgetting ball\n",Mem->MyNumber);
} 

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

void MobileObject::sanitize_times()
{
  Object::sanitize_times();
  Mem->sanitize_time(hptime);
  Mem->sanitize_time(hvtime);
}  


/********************************************************************************/
/********************************************************************************/
/********************************************************************************/
/*                        BallObject Class                                      */
/********************************************************************************/
/********************************************************************************/
/********************************************************************************/

void BallObject::Initialize(float max, float min_valid, float decay, float motion, float max_speed)
{
  MobileObject::Initialize(OBJ_Ball,max,min_valid,decay,motion,max_speed);
  reset();
  ektime = 0;
  use_pos_based_vel_estimate = TRUE;
  pos_based_vel_time = 0;
  past_kick_idx = 0;
  for (int i=0; i<num_past_kicks; i++)
    past_kicks[i].time = -1;
}

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

Bool BallObject::moving()
{ 
  /* experimentally checked that if just the player's moving, ball speed could register > .15 */
  return vel_valid() && get_speed() > Mem->CP_ball_moving_threshold ? TRUE : FALSE; 
} 

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

Bool BallObject::kickable(float buffer)
{
  if ( !pos_valid() || !Mem->MyConf() ) return FALSE;
  return get_dist() <= Mem->SP_kickable_area - buffer ? TRUE : FALSE; 
}

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

Bool BallObject::catchable()
{
  if ( !pos_valid() || !Mem->MyConf() ) return FALSE;
  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);

    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");

  /* update ball velocity from time to CurrentTime (before decay) */
  Vector kick_effect =
    Polar2Vector(get_kick_rate(time) * Mem->LastActionPower(), 
		 Mem->MyAng() + Mem->LastActionAngle());
  /* store this kick for later estimation */
  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::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)
{
  Time   prev_seen_time = last_seen_time;
  Vector prev_seen_pos  = last_seen_pos; 

  MobileObject::update(time);

  if ( kickable() ){
    effective_kick_rate = calc_kick_rate();
    ektime = time;
  }

  if ( !vel_valid() && pos_valid() &&
       prev_seen_time >= Mem->PreviousSightTime() &&
       !(use_pos_based_vel_estimate == FALSE &&
	 pos_based_vel_time == Mem->LastSightTime) ){

    /* Don't estimate velocity if the ball's far -- too much noise */
    if ( !Mem->MyConf() || get_dist() > Mem->SP_feel_distance ) return; 

    /* 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 positio	n 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 occilates 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 == Mem->LastSightTime) {
      //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);      
    } else 
      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;
    }
    //SMURF
    //cout << "gvel estimate: " << gvel << "mod: " << gvel.mod() << endl;
    
    /* old way - does not include recent kicks
    gvel = gpos - prev_seen_pos;	
    gvel /= SumGeomSeries(1,1/motion_decay,time-prev_seen_time);
    */
    //gvconf = Mem->CP_max_valid_conf; /* so it expires and can be over-ridden easily */
    gvconf = max_conf; /* so it overrides heard info */
    gvtime = time;
  }

  if ( vel_valid() && get_speed() > max_speed )
    gvel *= max_speed/get_speed();
}

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

void BallObject::update_seen(Time time)
{
  Vector epos;
  float estimate_valid = pos_valid();
  if ( estimate_valid ){
    epos = gpos; /* if LastSightTime == CurrentTime-1, current is an estimate */
    if ( Mem->LastSightTime == Mem->CurrentTime ){
      epos += gvel;     /* estimate is the old position + velocity */