visualsensor.cc

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

      vel.M_dir_chg = 0.0;
      vel.M_dist_chg = 0.0;
		}

  return vel;
}		


void VisualSensor_v1::ball_high_data_t::set ( const MPObject& ball, 
																							const Player& player )
{
  double ang = VisualSensor_v1::obj_dir_data_t::calcRadDir ( ball, player );
		
  double qstep;
#ifndef	NEW_QSTEP
  qstep = ServerParam::instance().dist_qstep;
#else
  qstep = player.dist_qstep_player;
#endif					
  double un_quant_dist = VisualSensor_v1::obj_dist_data_t::calcUnQuantDist ( ball, 
                                                                             player );
  double quant_dist = VisualSensor_v1::obj_dist_data_t::calcQuantDist ( un_quant_dist, 
                                                                        qstep );

  if ( fabs ( ang ) < player.vis_angle / 2.0 )
		{
      double prob;
						
      prob = ( quant_dist - UNUM_FAR_LENGTH ) 
        / ( UNUM_TOOFAR_LENGTH - UNUM_FAR_LENGTH );
						
      if ( prob > drand ( 0, 1 ) ) 
				{
          M_ufar_data.M_direction = VisualSensor_v1::obj_dir_data_t::calcDegDir ( ang );
          M_ufar_data.M_distance = quant_dist;
          M_range = VisualSensor_v1::UFAR;	
				}
      else
				{
          M_normal_data.M_direction = VisualSensor_v1::obj_dir_data_t::calcDegDir ( ang );
          M_normal_data.M_distance = quant_dist;
          M_normal_data.M_vel = VisualSensor_v1::obj_vel_data_t::calcVel ( ball, 
                                                                           player, 
                                                                           un_quant_dist,
                                                                           quant_dist );
          M_range = VisualSensor_v1::NORMAL;
				}
		}
  else if ( un_quant_dist <= player.vis_distance )
		{
      M_close_data.M_direction = VisualSensor_v1::obj_dir_data_t::calcDegDir ( ang );
      M_close_data.M_distance = quant_dist;
      M_range = VisualSensor_v1::CLOSE;
		}
  else
    M_range = VisualSensor_v1::OUT;
}

void VisualSensor_v1::player_high_data_t::set ( const Player& observed_player, 
																								const Player& player )
{
  double ang = VisualSensor_v1::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_v1::obj_dist_data_t::calcUnQuantDist ( observed_player, 
                                                                             player );
  double quant_dist = VisualSensor_v1::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_v1::obj_dir_data_t::calcDegDir ( ang );
          M_tfar_data.M_distance = quant_dist;
          M_tfar_data.M_name = VisualSensor_v1::obj_name_data_t::calcTFarName ( observed_player );
          M_range = VisualSensor_v1::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_v1::obj_dir_data_t::calcDegDir ( ang );
              M_ufar_data.M_distance = quant_dist;
              M_ufar_data.M_name = VisualSensor_v1::obj_name_data_t::calcUFarName ( observed_player );
              M_range = VisualSensor_v1::UFAR;	
						}
          else
						{
              M_normal_data.M_direction = VisualSensor_v1::obj_dir_data_t::calcDegDir ( ang );
              M_normal_data.M_distance = quant_dist;
              M_normal_data.M_vel = VisualSensor_v1::obj_vel_data_t::calcVel ( observed_player, 
                                                                               player, 
                                                                               un_quant_dist,
                                                                               quant_dist );
              M_normal_data.M_name = VisualSensor_v1::obj_name_data_t::calcName ( observed_player );
              M_range = VisualSensor_v1::NORMAL;
						}
				}
		}
  else if ( un_quant_dist <= player.vis_distance )
		{
      M_close_data.M_direction = VisualSensor_v1::obj_dir_data_t::calcDegDir ( ang );
      M_close_data.M_distance = quant_dist;
      M_range = VisualSensor_v1::CLOSE;
		}
  else
    M_range = VisualSensor_v1::OUT;
}


void VisualSensor_v1::flag_high_data_t::set ( const PObject& flag, 
																							const Player& player )
{
  double ang = VisualSensor_v1::obj_dir_data_t::calcRadDir ( flag, player );
				
  double qstep;
#ifndef	NEW_QSTEP
  qstep = ServerParam::instance().land_qstep;
#else
  qstep = player.land_qstep_player;
#endif					
  double un_quant_dist = VisualSensor_v1::obj_dist_data_t::calcUnQuantDist ( flag, 
                                                                             player );
  double quant_dist = VisualSensor_v1::obj_dist_data_t::calcQuantDist ( un_quant_dist, 
                                                                        qstep );
				
  if ( fabs ( ang ) < player.vis_angle / 2.0 )
		{
      double prob;
						
      prob = ( quant_dist - UNUM_FAR_LENGTH ) 
        / ( UNUM_TOOFAR_LENGTH - UNUM_FAR_LENGTH );
						
      if ( prob > drand ( 0, 1 ) ) 
				{
          M_ufar_data.M_direction = VisualSensor_v1::obj_dir_data_t::calcDegDir ( ang );
          M_ufar_data.M_distance = quant_dist;
          M_ufar_data.M_name = VisualSensor_v1::obj_name_data_t::calcName ( flag ); 
          M_range = VisualSensor_v1::UFAR;	
				}
      else
				{
          M_normal_data.M_direction = VisualSensor_v1::obj_dir_data_t::calcDegDir ( ang );
          M_normal_data.M_distance = quant_dist;
          M_normal_data.M_name = VisualSensor_v1::obj_name_data_t::calcName ( flag ); 
          M_normal_data.M_vel = VisualSensor_v1::obj_vel_data_t::calcVel ( flag, 
                                                                           player, 
                                                                           un_quant_dist,
                                                                           quant_dist );
          M_range = VisualSensor_v1::NORMAL;
				}
		}
  else if ( un_quant_dist <= player.vis_distance )
		{
      M_close_data.M_direction = VisualSensor_v1::obj_dir_data_t::calcDegDir ( ang );
      M_close_data.M_distance = quant_dist;
      M_range = VisualSensor_v1::CLOSE;
		}
  else
    M_range = VisualSensor_v1::OUT;
}


void VisualSensor_v1::goal_high_data_t::set ( const PObject& goal, 
																							const Player& player )
{
  double ang = VisualSensor_v1::obj_dir_data_t::calcRadDir ( goal, player );
				
  double qstep;
#ifndef	NEW_QSTEP
  qstep = ServerParam::instance().land_qstep;
#else
  qstep = player.land_qstep_player;
#endif					
  double un_quant_dist = VisualSensor_v1::obj_dist_data_t::calcUnQuantDist ( goal, 
                                                                             player );
  double quant_dist = VisualSensor_v1::obj_dist_data_t::calcQuantDist ( un_quant_dist, 
                                                                        qstep );
				
  if ( fabs ( ang ) < player.vis_angle / 2.0 )
		{
      double prob;
						
      prob = ( quant_dist - UNUM_FAR_LENGTH ) 
        / ( UNUM_TOOFAR_LENGTH - UNUM_FAR_LENGTH );
						
      if ( prob > drand ( 0, 1 ) ) 
				{
          M_ufar_data.M_direction = VisualSensor_v1::obj_dir_data_t::calcDegDir ( ang );
          M_ufar_data.M_distance = quant_dist;
          M_ufar_data.M_name = VisualSensor_v1::obj_name_data_t::calcName ( goal ); 
          M_range = VisualSensor_v1::UFAR;	
				}
      else
				{
          M_normal_data.M_direction = VisualSensor_v1::obj_dir_data_t::calcDegDir ( ang );
          M_normal_data.M_distance = quant_dist;
          M_normal_data.M_name = VisualSensor_v1::obj_name_data_t::calcName ( goal ); 
          M_normal_data.M_vel = VisualSensor_v1::obj_vel_data_t::calcVel ( goal, 
                                                                           player, 
                                                                           un_quant_dist,
                                                                           quant_dist );
          M_range = VisualSensor_v1::NORMAL;
				}
		}
  else if ( un_quant_dist <= player.vis_distance )
		{
      M_close_data.M_direction = VisualSensor_v1::obj_dir_data_t::calcDegDir ( ang );
      M_close_data.M_distance = quant_dist;
      M_range = VisualSensor_v1::CLOSE;
		}
  else
    M_range = VisualSensor_v1::OUT;
}

void VisualSensor_v1::line_high_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.pos.x == -PITCH_WIDTH/2.0 ) // top
    {
      line_normal = -PI/2;
      if ( player.pos.y < line.pos.x )
        line_normal = PI/2;
      player_2_line = line.pos.x - player.pos.y;
      line_start = -PITCH_LENGTH/2.0;
      line_stop = PITCH_LENGTH/2.0;
      vert = false;
    }
  else if ( line.pos.x == PITCH_WIDTH/2.0 ) // bottom
    {
      line_normal = PI/2;
      if ( player.pos.y > line.pos.x )
        line_normal = -PI/2;
      player_2_line = line.pos.x - player.pos.y;
      line_start = -PITCH_LENGTH/2.0;
      line_stop = PITCH_LENGTH/2.0;
      vert = false;
    }
  else
    {
      std::cerr << __FILE__ << ": " << __LINE__ 
           << ": Error, unknown line: " << line << std::endl;
      return;
    }

  double sight_2_line_ang = VisualSensor_v1::obj_dir_data_t::calcLineRadDir ( line, 
                                                                              player,
                                                                              line_normal );
  // angle between the players line of sight and the line's normal

  if ( fabs ( sight_2_line_ang ) >= PI/2 )
    {
      // if the angle between the line of sight and the line's
      // norm is not within -90.0 and 90 degrees then the player
      // is looking parallel or away from the line, thus it
      // cannot be visible.
      M_range = VisualSensor_v1::OUT;
      return;
    }

  double line_intersect = player_2_line * tan ( sight_2_line_ang );
  // this gives us the x or y offset from the player for where their
  // line of sight intersects the line

  if ( vert )
    line_intersect = player.pos.y - line_intersect;
  else
    line_intersect += player.pos.x;
  // this calculates the actual x or y value for where the line of
  // sight intersects the line.  Because the y axis is inverted, we
  // need to use -line_intersect if the line is vertical.

  if ( line_intersect < line_start 
       || line_intersect > line_stop )
    {
      // If the point that the players line of sight intersects
      // the line beyond it's beginning or end then the player
      // wont see this line
      M_range = VisualSensor_v1::OUT;
      return;
    }

  double qstep;
#ifndef	NEW_QSTEP
  qstep = ServerParam::instance().land_qstep;
#else
  qstep = player.land_qstep_player;
#endif					
  M_normal_data.M_direction = VisualSensor_v1::obj_dir_data_t::calcLineDegDir ( sight_2_line_ang ) ;
  M_normal_data.M_distance = VisualSensor_v1::obj_dist_data_t::calcLineDist ( sight_2_line_ang, player_2_line, qstep ) ;
  M_normal_data.M_name = VisualSensor_v1::obj_name_data_t::calcName ( line );
  M_range = VisualSensor_v1::NORMAL;		
}


void VisualSensor_v1::low_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_v1::goal_low_data_t data;
              data.set ( *( stadium.votable.object [ j ] ), player );
										
              if ( data.M_range != VisualSensor_v1::OUT )
                M_goals.push_back ( data );	
            }
          break;
						
        case MPObject::OT_FLAG:
          if ( stadium.votable.object [ j ]->obj_ver <= player.version )
            {
              VisualSensor_v1::flag_low_data_t data;
              data.set ( *( stadium.votable.object [ j ] ), player );
										
              if ( data.M_range != VisualSensor_v1::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_v1::ball_low_data_t data;
              data.set ( *( stadium.motable.object [ j ] ), player );
										
              if ( data.M_range != VisualSensor_v1::OUT )
                M_balls.push_back ( data );										
            }
          break;
																							
        default:
          break;
				}
		}

  for ( int j = 0 ; j < MAX_PLAYER * 2; j++) 
		{