position.h

足球机器人仿真组CMU97的源码

/* position.h
 * CMUnited-97 (soccer client for Robocup-97)
 * Peter Stone <[e-mail]>
 * Computer Science Department
 * Carnegie Mellon University
 * Copyright (C) 1997 Peter Stone
 *
 * CMUnited-97 was created by Peter Stone 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.
 */

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

#ifndef _POSITION_H
#define _POSITION_H

#include <math.h>
#include "peterclient.h"
#include "utils.h"

#define TEAM_SIZE 11

#define NUM_EDGE_MARKERS 8
#define NUM_INTERNAL_MARKERS 6
#define NUM_MARKERS (NUM_EDGE_MARKERS+NUM_INTERNAL_MARKERS)

#define GOAL_R   0
#define GOAL_L  (GOAL_R +1)
#define FLAG_RB (GOAL_L +1)
#define FLAG_B  (FLAG_RB+1)
#define FLAG_LB (FLAG_B +1)
#define FLAG_LT (FLAG_LB+1)
#define FLAG_T  (FLAG_LT+1)
#define FLAG_RT (FLAG_T +1)

#define FLAG_PRT (FLAG_RT +1)
#define FLAG_PRC (FLAG_PRT+1)
#define FLAG_PRB (FLAG_PRC+1)
#define FLAG_PLT (FLAG_PRB+1)
#define FLAG_PLC (FLAG_PLT+1)
#define FLAG_PLB (FLAG_PLC+1)

#define MY_GOAL     NUM_MARKERS
#define THEIR_GOAL (MY_GOAL   +1)
#define LB_FLAG    (THEIR_GOAL+1)
#define LC_FLAG    (LB_FLAG   +1)
#define LF_FLAG    (LC_FLAG   +1)
#define RB_FLAG    (LF_FLAG   +1)
#define RC_FLAG    (RB_FLAG   +1)
#define RF_FLAG    (RC_FLAG   +1)

#define MY_PL_FLAG    (RF_FLAG      +1)
#define MY_PC_FLAG    (MY_PL_FLAG   +1)
#define MY_PR_FLAG    (MY_PC_FLAG   +1)
#define THEIR_PL_FLAG (MY_PR_FLAG   +1)  // Left as I face THEIR goal
#define THEIR_PC_FLAG (THEIR_PL_FLAG+1)
#define THEIR_PR_FLAG (THEIR_PC_FLAG+1)

#define NO_LINE -1
#define LINE_R  GOAL_R
#define LINE_L  GOAL_L
#define LINE_B  FLAG_B
#define LINE_T  FLAG_T

#define MY_LINE    MY_GOAL
#define THEIR_LINE THEIR_GOAL
#define L_LINE     LC_FLAG
#define R_LINE     RC_FLAG

#define NUM_OBJECTS (NUM_MARKERS + (TEAM_SIZE+1)*2 + 1)

/* Vision widths and qualities */
#define SAME_WIDTH     0
#define NARROW_WIDTH  (SAME_WIDTH+1)
#define NORMAL_WIDTH  (NARROW_WIDTH+1)
#define WIDE_WIDTH    (NORMAL_WIDTH+1)

#define SAME_QUALITY  0
#define LOW_QUALITY  (SAME_QUALITY+1)
#define HIGH_QUALITY (LOW_QUALITY+1)

#define X0    52.5       // Dimensions of the field:  105 x 68 
#define Y0    34
#define PA_X (X0-16.5)   // Penalty area is 40.32 x 16.5
#define PA_Y (20.16)
#define GA_X (X0-5.5)    // goal area is 5.5 x 18.32
#define GA_Y (9.16)

#define FREE_KICK_BUFFER 9.15

#define PLAYER_SIZE 0.8
#define BALL_SIZE 0.085
#define KICK_DISTANCE (1 + PLAYER_SIZE + BALL_SIZE)
#define FEEL_DISTANCE 3
#define GOAL_WIDTH 14
#define DEFAULT_VIEW_ANGLE 90
#define STAMINA_MAX 2000
#define STAMINA_INC 20
#define GAME_LENGTH 6000

#define TRUE 1
#define FALSE 0

// Parameters
#define MINIMUM_OVERALL_CONFIDENCE 0.5
#define VALID_THRESHOLD            0.4
#define HEARSAY_CONFIDENCE         0.99

#define DECAY_S_ESTIMATE          0.9
#if SAVE_PASS_DATA
#define DECAY_M_ESTIMATE          0.99
#define DECAY_M_TICK               0.99
#else
#define DECAY_M_ESTIMATE          0.92    /* was .9, .96 */
#define DECAY_M_TICK               0.97  /* per time step */
#endif

// Geometry Functions
inline float rad_to_deg(float x)
{ return x * 180 / M_PI; }

inline float deg_to_rad(float x)
{ return x * M_PI / 180; }

// The coordinate system here gets a little tricky.  We work in two
//  coordinate systems, a polar and a cartesian.  Unfortunately
//  the angle in the polar coordinate system is with respect to
//  the y-axis in the cartesian coordinate system.  So, there
//  are two sets of conversion functions.  Ones between polar
//  and xy, the normal conversion, and the ones between polar
//  and relative xy, which reverses x and y.
inline void xy_to_polar(float x, float y, float *r, float *t)
{ *r = sqrt((x*x) + (y*y)); *t = (x==0 && y==0) ? 0 : atan2(y, x); }
inline void relxy_to_polar(float x, float y, float *r, float *t)
{ *r = sqrt((x*x) + (y*y)); *t = (x==0 && y==0) ? 0 : atan2(x, y); }

inline float polar_to_y(float r, float t)
{ return r * sin(t); }
inline float polar_to_x(float r, float t)
{ return r * cos(t); }
inline void polar_to_xy(float r, float t, float *x, float *y)
{ *x = polar_to_x(r,t); *y = polar_to_y(r,t); }

inline float polar_to_rely(float r, float t)
{ return r * cos(t); }
inline float polar_to_relx(float r, float t)
{ return r * sin(t); }
inline void polar_to_relxy(float r, float t, float *x, float *y)
{ *x = polar_to_relx(r,t); *y = polar_to_rely(r,t); }

inline float normalize(float a)
{ return a - ((2*M_PI) * (int)(a / M_PI)); }

inline int InPitch(float x, float y) { return ( fabs(x) < X0+10 && fabs(y) < Y0+10 );}


class Position {
public:
  Position();

  // Methods to get the position
  inline float get_t() { return cur_t; }
  inline float get_tDEG() { return rad_to_deg(cur_t); }
  inline float get_r() { return cur_r; }
  inline float get_x() { return polar_to_relx(cur_r, cur_t); }
  inline float get_y() { return polar_to_rely(cur_r, cur_t); }
  inline float get_rconf() { return ( cur_rconf > VALID_THRESHOLD ) ? 
			       cur_rconf : 0; }
  inline float get_tconf() { return ( cur_tconf > VALID_THRESHOLD ) ?
			       cur_tconf : 0; }
  inline float get_conf() { return (get_rconf() < get_tconf()) ?
			      get_rconf() : get_tconf(); }

  inline float get_new_r() { return new_r; }
  inline float get_new_t() { return new_t; }
  inline float get_new_tDEG() { return rad_to_deg(new_t); }
  inline float get_new_x() { return polar_to_relx(new_r, new_t); }
  inline float get_new_y() { return polar_to_rely(new_r, new_t); }
  inline float get_new_rconf() { return new_rconf; }
  inline float get_new_tconf() { return new_tconf; }
  inline float get_new_conf()  { return (new_rconf < new_tconf) ?
				   new_rconf : new_tconf; }

  // Methods to change the position
  // Changes do not take place until tick() is called.
  virtual float set_polar(float r, float t, float c);
  virtual float set_xy(float x, float y, float c);
  virtual float set_r(float r, float c);
  virtual float set_t(float t, float c);
  virtual float set_t_gospel(float t, float c);

  virtual float estimate_degrade(float c);
  virtual float translate(float dr, float dt, float c);

  // Method which processes all changes
  virtual void tick();
  virtual void reset();
  
  // Was the new_* position set via a set_* call.  If so, we accept
  //  it as gospel and rotate/translate calls cannot override it.
  int gospel;
  
protected:
  float cur_r, cur_t;  // relative polar coordinates
  float cur_rconf, cur_tconf;

  float new_r, new_t;
  float new_rconf, new_tconf;
};

class PositionStationary : public Position {
public:
  virtual float estimate_degrade();
  virtual float translate(float dr, float dt, float c);

  virtual void tick();

  void get_translate(float *dr, float *dt, float *c);
};

class PositionMobile : public Position {
public:
  PositionMobile();
  ~PositionMobile();

  virtual float estimate_degrade();
  virtual float translate(float dr, float dt, float c);
  virtual void tick(int time_steps);
  virtual void reset();

  float get_rel_vel_r(int dt); /* computed from relative ball positions */
  float get_rel_vel_t();
  float get_rel_vel_tDEG();

  float get_traj_mag();        /* computed from global ball positions   */
  float get_traj_dir();
  float get_traj_conf(int time, int dt);

  inline float get_old_r() { return old_r; }
  inline float get_old_t() { return old_t; }
  inline float get_old_x() { return polar_to_relx(old_r, old_t); }
  inline float get_old_y() { return polar_to_rely(old_r, old_t); }
  inline float get_old_conf() { return (old_rconf < old_tconf) ?
			      old_rconf : old_tconf; }

  inline float get_vel_dir() { return cur_vel_dir; }
  virtual void set_vel_dir(float );

  inline float get_vel_mag() { return cur_vel_mag; }
  virtual void set_vel_mag(float );

  virtual void set_vel(float, float);

  void clear_globalxy();
  void update_globalxy(float x, float y, int t); /* Not from external source */
  void set_globalxy(float x, float y, int t);
  inline void get_globalxy(float *x, float *y)
    {*x = global_x; *y = global_y;}

  inline int get_global_set_time() {return global_set_time;}
  int velocity_valid();

  int got_global_info;
  char side;
  int num;
  
private:
  float global_x, global_y;
  float old_global_x, old_global_y;
  PointQueue *points;

  int   global_set_time, old_global_set_time;  /* Times at which global values
						  were set */
  float old_r, old_t;            // Used to calculate relative velocity
  float old_rconf, old_tconf;

  float cur_vel_dir;
  float new_vel_dir;
  float cur_vel_mag;
  float new_vel_mag;