memposition.c

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

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

/* MemPosition.C
 * CMUnited99 (soccer client for Robocup99)
 * Peter Stone <[e-mail]>
 * Computer Science Department
 * Carnegie Mellon University
 * Copyright (C) 1999 Peter Stone
 *
 * CMUnited-99 was created by Peter Stone, Patrick Riley, and Manuela Veloso
 *
 * 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.
 * For more information, please see http://www.cs.cmu.edu/~robosoccer/
 */

 
#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_from_body()
{
  if ( pos_valid() || Mem->MyConf() )     
    return Mem->AngleToFromBody(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_neck + Mem->MyNeckRelAng();                 /* From relative */  
}

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

AngleDeg Object::get_ang_from_neck()
{
  if ( pos_valid() || Mem->MyConf() )     
    return Mem->AngleToFromNeck(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_neck;                                       /* From relative */  
}

/********************************************************************************/
/* we have some problems keeping these consitent I think,
   (especially during the update process, which has the positions in a state)
   that is nto equal to the current time 
   therefore, we'll just recompute them all the time */

Vector Object::get_rel_to_body_pos() /* relative */
{
  return (gpos - Mem->MyPos()).rotate(-Mem->MyBodyAng());
  /* see the somment above
  if ( rbtime != Mem->CurrentTime ){
    rbpos = (gpos - Mem->MyPos()).rotate(-Mem->MyBodyAng());
    rbtime = Mem->CurrentTime;
  }
  return rbpos;
  */
}

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

Vector Object::get_rel_to_neck_pos() /* relative */
{
  return (gpos - Mem->MyPos()).rotate(-Mem->MyNeckGlobalAng());
  /* see the somment above
  if ( rntime != Mem->CurrentTime ){
    rnpos = (gpos - Mem->MyPos()).rotate(-Mem->MyNeckGlobalAng());
    rntime = Mem->CurrentTime;
  }
  return rnpos;
  */
}

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

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

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

void Object::set_angle_from_neck(AngleDeg a, Time time)
{
  ang_from_neck = 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::clear_seen ()
{
  seen = FALSE;
}  

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

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(AngleDeg view_ang, 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_to_neck_pos();
  /* angle_buffer is the maximum distance that an object should be off to be forgotten
     We slide the object back before taking the angle in order to accomplish this */
  float slide_dist = angle_buffer / Sin(view_ang);
  //tmp_pos -= Vector(angle_buffer,0);  
  tmp_pos -= Vector(slide_dist,0);  

  if ( fabs(tmp_pos.dir()) <= view_ang ) {
    Mem->LogAction7(200, "Object::in_view_range, tmp_pos: (%.2f, %.2f) %.2f %.2f %.2f",
		    tmp_pos.x, tmp_pos.y, tmp_pos.dir(), view_ang, slide_dist);
    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_neck_global_ang)
{
  if ( type != OBJ_Marker ) my_error("Need to get pos with a marker\n");

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

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

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

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

  my_neck_ang = (ang_from_neck < 0) ? line_ang - (90 + ang_from_neck) : line_ang + (90 - ang_from_neck);

  NormalizeAngleDeg(&my_neck_ang);

  return my_neck_ang;
}


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

Vector StationaryObject::get_my_vel(AngleDeg my_neck_global_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_from_neck+my_neck_global_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_to_body_heading()
{
  float h = get_abs_heading() - Mem->MyBodyAng();
  NormalizeAngleDeg(&h);
  return h;
}

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

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

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

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

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

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

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

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