worldmodel.cpp

底层robocup3d源码。 suse１０．０ kdevelop 开发.

{
	return param.GoalDepth;
}
double Worldmodel::GetAgentMass()
{
	return param.AgentMass;
}
double Worldmodel::GetNowCycle()
{
	return NowCycle;
}
std::string Worldmodel::GetPlayMode()
{
	return param.PlayMode;
}
double Worldmodel::GetBallMass()
{
   return param.BallMass;
}
void  Worldmodel::SetMyPosition(const Vec v)
{
   Mystate.PositionV=v;
}
Vec   Worldmodel::GetTeamMatePosV(int number)
{
    Vision vi;
    TeamMateVQ[number-1].getAny(vi,TeamMateVQ[number-1].size());
    return  vi.posV;
}
Vec   Worldmodel::GetOpponentPosV(int number)
{
     Vision vi;
    OpponentVQ[number-1].getAny(vi,OpponentVQ[number-1].size());
    return  vi.posV;
}
Pol   Worldmodel::GetTeamMatePosP(int number)
{
    Vision vi;
    TeamMateVQ[number-1].getAny(vi,TeamMateVQ[number-1].size());
    return  vi.posP;
}
Pol   Worldmodel::GetOpponentPosP(int number)
{
    Vision vi;
    OpponentVQ[number-1].getAny(vi,OpponentVQ[number-1].size());
    return  vi.posP;
}
bool  Worldmodel::IsBallInAir()
{
	Vision Vi1;
	BallQ.getAny(Vi1,BallQ.size());
	if(Vi1.posP.Pz>0)return true;
	else return false;
}
 double Worldmodel::WhereBall()
{
	Vision Vi1,Vi2;
	BallQ.getAny(Vi1,BallQ.size());
	BallQ.getAny(Vi2,BallQ.size()-1);
	return Vi1.posP.Py-Vi2.posP.Py;
}
MoveState Worldmodel::GetBallState_ground(long cycle,int &reliable)
{
	reliable=0;
	MoveState tempMove;
	int queue_size=BallQ.size();
	if(queue_size<3)
	{	
		return tempMove;
	}
	else
	{
	    Vision Vi[2];
	    Vision temp_Vi;
	    Vec temp_V;
	    double s;
	    double t=0.0, Vt_x=0.0,V0_x=0.0,St_x=0.0;
	    double        Vt_y=0.0,V0_y=0.0,St_y=0.0;
	    BallQ.getAny(Vi[0],queue_size);
	    BallQ.getAny(Vi[1],queue_size-1);
	    BallStop=false;
	    Vec distanceV=Vi[0].posV-Vi[1].posV;
	    distanceV.Vz=0.0;
	    if(distanceV.GetMod()<0.1)
	    {
		    BallStop=true;
		    tempMove.PositionV=Vi[0].posV;
		    tempMove.speed=Vec(0,0,0);
		    return tempMove;
	    }
	    s=BALL_K_GROUND/GetBallMass();
	    for(int i=queue_size-2;i>0;i--)
	    {
		    t=(Vi[0].cycle-Vi[1].cycle)*0.01;
		    temp_V=Vi[0].posV-Vi[1].posV;
		    St_x=temp_V.Vx;
		    V0_x=s*St_x/(1.0-exp(-1.0*s*t));
		    St_y=temp_V.Vy;
		    V0_y=s*St_y/(1.0-exp(-1.0*s*t));
		    BallQ.getAny(temp_Vi,i);
		    t=(Vi[1].cycle-temp_Vi.cycle)*0.01;
		    St_x=V0_x*(1.0-exp(-1.0*s*t))/(s*exp(-1.0*s*t));
		    St_y=V0_y*(1.0-exp(-1.0*s*t))/(s*exp(-1.0*s*t));
		    temp_V.Vx=Vi[1].posV.Vx-St_x;
		    temp_V.Vy=Vi[1].posV.Vy-St_y;
		    temp_V=temp_V-temp_Vi.posV;
		    temp_V.Vz=0;
		    if(temp_V.GetMod()<0.35)
		    {
			    Vi[1]=temp_Vi;
		    }
		    else break;
		    reliable=queue_size-i;
	    }
	    t=(Vi[0].cycle-Vi[1].cycle)*0.01;
	    temp_V=Vi[0].posV-Vi[1].posV;
	    St_x=temp_V.Vx;
	    V0_x=s*St_x/(1.0-exp(-1.0*s*t));
	    St_y=temp_V.Vy;
	    V0_y=s*St_y/(1.0-exp(-1.0*s*t));
	    Vt_x=V0_x*exp(-1.0*s*t);
	    Vt_y=V0_y*exp(-1.0*s*t);
	    t=(cycle-Vi[0].cycle-DELAY)*0.01;
	    St_x=(1.0-exp(-1.0*s*t))*Vt_x/s;
	    St_y=(1.0-exp(-1.0*s*t))*Vt_y/s;
	    Vt_x=Vt_x*exp(-1.0*s*t);
	    Vt_y=Vt_y*exp(-1.0*s*t);
	    tempMove.speed.Vx=Vt_x;
	    tempMove.speed.Vy=Vt_y;
	    tempMove.PositionV.Vx=Vi[0].posV.Vx+St_x;
	    tempMove.PositionV.Vy=Vi[0].posV.Vy+St_y;
	    tempMove.PositionV.Vz=param.BallRadius;
	    return tempMove;
	}
}
MoveState Worldmodel::GetBallState_air(long cycle,int &reliable)
{
	reliable=0;
	MoveState tempMove;
	int queue_size=BallQ.size();
	if(queue_size<3)
	{	
		return tempMove;
	}
	else
	{
		Vision Vi[2];
		Vision temp_Vi;
		Vec temp_V;
		double s;
		double t=0.0, Vt_x=0.0,V0_x=0.0,St_x=0.0;
		double        Vt_y=0.0,V0_y=0.0,St_y=0.0;
		double        Vt_z=0.0,V0_z=0.0,St_z=0.0;
		BallQ.getAny(Vi[0],queue_size);
		BallQ.getAny(Vi[1],queue_size-1);
		s=BALL_K_AIR/GetBallMass();
		for(int i=queue_size-2;i>0;i--)
		{
			t=(Vi[0].cycle-Vi[1].cycle)*0.01;
			temp_V=Vi[0].posV-Vi[1].posV;
			St_x=temp_V.Vx;
			V0_x=s*St_x/(1.0-exp(-1.0*s*t));
			St_y=temp_V.Vy;
			V0_y=s*St_y/(1.0-exp(-1.0*s*t));
			St_z=temp_V.Vz;
			V0_z=Get_v0_Z_st(St_z,t);
			BallQ.getAny(temp_Vi,i);
			t=(Vi[1].cycle-temp_Vi.cycle)*0.01;
			St_x=V0_x*(1.0-exp(-1.0*s*t))/(s*exp(-1.0*s*t));
			St_y=V0_y*(1.0-exp(-1.0*s*t))/(s*exp(-1.0*s*t));
			V0_z=Get_v0_Z_vt(V0_z,t);
			St_z=Get_s_Z(V0_z,t);
			temp_V.Vx=Vi[1].posV.Vx-St_x;
			temp_V.Vy=Vi[1].posV.Vy-St_y;
			temp_V.Vz=Vi[1].posV.Vz-St_z;
			temp_V=temp_V-temp_Vi.posV;
			if(temp_V.GetMod()<0.4)
			{
				Vi[1]=temp_Vi;
			}
			else break;
			reliable=queue_size-i;
		}
		t=(Vi[0].cycle-Vi[1].cycle)*0.01;
		temp_V=Vi[0].posV-Vi[1].posV;
		St_x=temp_V.Vx;
		V0_x=s*St_x/(1.0-exp(-1.0*s*t));
		St_y=temp_V.Vy;
		V0_y=s*St_y/(1.0-exp(-1.0*s*t));
		St_z=temp_V.Vz;
		V0_z=Get_v0_Z_st(St_z,t);
		Vt_z=Get_Vt_Z(V0_z,t);
		Vt_x=V0_x*exp(-1.0*s*t);
		Vt_y=V0_y*exp(-1.0*s*t);
		t=(cycle-Vi[0].cycle-DELAY)*0.01;
		St_x=(1.0-exp(-1.0*s*t))*Vt_x/s;
		St_y=(1.0-exp(-1.0*s*t))*Vt_y/s;
		St_z=Get_s_Z(Vt_z,t);
		Vt_x=Vt_x*exp(-1.0*s*t);
		Vt_y=Vt_y*exp(-1.0*s*t);
		Vt_z=Get_Vt_Z(Vt_z,t);
		tempMove.speed.Vx=Vt_x;
		tempMove.speed.Vy=Vt_y;
		tempMove.speed.Vz=Vt_z;
		tempMove.PositionV.Vx=Vi[0].posV.Vx+St_x;
		tempMove.PositionV.Vy=Vi[0].posV.Vy+St_y;
		tempMove.PositionV.Vz=Vi[0].posV.Vz+St_z;
		if(tempMove.PositionV.Vz<param.BallRadius)tempMove.PositionV.Vz=0.111;
		return tempMove;
	}
}
void Worldmodel::UpBallState()
{
	int rely;
	MoveState state_ball;
	if(IsBallInAir())
		state_ball=GetBallState_air(param.CycleReceive,rely);
	else 
		state_ball=GetBallState_ground(param.CycleReceive,rely);
	Ballstate=state_ball;
}
Vec   Worldmodel::ForeseeFallPoint(double &t,Vec &Vec_vt,int &reliable)
{
	int  rely;
	MoveState BallMove=GetBallState_air(param.CycleReceive,rely);
	reliable=rely;
	if(BallMove.PositionV.Vz<=param.BallRadius)
	{
		t=0.0;
		return BallMove.PositionV;
	}
	else
	{
		Vec v=BallMove.PositionV;
		double s_z=param.BallRadius-BallMove.PositionV.Vz;
		double v0_z=BallMove.speed.Vz;
		double v0_x=BallMove.speed.Vx;
		double v0_y=BallMove.speed.Vy; 
	        t=Get_t_Z(v0_z,s_z);
		double s_x=Get_s_XY(v0_x,t,BALL_K_AIR);
		double s_y=Get_s_XY(v0_y,t,BALL_K_AIR);
		v.Vx=v.Vx+s_x;
		v.Vy=v.Vy+s_y;
		v.Vz=param.BallRadius;
		Vec_vt.Vx=Get_Vt_XY(v0_x,t,BALL_K_AIR);
		Vec_vt.Vy=Get_Vt_XY(v0_y,t,BALL_K_AIR);
		Vec_vt.Vz=Get_Vt_Z(v0_z,t);
		return v;
	}
}
MoveState Worldmodel::GetTeamMateState(int number,long cycle)
{
	Vision vi1,vi2;
	MoveState move;
	int size=TeamMateVQ[number-1].size();
	if(size>2)
	{
		TeamMateVQ[number-1].getAny(vi1,size);
		if((param.CycleReceive-vi1.cycle)<50)
		{
			TeamMateVQ[number-1].getAny(vi2,size-2);
			Vec dist=vi1.posV-vi2.posV;
			double t=(vi1.cycle-vi2.cycle)*0.01;
			move.speed=dist/t;
			t=(cycle-vi1.cycle)*0.01;
			move.PositionV=vi1.posV+move.speed*t;
			return move;
		}
	}
	move.PositionV=GetStrategy(number);
	return move;
}
MoveState Worldmodel::GetOpponentState(int number,long cycle)
{
	Vision vi1,vi2;
	MoveState move;
	int size=OpponentVQ[number-1].size();
	if(size>2)
	{
		OpponentVQ[number-1].getAny(vi1,size);
		OpponentVQ[number-1].getAny(vi2,size-2);
		Vec dist=vi1.posV-vi2.posV;
		double t=(vi1.cycle-vi2.cycle)*0.01;
		move.speed=dist/t;
		t=(cycle-vi1.cycle)*0.01;
		move.PositionV=vi1.posV+move.speed*t;
	}
	return move;
}
MoveState Worldmodel::GetMyState()
{
	return Mystate;
}
double Worldmodel::Get_t_Z(double v0,double s)
{
	double t=0.0;
	double tempt1=100.0,tempt2=0.0;
	double temps=0.0;
	double g=G,k=BALL_K_AIR,m=GetBallMass();
	if(s>=0.0)return 0.0;
	else
	{
		int i=0;
		while(temps-s>0.001||temps-s<-0.001)
		{
			i++;
			t=(tempt1+tempt2)/2;
			temps=-1.0*m*g*t/k+(m*m*g+m*v0*k)/(k*k)*(1.0-exp(-1.0*k*t/m));
			if(temps<=s)tempt1=t;
			else tempt2=t;
			if(i>100)break;
		}	
		return t;	
	}
}
double Worldmodel::Get_t_XY(double v0_XY,double s_XY,double k)
{
	double s=k/GetBallMass();
	double t=0.0;
	t=-1.0*log((v0_XY-s_XY*s)/v0_XY)/s;
	return t;
}
double Worldmodel::Get_Vt_Z(double v0_z,double t_z)
{
	double m=GetBallMass();
	double k=BALL_K_AIR;
	double Vt=0.0;
	double g=G;
	Vt=v0_z+(exp(-1.0*k*t_z/m)-1.0)*(m*g+v0_z*k)/k;
	return Vt;
}
double Worldmodel::Get_s_Z(double v0_z,double t_z)
{
	double m=GetBallMass();
	double k=BALL_K_AIR;
	double s=0.0;
	double g=G;
	s=-1.0*m*g*t_z/k+(1.0-exp(-1.0*k*t_z/m))*(m*m*g+m*v0_z*k)/(k*k);
	return s;
}
double Worldmodel::Get_v0_Z_st(double s_z,double t_z)
{
	double m=GetBallMass();
	double k=BALL_K_AIR;
	double v0=0.0;
	double g=G;
	v0=(s_z*k+m*g*t_z)/(m-m*exp(-1.0*k*t_z/m))-m*g/k;
	return v0;
}
double Worldmodel::Get_v0_Z_vt(double vt_z,double t_z)
{
	double m=GetBallMass();
	double k=BALL_K_AIR;
	double v0=0.0;
	double g=G;
	v0=vt_z/exp(-1.0*k*t_z/m)-m*g*(exp(-1.0*k*t_z/m)-1)/(k*exp(-1.0*k*t_z/m));
	return v0;
}
double Worldmodel::Get_s_XY(double v0_XY,double t_XY,double k)
{
	double s=k/GetBallMass();
	double St=0.0;
	St=v0_XY*(1.0-exp(-1.0*s*t_XY))/s;
	return St;
}
double Worldmodel::Get_V0_XY_tv(double t_XY,double v_t_XY,double k)
{
	double s=k/GetBallMass();
	double v0=0.0;
	v0=v_t_XY/exp(-1.0*s*t_XY);
	return v0;
}
double Worldmodel::Get_V0_XY_sv(double s_XY,double v_t_XY,double k)
{
	double s=k/GetBallMass();
	double v0=0.0;	
	v0=s_XY*s+v_t_XY;
	return v0;
}
double Worldmodel::Get_V0_XY_st(double s_XY,double t_XY,double k)
{
	double s=k/GetBallMass();
	double v0=0.0;
	v0=s_XY*s/(1.0-exp(-1.0*s*t_XY));
	return v0;
}
double Worldmodel::Get_Vt_XY(double v0_XY,double t_XY,double k)
{
	double s=k/GetBallMass();
	double Vt=0.0;
	Vt=v0_XY*exp(-1.0*s*t_XY);
	return Vt;
}
double Worldmodel::Get_t_XY_v(double v0_XY,double Vt_XY,double k)
{
	double s=k/GetBallMass();
	double t=0.0;
	t=-1.0*log(Vt_XY/v0_XY)/s;
	return t;
}
Worldmodel::~Worldmodel()
{
	
}

}