memposition.h

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

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

/* MemPosition.h
 * 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/
 */


#ifndef _POSITION_H_
#define _POSITION_H_

#include "MemPlayer.h"



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

class Object {
public:

  /* Methods to get the position  */
  float    get_dist();  /* relative */
  AngleDeg get_ang_from_body();
  AngleDeg get_ang_from_neck();

  inline Vector get_abs_pos()
    { if (!pos_valid()) my_error ("can't get_abs_pos %d",type);  return gpos; }
  Vector get_rel_to_body_pos();
  Vector get_rel_to_neck_pos();
  inline float get_x() { return get_abs_pos().x; }
  inline float get_y() { return get_abs_pos().y; }
  inline float pos_valid() { return ( (gconf >= min_valid_conf) ? gconf : 0 ); }
  
  /* Methods to change the position                      */
  /* Changes do NOT take effect until update() is called */
  virtual void set_polar_from_neck(float d, float a, Time time);
  virtual void set_angle_from_neck(AngleDeg a, Time time);
  virtual void set_chinfo(float dist, float dir, Time time);

  /* Method which processes all changes */
  virtual void update();  
  virtual void reset ();  /* 0 confidences */
  virtual void clear_seen();
  virtual void sanitize_times();
  
  ObjType type;
 
  inline Time GetSeenTime() { return seen_time; }
  inline Time GetSeenMovingTime() { return chtime; }
 
protected:

  float max_conf;
  float min_valid_conf;

  Bool    seen;
  Bool    seen_moving;
  Time    seen_time;

  /* Based on the object's current position, should it be seen? */
  virtual Bool in_view_range(AngleDeg view_ang, float angle_buffer, float distance_buffer);
  
  float    dist;  /* polar */
  AngleDeg ang_from_neck;
  Time     pdtime;
  Time     patime;

  inline Time ptime() { return Min(pdtime,patime); }

  float  distch;  /* polar */
  float  dirch;
  Time   chtime;

  Vector rbpos;   /* relative to body */
  Time   rbtime;
  Vector rnpos;   /* relative to neck */
  Time   rntime;
  
  Vector gpos;    /* global   */
  Time   gtime;
  float  gconf;
};



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

class StationaryObject : public Object {
public:

  void Initialize(MarkerType m, Vector pos, float max, float min_valid, Bool rotate);
  void Initialize(SideLine  l, Vector pos, float max, float min_valid, Bool rotate);

  Vector get_my_pos(AngleDeg my_neck_global_ang);
  AngleDeg get_my_neck_global_ang();
  Vector get_my_vel(AngleDeg my_neck_global_ang);

  int object_id;
};



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

class MobileObject : public Object {
public:

  virtual void Initialize(ObjType t, float max, float min_valid, float decay, float motion, float speed);

  AngleDeg get_rel_to_body_heading();
  AngleDeg get_rel_to_neck_heading();
  Vector get_rel_to_body_vel(); /* relative direction, absolute speed */
  Vector get_rel_to_neck_vel(); /* relative direction, absolute speed */

  /* Methods to get the velocity */
  inline Vector get_abs_vel()  /* global   */
    { if (!vel_valid()) my_error ("can't get_abs_vel %d",type); return gvel; }

  inline float    get_speed()       { return get_abs_vel().mod(); }
  inline AngleDeg get_abs_heading() { return get_abs_vel().dir(); }     
  inline float    vel_valid()       { return ( (gvconf >= min_valid_conf) ? gvconf : 0 ); }
  virtual inline Bool moving()      { return vel_valid() && get_speed() != 0 ? TRUE : FALSE; } 
      
    /* Methods to change the position                      */
  /* Changes do NOT take effect until update() is called */
  virtual void set_heard_info (float x, float y, float conf, float dist, Time time);
  virtual void set_heard_info (float x, float y, float pconf, float dx, float dy, float vconf, 
			       float dist, Time time);

  /* Method which processes all changes */
  virtual void  update_kick(Time time) = 0; /* pure virtual function */
  virtual void  estimate_pos(Time time);
  void          estimate_vel(Time time);
  virtual void  update(Time time);
  virtual void  update_seen(Time time);
  void          update_estimate(Time time);
  void          update_heard(Time time);
  virtual void  reset();
  virtual void  clear_seen();
  virtual void  forget();
  virtual void  sanitize_times();

  Vector        estimate_future_pos(int steps, Vector extra_vel=0.0, Vector extra_vel_per=0.0);

protected:

  float conf_decay;
  float motion_decay;

  float max_speed;

  Vector gvel;    /* global   */

  Time   gvtime;
  float  gvconf;

private:

  Bool heard;
  Bool heard_moving;

  Vector rbvel;    /* body -- relative direction, absolute speed */
  Time   rbvtime;
  Vector rnvel;    /* neck -- relative direction, absolute speed */
  Time   rnvtime;

  Vector hpos;    /* heard */
  Vector hvel;    
  Time   hptime;
  Time   hvtime;
  float  hpdist;
  float  hvdist;
  float  hpconf;
  float  hvconf;
};



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

class BallObject : public MobileObject {
public:
  void Initialize(float max, float min_valid, float decay, float motion, float max_speed);

  Bool moving();
  Bool kickable(float buffer = 0.0);
  Bool catchable();
  float get_kick_rate(Time time);

  void update_kick(Time time);
  void estimate_pos(Time time);
  void update(Time time);
  void update_seen(Time time);
        
  float calc_kick_rate(float dist, float ang);
  float calc_kick_rate() {return calc_kick_rate(get_dist(), get_ang_from_body());}

  void set_past_kick(float pow, AngleDeg ang, Time t);
  void forget_past_kick(Time t);

 protected:
  Bool  in_view_range(AngleDeg view_ang, float angle_buffer, float distance_buffer);
  
private:
  float effective_kick_rate;
  Time  ektime;

  Bool use_pos_based_vel_estimate;
  Time pos_based_vel_time;
  
  Time   last_seen_time;
  Vector last_seen_pos;

  /* we keep a record of the most recent kicks we try to do so that
     we can estimate the velocity of the ball from subsequent positions */
#define num_past_kicks (7)
  int past_kick_idx;
  struct PastKick {
    Time time;
    Vector kick_effect;
  } past_kicks[num_past_kicks];

  int past_kick_inc(int i)
    { return (i+1) % num_past_kicks; }
  int past_kick_dec(int i)
    { return ((i-1)+num_past_kicks) % num_past_kicks; }
};


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

class PlayerObject : public MobileObject {
public:
  void Initialize(float max, float min_valid, float decay, float motion, float max_speed);

  char side;
  Unum unum;