skills.cpp~

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

#include "skills.h"
extern MSpace::Geometry geometry;
namespace MSpace {

Skills::Skills()
{
}
void  Skills::DribbleTo(Angel direction,int speed)
{
	int rely=0;
	double k=0.0;
	MoveState ball_move,MyMove;
	MyMove=world.GetMyState();
	ball_move=world.GetBallState();
	double intercept_time;
	Vec intercept=world.InterceptBall(intercept_time,rely);
	if(rely>2)ball_move.PositionV=intercept;
	Vec temp_vector=ball_move.PositionV-MyMove.PositionV;
	Angel Angel1(temp_vector.GetXYAngel());
	Vec Force;
	double  KickAble=param.AgentRadius+param.BallRadius+0.2;
	if(fabs((direction-Angel1).angel)<35&&temp_vector.GetMod()<KickAble)
	{
		Force.Vx=cos(direction.angel);
		Force.Vy=sin(direction.angel);
		Force.SetMod(speed*5.0*PAI*world.GetBallMass());
		bool over;
		KickTo(Force,over);
	}
	else
	{
		Angel tempAngel=Angel1+Angel(30);
		if(fabs((direction-tempAngel).angel)>fabs((direction-Angel1).angel));
		tempAngel=Angel1-Angel(30);
		Force.Vx=cos(tempAngel.angel);
		Force.Vy=sin(tempAngel.angel);
		Force.SetMod(speed*5.0*PAI*world.GetBallMass());
		bool over;
		KickTo(Force,over);
	}
}
void Skills::DashTo(Vec position,Vec correction)
{
	MoveState state=world.GetMyState();
	Vec dist=position-state.PositionV;
	double vm_x=0.0;
	double vm_y=0.0;
	if(dist.GetMod()<0.5)
	{
		MoveTo(position,dist.GetMod()*80+5);
	}
	else
	{
	    vm_x=(DASH_K*dist.Vx-(1.0-exp(-DASH_K*0.01*STEP))*state.speed.Vx)/(exp(-DASH_K*0.01*STEP)+DASH_K*0.01*STEP-1);
	    vm_y=(DASH_K*dist.Vy-(1.0-exp(-DASH_K*0.01*STEP))*state.speed.Vy)/(exp(-DASH_K*0.01*STEP)+DASH_K*0.01*STEP-1);
	    Vec force;
	    if(vm_x>VMAX)
	    {
		    vm_y=vm_y/vm_x*VMAX;
		    vm_x=VMAX; 
	    }
	    else if(vm_x<-VMAX)
	    {
		    vm_y=vm_y/vm_x*-VMAX;
		    vm_x=-VMAX;  
	    }
	    if(vm_y>VMAX)
	    {
		    vm_x=vm_x/vm_y*VMAX;
		    vm_y=VMAX; 
	    }
	    else if(vm_y<-VMAX)
	    {
		    vm_x=vm_x/vm_y*-VMAX;
		    vm_y=-VMAX;  
	    }
	    force.Vx=vm_x/VMAX*100.0;
	    force.Vy=vm_y/VMAX*100.0;
	    force=force+correction;
	    command.PutDrive(force);
	}
}
void Skills::MoveTo(Vec position,double force)
{
	MoveState state=world.GetMyState();
	Vec dist=position-state.PositionV;
	double vm_x=0.0;
	double vm_y=0.0;
		vm_x=(DASH_K*dist.Vx-(1.0-exp(-DASH_K*0.02*STEP))*state.speed.Vx)/(exp(-DASH_K*0.02*STEP)+DASH_K*0.02*STEP-1);
		vm_y=(DASH_K*dist.Vy-(1.0-exp(-DASH_K*0.02*STEP))*state.speed.Vy)/(exp(-DASH_K*0.02*STEP)+DASH_K*0.02*STEP-1);
		if(vm_x>VMAX)
		{
			vm_y=vm_y/vm_x*VMAX; 
			vm_x=VMAX;
		}
		else if(vm_x<-VMAX)
		{
			vm_y=vm_y/vm_x*-VMAX;
			vm_x=-VMAX;
		}
		if(vm_y>VMAX)
		{
			vm_x=vm_x/vm_y*VMAX; 
			vm_y=VMAX;
		}
		else if(vm_y<-VMAX)
		{
			vm_x=vm_x/vm_y*-VMAX;
			vm_y=-VMAX;
		}
		Vec Force;
		Force.Vx=vm_x;
		Force.Vy=vm_y;
		Force.SetMod(force);
		command.PutDrive(Force);
}
void Skills::KickToAir(Vec position,double Vt,bool& over)
{	
	int rely=0;
	MoveState ball_move,MyMove;
	MyMove=world.GetMyState();
	ball_move=world.GetBallState();
	double k=0.0;
	Vec Vspeed;
	double intercept_time;
	Vec intercept=world.InterceptBall(intercept_time,rely);
	if(rely>3&&ball_move.speed.GetMod()>1.5)
		ball_move.PositionV=intercept;
	Vec dist=position-ball_move.PositionV;
	dist.Vz=0.0;
	double s=dist.GetMod();
	k=BALL_K_AIR;
	double v0=world.Get_V0_XY_sv(s,Vt,k);
	Vspeed=dist;
	Vspeed.SetMod(v0);
	Vspeed.Vz=v0*0.57735;
	Vspeed=Vspeed-ball_move.speed;
	Vec temp1=ball_move.PositionV-MyMove.PositionV;
	temp1.Vz=0.0;
	if(temp1.GetMod()<0.3)return;
	Angel angel1(temp1.GetXYAngel());
	Angel angel2(Vspeed.GetXYAngel());
	double  KickAble=param.AgentRadius+param.BallRadius+0.3;
	double direction=fabs((angel1-angel2).angel);
	if(direction<4.0&&temp1.GetMod()<KickAble)
	{
		over=true;
		Vec Force=Vspeed*(5.0*world.GetBallMass()*PAI);
		double f=Force.GetMod();
		double angel_z=Force.GetZAngel();
		if(f>100.0)f=100.0;
		if(angel_z>50.0)angel_z=50.0;
		else if(angel_z<0.0)angel_z=0.0;
		command.PutKick(angel_z,f);
	}
	else 
	{
		over=false;
		Vspeed.SetMod(0.001*direction+0.3);
		Vec pos=ball_move.PositionV-Vspeed;
		if(direction>100.0)
		{
			Line line;
			line=geometry.MakeLine(pos,MyMove.PositionV);
			Vec tempv=geometry.GetFoot(ball_move.PositionV,line);
			tempv=tempv-ball_move.PositionV;
			tempv.SetMod(0.5*(direction-100)/80.0);
			pos=pos+tempv;
			//DribbleTo(angel2,1);
		}
		Vec Force(0,0,0);
		DashTo(pos,Force);
	}
}
void Skills::KickToGround(Vec position,double Vt,bool &over)
{
	int rely=0;
	double k=0.0;
	Vec V;
	MoveState ball_move,MyMove;
	MyMove=world.GetMyState();
	ball_move=world.GetBallState();
	Vec dist=position-ball_move.PositionV;
	dist.Vz=0.0;
	double s=dist.GetMod();
	k=BALL_K_GROUND;
	Vec Vspeed=dist;
	Vspeed.Vz=0.0;
	double v0=world.Get_V0_XY_sv(s,Vt,k);
	Vspeed.SetMod(v0);
	Vspeed=Vspeed-ball_move.speed;
	Vspeed.Vz=0.0;
	Vec temp1=ball_move.PositionV-MyMove.PositionV;
	Angel angel1(temp1.GetXYAngel());
	Angel angel2(Vspeed.GetXYAngel());
	double  KickAble=param.AgentRadius+param.BallRadius+0.3;
	Vec Force=temp1;
	Force.SetMod(0.1);
	double direction=fabs((angel1-angel2).angel);
	//std::cout<<"  angel1"<<angel1.angel<<" speed"<<angel2.angel<<std::endl;
	if(direction<5&&temp1.GetMod()<KickAble)
	{
		over=true;
		Vec Force=Vspeed*(5.0*world.GetBallMass()*PAI);
		double f=Force.GetMod();
		double angel_z=Force.GetZAngel();
		if(f>100.0)f=100.0;
		command.PutKick(0.0,f);
	}
	else if(direction<5)
	{
		command.PutDrive(Force);
	}
	else 
	{
		over=false;
		Vspeed.SetMod(0.001*direction+0.2);
		double intercept_time;
		Vec intercept=world.InterceptBall(intercept_time,rely);
		if(rely>4&&ball_move.speed.GetMod()>3)ball_move.PositionV=intercept;
		Vec pos=ball_move.PositionV-Vspeed;
		if(direction>150)
		{
			Line line;
			line=geometry.MakeLine(pos,MyMove.PositionV);
			Vec tempv=geometry.GetFoot(ball_move.PositionV,line);
			tempv=tempv-ball_move.PositionV;
			tempv.SetMod(0.4);
			pos=pos+tempv;
			//DribbleTo(angel2,1);
		}
		DashTo(pos,Force);
	}
	
}
void Skills::AroundBallTo(Vec point,Vec ball)
{
	MoveState mystate=world.GetMyState();
	Vec tempV1=ball-point;
	Vec tempV2=point-mystate.PositionV;
	Vec tempV3=ball-mystate.PositionV;
	tempV1.Vz=0.0;
	Angel angelBall_Point(tempV1.GetXYAngel());
	Angel angelPoint_Me(tempV2.GetXYAngel());
	Angel acute_angel=angelBall_Point-angelPoint_Me;
	if(fabs(acute_angel.angel)<90.0)DashTo(point,Vec(0,0,0));
	else
	{	
		double dist=tempV3.GetMod();
		if(dist<0.4)dist=0.4;
		Angel Angel1(tempV3.GetXYAngel());
		Angel Angel2(asin(0.4/dist)/PAI*180.0);
		Angel Angel_out1=Angel1-Angel2;
		Angel Angel_out2=Angel1+Angel2;
		Angel Angel_out;
		Angel tempA1=Angel_out1-tempV1.GetXYAngel();
		Angel tempA2=Angel_out2-tempV1.GetXYAngel();
		if(fabs(tempA1.angel)>fabs(tempA2.angel))Angel_out=tempA1;
		else Angel_out=tempA2;
		if(fabs(Angel_out.angel-tempV1.GetXYAngel())<95.0)return;
		Vec Force;
		Force.Vx=80.0*cos(Angel_out.angel);
		Force.Vy=80.0*sin(Angel_out.angel);
		command.PutDrive(Force);
	}
}
void Skills::KickTo(Vec Force,bool& over)
{
	int rely=0;
	MoveState ball_move,MyMove;
	MyMove=world.GetMyState();
	ball_move=world.GetBallState();
	Vec temp1=ball_move.PositionV-MyMove.PositionV;
	Angel angel1(temp1.GetXYAngel());
	Angel angel2(Force.GetXYAngel());
	double  KickAble=param.AgentRadius+param.BallRadius+0.3;
	Vec attraction=Vec(0,0,0);
			//temp1*(180.0-fabs((angel1-angel2).angel));
	if(fabs((angel1-angel2).angel)<6&&temp1.GetMod()<KickAble)
	{
		over=true;
		double f=Force.GetMod();
		double angel_z=Force.GetZAngel();
		if(f>100.0)f=100.0;
		command.PutKick(0.0,f);
	}
	else if(fabs((angel1-angel2).angel)<5)
	{
		command.PutDrive(attraction);
	}
	else 
	{
		over=false;
		Force.SetMod(0.35);
		Vec pos=ball_move.PositionV-Force;
		double intercept_time;
		Vec intercept=world.InterceptBall(intercept_time,rely);
		if(rely>3)pos=intercept-Force;
		//Vec DriveForce=(pos-MyMove.PositionV)*40;
		//DriveForce.SetMod(30);
		if(fabs((angel1-angel2).angel)>155)
		{
			Line line;
			line=geometry.MakeLine(pos,MyMove.PositionV);
			Vec tempv=geometry.GetFoot(ball_move.PositionV,line);
			tempv=tempv-ball_move.PositionV;
			tempv.SetMod(4.0);
			pos=ball_move.PositionV+tempv;
		}
		DashTo(pos,attraction);
		//else command.PutDrive(DriveForce);
	}
}
void  Skills::FindBall()
{
	if(param.CycleSent==param.BallCycle)return;
	else 
	{
		double where=world.WhereBall();
		if(where>=0)
		{
			Angel angel1(90),angel2(0);
			command.TurnCamera(angel1,angel2);
		}
		else 
		{
			Angel angel1(-90),angel2(0);
			command.TurnCamera(angel1,angel2);
		}
	}
}


Skills::~Skills()
{
}


}