visualsensor.cc

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

// -*-c++-*-

/***************************************************************************
                               visualsensor.cc
    A class for storing the data from the players visual sensor
                             -------------------
    begin                : 25-NOV-2001
    copyright            : (C) 2001 by The RoboCup Soccer Server 
                           Maintainance Group.
    email                : [e-mail]
 ***************************************************************************/

/***************************************************************************
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU LGPL as published by the Free Software  *
 *   Foundation; either version 2 of the License, or (at your option) any  *
 *   later version.                                                        *
 *                                                                         *
 ***************************************************************************/

#include "visualsensor.h"
#include "player.h"
#include "field.h"
#include "random.h"


double VisualSensor_v1::obj_dir_data_t::calcRadDir ( const PObject& obj, 
																										 const Player& player )
{
  return normalize_angle ( player.vangle ( obj ) - player.angle_neck_committed );
}

int VisualSensor_v1::obj_dir_data_t::calcDegDir ( const double& rad )
{
  return Rad2IDeg ( rad );
}

int VisualSensor_v1::obj_dir_data_t::calcDir ( const PObject& obj, 
																							 const Player& player )
{
  return VisualSensor_v1::obj_dir_data_t::calcDegDir ( 
                                                      VisualSensor_v1::obj_dir_data_t::calcRadDir ( obj, player ) );
}

double 
VisualSensor_v1::obj_dir_data_t::calcLineRadDir( const PObject&,
                                                 const Player& player,
                                                 const double& line_normal )
{	
    return  normalize_angle ( line_normal 
                              - player.angle 
                              - player.angle_neck_committed );
}

int VisualSensor_v1::obj_dir_data_t::calcLineDegDir ( const double& sight_2_line_ang )
{
  if ( sight_2_line_ang > 0 )
    return Rad2IDeg ( sight_2_line_ang - PI/2 );
  else
    return Rad2IDeg ( sight_2_line_ang + PI/2 );
}

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

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

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


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

void VisualSensor_v1::player_low_data_t::set ( const Player& observed_player, 
																							 const Player& player )
{
  double ang = VisualSensor_v1::obj_dir_data_t::calcRadDir ( observed_player, player );
		
  double un_quant_dist = VisualSensor_v1::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_v1::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_v1::obj_dir_data_t::calcDegDir ( ang );
          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_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_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_range = VisualSensor_v1::CLOSE;
		}
  else
    M_range = VisualSensor_v1::OUT;
}

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

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

void VisualSensor_v1::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.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;
    }

  M_normal_data.M_direction = VisualSensor_v1::obj_dir_data_t::calcLineDegDir ( sight_2_line_ang ) ;
  M_normal_data.M_name = VisualSensor_v1::obj_name_data_t::calcName ( line );
  M_range = VisualSensor_v1::NORMAL;		
}


double VisualSensor_v1::obj_dist_data_t::calcUnQuantDist ( const PObject& obj, 
																													 const Player& player )
{
  return player.pos.distance ( obj.pos );
}		

double VisualSensor_v1::obj_dist_data_t::calcQuantDist ( const double& dist,
																												 const double& qstep )
{
  return Quantize ( exp ( Quantize ( log ( dist + EPS ), qstep ) ),	0.1 );
}		


double VisualSensor_v1::obj_dist_data_t::calcDist ( const PObject& obj, 
																										const Player& player, 
																										const double& qstep )
{
  return calcQuantDist ( calcUnQuantDist ( obj, player ), qstep );
}		

double VisualSensor_v1::obj_dist_data_t::calcLineDist ( const double& sight_2_line_ang, 
																												const double& player_2_line,
																												const double& qstep )
{
  return Quantize ( exp ( Quantize ( log ( fabs ( player_2_line 
                                                  / cos ( sight_2_line_ang ) ) 
                                           + EPS ), 
                                     qstep ) ), 0.1 ) ;
}		


VisualSensor_v1::obj_vel_data_t::vel_t
VisualSensor_v1::obj_vel_data_t::calcVel ( const MPObject& obj,
                                           const Player& player,
                                           const double& un_quant_dist,
                                           const double& quant_dist )
{
  VisualSensor_v1::obj_vel_data_t::vel_t vel;

  if ( un_quant_dist != 0.0 )
		{
      double qstep_dir;
#ifndef	NEW_QSTEP
      qstep_dir = 0.1;
#else
      qstep_dir = player.dir_qstep_player;
#endif	

      PVector vtmp = obj.vel - player.vel;
      PVector etmp = obj.pos - player.pos;
      etmp /= un_quant_dist;
								
      vel.M_dist_chg = vtmp.x * etmp.x + vtmp.y * etmp.y;
      vel.M_dir_chg = RAD2DEG * ( vtmp.y * etmp.x
                                  - vtmp.x * etmp.y ) / un_quant_dist;

      vel.M_dir_chg = ( vel.M_dir_chg == 0.0 ? 0.0 : Quantize ( vel.M_dir_chg, qstep_dir ) );
      vel.M_dist_chg = quant_dist * Quantize ( vel.M_dist_chg / un_quant_dist, 0.02 );
		}
  else
		{
      vel.M_dir_chg = 0.0;
      vel.M_dist_chg = 0.0;
		}

  return vel;
}		


VisualSensor_v1::obj_vel_data_t::vel_t 
VisualSensor_v1::obj_vel_data_t::calcVel ( const PObject& obj, 
                                           const Player& player,
                                           const double& un_quant_dist,
                                           const double& quant_dist )
{									
  VisualSensor_v1::obj_vel_data_t::vel_t vel;
		
  if ( un_quant_dist != 0.0 )
		{
      double qstep_dir;
#ifndef	NEW_QSTEP
      qstep_dir = 0.1;
#else
      qstep_dir = player.dir_qstep_player;
#endif					

      PVector vtmp = -player.vel;
      PVector etmp = obj.pos - player.pos;
      etmp /= un_quant_dist;
	
      vel.M_dist_chg = vtmp.x * etmp.x + vtmp.y * etmp.y;
      vel.M_dir_chg = RAD2DEG * ( vtmp.y * etmp.x
                                  - vtmp.x * etmp.y ) / un_quant_dist;

      vel.M_dir_chg = ( vel.M_dir_chg == 0.0 ? 0.0 : Quantize ( vel.M_dir_chg, qstep_dir ) );
      vel.M_dist_chg = quant_dist * Quantize ( vel.M_dist_chg / un_quant_dist, 0.02);
		}
  else
		{