worldmodel.cpp

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

#include "worldmodel.h"
extern MSpace::Param  param;
extern MSpace::Geometry geometry;
extern MSpace::Command command;
extern MSpace::File file;
namespace MSpace 
{
Worldmodel::Worldmodel()
{
    NowCycle=param.CycleSent;
    LastStateCycle=0;
    formation=0;
    MeStop=true;
    BallStop=true;
}
int   Worldmodel::GetMyNumber()
{
	return param.PlayerNumber;
}
MoveState Worldmodel::GetBallState()
{
	return Ballstate;
}
Vec Worldmodel::GetStrategy(int num)
{
	int rely=0;
	Vec position;
	Vec ballp=GetBallState().PositionV;
	position.Vz=param.AgentRadius;
	double *format;
	if(formation==0)return position;
	else if(formation==1)format=file.FT_initial[num-1];
	else if(formation==2)format=file.FT_433_offensive[num-1];
	else if(formation==3)format=file.FT_334_offensive[num-1];
	else if(formation==4)format=file.FT_defensive[num-1];
	else if(formation==5)format=file.FT_open_defensive[num-1];
	else if(formation==6)format=file.FT_343_attacking[num-1];	
	position.Vx=format[0]*(1-format[3])+ballp.Vx*(format[3]);
	position.Vy=format[1]*(1-format[4])+ballp.Vy*(format[4]);
	if(position.Vx<format[6]&&format[6]!=0.0)position.Vx=format[6];
	if(position.Vx>format[7]&&format[7]!=0.0)position.Vx=format[7];
	return position;
}
 Vec Worldmodel::GetMyStrategy()
{
	int num=GetMyNumber();
	Vec v=GetStrategy(num);
	return v;
}
int Worldmodel::WhoIsClosestToBall()
{
	int num;
	double dist=100.0,temp;
	int rely=0;
	double t;
	Vec ballp;
	Vec Vec_vt;
	if(IsBallInAir())ballp=ForeseeFallPoint(t,Vec_vt,rely);
	else ballp=GetBallState().PositionV;
	Vec distv;
	Vision vi;
	int size;
	for(int i=1;i<=11;i++)
	{
		size=TeamMateVQ[i-1].size();
		TeamMateVQ[i-1].getAny(vi,size);
		if(param.CycleReceive-vi.cycle<20&&size!=0)
		{
			distv=ballp-vi.posV;
			distv.Vz=0.0;
			temp=distv.GetMod();
			if(temp<dist)
			{
				dist=temp;
				num=i;
			}
		}
	}
	Vec distance_me=ballp-Mystate.PositionV;
	distance_me.Vz=0.0;
	temp=distance_me.GetMod();
	if(temp<dist)
	{
		dist=temp;
		num=GetMyNumber();
	}
	return num;
}
bool Worldmodel::IsMeClosestToBall()
{
	return WhoIsClosestToBall()==GetMyNumber();
}
void Worldmodel::SetFormation(int n)
{
	formation=n;
}
bool  Worldmodel::IsUsKickOff()
{
	return((param.IsLeft&&(param.PlayMode=="KickOff_Left"))||(!param.IsLeft&&(param.PlayMode=="KickOff_Right")));	
}
Vec   Worldmodel::GetBallV()
{
	Vision vi;
	BallQ.getAny(vi,BallQ.size());
	return  vi.posV;
}
void   Worldmodel::UpMystate()
{
	
	Vision  vi,temp_vi;
	MeQ.getAny(vi,MeQ.size());
	MeQ.getAny(temp_vi,MeQ.size()-1);
	MeStop=false;
	Vec distanceV=vi.posV-temp_vi.posV;
	distanceV.Vz=0.0;
	if(distanceV.GetMod()<0.1)
	{
		MeStop=true;
		Mystate.speed=Vec(0,0,0);
		Mystate.PositionV=vi.posV;
		return;
	}
	Vec    F=command.GetLastDriveForce();
	double t1=(param.CycleReceive-vi.cycle)*0.01;
	double t2=(param.CycleReceive-LastStateCycle)*0.01;
	double vm_x=F.Vx/100.0*VMAX;
	double vm_y=F.Vy/100.0*VMAX;
	Vec  dist;
	dist.Vx=(Mystate.speed.Vx-vm_x+vm_x*t1*DASH_K-exp(-DASH_K*t1)*(Mystate.speed.Vx-vm_x))/DASH_K;
	dist.Vy=(Mystate.speed.Vy-vm_y+vm_y*t1*DASH_K-exp(-DASH_K*t1)*(Mystate.speed.Vy-vm_y))/DASH_K;
	Mystate.PositionV=dist+vi.posV;	
	Mystate.speed.Vx=vm_x+exp(-DASH_K*t2)*(Mystate.speed.Vx-vm_x);
	Mystate.speed.Vy=vm_y+exp(-DASH_K*t2)*(Mystate.speed.Vy-vm_y);
	Mystate.speed.Vz=0.0;
	double t=(vi.cycle-temp_vi.cycle)*0.01;
	Vec temp_speed=distanceV/t;
	temp_speed.Vz=0.0;
	if(Mystate.speed.GetMod()-temp_speed.GetMod()>0.6)Mystate.speed=Mystate.speed*0.5+temp_speed*0.5;
	LastStateCycle=param.CycleReceive;
}
Vec   Worldmodel::InterceptBall(double &leftTime,int &reliable)
{
	double ball_k;
	int rely=0;
	double t;
	long Cycle=0;
	bool InAir=false;
	MoveState tempState;
	Vec ballp,Intercept;
	if(IsBallInAir())
	{
		Vec Vec_vt;
		Intercept=ForeseeFallPoint(t,Vec_vt,rely);
		reliable=rely;
		Vec F=Intercept-Mystate.PositionV;
		F.Vz=0.0;
		if(F.GetMod()/(VMAX/2)<t)return Intercept+Vec_vt*0.04;
		else InAir=true;
	}
	if(InAir)
	{
		ball_k=BALL_K_AIR;
		tempState=GetBallState_air(param.CycleReceive,rely);
	}
	else 
	{
		tempState=GetBallState_ground(param.CycleReceive,rely);
		ball_k=BALL_K_GROUND;
	}
	ballp=tempState.PositionV;
	Vec dist=ballp-Mystate.PositionV;
	dist.Vz=0.0;
	reliable=rely;
	int i=0;
	double s_x=dist.Vx,s_y=dist.Vy;
	double temp_s_x=1000.0,temp_s_y=1000.0;
	double t_x=0.0,t_y=0.0,St_x=0.0,St_y=0.0,OK_x=10.0,OK_y=10.0;
	double tempt1_x=50.0,tempt2_x=0.0;
	double tempt1_y=50.0,tempt2_y=0.0;
	double Vmax_x,Vmax_y;
	Vmax_x=dist.Vx/dist.GetMod()*VMAX;
	Vmax_y=dist.Vy/dist.GetMod()*VMAX;
	double va_x=Mystate.speed.Vx;
	double va_y=Mystate.speed.Vy;
	double vb_x=tempState.speed.Vx;
	double vb_y=tempState.speed.Vy;	
	double sa=DASH_K;
	double sb=ball_k/GetBallMass();
	while(i<30&&(fabs(temp_s_x)>0.02||fabs(temp_s_y)>0.02))
	{
		i++;
		t_x=(tempt1_x+tempt2_x)/2.0;
		t_y=(tempt1_y+tempt2_y)/2.0;
		temp_s_x=(va_x-Vmax_x+Vmax_x*sa*t_x-exp(-1.0*sa*t_x)*(va_x-Vmax_x))/sa-vb_x*(1.0-exp(-1.0*sb*t_x))/sb-s_x;
		temp_s_y=(va_y-Vmax_y+Vmax_y*sa*t_y-exp(-1.0*sa*t_y)*(va_y-Vmax_y))/sa-vb_y*(1.0-exp(-1.0*sb*t_y))/sb-s_y;
		if(s_x*temp_s_x<0)tempt2_x=t_x;
		else tempt1_x=t_x;
		if(s_y*temp_s_y<0)tempt2_y=t_y;
		else tempt1_y=t_y;	
	}
	if(t_x>t_y)t=t_x;
	else t=t_y;
	St_x=Get_s_XY(vb_x,t,ball_k);
	St_y=Get_s_XY(vb_y,t,ball_k);
	Vec V_b_t;
	V_b_t.Vx=Get_Vt_XY(vb_x,t,ball_k);
	V_b_t.Vy=Get_Vt_XY(vb_y,t,ball_k);
	Intercept.Vx=ballp.Vx+St_x;
	Intercept.Vy=ballp.Vy+St_y;
	leftTime=t;
	Intercept=Intercept+V_b_t*0.02;
	return Intercept;
}
MoveState    Worldmodel::GetFutureMyState(long Cycle,Vec F)
{
	MoveState move;
	double t=(Cycle-param.CycleReceive)*0.01;
	double vm_x=F.Vx/100.0*VMAX;
	double vm_y=F.Vy/100.0*VMAX;
	Vec  dist;
	dist.Vx=(Mystate.speed.Vx-vm_x+vm_x*t*DASH_K-exp(-DASH_K*t)*(Mystate.speed.Vx-vm_x))/DASH_K;
	dist.Vy=(Mystate.speed.Vy-vm_y+vm_y*t*DASH_K-exp(-DASH_K*t)*(Mystate.speed.Vy-vm_y))/DASH_K;
	move.PositionV=Mystate.PositionV+dist;
	move.speed.Vx=vm_x+exp(-DASH_K*t)*(Mystate.speed.Vx-vm_x);
	move.speed.Vy=vm_y+exp(-DASH_K*t)*(Mystate.speed.Vy-vm_y);
	move.speed.Vz=0.0;
	return move;
}
void  Worldmodel::ForeseeMyState()
{
	if(MeStop)return;
	Vec    F=command.GetLastDriveForce();
	double t=(param.CycleReceive-LastStateCycle)*0.01;
	double vm_x=F.Vx/100.0*VMAX;
	double vm_y=F.Vy/100.0*VMAX;
	Vec  dist;
	dist.Vx=(Mystate.speed.Vx-vm_x+vm_x*t*DASH_K-exp(-DASH_K*t)*(Mystate.speed.Vx-vm_x))/DASH_K;
	dist.Vy=(Mystate.speed.Vy-vm_y+vm_y*t*DASH_K-exp(-DASH_K*t)*(Mystate.speed.Vy-vm_y))/DASH_K;
	Mystate.PositionV=Mystate.PositionV+dist;
	Mystate.speed.Vx=vm_x+exp(-DASH_K*t)*(Mystate.speed.Vx-vm_x);
	Mystate.speed.Vy=vm_y+exp(-DASH_K*t)*(Mystate.speed.Vy-vm_y);
	Mystate.speed.Vz=0.0;
	LastStateCycle=param.CycleReceive;
}
 Vec  Worldmodel::GetMeV()
{
	Vision vi;
	MeQ.getAny(vi,MeQ.size());
	return  vi.posV;
}
Pol   Worldmodel::GetBallP()
{
	Vision vi;
	BallQ.getAny(vi,BallQ.size());
	return  vi.posP;
}
Vec  Worldmodel::GetMyPosition()
{
	bool done=false;
	double left=1.0;
	Vec position1,position2,flag;
	double fx1=0.0,fx2=0.0,fy1=0.0,fy2=0.0,kx=0.5,ky=0.5;
	Angel a1(param.pan);
	Angel a2(param.tilt);
	Angel a3;
	Angel a4;
	Angel ay;
	Angel az;
	if(param.IsLeft)
		left=1.0;
	else left=-1.0; 
	for(int j=0;j<8;j++)
	{
		if(j==0)flag=Vec(-1.0*GetHalfLength()*left,-1.0*GetHalfWidth()*left,0);
		else if(j==1)flag=Vec(-1.0*GetHalfLength()*left,GetHalfWidth()*left,0);
		else if(j==2)flag=Vec(GetHalfLength()*left,-1.0*GetHalfWidth()*left,0);
		else if(j==3)flag=Vec(GetHalfLength()*left,GetHalfWidth()*left,0);
		else if(j==4)flag=Vec(-1.0*GetHalfLength()*left,-0.5*param.GoalWidth*left,0);
		else if(j==5)flag=Vec(-1.0*GetHalfLength()*left,0.5*param.GoalWidth*left,0);
		else if(j==6)flag=Vec(GetHalfLength()*left,-0.5*param.GoalWidth*left,0);
		else  flag=Vec(GetHalfLength()*left,0.5*param.GoalWidth*left,0);
		if(!done&&param.FGCycle[j]==param.CycleSent)
		{	
			a3=Angel(param.FG_Pol[j].Py);
			a4=Angel(param.FG_Pol[j].Pz);
			ay=a1+a3;
			az=a2+a4;
			param.FG_Pol[j].Py=ay.angel;
			param.FG_Pol[j].Pz=az.angel;
			Vec  opV=geometry.Pol_Vec(param.FG_Pol[j]);
			position1=flag-opV;
			done=true;
		}
		else if(done&&param.FGCycle[j]==param.CycleSent)
		{
			a3=Angel(param.FG_Pol[j].Py);
			a4=Angel(param.FG_Pol[j].Pz);
			ay=a1+a3;
			az=a2+a4;
			param.FG_Pol[j].Py=ay.angel;
			param.FG_Pol[j].Pz=az.angel;
			position2=flag-geometry.Pol_Vec(param.FG_Pol[j]);
			fx1=(position2.Vx-position1.Vx)*(position2.Vx-position1.Vx);
			fy1=(position2.Vy-position1.Vy)*(position2.Vy-position1.Vy);
			position1.Vx=position1.Vx+kx*(position2.Vx-position1.Vx);
			position1.Vy=position1.Vy+ky*(position2.Vy-position1.Vy);
			fx2=fx1-kx*fx1;fy2=fy1-ky*fy1;
			kx=fx1/(fx1+fx2);ky=fy1/(fy1+fy2);	
		}
	}
	position1.Vz=param.AgentRadius;
	if(done)return position1;
	else return Vec(0,0,0);
}
bool  Worldmodel::Updata()
{
	NowCycle=param.CycleSent;
	Vec V0(0,0,0);
	Vision vi;
	Vec posU=GetMyPosition();
	if(!(posU==V0))
	{
		Angel a1(param.pan);
		Angel a2(param.tilt);
		Angel a3;
		Angel a4;
		Angel ay;
		Angel az;
		vi.posV=posU;
		vi.cycle=param.CycleSent;
		MeQ.enQueue(vi);
		for(int j=0;j<11;j++)
		{
			if(param.MateCycle[j]==NowCycle)
			{
				a3=Angel(param.Player_teammate[j].Py);
				a4=Angel(param.Player_teammate[j].Pz);
				ay=a1+a3;
				az=a2+a4;
				param.Player_teammate[j].Py=ay.angel;
				param.Player_teammate[j].Pz=az.angel;
				vi.posP=param.Player_teammate[j];
				vi.posV=posU+geometry.Pol_Vec(param.Player_teammate[j]);
				vi.posV.Vz=param.AgentRadius;
				vi.cycle=NowCycle;
				TeamMateVQ[j].enQueue(vi);
			}
			if(param.OpponentCycle[j]==NowCycle)
			{
				a3=Angel(param.Player_opponent[j].Py);
				a4=Angel(param.Player_opponent[j].Pz);
				ay=a1+a3;
				az=a2+a4;
				param.Player_opponent[j].Py=ay.angel;
				param.Player_opponent[j].Pz=az.angel;
				vi.posP=param.Player_opponent[j];
				vi.posV=posU+geometry.Pol_Vec(param.Player_opponent[j]);
				vi.posV.Vz=param.AgentRadius;
				vi.cycle=NowCycle;
				OpponentVQ[j].enQueue(vi);
			}
		}
		if(param.BallCycle==NowCycle)
		{
			a3=Angel(param.ball.Py);
			a4=Angel(param.ball.Pz);
			ay=a1+a3;
			az=a2+a4;
			param.ball.Py=ay.angel;
			param.ball.Pz=az.angel;
			vi.cycle=NowCycle;
			vi.posP=param.ball;
			vi.posV=posU+geometry.Pol_Vec(param.ball);
			if(vi.posV.Vz<param.BallRadius)vi.posV.Vz=param.BallRadius;
			BallQ.enQueue(vi);
		}
	}
	else 
	{
		return false;
	}
	return true;
}
double Worldmodel::GetHalfLength()
{
    return param.FieldLength/2.0;
}
double Worldmodel::GetHalfWidth()
{
   return param.FieldWidth/2.0;
}
double Worldmodel::GetHalfGoalWidth()
{
	return param.GoalWidth/2.0;
}
Vec Worldmodel::GetOpponentGoal()
{
	Vec goal;
	goal.Vx=GetHalfLength();
	return goal;
}
Vec Worldmodel::GetOurGoal()
{
	Vec goal;
	goal.Vx=-1.0*GetHalfLength();
	return goal;
}
double Worldmodel::GeGoalDepth()