basicworldmodel.cpp

东南大学最新的校内比赛的源代码

	{
		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");
	}
}