worldmodel.cpp

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

/*****************************************************************************************
 *                                      SEU-3D
 *                     -------------------------------------------------
 * Copyright (c) 2005, Yuan XU<[e-mail]>,Chang'e SHI<[e-mail]>
 * Copyright (c) 2006, Yuan XU<[e-mail]>,Chunlu JIANG<[e-mail]>
 * 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 "WorldModel.h"
 #include "Skill.h"
 #include "Formation.h"

WorldModel::WorldModel()
{
	/** offside line */
	_ourOffSideLine = max_field_length*0.5;
	_oppOffSideLine = -_ourOffSideLine;
}

bool WorldModel::predictBallStateAfterNrSteps(	Vector3f &pos, Vector3f &prePos, Vector3f &preprePos,
										Vector3f &vel, Vector3f &preVel, Vector3f &prepreVel,
										int iStep,
										int iStepMax ) const
{
	return _ball.predictStateAfterNrSteps( pos, prePos, preprePos,
											 vel, preVel, prepreVel,
											iStep,
											iStepMax);
}

bool WorldModel::isGroundBall( const Vector3f &posBall, const Vector3f &prePosBall, const Vector3f &preprePosBall,
						const Vector3f &velBall, const Vector3f &preVelBall, const Vector3f &prepreVelBall) const
{
	/*vector<float> nnInput;
	nnInput.resize(6);
	nnInput.push_back( posBall.z());
	nnInput.push_back( prePosBall.z());
	nnInput.push_back( preprePosBall.z());
	nnInput.push_back( velBall.z());
	nnInput.push_back( preVelBall.z());
	nnInput.push_back( prepreVelBall.z());
	return _nnIsGroundBall.sim( nnInput.begin() );
	*/
	return _ball.isGroundBall( posBall, prePosBall, preprePosBall,
								velBall, preVelBall, prepreVelBall );
}

bool WorldModel::isBallStop( const float minSpeed ) const
{
	return _ball.isBallStop( getBallGlobalPos(), getBallGlobalPos(1), getBallGlobalPos(2),
								getBallGlobalVel(), getBallGlobalVel(Step(1)), getBallGlobalVel(Step(2)) ,minSpeed);
}

/*!判断是不是地滚球
	\param 球的当前位置
	\param 球的但前速度
	\return 是不是地滚球
*/
bool WorldModel::isFallOnGround( const Vector3f &posBall, const Vector3f &prePosBall, const Vector3f &preprePosBall,
						const Vector3f &velBall, const Vector3f &preVelBall, const Vector3f &prepreVelBall) const
{
	return _ball.isFallOnGround( posBall, prePosBall, preprePosBall,
								velBall, preVelBall, prepreVelBall );
}

/*!判断球是不是刚好落地反弹
\param 球的当前位置
\param 球的但前速度
\return 是不是落地反弹
*/
bool WorldModel::isKickable( Vector3f posPlayer, Vector3f posBall )
{
	return getBallRelativePos().Length() < 3.0;
}

bool WorldModel::isKickable()
{
	return isKickable( getMyGlobalPos(), getBallGlobalPos() );
}

State2Ball WorldModel::predictOurFastest()
{
	if ( getBallRelativePos().Length() > max_kick_distance )
		return S2B_ILLEGAL;
	LOG( 40,"dist2ball: %f",getBallRelativePos().Length() );
	LOG( 40,"realtive ball pos: %f, %f, %f ",getBallRelativePos().x(),getBallRelativePos().y(),getBallRelativePos().z());
	//-* algorithm 1
	//-- the fastest one is the one who is clostest to the first point
	//-- it suits air ball, but not good enough for the ground ball
	/*Vector3f posFight = _firstPoint;
	float distMe = ( getMyGlobalPos() - posFight ).Length();
	for ( int i=1; i<=11; i++ )
	{
		float distTeammate = ( getTeammateGlobalPos(i) - posFight ).Length();
		if ( distTeammate < distMe - 1.0 )
			return false;
	}
	return true;
	*/
	
	//-* algorithm 2
	//-- predict all Player's need time when he intercept the ball
	//-- its performance relay on the predictInterceptPos function
	/*Step iStepIntercept = 0;
	_prePosIntercept = _posIntercept;
	_posIntercept = predictInterceptPos( iStepIntercept, getMyGlobalPos() );
	if ( iStepIntercept >= max_intercept_steps ) return false;
	LOG( 12,"my intercept step: %d",iStepIntercept);
	//LOG( 12,"my intercept position: %f, %f, %f",_posIntercept.x(),_posIntercept.y(),_posIntercept.z());
	Vector3f dPosIntercept = _posIntercept - _prePosIntercept; 
	LOG( 12,"intercept delta: %f, %f, %f",dPosIntercept.x(),dPosIntercept.y(),dPosIntercept.z());
	for ( unsigned int i=1; i<=11; i++ )
	{
		if ( i == getMyNum() ) continue;
		Step teammateStep = 0;
		predictInterceptPos( teammateStep, getTeammateGlobalPos(i), iStepIntercept+10 );
		LOG( 40,"teammate[%d] step: %d",i,teammateStep);
		if ( teammateStep < iStepIntercept - 3 )
			return false;
	}
	return true;
	*/
	
	//-* algorithm 3
	//-- 2005-10-27
	//-- predict all Player's need *time*(in seconds) when he intercept the ball
	//-- its performance relay on the predictInterceptPos function
	//-- but it ne
	/*Time myTime = 0;
	_posIntercept = predictInterceptPos( myTime, getMyGlobalPos(), getMyGlobalVel() );
	if ( myTime < 0.5 )
		return S2B_FASTEST_TEAM;
	if ( myTime >= max_intercept_time )
		return S2B_ILLEGAL;
	for ( unsigned int i=1; i<=teammates_num; i++ )
	{
		if ( i == getMyNum() ) continue;
		Time teammateTime = 0;
		predictInterceptPos( teammateTime, getTeammateGlobalPos(i), getTeammateGlobalVel(i),myTime+2 );
		if ( teammateTime < myTime - 3*sim_step_time )
			return S2B_ILLEGAL;
	}
	return S2B_FASTEST_TEAM;
	*/
	//-* algorithm 4
	//-- 2005-11-22
	//-- base on algorithm 3
	//-- partition the same type and different type
	Time myTime = 0;
	_posIntercept = predictInterceptPos( myTime, getMyGlobalPos(), getMyGlobalVel() );
	//_posIntercept = (_posIntercept + predictInterceptPos( myTime, getMyGlobalPos(), getMyGlobalVel() ))*0.5;
	LOG( 40,"_posIntercept=( %f, %f, %f)",_posIntercept.x(),_posIntercept.y(),_posIntercept.z());
	if ( myTime < 0.5 )
		return S2B_FASTEST_TEAM;
	if ( myTime >= max_intercept_time )
		return S2B_ILLEGAL;
	set<Num>::iterator iter;
	Time teammateTime;
	for ( iter=_sameTypeTeammateNum.begin(); iter!=_sameTypeTeammateNum.end(); iter++ )
	{
		teammateTime = 0;
		predictInterceptPos( teammateTime, getTeammateGlobalPos(*iter), getTeammateGlobalVel(*iter),myTime+2 );
		if ( teammateTime < myTime - 3*sim_step_time )
			return S2B_ILLEGAL;
	}
	for ( iter=_diffTypeTeammateNum.begin(); iter!=_diffTypeTeammateNum.end(); iter++ )
	{
		teammateTime = 0;
		predictInterceptPos( teammateTime, getTeammateGlobalPos(*iter), getTeammateGlobalVel(*iter),myTime+2 );
		if ( teammateTime < myTime - 3*sim_step_time )
			return S2B_FASTEST_TYPE;
	}
	return S2B_FASTEST_TEAM;
}

Vector3f WorldModel::getInterceptPos( const Vector3f &goal) const
{
	Vector3f pos;
	Vector3f goalDriveLine( goal - _posIntercept);
    goalDriveLine.Normalize();
	pos[0] = _posIntercept[0] - goalDriveLine[0]*kick_pos_dist;
	pos[1] = _posIntercept[1] - goalDriveLine[1]*kick_pos_dist;
	pos[2] = _myself.getRadius();
	
	return pos;
}

Vector3f WorldModel::predictInterceptPos(Step &iStepPre,
										Vector3f posPlayer,
										Step iStepMax ) const
{
	float mySpeed = 1.0;
	//float mySpeed = real_agent_max_speed - 0.3;
	Vector3f velBall = getBallGlobalVel();
	//if ( velBall[0] > 0 )
	//	mySpeed = real_agent_max_speed + 0.3;
	//mySpeed*=0.5;//tmp test 2005-10-12

	Vector3f posBall = getBallGlobalPos();
	if ( (posBall - posPlayer).Length() > max_kick_distance )
	{
		iStepPre = iStepMax+1;
		return vector3f_illegal;
	}
	Vector3f prePosBall = getBallGlobalPos(1);
	Vector3f preprePosBall = getBallGlobalPos(2);
	Vector3f preVelBall = getBallGlobalVel(1);
	Vector3f prepreVelBall = getBallGlobalVel(2);
	
	
	Vector3f posIntercept;

	//default(默认截球位置，即球当前位置)
	posIntercept = posBall;

	float speedBall = getBallGlobalVel().Length();
	float dist = sqrt((posBall[0] - posPlayer[0])*(posBall[0] - posPlayer[0])+(posBall[1] - posPlayer[1])*(posBall[1] - posPlayer[1]));
	float runRadious = 0;
	//球停止了，所以不用计算
	//2005-10-04-test
	//if ( speedBall < 0.1 && isGroundBall( posBall,velBall )	)	
	//if ( _ball.isBallStop( posBall,prePosBall,preprePosBall,velBall,preVelBall,prepreVelBall) )
	//if ( speedBall < 0.2 && isGroundBall( posBall,prePosBall,preprePosBall,velBall,preVelBall,prepreVelBall)	)
	if ( speedBall < 0.01 && isGroundBall( posBall,prePosBall,preprePosBall,velBall,preVelBall,prepreVelBall)	)
	{
		iStepPre = Second2Step(dist /  mySpeed);
		return posIntercept;
	}
	//地滚球
	else if ( isGroundBall(posBall,prePosBall,preprePosBall,velBall,preVelBall,prepreVelBall) )//ground intercept!
	{
groundloop:		while( dist > runRadious )
		{
			iStepPre++;
			predictBallStateAfterNrSteps( posBall,prePosBall,preprePosBall,velBall,preVelBall,prepreVelBall );
			dist = sqrt((posBall[0] - posPlayer[0])*(posBall[0] - posPlayer[0])+(posBall[1] - posPlayer[1])*(posBall[1] - posPlayer[1]));
			runRadious = mySpeed*sim_step_time*iStepPre;
			if ( iStepPre > iStepMax ) break;
		}
	}

	else//空中球
	{
		int iStepFly = 0;
		while( dist > runRadious )
		{
			if ( isGroundBall( posBall,prePosBall,preprePosBall,velBall,preVelBall,prepreVelBall ) ) 
				goto groundloop;//落地之后成为地滚球
			
			predictBallStateAfterNrSteps( posBall,prePosBall,preprePosBall,velBall,preVelBall,prepreVelBall );
			iStepFly = predictFirstPoint( posBall,prePosBall,preprePosBall,velBall,preVelBall,prepreVelBall );
			iStepPre += (iStepFly + 1);
			dist = sqrt((posBall[0] - posPlayer[0])*(posBall[0] - posPlayer[0])+(posBall[1] - posPlayer[1])*(posBall[1] - posPlayer[1]));
			runRadious = mySpeed*sim_step_time*iStepPre;
			
			if ( iStepPre > iStepMax ) break;
		}
	}
	
	//predictBallStateAfterNrSteps(posBall,velBall,40);
	posIntercept = posBall;
	return posIntercept;
}

Vector3f WorldModel::predictInterceptPos(Time &time,
										const Vector3f &posPlayer,
										const Vector3f &velPlayer,
										Time timeMax ) const
{
	Vector3f posBall = getBallGlobalPos();
	if ( (posBall - posPlayer).Length() > max_kick_distance )
	{//-* too far away, not consider
		time = timeMax+1;
		return vector3f_illegal;
	}
	const Step predicateSteps = step_num_per_cycle;
	Vector3f prePosBall = getBallGlobalPos(1);
	Vector3f preprePosBall = getBallGlobalPos(2);
	Vector3f velBall = getBallGlobalVel();
	Vector3f preVelBall = getBallGlobalVel(1);
	Vector3f prepreVelBall = getBallGlobalVel(2);
	Time ballTime = 0;
	Skill::driveTo(posPlayer,velPlayer,posBall,Vector3f(0,0,0),time);
	bool isBallOnGround = false;
	
	//-- new add by XuYuan @2005-11-11
	//-- close intercept, if I near the ball, and the ball's speed is slow
	//-- directly intercept it.
	//-- *TODO*
	//-- the closeInterceptSpeed and closeInterceptDist should depend on the situation and opponent model
	/*const float close_intercept_speed = 0.5;
	const float close_intercept_dist = 1.0;
	const float minDist = getMyRadius() + getBallRadius();
	const Step close_intercept_Step = 3;
	if ( _ball.isBallStop( posBall,prePosBall,preprePosBall,velBall,preVelBall,prepreVelBall,close_intercept_speed )
		&& (posBall-posPlayer).Length() < close_intercept_dist )
	{
		//Vector3f PlayerPos = posPlayer;
		//Vector3f PlayerVel = velPlayer;	
		for ( Step i=0; i< close_intercept_Step; i++ )
		{
			predictBallStateAfterNrSteps( posBall,prePosBall,preprePosBall,velBall,preVelBall,prepreVelBall,predicateSteps );
			//predictPlayerStateAfterOneStep(PlayerPos,PlayerVel,getMyDriveForce());
			if ( isCollide(posPlayer,posBall,minDist) )
			{
				posBall = prePosBall;
				break;
			}
		}	
		return posBall;
	}*/
	//float speedBall = velBall.Length();
	while ( time > ballTime && ballTime < timeMax )
	{
		//-- ball is stopped
		/*if (  (speedBall < 0.5 && isGroundBall( posBall,prePosBall,preprePosBall,velBall,preVelBall,prepreVelBall) )
			|| _ball.isBallStop( posBall,prePosBall,preprePosBall,velBall,preVelBall,prepreVelBall )	)
		*/
		if ( _ball.isBallStop( posBall,prePosBall,preprePosBall,velBall,preVelBall,prepreVelBall )	)
		{
			return posBall;
		}
		//-- ball is on the ground
		else if ( isBallOnGround
			|| isGroundBall(posBall,prePosBall,preprePosBall,velBall,preVelBall,prepreVelBall) )//ground intercept!
		{
			isBallOnGround = true;
			predictBallStateAfterNrSteps( posBall,prePosBall,preprePosBall,velBall,preVelBall,prepreVelBall,predicateSteps );
			Skill::driveTo(posPlayer,velPlayer,posBall,Vector3f(0,0,0),time);
			ballTime += Step2Second(predicateSteps);
		}
		//-- ball is on the air
		else
		{
			Step iStepFly = predictFirstPoint( posBall,prePosBall,preprePosBall,velBall,preVelBall,prepreVelBall );
			predictBallStateAfterNrSteps( posBall,prePosBall,preprePosBall,velBall,preVelBall,prepreVelBall,predicateSteps );
			Skill::driveTo(posPlayer,velPlayer,posBall,Vector3f(0,0,0),time);
			ballTime += Step2Second(iStepFly);
			ballTime += Step2Second(predicateSteps);
		}
		
	}

	return posBall;
}

int WorldModel::predictFirstPoint( Vector3f &posBall, Vector3f &prePosBall, Vector3f &preprePosBall,
										Vector3f &velBall, Vector3f &preVelBall, Vector3f &prepreVelBall,
										int iStepMax ) const
{
	int iFlySteps = 0;
	while ( ! ( isFallOnGround( posBall,prePosBall,preprePosBall,velBall,preVelBall,prepreVelBall )  
				|| isGroundBall( posBall,prePosBall,preprePosBall,velBall,preVelBall,prepreVelBall ) ) )
	{
		iFlySteps++;
		predictBallStateAfterNrSteps( posBall,prePosBall,preprePosBall,velBall,preVelBall,prepreVelBall );
		if ( iFlySteps > iStepMax ) break;
	}
	return iFlySteps;
}

bool WorldModel::predictPlayerStateAfterNrSteps(	Vector3f &pos,
										Vector3f &vel,
										int iStep,
										int iStepMax ) const
{
	Vector3f power = _myself.getDriveForce();
	/*Vector3f a = (power*0.4848 - vel*30)/14.43;
	for ( int i=0;i<iStep;i++)
	{
		pos = pos + vel*sim_step_time;
		//-* new adapt @2005-11-08-by-XuYuan
		//-* S = S0 + V0*t + 0.5*a*t^2
		//pos = pos + vel*sim_step_time + a*pow2(sim_step_time)*0.5; 
		pos[2] = _myself.getRadius();
		a = (power*0.4848 - vel*30)/14.43;
		vel += a*sim_step_time;
		vel[2] = 0;
		if ( i > iStepMax ) return false;
	}*/
	for ( int i=0; i<iStep;i++)
	{
		predictPlayerStateAfterOneStep(pos,vel,power);
		if ( i > iStepMax ) return false;
	}
	return true;
}

void WorldModel::predict()
{
	//-* 1.predict my global position and velocity in real time
	predictRealTime();
	
	//-* 2.predict first point;
	Vector3f posBall = getBallGlobalPos();