memposition.c

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

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

void MobileObject::set_heard_info(float x, float y, float pconf, float dx, float dy, float vconf, float dist, Time time)
{
  /* Don't overwrite better infor from the same cycle      */
  /* If I have better motion info, I have better pos. info */
  if ( heard_moving && 
       (vconf < hvconf || (vconf == hvconf && hvdist > dist)) )
    return;

  set_heard_info(x,y,pconf,dist,time);
  hvel = Vector(dx,dy);
  hvconf = vconf;
  hvdist = dist;
  hvtime = time;
  heard_moving = TRUE;

  float speed = hvel.mod();
  if ( speed > max_speed ){
    if ( speed > max_speed + .1 )
      my_error("object CAN'T be moving that fast %.0f %.2f",(float) type,speed);
    hvel *= max_speed/speed;
  }
}

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

void  MobileObject::estimate_pos(Time time)
{
  if ( gtime >= time ) my_error("pos already updated");

  if ( pos_valid() && vel_valid() )
    gpos += gvel;
}

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

void  MobileObject::estimate_vel(Time time)
{
  if ( gvtime >= time ) {
    my_error("vel already updated");
  }

  if ( vel_valid() )
    gvel *= motion_decay;
  else
    my_error("Shouldn't be updating invalid vel");
}

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

void  MobileObject::update(Time time)
{
  if ( Mem->NewSight && (seen || seen_moving) )
    update_seen(Mem->LastSightTime);

  /* change_view happens instantaneously, so last sight could've been from either view angle */
  AngleDeg forget_view_ang = Min(Mem->MyViewAngle(time),Mem->MyViewAngle(time-1));
  float ang_buff = (type == OBJ_Ball ?
		    Mem->CP_ball_forget_angle_buf :
		    Mem->CP_player_forget_angle_buf);
  float dist_buff = (type == OBJ_Ball ?
		    Mem->CP_ball_forget_dist_buf :
		    Mem->CP_player_forget_dist_buf);
  
  /* If sight was from time-1, and should be in view at this point but isn't, reset */
  if (Mem->NewSight && Mem->LastSightTime == time-1 &&
      !seen && in_view_range(forget_view_ang, ang_buff, dist_buff) )
    forget();

  if ( gvtime < time || gtime < time )
    update_estimate(time);

  /* If sight was from time, and should be in view at this point but isn't, reset */
  if (Mem->NewSight && Mem->LastSightTime == time &&
      !seen && in_view_range(forget_view_ang, ang_buff, dist_buff) )
    forget();

  if ( heard || heard_moving )
    update_heard(time);

  Object::update();
  heard = heard_moving = FALSE;
}

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

void  MobileObject::update_seen(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? %d %d (%d %d) type %d %c %d",
	       seen_time.t, Mem->LastSightTime.t, seen, seen_moving, type, 
	       ((PlayerObject *)this)->side,((PlayerObject *)this)->unum);
      //}
    return;
  }

  if ( Mem->MyUpdateTime() != time ) my_error("Must have missed a cycle (mobile)"); 
  if ( Mem->LastSightTime != Mem->CurrentTime && Mem->LastSightTime != Mem->CurrentTime-1 ) 
    my_error("Didn't see in the past cycle"); 

  /****** 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 */
    }
    rnpos = Polar2Vector(dist,ang_from_neck);
    rntime = time;
    gpos = Mem->MyPos() + rnpos.rotate(Mem->MyNeckGlobalAng());
    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 */
    /* this way of computing the realtive velocity didn't work all the time
       especially when the ball was approaching you 
    Vector temp_rvel = Vector(distch, dist*Tan(dirch)).rotate(ang_from_neck);
    */
    /* printf("rvel:  (%f %f)\n",rvel.mod(),rvel.dir()); */

    /* Now compute the relative velocity the right way and see if they agree */
    Vector temp2_rvel;
    Vector rpos = Polar2Vector(dist, ang_from_neck);
    rpos = rpos.Normalize();
    temp2_rvel.x = distch * rpos.x - (dirch * M_PI / 180 * dist * rpos.y);
    temp2_rvel.y = distch * rpos.y + (dirch * M_PI / 180 * dist * rpos.x);
#ifdef NOT_NEEDED_NOW	
    /* now check to see if we get different answers */
    if (fabs(temp_rvel.x - temp2_rvel.x) > .01 ||
	fabs(temp_rvel.y - temp2_rvel.y) > .01) {
      //my_error("rpos: (%6.2f, %6.2f)",rpos.x, rpos.y);
      Vector temp_gvel = Mem->MyVel() + temp_rvel.rotate(Mem->MyNeckGlobalAng());
      Vector temp2_gvel = Mem->MyVel() + temp2_rvel.rotate(Mem->MyNeckGlobalAng());
      my_error("Diff vels! old: (%6.2f, %6.2f)   new: (%6.2f, %6.2f)",
	       temp_gvel.x, temp_gvel.y, temp2_gvel.x, temp2_gvel.y);
    }
#endif
    
    //gvel = Mem->MyVel() + temp_rvel.rotate(Mem->MyNeckGlobalAng());
    gvel = Mem->MyVel() + temp2_rvel.rotate(Mem->MyNeckGlobalAng());
    gvtime = time;
    gvconf = max_conf;
  }
}

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

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 || !pos_valid() || 
	 (decayed_hpconf == gconf && get_dist() > hpdist) ){
      /* Update gpos from hpos     */
      gpos  = hpos;
      gtime = hptime;
      gconf = hpconf;
      if (type == OBJ_Ball)
	Mem->LogAction2(200,"Updating the ball's position based on heard info");
    }
  }

  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 ( (!pos_valid() || get_dist() > Mem->SP_feel_distance || !vel_valid()) &&  
	 /* Don't listen to vel if object's near */
	 (decayed_hvconf > gvconf || !pos_valid() ||
	  (decayed_hvconf == gvconf && get_dist() > hvdist)) ){
      /* Update gvel from hvel */
      gvel  = hvel;
      gvtime = hvtime;
      gvconf = hvconf;
      if (type == OBJ_Ball)
	Mem->LogAction2(200,"Updating the ball's velocity based on heard info");
    }
  }

  /* 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::clear_seen()
{
  Object::clear_seen();
  seen_moving = FALSE;
} 

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

void  MobileObject::forget()                                                           
{                                                                                      
  gconf = gvconf = 0;                                                                  
#ifndef NO_ACTION_LOG                                                                  
  if (type == OBJ_Ball) {
    Vector pos = get_rel_to_neck_pos();
    Mem->LogAction6(175,"Forgetting ball: (%.1f, %.1f) (%.1f, %.1f)",
                   pos.x, pos.y, (pos - Vector(3, 0)).x, (pos - Vector(3, 0)).y);
    Mem->LogAction4(175,"Forgetting ball (still): old va: %.0f  new va: %.0f",
		   Mem->MyViewAngle(Mem->LastSightTime),
		   Mem->MyViewAngle(Mem->CurrentTime));
  }
#endif                                                                                 
}


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

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;