/***************************************************************************************** *                                      SEU-3D *                     ------------------------------------------------- * Copyright (c) 2005, Yuan XU<xychn15@yahoo.com.cn>,Chang'e SHI<evelinesce@yahoo.com.cn> * Copyright (c) 2006, Yuan XU<xuyuan.cn@gmail.com>,Chunlu JIANG<JamAceWatermelon@gmail.com> * Southeast University ,China * All rights reserved. * * Additionally,this program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the * GNU Library General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program; if not, write to the Free Software * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. ****************************************************************************************/ #include "BasicWorldModel.h"#include "Logger.h"#include "Parser.h"#include <vector>using namespace std;/************************************************************************//************************ BASIC WORLD MODEL *****************************//************************************************************************/BasicWorldModel::BasicWorldModel(){	//-* set default field parameter    _fieldLength = 105.0;    _fieldWidth = 68.0;    _fieldHeight = 20.0;    _goalWidth = 7.32;    _goalDepth = 2.0;    _goalHeight = 0.5; //-* fifa: 2.44;    _borderSize = 4.0f;		//-* set the Players' sum	_teammate.resize( teammates_num );	_opponent.resize( opponents_num );		//-* set oppTeamName to ""	_oppTeamName = "";	//-* init "K" think time;	_thinkTimeLast = 0;	_numK = 0;	_sumThinkTime = 0;	//-* init "C" think time	_thinkTimeCurrent = 0;		/** offside line */	_ourOffSideLine = max_field_length*0.5;	_oppOffSideLine = -_ourOffSideLine;		setupPlayModeMap( _playModeMap );    setupVisionObjectMap();}BasicWorldModel::~BasicWorldModel(){}/*void BasicWorldModel::parseGameState(){	LOG( 2,"seu parseGameState");	while ( !PA->isBracketClose() )	{		//-* read the state name        string StateName = "";        if ( !PA->getNextParamName(StateName) )        {            continue;        }		LOG( 2,"StateName= "+StateName);		//-* field state		if (StateName == "FieldLength")		{			PA->getNextValue(_fieldLength);			LOG( 2,"res=%f",_fieldLength);		}		else if (StateName == "FieldWidth")		{			PA->getNextValue(_fieldWidth);			LOG( 2,"res=%f",_fieldWidth);		}		else if (StateName == "FieldHeight")		{			PA->getNextValue(_fieldHeight);			LOG( 2,"res=%f",_fieldHeight);		}		else if (StateName == "GoalWidth")		{			PA->getNextValue(_goalWidth);			LOG( 2,"res=%f",_goalWidth);		}		else if (StateName == "GoalDepth")		{			PA->getNextValue(_goalDepth);			LOG( 2,"res=%f",_goalDepth);		}		else if (StateName == "GoalHeight")		{			PA->getNextValue(_goalHeight);			LOG( 2,"res=%f",_goalHeight);		}		else if (StateName == "BorderSize")		{			PA->getNextValue(_borderSize);			LOG( 2,"res=%f",_borderSize);			setupFixedObjects();		}		//-* time		else if (StateName == "time")		{			PA->getNextValue(_gameTime);			LOG( 2,"res=%f",_gameTime);		}		//-* my num		else if (StateName == "unum")		{			int num = _myself.getNum();			PA->getNextValue(num);			_myself.setNum( num );			LOG( 0,"I am Num %d",num );			LOG( 2,"res=%f",num);		}		//-* agent radius		else if (StateName == "AgentRadius")		{			float agentRadius = _myself.getRadius();			PA->getNextValue(agentRadius);			_myself.setRadius( agentRadius );			LOG( 2,"res=%f",agentRadius);		}		//-* agent mass		else if (StateName == "AgentMass")		{			float agentMass = _myself.getMass();			PA->getNextValue(agentMass);			_myself.setMass( agentMass );			LOG( 2,"res=%f",agentMass);		}		//-* agent max speed		else if (StateName == "AgentMaxSpeed")		{			float agentMaxSpeed = _myself.getMaxSpeed();			PA->getNextValue(agentMaxSpeed);			_myself.setMaxSpeed( agentMaxSpeed );			LOG( 2,"res=%f",agentMaxSpeed);		}		//-* ball radius		else if (StateName == "BallRadius")		{			float ballRadius = _ball.getRadius();			PA->getNextValue(ballRadius);			_ball.setRadius( ballRadius );			LOG( 2,"res=%f",ballRadius);		}		//-* ball mass		else if (StateName == "BallMass")		{			float ballMass = _ball.getMass();			PA->getNextValue(ballMass);			_ball.setMass( ballMass );			LOG( 2,"res=%f",ballMass);		}		//-* team index		else if (StateName == "team")		{			string team="";			PA->getNextValue(team);			parseTeamIndex( team );			LOG( 2,"res="+team);		}		//-* play mode		else if (StateName == "playmode")		{			string mode="";			if (PA->getNextValue(mode))			{				parsePlayMode(mode);			}			LOG( 2,"palymode= %d",getPlayMode());		}		else		{			LOGERR("[Warning](BasicWorldModel)unknown GameState: "+StateName+"\n");		}		PA->gotoNextLeftBracket();   	}	}*//*void BasicWorldModel::parse(const char* msgBegin, const unsigned int msgSize){	LOG( 2, "seu parse, Length=%d",msgSize);	PA->input(msgBegin,msgBegin+msgSize);	    switch( *msgBegin )	{		case 'C' :			parseMsgC();			break;				case 'D' :            break;		        case 'K' :			parseMsgK();            break;        case 'S' :			parseMsgS();			break;				case 'T' :			parseMsgT();			break;				case 'X':			break;				case 'O':			LOGERR("[Warning]thinking too long time?\nreceived \"O\"");			break;				case 'I':			LOGERR("[Warning]thinking too long time?\nreceived \"I\"");			break;		        default:            LOGERR("[Warning](BasicWorldModel)received unknown data: \n");            break;	}}*//*void BasicWorldModel::parseTeamIndex(const string &team){    //-* parse team index    if (team == "left")        _teamIndex = TI_LEFT;    else if (team == "right")        _teamIndex = TI_RIGHT;    else        {            _teamIndex = TI_NONE;            LOGERR("(BasicWorldModel) received TeamIndex TI_NONE\n");        }}void BasicWorldModel::parsePlayMode(const string &mode){    //-* parse play mode    TPlayModeMap::iterator iter = _playModeMap.find(mode);    if (iter != _playModeMap.end())        {            _playMode = (TPlayMode)(*iter).second;        } else            {                LOGERR(                     			"[Error](BasicWorldModel) could not parse playmode: "+                     			mode+"\n"								);            }}*//*void BasicWorldModel::parseMsgT(){	int iRes = 0 ;	if ( PA->gotoNext() && PA->parseNextInt(iRes))	{    	_realTime = iRes;	}}*//*void BasicWorldModel::parseMsgC(){	LOG( 2,"parseMsgC");	int iRes = 0 ;	if ( PA->gotoNext() && PA->parseNextInt(iRes))	{		_thinkTimeCurrent = iRes;    	_realTime += iRes;		_sumThinkTime += _thinkTimeCurrent;		_numK ++;		LOG( 2,"complete parseMsgC");	}}*//*void BasicWorldModel::parseMsgK(){	int iRes = 0 ;    if ( PA->gotoNext() && PA->parseNextInt(iRes))	{		_thinkTimeLast = iRes;		_numK ++;		_sumThinkTime += _thinkTimeLast;		LOG( 2,"parse Msg K");	}}*//*void BasicWorldModel::parseMsgS(){	LOG( 2,"seu parseMsgS");	int iRes = 0;	//-* parse the time of the vision message	if ( PA->gotoNext() && PA->parseNextInt(iRes) )	{		LOG( 2,"_timeLastSeen=%d",iRes);    	_timeLastSeen = iRes;	}		iRes = 0;	//-* parse the current time	if ( PA->parseNextInt(iRes) )	{		LOG( 2,"_realTime=%d",iRes);		_realTime = iRes;	}    while ( !PA->isEnd() )        {			string strParamType;			if (!PA->getNextParamName(strParamType))return;			//cout<<"strParamType "<<strParamType<<endl;			if( strParamType == "GameState")			{                parseGameState();			}			else if ( strParamType == "Vision")			{				//-* check for the Vision perceptor				parseVision();			}            else if ( strParamType == "AgentState")			{				parseAgentState();			}			else			{				LOGERR(                			"[Warning](WorldModel) skipping unknown predicate: "+                			strParamType + "\n");			}                    }		LOG( 2,"complete seu parseS");}*//*void BasicWorldModel::parseVision(){	LOG( 2,"seu parseVision");	while ( !PA->isBracketClose() )	{			//-* read the object name        string name = "";        PA->getNextParamName(name);		if ( name.empty() )		{			while ( !PA->isBracketClose() )			{				PA->gotoNext();			}			return;		}						//-* try to read 'team' section		string strTeam = "";		if ( PA->getNextParamValue("team", strTeam))        {            name += strTeam;			//-* parse opponents' teamName			if ( ""==_oppTeamName && strTeam != getMyTeamName() )			{				setOppTeamName(strTeam);			}        }		//-* try read the 'id' section        string strId = "";        if ( PA->getNextParamValue("id", strId))        {            name += strId;        }		LOG( 2, "objName= "+name );        //-* try to lookup the VisionObject        TVisionObjectMap::iterator iterVisionObject = _visionObjectMap.find(name);        if (iterVisionObject == _visionObjectMap.end())        {			//if(!PA->gotoNextRightBracket())return;            continue;        }        VisionObject vo = (*iterVisionObject).second;        //-* find  to the 'pol' entry in the object's section		VisionSense sense;        if ( !PA->getNextParamValue("pol",sense))        {			LOG( 2,"failure in parse pol");            continue;        }		LOG( 2,"success in parse pol ( %f, %f, %f)",sense.distance,sense.theta,sense.phi);        //-* update the vision map        _visionMap[vo] = sense;				PA->gotoNextLeftBracket();	}}*/void BasicWorldModel::setupVisionObjectMap(){    _visionObjectMap.clear();