visualsensor.cc

在LINUX下运行的仿真机器人服务器源代码

		}

  for ( std::list < VisualSensor_v4::ball_high_data_t >::const_iterator i = data.M_balls.begin ();
        i != data.M_balls.end (); ++i )
		{
      toStr ( o << " ", *i ); 
		}

  for ( std::list < VisualSensor_v4::player_high_data_t >::const_iterator i = data.M_players.begin ();
        i != data.M_players.end (); ++i )
		{
      toStr ( o << " ", *i ); 
		}

  return o << ")";
}


int VisualSensor_v5::obj_head_dir_data_t::calcHeadDir ( const Player& observed_player, 
																												const Player& player )
{
  return Rad2IDeg ( normalize_angle ( observed_player.angle_neck_committed 
                                      + observed_player.angle
                                      - player.angle 
                                      - player.angle_neck_committed ) );
}

void VisualSensor_v5::player_high_data_t::set ( const Player& observed_player, 
																								const Player& player )
{
  double ang = VisualSensor_v5::obj_dir_data_t::calcRadDir ( observed_player,
                                                             player );
				
  double qstep;
#ifndef	NEW_QSTEP
  qstep = ServerParam::instance().dist_qstep;
#else
  qstep = player.dist_qstep_player;
#endif					
  double un_quant_dist = VisualSensor_v5::obj_dist_data_t::calcUnQuantDist ( observed_player, 
                                                                             player );
  double quant_dist = VisualSensor_v5::obj_dist_data_t::calcQuantDist ( un_quant_dist, 
                                                                        qstep );
				
  if ( fabs ( ang ) < player.vis_angle / 2.0 )
		{
      double prob = ( quant_dist - TEAM_FAR_LENGTH ) 
        / ( TEAM_TOOFAR_LENGTH - TEAM_FAR_LENGTH );
						
      if ( prob > drand ( 0, 1 ) ) 
				{
          M_tfar_data.M_direction = VisualSensor_v5::obj_dir_data_t::calcDegDir ( ang );
          M_tfar_data.M_distance = quant_dist;
          M_tfar_data.M_name = VisualSensor_v5::obj_name_data_t::calcTFarName ( observed_player );
          M_range = VisualSensor_v5::TFAR;	
				}
      else
				{
          prob = ( quant_dist - UNUM_FAR_LENGTH ) 
            / ( UNUM_TOOFAR_LENGTH - UNUM_FAR_LENGTH );
								
          if ( prob > drand ( 0, 1 ) ) 
						{
              M_ufar_data.M_direction = VisualSensor_v5::obj_dir_data_t::calcDegDir ( ang );
              M_ufar_data.M_distance = quant_dist;
              M_ufar_data.M_name = VisualSensor_v5::obj_name_data_t::calcUFarName ( observed_player );
              M_range = VisualSensor_v5::UFAR;	
						}
          else
						{
              M_normal_data.M_direction = VisualSensor_v5::obj_dir_data_t::calcDegDir ( ang );
              M_normal_data.M_distance = quant_dist;
              M_normal_data.M_vel = VisualSensor_v5::obj_vel_data_t::calcVel ( observed_player, 
                                                                               player, 
                                                                               un_quant_dist,
                                                                               quant_dist );
              M_normal_data.M_name = VisualSensor_v5::obj_name_data_t::calcName ( observed_player );
              M_normal_data.M_body_dir = VisualSensor_v5::obj_body_dir_data_t::calcBodyDir ( observed_player,
                                                                                             player );
              M_normal_data.M_head_dir = VisualSensor_v5::obj_head_dir_data_t::calcHeadDir ( observed_player,
                                                                                             player );
              M_range = VisualSensor_v5::NORMAL;
						}
				}
		}
  else if ( un_quant_dist <= player.vis_distance )
		{
      M_close_data.M_direction = VisualSensor_v5::obj_dir_data_t::calcDegDir ( ang );
      M_close_data.M_distance = quant_dist;
      M_range = VisualSensor_v5::CLOSE;
		}
  else
    M_range = VisualSensor_v5::OUT;
}

void VisualSensor_v5::high_data_t::set ( const Stadium& stadium,
																				 const Player& player )
{
  M_time = stadium.time;

  for( int j = 0 ; j < stadium.votable.n ; j++ )
		{
      switch ( stadium.votable.object [ j ]->getObjectType () )
				{
        case MPObject::OT_GOAL:
          if ( stadium.votable.object [ j ]->obj_ver <= player.version )
            {
              VisualSensor_v5::goal_high_data_t data;
              data.set ( *( stadium.votable.object [ j ] ), player );
										
              if ( data.M_range != VisualSensor_v5::OUT )
                M_goals.push_back ( data );
            }	
          break;

        case MPObject::OT_FLAG:
          if ( stadium.votable.object [ j ]->obj_ver <= player.version )
            {
              VisualSensor_v5::flag_high_data_t data;
              data.set ( *( stadium.votable.object [ j ] ), player );
										
              if ( data.M_range != VisualSensor_v5::OUT )
                M_flags.push_back ( data );
            }	
          break;

        default:
          break;
				}
		}

  for( int j = 0 ; j < stadium.motable.n ; j++ )
		{
      switch ( stadium.motable.object [ j ]->getObjectType () )
				{
        case MPObject::OT_BALL:
          if ( stadium.motable.object [ j ]->obj_ver <= player.version )
            {
              VisualSensor_v5::ball_high_data_t data;
              data.set ( *( stadium.motable.object [ j ] ), player );

              if ( data.M_range != VisualSensor_v5::OUT )
                M_balls.push_back ( data );
            }
          break;
												
        default:
          break;
				}
		}

  for ( int j = 0 ; j < MAX_PLAYER * 2; j++ ) 
		{
      if ( stadium.player[ j ] != &player
           && stadium.player[ j ]->alive != DISABLE 
           && stadium.player [ j ]->obj_ver <= player.version )
				{
          VisualSensor_v5::player_high_data_t data;
          data.set ( *( stadium.player [ j ] ), player );
				
          if ( data.M_range != VisualSensor_v5::OUT )
						M_players.push_back ( data );
				}
		}
}

std::ostream& toStr ( std::ostream& o, const VisualSensor_v5::player_high_normal_data_t& data )
{
  return o << "(" << data.M_name
           << " " << data.M_distance
           << " " << data.M_direction 
           << " " << data.M_vel.M_dist_chg
           << " " << data.M_vel.M_dir_chg 
           << " " << data.M_body_dir 
           << " " << data.M_head_dir 
           << ")";		
}

std::ostream& toStr ( std::ostream& o, const VisualSensor_v5::player_high_data_t& data )
{
  switch ( data.M_range )
		{
    case VisualSensor_v5::CLOSE:
      return toStr ( o, data.M_close_data );
    case VisualSensor_v5::NORMAL:
      return toStr ( o, data.M_normal_data );
    case VisualSensor_v5::UFAR:
      return toStr ( o, data.M_ufar_data );
    case VisualSensor_v5::TFAR:
      return toStr ( o, data.M_tfar_data );
    default:
      return o;
		};
}

std::ostream& toStr ( std::ostream& o, const VisualSensor_v5::high_data_t& data )
{
  o << "(see " << data.M_time;

  for ( std::list < VisualSensor_v5::goal_high_data_t >::const_iterator i = data.M_goals.begin ();
        i != data.M_goals.end (); ++i )
		{
      toStr ( o << " ", *i ); 
		}

  for ( std::list < VisualSensor_v5::flag_high_data_t >::const_iterator i = data.M_flags.begin ();
        i != data.M_flags.end (); ++i )
		{
      toStr ( o << " ", *i ); 
		}

  for ( std::list < VisualSensor_v5::ball_high_data_t >::const_iterator i = data.M_balls.begin ();
        i != data.M_balls.end (); ++i )
		{
      toStr ( o << " ", *i ); 
		}

  for ( std::list < VisualSensor_v5::player_high_data_t >::const_iterator i = data.M_players.begin ();
        i != data.M_players.end (); ++i )
		{
      toStr ( o << " ", *i ); 
		}

  return o << ")";
}



const char* VisualSensor_v6::obj_name_data_t::calcName ( const PObject& obj )
{
  return obj.short_name;
}

const char* VisualSensor_v6::obj_name_data_t::calcUFarName ( const Player& obj )
{
  return obj.short_name_far;
}

const char* VisualSensor_v6::obj_name_data_t::calcTFarName ( const Player& obj )
{
  return obj.short_name_toofar;
}


void VisualSensor_v6::ball_low_data_t::set ( const MPObject& ball, 
																						 const Player& player )
{
  double ang = VisualSensor_v6::obj_dir_data_t::calcRadDir ( ball, player );
				
  if ( fabs ( ang ) < player.vis_angle / 2.0 )
		{
      M_normal_data.M_direction = VisualSensor_v6::obj_dir_data_t::calcDegDir ( ang );
      M_range = VisualSensor_v6::NORMAL;
		}
  else if ( player.pos.distance ( ball.pos ) <= player.vis_distance )
		{
      M_close_data.M_direction = VisualSensor_v6::obj_dir_data_t::calcDegDir ( ang );
      M_range = VisualSensor_v6::CLOSE;
		}
  else
    M_range = VisualSensor_v6::OUT;
}


void VisualSensor_v6::player_low_data_t::set ( const Player& observed_player, 
																							 const Player& player )
{
  double ang = VisualSensor_v6::obj_dir_data_t::calcRadDir ( observed_player, player );
		
  double un_quant_dist = VisualSensor_v6::obj_dist_data_t::calcUnQuantDist ( observed_player, 
                                                                             player );

  if ( fabs ( ang ) < player.vis_angle / 2.0 )
		{
      double qstep;
#ifndef	NEW_QSTEP
      qstep = ServerParam::instance().dist_qstep;
#else
      qstep = player.dist_qstep_player;
#endif					
      double quant_dist = VisualSensor_v6::obj_dist_data_t::calcQuantDist ( un_quant_dist, 
                                                                            qstep );
						
      double prob = ( quant_dist - TEAM_FAR_LENGTH ) 
        / ( TEAM_TOOFAR_LENGTH - TEAM_FAR_LENGTH );
						
      if ( prob > drand ( 0,1 ) ) 
				{
          M_tfar_data.M_direction = VisualSensor_v6::obj_dir_data_t::calcDegDir ( ang );
          M_tfar_data.M_name = VisualSensor_v6::obj_name_data_t::calcTFarName ( observed_player );
          M_range = VisualSensor_v6::TFAR;
				}
      else
				{
          prob = ( quant_dist - UNUM_FAR_LENGTH ) 
            / ( UNUM_TOOFAR_LENGTH - UNUM_FAR_LENGTH );
								
          if ( prob > drand ( 0,1 ) ) 
						{
              M_ufar_data.M_direction = VisualSensor_v6::obj_dir_data_t::calcDegDir ( ang );
              M_ufar_data.M_name = VisualSensor_v6::obj_name_data_t::calcUFarName ( observed_player );
              M_range = VisualSensor_v6::UFAR;
						}
          else
						{
              M_normal_data.M_direction = VisualSensor_v6::obj_dir_data_t::calcDegDir ( ang );
              M_normal_data.M_name = VisualSensor_v6::obj_name_data_t::calcName ( observed_player );
              M_range = VisualSensor_v6::NORMAL;
						}
				}
		}
  else if ( un_quant_dist <= player.vis_distance )
		{
      M_close_data.M_direction = VisualSensor_v6::obj_dir_data_t::calcDegDir ( ang );
      M_range = VisualSensor_v6::CLOSE;
		}
  else
    M_range = VisualSensor_v6::OUT;
}

void VisualSensor_v6::flag_low_data_t::set ( const PObject& flag, 
																						 const Player& player )
{
  double ang = VisualSensor_v6::obj_dir_data_t::calcRadDir ( flag, player );
				
  if ( fabs ( ang ) < player.vis_angle / 2.0 )
		{
      M_normal_data.M_direction = VisualSensor_v6::obj_dir_data_t::calcDegDir ( ang );
      M_normal_data.M_name = VisualSensor_v6::obj_name_data_t::calcName ( flag );
      M_range = VisualSensor_v6::NORMAL;
		}
  else if ( player.pos.distance ( flag.pos ) <= player.vis_distance )
		{
      M_close_data.M_direction = VisualSensor_v6::obj_dir_data_t::calcDegDir ( ang );
      M_range = VisualSensor_v6::CLOSE;
		}
  else
    M_range = VisualSensor_v6::OUT;
}				

void VisualSensor_v6::goal_low_data_t::set ( const PObject& goal, 
																						 const Player& player )
{
  double ang = VisualSensor_v6::obj_dir_data_t::calcRadDir ( goal, player );

  if ( fabs ( ang ) < player.vis_angle / 2.0 )
		{
      M_normal_data.M_direction = VisualSensor_v6::obj_dir_data_t::calcDegDir ( ang );
      M_normal_data.M_name = VisualSensor_v6::obj_name_data_t::calcName ( goal );
      M_range = VisualSensor_v6::NORMAL;
		}
  else if ( player.pos.distance ( goal.pos ) <= player.vis_distance )
		{
      M_close_data.M_direction = VisualSensor_v6::obj_dir_data_t::calcDegDir ( ang );
      M_range = VisualSensor_v6::CLOSE;
		}
  else
    M_range = VisualSensor_v6::OUT;
}

void VisualSensor_v6::line_low_data_t::set ( const PObject& line, 
																						 const Player& player )
{
  double line_normal;		// the angle of an outward pointing
  // normal ( 90degs to ) to the line

  double player_2_line;		// perp distance of the player from
  // the line

  double line_start; // the x of y value of where the line starts
		
  double line_stop; // the x or y value of where the line stops
		
  bool vert; // a flag to specify if the line is vertical or
  // horizontal

  // be very carefull here.  The lines pos.x is actually it's distance
  // from the center of the field, not neccesarily it's x position.
  if ( line.pos.x == -PITCH_LENGTH/2.0 ) // left
    {
      line_normal = PI;
      if ( player.pos.x < line.pos.x )
        line_normal = 0.0;
      player_2_line = line.pos.x - player.pos.x;
      line_start = -PITCH_WIDTH/2.0;
      line_stop = PITCH_WIDTH/2.0;
      vert = true;
    }
  else if ( line.pos.x == PITCH_LENGTH/2.0 ) // right
    {
      line_normal = 0.0;
      if ( player.pos.x > line.pos.x )
        line_normal = PI;
      player_2_line = line.pos.x - player.pos.x;
      line_start = -PITCH_WIDTH/2.0;
      line_stop = PITCH_WIDTH/2.0;
      vert = true;
    }
  else if ( line.