worldmodel.c

机器足球2D比赛程序 对trlen_base_2002的改进

		if( isConfidenceGood( Teammates[i].getType() ) )
			Teammates[i].show( getTeamName() );
	os << "Opponents: " << endl;
	for( i = 0; i < MAX_OPPONENTS ; i++ )
		if( isConfidenceGood( Opponents[i].getType() ) )
			Opponents[i].show( DEFAULT_OPPONENT_NAME );
	os << "Agent: " << endl;
	agentObject.show( getTeamName() );

	os << "General Info: " << endl <<
		"side: "      << SoccerTypes::getSideStr( getSide() )    << endl <<
		"kicks: "     << getNrOfCommands( CMD_KICK )             << endl <<
		"turns: "     << getNrOfCommands( CMD_TURN )             << endl <<
		"dashes: "    << getNrOfCommands( CMD_DASH )             << endl <<
		"turnnecks: " << getNrOfCommands( CMD_TURNNECK )         << endl <<
		"says: "      << getNrOfCommands( CMD_SAY )              << endl <<
		"playmode: "  << SoccerTypes::getPlayModeStr( playMode ) << endl <<
		"===================================="                   << endl;

}

/*! This method prints all the objects and information contained in the object
  set 'set' to specified outputstream. 
  \param os output stream to which output is written (default cout). */
void WorldModel::show( ObjectSetT set, ostream & os )
{
	int iIndex;
	for( ObjectT o = iterateObjectStart( iIndex, set, -1.0 );
			o != OBJECT_ILLEGAL;
			o = iterateObjectNext( iIndex, set, -1.0 ) )
		show( o, os );
	os << endl;
}

/*! This method prints the queued commands - commands that are sent by the
  ActHandler to the server - to the specified output stream.
  \param os output stream to which information is printed (default cout)*/
void WorldModel::showQueuedCommands( ostream & os )
{
	os << "Commands in queue:\n" ;
	for( int i = 0; i < MAX_COMMANDS; i++ )
		if( queuedCommands[i].commandType != CMD_ILLEGAL )
			queuedCommands[i].show( os );
}

/*! This method prints the information about the Object o to the output stream
  os.
  \param o object of which information should be printed
  \param os output stream to which information is printed. */
void WorldModel::show( ObjectT o, ostream &os )
{
	Object *object = getObjectPtrFromType( o );
	if( object != NULL )
	{
		if( SoccerTypes::isPlayer( o ) )
		{
			PlayerObject *pobj = (PlayerObject*) object ;
			if( SoccerTypes::isTeammate( o ) )
				pobj->show( getTeamName(), os );
			else
				pobj->show( "Opponent", os );
		}
		else
			object->show( os );    
	}
}

/*! This method blocks till new information has arrived. Information is either
  a sense_body message or a see message. If there isn't received information
  from the server for longer than 3 seconds, server is assumed dead and
  false is returned.
  \return true when new info has arrived, false if server is dead */
bool WorldModel::waitForNewInformation( )
{
	bool bReturn = true;
	if( bNewInfo == false ) // there hasn't arrived any information yet
	{
		struct timeval now;
		struct timespec timeout;
		gettimeofday(&now, NULL);
		timeout.tv_sec = now.tv_sec + PS->getServerTimeOut();
		timeout.tv_nsec = now.tv_usec*1000;

		// lock mutex and wait till it is unlocked by Sense thread
		// this happens in setTimeLastSeeMessage, setTimeLastSenseMessage
		// or setTimeLastSeeGlobalMessage
		pthread_mutex_lock( &mutex_newInfo );
		int ret;
		Log.logWithTime( 2, "go into conditional wait" );
		while( (ret = pthread_cond_timedwait( &cond_newInfo,
						&mutex_newInfo, &timeout) ) == EINTR )
			printf("(WorldModel::waitForNewInformation) failure in loop!!\n");
		Log.logWithTime( 2, "go out of conditional wait" );     
		if( ret == ETIMEDOUT ) // if no information was received but timer timed out
			bReturn = false;
		pthread_mutex_unlock( &mutex_newInfo );
	}
	// reset the indication of new visual information
	bNewInfo = false;

	return bReturn;
}

/*! This method logs all object information that is currently stored in the
  World Model. The output is formatted as follows. First the current time
  (cycle_nr,cycle_stopped) is printed, followed by the object information
  of the specified object 'obj'. The global x and global y position are first
  printed, followed by the global x and global y velocity. Finally the body
  and the neck angle are printed. Then the information of the ball
  "pos_x pos_y vel_x vel_y conf" is printed, followed by the information of
  all eleven teammates "nr pos_x pos_y vel_x vel_y conf" and the same
  information about the eleven opponents. Values that are currently not known
  are replaced by the value -10.0000. This method is normally used by a
  player to log every cycle all its information contained in the world model
  to a file. If the coach (with perfect information) does the same, these two
  files can be analyzed to calculate the error for the different values.
  \param iLogLevel loglevel for which information should be printed
  \param obj object of which information should be printed at start line
  of this object also the body and neck angle are printed. */
void WorldModel::logObjectInformation( int iLogLevel, ObjectT obj )
{

	char   str[2048];
	double dConf     = PS->getPlayerConfThr();
	sprintf( str, "(%4d,%3d) ", getCurrentTime().getTime(),
			getCurrentTime().getTimeStopped() );
	if( obj != OBJECT_ILLEGAL )
		sprintf( str, "%12.6f %12.6f %12.6f %12.6f %12.6f %12.6f",
				getGlobalPosition(obj).getX(),
				getGlobalPosition(obj).getY(),
				getGlobalVelocity(obj).getX(),
				getGlobalVelocity(obj).getY(),
				getGlobalBodyAngle(obj),
				getGlobalNeckAngle(obj) );

	if( getConfidence      ( OBJECT_BALL ) > dConf &&
			getRelativeDistance( OBJECT_BALL ) < 20.0 )
		sprintf( &str[strlen(str)], " %12.6f %12.6f",
				getGlobalPosition(OBJECT_BALL).getX(),
				getGlobalPosition(OBJECT_BALL).getY() );
	else
		sprintf( &str[strlen(str)], " %12.6f %12.6f", -10.0, -10.0 );

	if( getTimeGlobalVelocity( OBJECT_BALL ) > getTimeFromConfidence( dConf ) &&
			getRelativeDistance  ( OBJECT_BALL ) < 20.0 )
		sprintf( &str[strlen(str)], " %12.6f %12.6f",
				getGlobalVelocity(OBJECT_BALL).getX(),
				getGlobalVelocity(OBJECT_BALL).getY() );
	else
		sprintf( &str[strlen(str)], " %12.6f %12.6f", -10.0, -10.0 );
	sprintf( &str[strlen(str)], " %12.6f", getConfidence(OBJECT_BALL) );

	int    iIndex=-1;
	int    iIndexPlayer;
	for( ObjectT o = iterateObjectStart( iIndex, OBJECT_SET_PLAYERS, 0.0 );
			o != OBJECT_ILLEGAL;
			o = iterateObjectNext( iIndex, OBJECT_SET_PLAYERS, 0.0 ) )
	{
		bool bPrint = false;

		iIndexPlayer = (SoccerTypes::isTeammate(o))
			? SoccerTypes::getIndex(o) + 1
			: SoccerTypes::getIndex(o) + 12;
		sprintf( &str[strlen(str)], " %d", iIndexPlayer );
		if( getConfidence      ( o ) > dConf && isKnownPlayer(o) &&
				getRelativeDistance( o ) < 20.0 )
		{
			sprintf( &str[strlen(str)], " %12.6f %12.6f",
					getGlobalPosition(o).getX(),
					getGlobalPosition(o).getY() );
			bPrint = true;
		}
		else
			sprintf( &str[strlen(str)], " %12.6f %12.6f", -10.0, -10.0 );

		if( getTimeGlobalVelocity( o ) > getTimeFromConfidence( dConf ) &&
				getRelativeDistance  ( o ) < 20.0 && isKnownPlayer(o)  )
		{
			sprintf( &str[strlen(str)], " %12.6f %12.6f",
					getGlobalVelocity(o).getX(),
					getGlobalVelocity(o).getY() );
			bPrint = true;
		}
		else
			sprintf( &str[strlen(str)], " %12.6f %12.6f", -10.0, -10.0 );

		if( bPrint )
			sprintf( &str[strlen(str)], " %12.6f", getConfidence(o) );
		else
			sprintf( &str[strlen(str)], " %12.6f", -10.0 );
	}
	if( getCurrentCycle() != 3000  )
		Log.log( iLogLevel, str );
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//                                                  Orit                                                         //
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

/*! 判断当前是否是我开任意球。 
 *
 */
bool WorldModel::isMyFreeKick ()
{
	if ( !isFreeKickUs ())
		return false;
	else{
		VecPosition   posBall  = getBallPos ();
		if ( ((posBall.getX()>0) && (getPlayerNumber()==3)) 
				|| ((posBall.getX()<=0) && (getPlayerNumber()==4)) )
			return true;
		else
			return false;
	}

}

/*! 判断当前是否是我角球。 
 *
 */
bool WorldModel::isMyCornerKick ()
{
	if ( !isCornerKickUs ())
		return false;
	else{
		VecPosition   posBall  = getBallPos ();
		if ( ((posBall.getY()>0) && (getPlayerNumber()==7)) || ((posBall.getX()<=0) && (getPlayerNumber()==8)) )
			return true;
		else
			return false;
	}

}

/*! 判断当前是否是我越位球。 
 *
 */
bool WorldModel::isMyOffsideKick ()
{
	if ( !isOffsideThem ())
		return false;
	else{
		VecPosition   posBall  = getBallPos ();
		if ( ((posBall.getY()>0) && (getPlayerNumber()==7)) 
				|| ((posBall.getX()<=0) && (getPlayerNumber()==8)) )
			return true;
		else
			return false;
	}

}

/*! 得到当前的视觉角度
 *
 */
ViewAngleT WorldModel::getViewAngle ()
{
	return agentObject.getViewAngle ();
}

/*! 本函数计算点pos所在的区域。
 *
 */
int
WorldModel::getPosArea (VecPosition pos)
{
	int area = 0;

	double x = pos.getX();
	double y = pos.getY();

    if (x < -3){
		area = 1;
	}
    else if (fabs(y)<14){
	    if (x < 33)
	  	    area = 2;
	    else
		    area = 6;

    }
    else{
	    if (x < 30)
		    area = 3;
	    else if (x < 39)
		    area = 4;
	    else
		    area = 5;
    }

	return area;
}



/*!该函数返回球运动到某一点所要的周期数, 前提是该点在球的运动轨迹上。
   \参数 ballSpeed : 球的运动速度;
   \posTo ： 球要运动到的位置；
   \return : 所用的周期数。
*/
int
WorldModel::getNrCyclesBallToPoint (double ballSpeed, VecPosition posTo)
{
  VecPosition posBall = getBallPos ();
  int iCyclesToPos = 0;
  double distBallToPos = posBall.getDistanceTo (posTo);
  double posThr = 1;		/* we think that the ball has arrived the posTo when the distance is less than posThr */
  double distPre = 0;
  double distAdd = ballSpeed;

  if (distBallToPos < posThr)
    {
      return 1;
    }

  while ((iCyclesToPos <= 100) && (distPre < distBallToPos))
    {
      iCyclesToPos++;
      distPre += distAdd;
      distAdd = distAdd * (SS->getBallDecay ());
    }

  return iCyclesToPos;

}

/*!该函数返回球运动到某一点所要的周期数, 前提是该点在球的运动轨迹上。
   \参数 ballSpeed : 球的运动速度;
   \posTo ： 球要运动到的位置；
   \return : 所用的周期数。
*/
double
WorldModel::getEndSpeedFromFirstSpeed (double ballSpeed, VecPosition posTo)
{
  VecPosition posBall = getBallPos ();
  int iCyclesToPos = 0;
  double distBallToPos = posBall.getDistanceTo (posTo);
  double posThr = 1;		/* we think that the ball has arrived the posTo when the distance is less than posThr */
  double distPre = 0;
  double distAdd = ballSpeed;

  if (distBallToPos < posThr)
    {
      return ballSpeed;
    }

  while ((iCyclesToPos <= 100) && (distPre < distBallToPos))
    {
      iCyclesToPos++;
      distPre += distAdd;
      distAdd = distAdd * (SS->getBallDecay ());
    }

  return distAdd;

}