formations.cpp

自己写的robocup-2d程序

    \return bool indicating whether update was succesfull. */
bool FormationTypeInfo::setPlayerTypeInfo( PlayerTypeInfo info, int atIndex )
{
  playerTypeInfo[ atIndex ] = info;
  return true;
}

/*! This method returns (a pointer to) the player type information for the
    player type at position 'atIndex'
    \param atIndex index of which player type information should be returned
    \return pointer to player type information located at index 'atIndex' */
PlayerTypeInfo* FormationTypeInfo::getPlayerTypeInfo( int atIndex )
{
  return &playerTypeInfo[ atIndex ];
}


/*! This method returns (a pointer to) the player type information for the
    player with role number 'iPlayerInFormation'.
    \param iPlayerInFormation role number for which info should be returned
    \return pointer to information for role 'iPlayerInFormation'*/
PlayerTypeInfo* FormationTypeInfo::getPlayerTypeInfoOfPlayer(
                                                       int iPlayerInFormation )
{
  return &playerTypeInfo[ playerType[iPlayerInFormation] ];
}

/*****************************************************************************/
/********************** CLASS FORMATIONS *************************************/
/*****************************************************************************/

/*! This is the constructor for the Formations class and needs as arguments
    a formation configuration file, the current formation and the number of the
    agent in this formation (normally at start-up this equals the player
    number).
    \param strFile string representation of the formation configuration file
    \param curFt current formation type (default FT_ILLEGAL)
    \param iNr number of the agent in this formation (default 1)*/
Formations::Formations( const char *strFile, FormationT curFt, int iNr )
{
  if( strFile[0] == '\0' )
  {
    cerr << "(Formations::Formations) No Filename given" << endl;
    return;
  }

  if( readFormations( strFile ) == false )
    cerr << "(Formations::Formations) Error reading file " << strFile << endl;
  curFormation = curFt;
  setPlayerInFormation( iNr );
}

/*! This methods prints all the information of the different formation types to
    the output stream os and furthermore prints the current formation and the
    role number of the agent in this formation.
    \param os output stream to which output is written. */
void Formations::show( ostream &os )
{
  for( int i = 0 ; i < MAX_FORMATION_TYPES; i ++ )
    formations[i].show( os );
  os << "Current formation: " << (int)curFormation << endl
       << "Player nr in formation: " << iPlayerInFormation ;
}

/*! This method returns the strategic position for a player. It calculates this
    information by taking the home position of the current role in the
    current formation and combines this with the position of the ball using the
    attraction values for the current player type. The attraction values
    defines the percentage of the ball coordinate that is added to the home
    position of the current player type. So when the x coordindate of the home
    position is 10.0, x coordinate ball is 20.0 and x attraction is 0.25. The
    x coordinate of the strategic position will become 10.0 + 0.25*20.0 = 15.0.
    When this value is smaller than the minimal x coordinate or larger than
    the maximal x coordinate, the coordinate is changed to this minimal or
    maximal coordinate respectively. Also when the behind ball value is set,
    the x coordinate of the strategic position is set to this ball coordinate.
    Furthermore when the strategic position is in front of the supplied
    argument dMaxXInPlayMode, the x coordinate is adjusted to this value.
    During normal play mode the supplied value is often the offside line.
    \param iPlayer player number in formation of which strategic position
                   should be determined.
    \param posBall position of the ball
    \param dMaxXInPlayMode, max x coordinate allowed in current play mode. */
VecPosition Formations::getStrategicPosition( int iPlayer, VecPosition posBall,
     double dMaxXInPlayMode, bool bInBallPossession, double dMaxYPercentage,
     FormationT ft )
{
/*
  if( ft == FT_ILLEGAL )
    ft = curFormation;
  VecPosition     posHome;
  PlayerTypeInfo* ptInfo = formations[ft].
                            getPlayerTypeInfoOfPlayer( iPlayer );
  double x, y;

  // get the home position and calculate the associated strategic position
  posHome = formations[ft].getPosHome( iPlayer );
  y = posHome.getY() + posBall.getY() * ptInfo->getAttrY();
  x = posHome.getX() + posBall.getX() * ptInfo->getAttrX();

  // do not move too much to the side
  if( fabs( y ) > 0.5*dMaxYPercentage*PITCH_WIDTH )
    y = sign(y)*0.5*dMaxYPercentage*PITCH_WIDTH;

  // when behind ball is set, do not move to point in front of ball
  if( ptInfo->getBehindBall() == true && x > posBall.getX() )
    x = posBall.getX();

  // do not move past maximal x or before minimal x
  if( x > ptInfo->getMaxX() )
    x = ptInfo->getMaxX();
  else if( x < ptInfo->getMinX() )
    x = ptInfo->getMinX();

  // when x coordinate is in front of allowed x value, change it
  if( x > dMaxXInPlayMode )
    x = dMaxXInPlayMode;

  return VecPosition( x, y );
*/
	VecPosition pos = getPosition(iPlayer+1,posBall.getX(),posBall.getY());
	if( ft == FT_INITIAL && pos.getX()>0)
	{
		pos.setX(-1);
	}
	if ( pos.getX() > dMaxXInPlayMode )
		pos.setX(dMaxXInPlayMode);
	return pos;
//return (0, 0);
}

/*! This method reads the formations from the file 'strFile' and has the
    following format:
       - x coordinate of the home position for all the roles
       - y coordinate of the home position for all the roles
       - the player types for all the roles
       - the x attraction for all the player types
       - the y attraction for all the player types
       - indication whether to stay behind the ball for all the player types
       - minimal x coordinate for all the player types
       - maximal x coordinate for all the player types
       - extra y distance added to strat. pos. when in ball possession
    \param strFile string representation of the file.
    \return bool when file was read in succesfully. */
bool Formations::readFormations( const char *strFile )
{
  ifstream in( strFile );
  if( !in )
  {
    cerr << "(readValues::readValues) Could not open file '" <<
    strFile << "'" << endl;
    return false;
  }

  char strLine[256], *str;
  int            iLineNr          = 0, i;
  int            iForm            = 0; // current formation type that is parsed
  int            iLineInFormation = 0; // current offset of line in formation
  bool           bReturn          = true;
  PlayerTypeInfo *pt_info;

  // read all lines
  while( bReturn && in.getline( strLine, sizeof(strLine) ) )
  {
    str = &strLine[0];
    iLineNr++;
    // comment and empty lines should be skipped
    if( !(strLine[0] == '\n' || strLine[0] == '#' || strLine[0]=='\0' ||
          Parse::gotoFirstNonSpace( &str ) == '\0' ) )
    {
      // there are ten different lines in a formation (see comment above)
      // all values for each line are parsed in one iteration
      // after all 10 lines are parsed, the sequence it is resetted.
      switch( iLineInFormation )
      {
        case 0: // first line is the number of the formation
          iForm = Parse::parseFirstInt( &str );
          break;
        case 1: // the x coordinate of the home pos for all the players
          for( i = 0 ; i < MAX_TEAMMATES ; i ++ )
            formations[iForm].setXPosHome(Parse::parseFirstDouble(&str), i);
          break;
        case 2: // the y coordinate of the home pos for all the players
          for( i = 0 ; i < MAX_TEAMMATES ; i ++ )
            formations[iForm].setYPosHome(Parse::parseFirstDouble(&str), i);
          break;
        case 3: // the player types for all the players
          for( i = 0 ; i < MAX_TEAMMATES ; i ++ )
            formations[iForm].setPlayerType(
                (PlayerT) Parse::parseFirstInt(&str), i);
          break;
        case 4: // the x attraction for all the player types
          for( i = 0 ; i < MAX_PLAYER_TYPES ; i ++ )
          {
            pt_info = formations[iForm].getPlayerTypeInfo( i );
            pt_info->setAttrX( Parse::parseFirstDouble( &str ) );
          }
          break;
        case 5: // the y attraction for all the player types
          for( i = 0 ; i < MAX_PLAYER_TYPES ; i ++ )
          {
            pt_info = formations[iForm].getPlayerTypeInfo( i );
            pt_info->setAttrY( Parse::parseFirstDouble( &str ) );
          }
          break;
        case 6: // stay behind the ball for all the player types
          for( i = 0 ; i < MAX_PLAYER_TYPES ; i ++ )
          {
            pt_info = formations[iForm].getPlayerTypeInfo( i );
            if( Parse::parseFirstInt( &str ) == 1 )
              pt_info->setBehindBall( true );
            else
              pt_info->setBehindBall( false );          ;
          }
          break;
        case 7: // the minimal x coordinate for all the player types
          for( i = 0 ; i < MAX_PLAYER_TYPES ; i ++ )
          {
            pt_info = formations[iForm].getPlayerTypeInfo( i );
            pt_info->setMinX( Parse::parseFirstDouble( &str ));
          }
          break;
        case 8:// the maximal x coordinate for all the player types
          for( i = 0 ; i < MAX_PLAYER_TYPES ; i ++ )
          {
            pt_info = formations[iForm].getPlayerTypeInfo( i );
            pt_info->setMaxX( Parse::parseFirstDouble( &str ));
          }
          break;
        default:
          cerr << "(Formations::readFormations) error line " << iLineNr <<endl;
          return false;
      }

      iLineInFormation++;          // go one line further
      if( iLineInFormation == 9 ) // each formation consists of ten lines
        iLineInFormation = 0;
    }
  }
  return true;
}

/*! This method sets the current formation.
    \param formation new current formation
    \return bool indicating whether the update was successful */
bool Formations::setFormation( FormationT formation )
{
  curFormation = formation;
  return true;
}

/*! This method returns the current formation.
    \return current formation  */
FormationT Formations::getFormation( ) const
{
  return curFormation;
}

/*! This method sets the player number of the agent in the current formation to
    'iNumber'.
    \param iNumber new player number for this agent
    \return bool indicating whether the update was succesfull */
bool Formations::setPlayerInFormation( int iNumber )
{
  iPlayerInFormation = iNumber;
  return true;
}

/*! This method returns the role number of the agent in the current formation
    \return player number for this agent in the current formation */
int Formations::getPlayerInFormation( ObjectT obj  ) const
{
  int i;
  if( obj == OBJECT_ILLEGAL )
    i = iPlayerInFormation;
  else
    i = SoccerTypes::getIndex( obj );

  return i;
}

/*! This method returns the player type for the specified object
    \return player type for the agent in the current formation */
PlayerT Formations::getPlayerType( ObjectT obj, FormationT ft ) const
{
  if( ft == FT_ILLEGAL )
    ft = curFormation;
  return formations[ ft ].getPlayerType( 
    SoccerTypes::getIndex( obj ) );
}

/*! This method returns the player type for the agent in the current formation
    \return player type for the agent in the current formation */
PlayerT Formations::getPlayerType( int iIndex, FormationT ft ) const
{
  if( ft == FT_ILLEGAL )
    ft = curFormation;
  if( iIndex == -1  )
    iIndex = iPlayerInFormation;
  return formations[ ft ].getPlayerType( iIndex );
}

VecPosition Formations::getPosition(int player,double x_Ball,double y_Ball)
{
	double x, y;
	CBpline Qu;
	double Fieldlength=PITCH_LENGTH;
	double u=(x_Ball+Fieldlength/2)/Fieldlength*position_cp.CpNum[player-1]+1.1;
	int m=0;
	for(m=0;m<position_cp.CpNum[player-1];m++)
	{	
		tempCP[m]=position_cp.CP[player-1][m];
	}
	if(m!=0)
	{
		tempCP[m]=position_cp.CP[player-1][m-1];
		tempCP[m+1]=position_cp.CP[player-1][m-1];
	}
	if(position_cp.CpNum[player-1]!=0)
	{
		
		VecPosition pos = Qu.Qu(u,position_cp.CpNum[player-1],tempCP,position_cp.weight[player-1]);
		x=pos.getX();
		y=pos.getY()+getYShift(y_Ball);
		
//		return (0,0);
		if (x>PITCH_LENGTH/2)
			x = PITCH_LENGTH/2;
		if(x < -PITCH_LENGTH/2)
			x = -PITCH_LENGTH/2;
		if (y > PITCH_WIDTH /2)
			y = PITCH_WIDTH /2;
		if (y < -PITCH_WIDTH /2)
			y = -PITCH_WIDTH /2;
		return VecPosition(x, y);
	}
	else
		return VecPosition(0, 0);
		
	
}

double Formations::getYShift(double y_Ball)
{	
	double Yshift;
	CBpline Qu;
	double FieldWide=PITCH_WIDTH;

	for(int n=0;n<6;n++)
	{
		Y_CP[n].setY(n*3);
		Y_CP[n].setX(0);
		Y_weight[n]=1;
	}
	
	double u;
	VecPosition pos;
	
	if(y_Ball>=0)
	{
		 u=y_Ball/(FieldWide/2)*6+1.1;
		 pos = Qu.Qu(u,4,Y_CP,Y_weight);
		 Yshift=pos.getY();
		 return Yshift;
	}
	else
	{
		u=-1*y_Ball/(FieldWide/2)*6+1.1;
		pos = Qu.Qu(u,4,Y_CP,Y_weight);
		Yshift=pos.getY();	
		return -Yshift;
	}

}

void ControlPoint::GetPositionCP(char * posFile)
{
	ifstream in(posFile,ios::in);
	if(!in) 
		{ cerr<<"open error.\n"; exit(0); } 
	string line;
	
	int n;
	float x, y;
	int w;
	
	for(int j=0;j<15;j++)
		for(int k=0;k<50;k++)
			weight[j][k]=1;
	while (getline(in, line)) {
		
		sscanf(line.data(), "(Player %d): [%f,%f,%d]", &n, &x, &y,&w);
		CP[n-1][CpNum[n-1]].setX(x);
		CP[n-1][CpNum[n-1]].setY(y);
		weight[n-1][CpNum[n-1]]=w;
		CpNum[n-1]++;		
	}
	
}

/*************************** TESTING PURPOSES ********************************/

/*
int main( void )
{
  Formations fs( "formations.conf" );
  fs.show( cout );
}
*/