//-* <ObjectName>+<Id> -> VisionObject	//-* fixed objects    _visionObjectMap["Flag1_l"] = VO_FLAG1L;    _visionObjectMap["Flag1_r"] = VO_FLAG1R;    _visionObjectMap["Flag2_l"] = VO_FLAG2L;    _visionObjectMap["Flag2_r"] = VO_FLAG2R;    _visionObjectMap["Goal1_l"] = VO_GOAL1L;    _visionObjectMap["Goal1_r"] = VO_GOAL1R;    _visionObjectMap["Goal2_l"] = VO_GOAL2L;    _visionObjectMap["Goal2_r"] = VO_GOAL2R;		//-* ball    _visionObjectMap["Ball"]    = VO_BALL;	    //-* opponents	string oppTeamName = _oppTeamName;    _visionObjectMap["Player"+oppTeamName+"1"]  =  VO_OPPONENT_1;    _visionObjectMap["Player"+oppTeamName+"2"]  =  VO_OPPONENT_2;    _visionObjectMap["Player"+oppTeamName+"3"]  =  VO_OPPONENT_3;    _visionObjectMap["Player"+oppTeamName+"4"]  =  VO_OPPONENT_4;    _visionObjectMap["Player"+oppTeamName+"5"]  =  VO_OPPONENT_5;    _visionObjectMap["Player"+oppTeamName+"6"]  =  VO_OPPONENT_6;    _visionObjectMap["Player"+oppTeamName+"7"]  =  VO_OPPONENT_7;    _visionObjectMap["Player"+oppTeamName+"8"]  =  VO_OPPONENT_8;    _visionObjectMap["Player"+oppTeamName+"9"]  =  VO_OPPONENT_9;    _visionObjectMap["Player"+oppTeamName+"10"] =  VO_OPPONENT_10;    _visionObjectMap["Player"+oppTeamName+"11"] =  VO_OPPONENT_11;	//-* teammates	string teamName = getMyTeamName();    _visionObjectMap["Player"+teamName+"1"]  =  VO_TEAMMATE_1;    _visionObjectMap["Player"+teamName+"2"]  =  VO_TEAMMATE_2;    _visionObjectMap["Player"+teamName+"3"]  =  VO_TEAMMATE_3;    _visionObjectMap["Player"+teamName+"4"]  =  VO_TEAMMATE_4;    _visionObjectMap["Player"+teamName+"5"]  =  VO_TEAMMATE_5;    _visionObjectMap["Player"+teamName+"6"]  =  VO_TEAMMATE_6;    _visionObjectMap["Player"+teamName+"7"]  =  VO_TEAMMATE_7;    _visionObjectMap["Player"+teamName+"8"]  =  VO_TEAMMATE_8;    _visionObjectMap["Player"+teamName+"9"]  =  VO_TEAMMATE_9;    _visionObjectMap["Player"+teamName+"10"] =  VO_TEAMMATE_10;    _visionObjectMap["Player"+teamName+"11"] =  VO_TEAMMATE_11;}VisionSense BasicWorldModel::getVisionSense( VisionObject vo ){    TVisionMap::iterator iter = _visionMap.find( vo );    if (iter == _visionMap.end())        {            //LOGERR("[ERROR](BasicWorldMode::getVisionSeen) unknown VisionObject: %i",vo);            return VisionSense();        }    return (*iter).second;		}void BasicWorldModel::setMyTeamName( string teamName ) {	//LOG( 1,"Set teamName to :"+teamName );	_myself.setTeamName( teamName ); 		setupVisionObjectMap();}void BasicWorldModel::setMyGlobalPosWhileBeam( Vector3f posGlobal ){		LOG( 23,"setMyGlobalPosWhileBeam %f,%f,%f",posGlobal[0],posGlobal[1],posGlobal[2]);	Time time = getRealTime();	_myself.updateDynamic( time+action_latecy, posGlobal, posGlobal, Vector3f(0,0,0), Vector3f(0,0,0) );	_myself.setDriveForce( Vector3f(0,0,0) );		Vector3f pos = _myself.getGlobalPos();	LOG( 23,"setMyGlobalPosWhileBeam after %f,%f,%f",pos[0],pos[1],pos[2]);	}void BasicWorldModel::setBallBeforeKickOff(){	_ball.updateDynamic( getRealTime(), Vector3f(0,0,0), _myself.getGlobalPos(), Vector3f(0,0,0), Vector3f(0,0,0) );}void BasicWorldModel::setOppTeamName( const string &teamName ){	cout<<" opp team name is : "<<teamName<<'\n';	_oppTeamName = teamName;		setupVisionObjectMap();}/*void BasicWorldModel::parseAgentState(){	LOG( 2,"seu parse agent state");	while ( !PA->isBracketClose() )	{		//-* read the state name        string StateName;		//cout<<"StateName: "<<StateName<<endl;        if ( !PA->getNextParamName(StateName) )        {            continue;        }		//cout<<"StateName: "<<StateName<<endl;		//-* update the agent state		if ( StateName == "battery")		{			float battery = 0;			PA->getNextValue(battery);			_myself.setBattery( battery );		}		else if ( StateName == "temp" )		{			float temp = 0;			PA->getNextValue(temp);			_myself.setTemperature( temp );		}		else		{			LOGERR("[Warning](BasicWorldModel::parseAgentState) unknown agent state!");		}		PA->gotoNextLeftBracket();			}		}*/void BasicWorldModel::update(){	//-* 1.parse	//parse( message );	//-* 2.updateAll	updateAll();	LOG( 2,"complete updateAll");	//-* 3.predict	predict();	LOG( 2,"complete predict");	LOG( 2,"update ok");}void BasicWorldModel::updateAll(){	//-* 1. update the information of the flags and goals	//-- for agent localization need these infromation	updateFixedObjects();		//-* 2. update agent:	//-- localization	updateAgent();		//-* 3. update ball's information	newupdateBall();		//-* 4. update other Players	updatePlayers();		/** 5. update offside lines */	updateOffSideLine();}void BasicWorldModel::updateFixedObjects(){	_flag_1_l.updateLastSeen( _timeLastSeen, getVisionSense( VO_FLAG1L ) );	_flag_1_r.updateLastSeen( _timeLastSeen, getVisionSense( VO_FLAG1R ) );	_flag_2_l.updateLastSeen( _timeLastSeen, getVisionSense( VO_FLAG2L ) );	_flag_2_r.updateLastSeen( _timeLastSeen, getVisionSense( VO_FLAG2R ) );	_goal_1_l.updateLastSeen( _timeLastSeen, getVisionSense( VO_GOAL1L ) );	_goal_1_r.updateLastSeen( _timeLastSeen, getVisionSense( VO_GOAL1R ) );	_goal_2_l.updateLastSeen( _timeLastSeen, getVisionSense( VO_GOAL2L ) );	_goal_2_r.updateLastSeen( _timeLastSeen, getVisionSense( VO_GOAL2R ) );	/*VisionSense bv = _flag_1_l.getVisionSeen( 0 );	LOG( 11,"flag_1_l updated: %f:%f,%f,%f", _flag_1_l.getTimeSeen( 0 ),bv.distance,bv.theta,bv.phi);	Vector3f pos = _flag_1_l.getGlobalPos();	LOG( 11,"flag_1_l global pos: %f,%f,%f",pos[0],pos[1],pos[2] );	*/}void BasicWorldModel::updateAgent(){	localization();	//Vector3f posMe = _myself.getGlobalPos();	//LOG(11,"mypos: %f,%f,%f",posMe[0],posMe[1],posMe[2]);		//Vector3f driveForce = _myself.getDriveForce();	//LOG(11,"driveForce: %f,%f,%f",driveForce[0],driveForce[1],driveForce[2]);}void BasicWorldModel::newupdateBall(){	//-* update the sense about the ball	_ball.updateLastSeen( _timeLastSeen, getVisionSense( VO_BALL ) );	//-* -----update the global position and velocity of the ball---------	Time timeDelta = _ball.getTimeSeen() - _ball.getTime();	if ( timeDelta <= 0 )		return;	Step iStep = Time2Step(timeDelta);	//LOG( 21," timeDelta: %f", timeDelta );	//-* calculate ball global pos and global velocity	Vector3f globalPos, globalVelocity;	//-* in playon mode, we must calculate both ball's position and velocity	//-- but in unplayon mode, we only need to set the vaule of some var	if ( getPlayMode() == PM_PlayOn 		 || getPlayMode() == PM_GOAL_KICK_LEFT		 || getPlayMode() == PM_GOAL_KICK_RIGHT)	{		globalPos = _ball.getGlobalPos();		Vector3f globalPos1 = _ball.getGlobalPos(1);		Vector3f globalPos2 = _ball.getGlobalPos(2);		globalVelocity = _ball.getGlobalVelocity();		Vector3f globalVelocity1 = _ball.getGlobalVelocity(Step(1));		Vector3f globalVelocity2 = _ball.getGlobalVelocity(Step(2));		calculateBallState( globalPos, globalPos1, globalPos2,							globalVelocity, globalVelocity1, globalVelocity2,							iStep);	}	else	{		globalPos = getBallPositionUnPlayOn();		globalVelocity.Zero();	}	if ( globalPos.z() < _ball.getRadius() )		globalPos.z() = _ball.getRadius();	_ball.updateDynamic( _ball.getTimeSeen(), globalPos, _myself.getGlobalPos(), globalVelocity, Vector3f(0,0,0) );}Vector3f BasicWorldModel::getBallPositionUnPlayOn(){	Vector3f posBall = _ball.calculateGlobalPosFromVision( _myself.getGlobalPos());	Vector3f posOld = _ball.getGlobalPos();	Vector3f velOld = _ball.getGlobalVelocity();	switch ( getPlayMode())	{		case PM_BeforeKickOff :		case PM_KickOff_Left :        case PM_KickOff_Right :			posBall.Zero();            break;        case PM_KickIn_Left :        case PM_KickIn_Right :		{			posBall.y() = sign( posBall.y())*getFieldWidth()/2;			posBall.z() = _ball.getRadius();            break;		}				case PM_CORNER_KICK_LEFT:		case PM_CORNER_KICK_RIGHT:		{			posBall.x() = sign( posBall.x())*getFieldLength()/2;			posBall.y() = sign( posBall.y())*getFieldWidth()/2;			posBall.z() = _ball.getRadius();			break;		}        default:            break;	}	//if pre cycle have been unPlayOn	if ( velOld == Vector3f(0,0,0))	{		posBall = ( posBall + posOld)/2;	}		return posBall;}void BasicWorldModel::calculateBallState( Vector3f &posBall, Vector3f &prePosBall, Vector3f &preprePosBall,										Vector3f &velBall, Vector3f &preVelBall, Vector3f &prepreVelBall,										const Step iStep){	//LOG( 25,"deltaTime: %f s",Step2Second(iStep));	const float max_dist_gap = 0.5;	Vector3f posSee, velSee, posSim, velSim;	posSee = _ball.calculateGlobalPosFromVision( _myself.getGlobalPos());	//posSim = _ball.calculateGloabalPosFromMemory( deltaTime);	posSim = posBall;	velSim = velBall;	Vector3f posSim1 = prePosBall;	Vector3f posSim2 = preprePosBall;	Vector3f velSim1 = preVelBall;	Vector3f velSim2 = prepreVelBall;	_ball.updateKalman(iStep);	Vector3f visionError = _ball.getVisionError();	Vector3f velPol = _ball.calculateGlobalVelFromVision( _myself.getGlobalVelocity());	Vector3f velPolErr = _myself.getKalmanP11() + visionError;	//-* if collide happened, the see information only used	//-- if no collide, use both information form see and simulate	if ( _ball.predictStateAfterNrSteps( posSim, posSim1, posSim2, velSim, velSim1, velSim2, iStep) 		&& (posSee -posSim).Length() < max_dist_gap	)	{		//LOG( 25,"posSim: %f, %f, %f",posSim[0],posSim[1],posSim[2]);		//LOG( 25,"posSee: %f, %f, %f",posSee[0],posSee[1],posSee[2]);		Vector3f simulatePosError = _ball.getKalmanP22();		Vector3f simulateVelError = _ball.getKalmanP12();		LOG( 25,"visionError: %f, %f, %f",visionError[0],visionError[1],visionError[2]);		LOG( 25,"simulatePosError: %f, %f, %f",simulatePosError[0],simulatePosError[1],simulatePosError[2]);		Vector3f weightedPos = posSee;		float k = 0.0;		for ( int i=0; i<3; i++)		{			k = visionError[i] / ( simulatePosError[i] +visionError[i]);			weightedPos[i] += k*( posSim[i] - weightedPos[i]);			simulatePosError[i] = visionError[i]*(1-k);			LOG( 25,"K[%d]=%f",i,k);			velSee[i] = ( weightedPos[i] - posBall[i])/Step2Second(iStep);			k = visionError[i] / ( simulateVelError[i] +visionError[i]);			velBall[i] = velSee[i] + k*( velSim[i] - velSee[i]);			visionError[i] -= k*visionError[i];			k = visionError[i]/ ( velPolErr[i] + visionError[i] );			velBall[i] += k*( velPol[i] - velBall[i] );		}		_ball.setKalmanP22(simulatePosError);		LOG( 25,"weightedPos: %f, %f, %f",weightedPos[0],weightedPos[1],weightedPos[2]);		//LOG( 25,"velSee: %f, %f, %f",velSee[0],velSee[1],velSee[2]);		//LOG( 25,"velSim: %f, %f, %f",velSim[0],velSim[1],velSim[2]);		posBall = weightedPos;	}	else	{		LOG( 25,"more than max gap!");		_ball.setKalmanP22(visionError);		Vector3f posOld = posBall;		posBall = posSee;		//posBall = lowFreqSignalFilter( posSee, posBall);		velSee = (posBall - posOld)/Step2Second(iStep); 		velBall = ( velSee + velPol*0.5)/1.5;		//velBall = lowFreqSignalFilter( velSee, velBall);	}		//	//Vector3f velD = velBall -  velPol;	//LOG( 25,"*");	//LOG( 25,"velPol : %f,%f,%f",velPol[0],velPol[1],velPol[2]);	//LOG( 25,"velBall: %f,%f,%f",velBall[0],velBall[1],velBall[2]);	//LOG( 25,"velD : %f,%f,%f",velD[0],velD[1],velD[2]);	//}void BasicWorldModel::updateBall(){	//-* update the sense about the ball	_ball.updateLastSeen( _timeLastSeen, getVisionSense( VO_BALL ) );	//VisionSense bv = _ball.getVisionSeen( 0 );	//LOG( 11,"ball updated: %f:%f,%f,%f", _ball.getTimeSeen( 0 ),bv.distance,bv.theta,bv.phi);	//-* -----update the global position and velocity of the ball---------	Time timeDelta = ( _ball.getTimeSeen() - _ball.getTime() ) * 0.01;	if ( timeDelta <= 0 )		return;	//LOG( 21," timeDelta: %f", timeDelta );	//-* calculate ball global pos	Vector3f tmp1, tmp2, globalPos, globalVelocity;	tmp1 = _myself.getGlobalPos() + _ball.getPosRelativeSeen();	tmp2 = _ball.getGlobalPos() + _ball.getGlobalVelocity()*timeDelta;	globalPos = ( tmp1 + tmp2*0.5 )/1.5;	//LOG( 21,"_ball.getPosRelativeSeen: %f,%f,%f",_ball.getPosRelativeSeen()[0],_ball.getPosRelativeSeen()[1],_ball.getPosRelativeSeen()[2]);	//LOG( 21," tmp1: %f,%f,%f", tmp1[0],tmp1[1],tmp1[2]);	//LOG( 21," tmp2: %f,%f,%f", tmp2[0],tmp2[1],tmp2[2]);	//LOG( 21," poaBall 1: %f,%f,%f", globalPos[0],globalPos[1],globalPos[2]);	globalPos = lowFreqSignalFilter( globalPos, _ball.getGlobalPos() );	//LOG( 21," poaBall 2: %f,%f,%f", globalPos[0],globalPos[1],globalPos[2]);	globalPos[2] = tmp1[2];	globalPos[2] = setMaxNMin( globalPos[2], getFieldHeight(), _ball.getRadius() );	//LOG( 21," poaBall 3: %f,%f,%f", globalPos[0],globalPos[1],globalPos[2]);	//-* calculate ball velocity 	tmp1 = ( globalPos - _ball.getGlobalPos() )/timeDelta;	if ( globalPos[2] > _ball.getRadius() 		&& _ball.getGlobalVelocity()[2] > 1 )//-* air ball	{		tmp2 = _ball.getGlobalVelocity()*( 1-timeDelta*air_friction_factor/_ball.getMass() );		tmp2[2] = tmp1[2];	}	else//-* ground ball	{		tmp2 = _ball.getGlobalVelocity()*( 1-timeDelta*ground_friction_factor/_ball.getMass() );		tmp2[2] = tmp1[2];	}	//LOG( 21," tmp1: %f,%f,%f", tmp1[0],tmp1[1],tmp1[2]);	//LOG( 21," tmp2 = v*( 1-t*u/m ): v: %f,%f,%f m: %f", 	//			_ball.getGlobalVelocity()[0],_ball.getGlobalVelocity()[1],_ball.getGlobalVelocity()[2],_ball.getMass());	//LOG( 21," tmp2: %f,%f,%f", tmp2[0],tmp2[1],tmp2[2]);	globalVelocity = (tmp1 + tmp2)/2;	globalVelocity[0] = lowFreqSignalFilter( globalVelocity[0], _ball.getGlobalVelocity()[0] );	globalVelocity[1] = lowFreqSignalFilter( globalVelocity[1], _ball.getGlobalVelocity()[1] );		_ball.updateDynamic( _ball.getTimeSeen(), globalPos, _myself.getGlobalPos(), globalVelocity, Vector3f(0,0,0) );}void BasicWorldModel::updatePlayers(){	updateMyselfInTeam();//-* this step munst before update our team!!	updateTeam( _teammate, VO_TEAMMATE_1 );	updateTeam( _opponent, VO_OPPONENT_1 );	}/*! update the Player's info, who is myself in team*/void BasicWorldModel::updateMyselfInTeam(){	//-* if can not make sure my number, only in the beginning	//-- do nothing	if ( getMyNum() > 0 && getMyNum() <= teammates_num )	{		vector< PlayerObject >::iterator iter = _teammate.begin()+getMyNum()-1;		iter->updateDynamic( getTimeLastSeen(),									getMyGlobalPos(), getMyGlobalPos(), getMyGlobalVel(), Vector3f(0,0,0) );	}}void BasicWorldModel::updateTeam( vector<PlayerObject> &team ,VisionObject num_1 ){	Vector3f posMe = _myself.getGlobalPos();	//LOG( 21,"posMe: %f,%f,%f",posMe[0],posMe[1],posMe[2]);	//-* Team	unsigned int num = num_1;	for ( vector< PlayerObject >::iterator iter = team.begin();			iter != team.end();			iter++, ++num )