memposition.c

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

/* -*- Mode: C -*- */

/* MemPosition.C
 * CMUnited98 (soccer client for Robocup98)
 * Peter Stone <[e-mail]>
 * Computer Science Department
 * Carnegie Mellon University
 * Copyright (C) 1998 Peter Stone
 *
 * CMUnited-98 was created by Peter Stone, Manuela Veloso, and Patrick Riley
 *
 * You may copy and distribute this program freely as long as you retain this notice.
 * If you make any changes or have any comments we would appreciate a message.
 */

/* MemPosition.C stores all the information relating to other objects on the field:
 * other players, the ball, markers, etc.
 */

#include "MemPosition.h"
#include "client.h"


/********************************************************************************/
/********************************************************************************/
/********************************************************************************/
/*                        Object Class                                          */
/********************************************************************************/
/********************************************************************************/
/********************************************************************************/

float Object::get_dist()
{
  if ( pos_valid() && Mem->MyConf() )
    return Mem->DistanceTo(gpos);                             /* From global   */

  /* dump_core("dump"); */
  my_error("Can't get_dist %d",type);
  return 0;

  if ( pdtime < Mem->CurrentTime-1 )                          /* In sight      */
    my_error("Can't get_dist %d",type);

  printf("Using in-sight estimate of dist\n");
  return dist;                                              /* From relative */ 
}

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

AngleDeg Object::get_ang()
{
  if ( pos_valid() || Mem->MyConf() )     
    return Mem->AngleTo(gpos);                                /* From global   */

  my_error("Can't get_ang %d",type);
  return 0;

  if ( patime < Mem->CurrentTime-1 )
    my_error("Can't get_ang %d",type);

  /* In sight */
  printf("Using in-sight estimate of ang\n");
  return ang;                                                 /* From relative */  
}

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

Vector Object::get_rel_pos() /* relative */
{
  if ( rtime != Mem->CurrentTime ){
    rpos = (gpos - Mem->MyPos()).rotate(-Mem->MyAng());
    rtime = Mem->CurrentTime;
  }
  return rpos;
}

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

void Object::set_polar(float d, float a, Time time)
{ 
  dist = d;
  ang  = a;
  pdtime = patime = time;
  seen = TRUE;
  seen_time = time;
}

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

void Object::set_angle(AngleDeg a, Time time)
{
  ang = a;
  patime = time;
  seen = TRUE;
  seen_time = time;
}

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

void Object::set_chinfo(float dist, float dir, Time time)
{
  distch = dist;
  dirch  = dir;
  chtime = time;
  seen_moving = TRUE;
}

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

void Object::update()
{
  seen = seen_moving = FALSE;
}

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

void Object::reset ()
{
  chtime = pdtime = patime = 0;
  seen = FALSE;
  seen_time = -3;
}  

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

void Object::sanitize_times()
{
  Mem->sanitize_time(seen_time);
  Mem->sanitize_time(pdtime);
  Mem->sanitize_time(patime);
  Mem->sanitize_time(chtime);
}  

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

Bool Object::in_view_range(Time time, float angle_buffer, float distance_buffer)
{
  if ( !pos_valid() || !Mem->MyConf() ) return FALSE;
  if ( distance_buffer != 0 ) my_error("Shouldn't have valid distance_buffer at this point");

  Vector tmp_pos = get_rel_pos();
  /* angle_buffer is a distance I should move forward before drawing the angle */
  tmp_pos -= Vector(angle_buffer,0);  

  if ( fabs(tmp_pos.dir()) <= Mem->MyViewAngle(time) ) return TRUE;

  /* if ( fabs(get_ang()) <= Mem->MyViewAngle() - angle_buffer ) return TRUE; */

  return FALSE; 
}

/********************************************************************************/
/********************************************************************************/
/********************************************************************************/
/*                        StationaryObject Class                                */
/********************************************************************************/
/********************************************************************************/
/********************************************************************************/

void StationaryObject::Initialize(MarkerType m, Vector pos, float max, float min_valid, Bool rotate)
{
  gpos = rotate ? -pos : pos;  /* If on the right side of the field, flip all coords */

  max_conf = max;
  min_valid_conf = min_valid;

  gconf = max_conf;

  type = OBJ_Marker;
  object_id = m;
}

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

void StationaryObject::Initialize(SideLine l, Vector pos, float max, float min_valid, Bool rotate)
{
  gpos = rotate ? -pos : pos;  /* If on the right side of the field, flip all coords */

  max_conf = max;
  min_valid_conf = min_valid;

  gconf = max_conf;

  type = OBJ_Line;
  object_id = l;
}

/********************************************************************************/
 
Vector StationaryObject::get_my_pos(AngleDeg my_ang)
{
  if ( type != OBJ_Marker ) my_error("Need to get pos with a marker\n");

  AngleDeg abs_ang = ang + my_ang;
  NormalizeAngleDeg(&abs_ang);
  Vector rpos = Polar2Vector(dist,abs_ang);
  
  return gpos - rpos;
}

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

AngleDeg StationaryObject::get_my_ang(){
  
  if ( type != OBJ_Line ){ my_error("Need to get angle with a line\n"); dump_core("dump"); }

  AngleDeg line_ang = gpos.dir();
  AngleDeg my_ang;

  my_ang = (ang < 0) ? line_ang - (90 + ang) : line_ang + (90 - ang);

  NormalizeAngleDeg(&my_ang);

  return my_ang;
}


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

Vector StationaryObject::get_my_vel(AngleDeg my_ang)
{
  my_error("Shouldn't be estimating velocity from markers -- use sense_body");
  if ( type != OBJ_Marker ) my_error("Need to get vel with a marker\n");

  Vector rvel = Vector(distch, dist*Tan(dirch)).rotate(ang+my_ang);
  return -rvel; /* Assume the object's not moving */
}




/********************************************************************************/
/********************************************************************************/
/********************************************************************************/
/*                        MobileObject Class                                    */
/********************************************************************************/
/********************************************************************************/
/********************************************************************************/

void MobileObject::Initialize(ObjType t, float max, float min_valid, float decay, float motion, float speed)
{
  max_conf = max;
  min_valid_conf = min_valid;
  conf_decay = decay;
  max_speed = speed;
  motion_decay = motion;
  type = t;
  reset();
}

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

AngleDeg MobileObject::get_rel_heading()
{
  float h = get_abs_heading() - Mem->MyAng();
  NormalizeAngleDeg(&h);
  return h;
}

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

Vector MobileObject::get_rel_vel() /* relative */
{
  if ( rvtime != Mem->CurrentTime ){
    /* rvel = (gvel - Mem->MyVel()).rotate(-Mem->MyAng()); */   /* TRUE RELATIVE VEL            */
    rvel = gvel.rotate(-Mem->MyAng());                          /* ABSOLUTE SPEED, RELATIVE DIR */
    rvtime = Mem->CurrentTime;
  }
  return rvel;
}

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

void MobileObject::set_heard_info(float x, float y, float conf, float dist, Time time)
{
  /* Don't overwrite better infor from the same cycle */
  if ( heard && 
       (conf < hpconf || (conf == hpconf && hpdist > dist)) )
    return;

  hpos = Vector(x,y);
  hpconf = conf;
  hpdist = dist;
  hptime = time;
  heard = TRUE;
}

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

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

  /* 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(time-1) )
    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(time) )
    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",
	       seen_time.t, seen_time.s, Mem->LastSightTime.t, Mem->LastSightTime.s);
    }
    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");