{		Time timeDelta = ( getTimeLastSeen() - iter->getTime() ) * 0.01;		if ( timeDelta > 0 )		{			//LOG( 21,"num: %i",num);			//-* update vision			iter->updateLastSeen( _timeLastSeen, getVisionSense( VisionObject(num) ) );			//LOG( 21, "vision: %f,%f,%f",iter->getVisionSeen().distance,iter->getVisionSeen().theta,iter->getVisionSeen().phi);			//-* cal global pos			Vector3f globalPos = posMe + iter->getPosRelativeSeen(); 			globalPos[2] = iter->getRadius();			//LOG( 21, "globalPos: %f,%f,%f",globalPos[0],globalPos[1],globalPos[2]);			//-* cal vel			Vector3f globalVelocity = ( globalPos - iter->getGlobalPos() )/timeDelta;					//-* update			iter->updateDynamic( getTimeLastSeen(),									globalPos, posMe, globalVelocity, Vector3f(0,0,0) );			}	}}void BasicWorldModel::setupFixedObjects(){	//-* complete test	//-- the flag global position is not we see in the monitor	//-- they are just on the ground ( z = 0 )	const float flagHight = 0;	//-* side: left or right?	int side = 1;	if( getTeamIndex() == TI_RIGHT )		side = -1;	_flag_1_l.setGlobalPos( Vector3f( -_fieldLength/2*side, -_fieldWidth/2*side, flagHight ) );	_flag_1_r.setGlobalPos( Vector3f(  _fieldLength/2*side, -_fieldWidth/2*side, flagHight ) );	_flag_2_l.setGlobalPos( Vector3f( -_fieldLength/2*side,  _fieldWidth/2*side, flagHight ) );	_flag_2_r.setGlobalPos( Vector3f(  _fieldLength/2*side,  _fieldWidth/2*side, flagHight ) );	_goal_1_l.setGlobalPos( Vector3f( -_fieldLength/2*side, -_goalWidth/2, 0 ) );	_goal_1_r.setGlobalPos( Vector3f(  _fieldLength/2*side, -_goalWidth/2, 0 ) );	_goal_2_l.setGlobalPos( Vector3f( -_fieldLength/2*side,  _goalWidth/2, 0 ) );	_goal_2_r.setGlobalPos( Vector3f(  _fieldLength/2*side,  _goalWidth/2, 0 ) );}//////////////////// localization ////////////////////////void BasicWorldModel::localization(){	//-* if needed localization?	if ( !getRealTime() > _myself.getTime())		return;		//-* constants	const float max_speed = real_agent_max_speed*2;		Vector3f weightedPos;	VisionSense flagSense;	// for kalman	float k = 0.0;	Vector3f var = Vector3f(0,0,0);	Vector3f totalVar = Vector3f(0,0,0);	Vector3f newPos;	// iterate through all flags, localize the Player and	// finally create a weighted average of all these positions	for( int flag = VO_FLAG1L; flag <= VO_GOAL2R; ++flag)	{		newPos = localizeWithOneFlag( VisionObject(flag) );		const FixedObject *flagPtr = getFlagPtr( VisionObject(flag) );		var = flagPtr->getVisionError();				// first flag, so intialize pos		if( flag == VO_FLAG1L )		{			totalVar  = var;            			weightedPos = newPos;		}		else		{			//-* otherwise use new position based on weighted variance kalman filtering technique			for (int i=0; i<3; i++)			{				k = totalVar[i] / ( var[i] + totalVar[i] );				weightedPos[i] += k*( newPos[i] - weightedPos[i] );				totalVar[i] -= k*totalVar[i];			}		}	}		Vector3f posGlobal = _myself.getGlobalPos();	Vector3f velGlobal = _myself.getGlobalVelocity();	unsigned int deltaSteps = static_cast<unsigned int>(getRealTime()-_myself.getTime());	Time deltaTime = deltaSteps/100.0f;	_myself.updateKalman( deltaTime);	LOG( 24,"deltaTime: %f",deltaTime);	LOG( 24,"vision pos: %f,%f,%f",weightedPos[0],weightedPos[1],weightedPos[2]);	if ( (weightedPos-posGlobal).Length() > max_speed*deltaTime )//-* be moved?	{		_myself.updateDynamic( getRealTime(), weightedPos, weightedPos, Vector3f(0,0,0), Vector3f(0,0,0) );	}	else	{		Vector3f weightedVel;		Vector3f oldPos = posGlobal;		predictPlayerStateAfterNrSteps( posGlobal, velGlobal, deltaSteps);		LOG( 24,"drive pos: %f,%f,%f",posGlobal[0],posGlobal[1],posGlobal[2]);				Vector3f kalmanP22 = _myself.getKalmanP22();		for ( int i =0; i<3; i++ )		{			var[i] = kalmanP22[i];			k = totalVar[i] / ( var[i] + totalVar[i] );			weightedPos[i] += k*( posGlobal[i] - weightedPos[i] );			//kalmanP22[i] -= k*kalmanP22[i];			kalmanP22[i] = totalVar[i]*(1-k);						k = totalVar[i] / ( (_myself.getKalmanP12())[i]+ totalVar[i] );			weightedVel[i] = ( weightedPos[i] - oldPos[i])/deltaTime;			weightedVel[i] += k*( velGlobal[i] - weightedVel[i] );		}		_myself.setKalmanP22( kalmanP22);		LOG( 24,"total vision var: %f,%f,%f",totalVar[0],totalVar[1],totalVar[2]);		LOG( 24,"drive var: %f,%f,%f",var[0],var[1],var[2]);		weightedVel[2] = 0.0;		LOG( 24,"Global pos: %f,%f,%f",weightedPos[0],weightedPos[1],weightedPos[2]);		if ( weightedVel.Length() >real_agent_max_speed)//-* error?		{			weightedVel.Normalize();			weightedVel = weightedVel*real_agent_max_speed;		}		_myself.updateDynamic( getRealTime(), weightedPos, weightedPos, weightedVel, Vector3f(0,0,0) );	}}//-* this localization use all 8 flags in the field//-- beacuse of symmetry//-- myPos = SIGMA(flagPos)/(-8)Vector3f BasicWorldModel::localizationWithAllFixedObjects() const{	Vector3f posAgent =   _flag_1_l.getPosRelativeLastSeen()						+ _flag_1_r.getPosRelativeLastSeen()						+ _flag_2_l.getPosRelativeLastSeen()						+ _flag_2_r.getPosRelativeLastSeen()						+ _goal_1_l.getPosRelativeLastSeen()						+ _goal_1_r.getPosRelativeLastSeen()						+ _goal_2_l.getPosRelativeLastSeen()						+ _goal_2_r.getPosRelativeLastSeen();	posAgent[0] /= -8;	posAgent[1] /= -8;	posAgent[2] = _myself.getRadius();	return posAgent;}Vector3f BasicWorldModel::localizeWithOneFlag( VisionObject flag ) const{	const FixedObject *flagPtr = getFlagPtr( flag );	Vector3f flagPos = getFlagGlobalPos(flag);	Vector3f pos2Flag  = flagPtr->getRelativePosSeen();		// appropriate coord. quadrant	// BUG?	/*if( getTeamIndex() == TI_RIGHT )	{		pos2Flag[0] = -pos2Flag[0];		pos2Flag[1] = -pos2Flag[1];	}*/	return flagPos - pos2Flag;}//-* get Vector3f in a vector< PlayerObject > Vector3f BasicWorldModel::getVector3fInVector( Vector3fType vt, vector< PlayerObject > team, unsigned int num ) const{	vector< PlayerObject >::iterator iter = team.begin()+num-1;		//-* if wrong num, return illegal value	if ( iter >= team.begin() && iter < team.end() )	{		Vector3f res = vector3f_illegal;		switch ( vt )		{			case VT_GLOBAL_POS :				res = iter->getGlobalPos();			break;						case VT_GLOBAL_VEL :				res = iter->getGlobalVelocity();			break;						case VT_GLOBAL_ACC :				res = iter->getGlobalAcceletation();			break;						case VT_RELATIVE_POS :				res = iter->getRelativePos();			break;						case VT_RELATIVE_POS_SEEN :				res = iter->getRelativePosSeen();			break;						default:				break;		}		return res;	}	else		return vector3f_illegal;}Vector3f BasicWorldModel::getPlayerGlobalPosInTeam( vector<PlayerObject> team, unsigned int num ) const{	return getVector3fInVector( VT_GLOBAL_POS, team, num );}Vector3f BasicWorldModel::getPlayerGlobalVelInTeam( vector<PlayerObject> team, unsigned int num ) const{	return getVector3fInVector( VT_GLOBAL_VEL, team, num );}Vector3f BasicWorldModel::getPlayerGlobalPos( bool isTeammate, unsigned int num ) const{	return  isTeammate?			getTeammateGlobalPos( num ):			getOpponentGlobalPos( num );		}Vector3f BasicWorldModel::getPlayerGlobalVel( bool isTeammate, unsigned int num ) const{	return  isTeammate?			getTeammateGlobalVel( num ):			getOpponentGlobalVel( num );		}/*!	get player's position relative to me	\param team, indicate which team is the player in	\param num, indicate the player's num	\return relative position*/Vector3f BasicWorldModel::getPlayerRelativePosInTeam( vector<PlayerObject> team, Num num ) const{	return getVector3fInVector( VT_RELATIVE_POS, team, num );}/*!	get player's position relative to me	\param isTeammate, indicate which team is the player in	\param num, indicate the player's num	\return relative position*/Vector3f BasicWorldModel::getPlayerRelativePos( bool isTeammate, unsigned int num ) const{	return  isTeammate?			getTeammateRelativePos( num ):			getOpponentRelativePos( num );}/*!	get player's position in sense to relative me	\param team, indicate which team is the player in	\param num, indicate the player's num	\return relative position*/Vector3f BasicWorldModel::getPlayerRelativePosSeenInTeam( vector<PlayerObject> team, Num num ) const{	return getVector3fInVector( VT_RELATIVE_POS_SEEN, team, num );}/*!	get player's position  in sense relative to me	\param isTeammate, indicate which team is the player in	\param num, indicate the player's num	\return relative position*/Vector3f BasicWorldModel::getPlayerRelativePosSeen( bool isTeammate, Num num ) const{	return  isTeammate?			getTeammateRelativePos( num ):			getOpponentRelativePos( num );}const FixedObject * BasicWorldModel::getFlagPtr( VisionObject flag ) const{	const FixedObject *flagPtr;	switch ( flag )	{		case VO_FLAG1L :			flagPtr = &_flag_1_l;			break;		case VO_FLAG1R :			flagPtr = &_flag_1_r;			break;		case VO_FLAG2L :			flagPtr = &_flag_2_l;			break;		case VO_FLAG2R :			flagPtr = &_flag_2_r;			break;		case VO_GOAL1L :			flagPtr = &_goal_1_l;			break;		case VO_GOAL1R :			flagPtr = &_goal_1_r;			break;		case VO_GOAL2L :			flagPtr = &_goal_2_l;			break;		case VO_GOAL2R :			flagPtr = &_goal_2_r;			break;		default:			return NULL;	}	return flagPtr;}bool BasicWorldModel::isThinkTime( const Time& time) const{	return ( find( _thinkTimeSet.begin(), _thinkTimeSet.end(), time) 				!= _thinkTimeSet.end() );}/** calculate offside line \param isOur is Our or Opp?\return the offside line*/float BasicWorldModel::calculateOffsideLine( bool isOur ){		/** find the second nearst to the base line in the field */	int sideF = 1;	if ( !isOur ) sideF = -1;	float lastX = 0;	float last2X = 0; // the offside line minimum is 0	float tmp;	float baseLine = getOppBaseLine();	for ( Num i=1; i<=players_num; i++)	{		tmp = sideF*getPlayerGlobalPos(!isOur,i).x();		if ( tmp < baseLine && tmp > 0 )		{			if ( tmp > last2X )			{				swap( tmp, last2X );			}			if ( last2X > lastX )			{				swap( lastX, last2X );			}		}	}	// behind ball is not offside	float ballX = sideF*getBallGlobalPos().x();	if ( ballX > last2X )		last2X = ballX;		return last2X*sideF;}/** update the two offside lines*/void BasicWorldModel::updateOffSideLine(){	_ourOffSideLine = calculateOffsideLine(true);	_oppOffSideLine = calculateOffsideLine(false);}bool BasicWorldModel::updateObjectVisionPosition( const std::string& obj, const Polar& pol){	//-* try to lookup the VisionObject	TVisionObjectMap::iterator iterVisionObject = _visionObjectMap.find(obj);	if (iterVisionObject == _visionObjectMap.end())	{		LOGERR("unknown object: %s",obj.c_str());		return false;	}	VisionObject vo = iterVisionObject->second;	//-* update the vision map	_visionMap[vo] = pol;	LOG( 2,"success in update obj %s, pol ( %f, %f, %f)",obj.c_str(),pol.distance,pol.theta,pol.phi);	return true;}bool BasicWorldModel::setPlayMode( const string& playMode ){	//-* try to lookup the play mode	TPlayModeMap::iterator iter = _playModeMap.find(playMode);	if ( iter != _playModeMap.end() )	{		_playMode = (TPlayMode)(*iter).second;		return true;	}	else	{		LOGERR(	"[Error](BasicWorldModel) could not parse playmode: "+					playMode+"\n" );		return false;	}}void BasicWorldModel::setTeamIndex( const string& team ){	//-* look up for 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");	}}