visualsensor.cc

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

std::ostream& toStr ( std::ostream& o, const VisualSensor_v1::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 << ")";		
}

std::ostream& toStr ( std::ostream& o, const VisualSensor_v1::player_high_ufar_data_t& data )
{
  return o << "(" << data.M_name
           << " " << data.M_distance
           << " " << data.M_direction << ")";		
}

std::ostream& toStr ( std::ostream& o, const VisualSensor_v1::player_high_tfar_data_t& data )
{
  return o << "(" << data.M_name
           << " " << data.M_distance
           << " " << data.M_direction << ")";		
}

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



std::ostream& toStr ( std::ostream& o, const VisualSensor_v1::flag_high_close_data_t& data )
{
  return o << "(" << O_TYPE_FLAG_NAME
           << " " << data.M_distance
           << " " << data.M_direction << ")";		
}

std::ostream& toStr ( std::ostream& o, const VisualSensor_v1::flag_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 << ")";		
}

std::ostream& toStr ( std::ostream& o, const VisualSensor_v1::flag_high_ufar_data_t& data )
{
  return o << "(" << data.M_name 
           << " " << data.M_distance
           << " " << data.M_direction << ")";		
}

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

std::ostream& toStr ( std::ostream& o, const VisualSensor_v1::goal_high_close_data_t& data )
{
  return o << "(" << O_TYPE_GOAL_NAME
           << " " << data.M_distance
           << " " << data.M_direction << ")";		
}

std::ostream& toStr ( std::ostream& o, const VisualSensor_v1::goal_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 << ")";		
}

std::ostream& toStr ( std::ostream& o, const VisualSensor_v1::goal_high_ufar_data_t& data )
{
  return o << "(" << data.M_name 
           << " " << data.M_distance
           << " " << data.M_direction << ")";		
}

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

std::ostream& toStr ( std::ostream& o, const VisualSensor_v1::line_high_normal_data_t& data )
{
  return o << "(" << data.M_name 
           << " " << data.M_distance
           << " " << data.M_direction << ")";		
}

std::ostream& toStr ( std::ostream& o, const VisualSensor_v1::line_high_data_t& data )
{
  switch ( data.M_range )
		{
    case VisualSensor_v1::NORMAL:
      return toStr ( o, data.M_normal_data );
    default:
      return o;
		}
}

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

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

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

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

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

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

  return o << ")";
}

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

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

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

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

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

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

  return o << ")";
}


int VisualSensor_v4::obj_body_dir_data_t::calcBodyDir ( const Player& observed_player, 
                                                        const Player& player )
{
  return Rad2IDeg ( normalize_angle ( observed_player.angle 
                                      - player.angle 
                                      - player.angle_neck_committed ) );
}


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

void VisualSensor_v4::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_v4::goal_high_data_t data;
              data.set ( *( stadium.votable.object [ j ] ), player );
										
              if ( data.M_range != VisualSensor_v4::OUT )
                M_goals.push_back ( data );
            }	
          break;

        case MPObject::OT_FLAG:
          if ( stadium.votable.object [ j ]->obj_ver <= player.version )
            {
              VisualSensor_v4::flag_high_data_t data;
              data.set ( *( stadium.votable.object [ j ] ), player );
										
              if ( data.M_range != VisualSensor_v4::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_v4::ball_high_data_t data;
              data.set ( *( stadium.motable.object [ j ] ), player );

              if ( data.M_range != VisualSensor_v4::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_v4::player_high_data_t data;
          data.set ( *( stadium.player [ j ] ), player );
						
          if ( data.M_range != VisualSensor_v4::OUT )
            M_players.push_back ( data );
				}
		}
}

std::ostream& toStr ( std::ostream& o, const VisualSensor_v4::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
           << ")";		
}

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


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

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

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