/***************************************************************************************** *                                      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 "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();