situation.cpp

浙江大学 RoboCup3D 2006 源代码

}



bool Situation::isBallInOurControl()



{



	if(!(opp_ITInfo[oppByITCyc[0]].nCyc <4||opp_ITInfo[oppByDist2Ball[0]].dDist2Ball < SS->ballSize+SS->playerSize + 0.1)



		&&



		(our_ITInfo[ourByITCyc[0]].nCyc <4||our_ITInfo[ourByDist2Ball[0]].dDist2Ball < SS->ballSize+SS->playerSize + 0.1)



		)



		return true;



	return false;



}

bool Situation::getPassVel(double & power, double & theta, double start, double end)
{
	bool isShoot;
	return getPassVel(power, theta, isShoot, start, end);
}

bool Situation::getPassVel(double &power, double & theta, bool &isShoot, double start, double end)
{
	isShoot = false;

	bool res = false;

	Vector3 goal(X(100),0);

	Vector3 goalour(-X(100),0);

	Vector3 g1(X(100),SS->mGoalWidth/2.);

	Vector3 g2(X(100),-SS->mGoalWidth/2.);

	Vector3 g3(-X(100),SS->mGoalWidth/2.);

	Vector3 g4(-X(100),-SS->mGoalWidth/2.);

	Vector3 ball=global.wm.ballPos+global.wm.ballVel;

	Vector3 agent=global.wm.nextPos;

	Angle kick_ang = getKickAngle();

	theta = 0;

	double len =20;

	Vector3 pos = getWhereToKickTo(start, end);

	double kick_value=pos.z;//value termination point of the pass action

	pos.z=0;

	Vector3 vcurpos_value=EvaluatePosVal(ball);

	double curpos_value=vcurpos_value.z;//value of current ball pos

	bool isballin_shootrange=false;

	if((ball-goal).mod()<18)  isballin_shootrange=true;

	if(global.wm.myNumber>=2&&global.wm.myNumber<=5)
	{
		if(kick_value<40&&vcurpos_value.x>50&&ball.getDistToOurGoal()>20)
		{
			power=0;theta=0;

			res=false;

			return res;
		}
		
		Angle a_b2ourG=(ball-goalour).ang();
		
		if(fabs(ball.y)>SS->mGoalWidth&&fabs(a_b2ourG)>45)
		{
			if((pos-ball).mod()>23)
              		pos=ball+Vector3(23,kick_ang,0,POLAR);

			if(fabs(pos.y)<7&&pos.x<-X(100)+12&&kick_value<60)
			{
				power=0;theta=0;

				res=false;

				return res;
			}
			
			if(ball.y<0)
			{
				if(kick_ang>75&&kick_ang<100&&(pos-ball).mod()>7)
				{
					power=0;theta=0;

					res=false;
	
					return res;

				}
					
			}
			else
			{
				if(kick_ang<-75&&kick_ang>-100&&(pos-ball).mod()>7)
				{
					power=0;theta=0;

					res=false;

					return res;
				}

			}

		}
	}
	
	if(global.wm.myNumber>=6&&global.wm.myNumber<=11)//////add by yzp
		if(kick_value<40&&isballin_shootrange==false&&ball.getDistToOurGoal()>20){//////////yzp0605
				Vector3 opp2me=global.wm.oppPos[0]-global.wm.nextPos;
				Vector3 goal2me=Vector3(X(100),0,0)-global.wm.nextPos;
				Vector3 ball2me=global.wm.ballPos+global.wm.ballVel-global.wm.nextPos;
				
				double ang_goal2me=Normalize(goal2me.ang());
				double ang_ball2me=Normalize(ball2me.ang());
				double test=fabs(ang_goal2me-ang_ball2me);
			
			if(global.wm.myPos.x>X(80)||fabs(ang_goal2me-ang_ball2me)>45||global.wm.myNumber>9){//////yzp0609

				power=0;theta=0;
				
				res=false;
				
				return res;

			}
			
			for(int k=0;k<11;k++){
				opp2me=global.wm.oppPos[k]-global.wm.nextPos;
				if(opp2me.mod()>2.5||fabs(Normalize(opp2me.ang()-goal2me.ang()))>80)
					continue;
				
				if((pos-global.wm.nextPos).ang()<80){
					if(kick_value>10){
						if(getKickPower(pos, theta, power)==0) {
							
							theta=30;
							
							power=100;
						}
						
					}else {
						theta=30;
						
						power=100;
						
					}
					
					res=true;
					
					return res;
				}
			}
			
			power=0;theta=0;
			
			res=false;
			
			return res;
		}

	if(ball.getDistToOurGoal()<16&&vcurpos_value.x<50&&(kick_value<55||(kick_value<70&&fabs(kick_ang)>85)))
	{
		Angle ang3=(ball-g3).ang()-5;
		Angle ang4=(ball-g4).ang()+5;
		Angle kick_ang2=Normalize(kick_ang+180);
		if( (agent.y>SS->mGoalWidth&&kick_ang2<ang3)||
		     (agent.y<-SS->mGoalWidth&&kick_ang2>ang4)||
		     (agent.y<SS->mGoalWidth&&agent.y>-SS->mGoalWidth&&kick_ang2>ang4+5&&kick_ang2<ang3-5))
		{
			res=true;

			power=100;

			if(ball.getDistToOurGoal()<8)

				theta=50;

			else

				theta=30;

			return res;

		}
	/*	res=true;

		power=100;

		if(fabs(kick_ang)<90||(global.wm.ballPos.x>-40&&fabs(global.wm.ballPos.y)>10))

			theta=30;

		else

			theta=50;

		return res;*/
	}

	if(global.wm.myNumber==1&&vcurpos_value.x<70&&kick_value<70)
	{
		res=true;

		power=100;

		if(fabs(kick_ang)<90||(global.wm.ballPos.x>-40&&fabs(global.wm.ballPos.y)>10))

			theta=30;

		else

			theta=50;

		return res;
	}

	double dist2goal=fabs((X(100)-ball.x)/cos(kick_ang/180*PI));

	if(isballin_shootrange||dist2goal<18)
	{

	//	Angle ang1 = (g1-ball).getDirection()+5;

	//	Angle ang2 = (g2-ball).getDirection()-5;

		Angle ang1 = (g1-ball).getDirection()-1;

		Angle ang2 = (g2-ball).getDirection()+1;

		if(kick_ang>ang2&&kick_ang<ang1)
		{
			isShoot=true;

	//		double dist = fabs((ball.x - X(100)) / cos(kick_ang * 3.14159 / 180.));

          //        theta = dist < 10 ? 4 : 10;

	   //       	power=100;

		  	getShootVel(theta, power,kick_ang);/////////yzp0529

			res=true;

			return res;

		}



		if((kick_value<50&&vcurpos_value.x>70)||kick_value<40)
		{
			res=false;

			return res;
		}

	}


	double goal_ang = (goal-ball).getDirection();

	if(fabs(Normalize(goal_ang-kick_ang))<40||fabs(kick_ang)<85)
	{
		Vector3 p(len,kick_ang,0,POLAR);

		p += ball;

		Angle ang1 = (g1-ball).getDirection()+10;

	       Angle ang2 = (g2-ball).getDirection()-10;

		if(p.isInField(1)||((ball-goal).mod()<25&&kick_ang>ang2&&kick_ang<ang1))

		res = true;

	}

	else
	{
		if(kick_value>50&&kick_value>vcurpos_value.x+10&&pos.x>global.wm.ballPos.x-15&&pos.getDistToOurGoal()>25) 
			res=true;
	}

	if(getKickPower(pos, theta, power)==0) 
	{
		theta=30;

		power=100;
	}
	if(global.wm.myNumber==1&&(kick_value<40||(kick_value<55&&agent.getDistToOurGoal()>10)))
	{
		theta=30; 
		
		power=100;
	}

	return res;


}



Angle Situation::getKickAngle() //amended by fx
{
	Angle ang = (global.wm.nextBallPos - global.wm.nextPos).ang();

	Vector3 nextBallPos(global.wm.nextBallPos.x, global.wm.nextBallPos.y);

	Vector3 nextAgentPos(global.wm.nextPos.x, global.wm.nextPos.y);

#ifdef WIN32
		if(global.wm.collisionFlag)
			ShowMessage("COLLISION between agent and ball!");

		nextBallPos = nextAgentPos -nextBallPos;

		nextBallPos = nextBallPos * 1000 + nextAgentPos;

		ShowCircle(global.wm.nextBallPos.x, global.wm.nextBallPos.y, 0.111, "128 0 0 2");

		ShowCircle(global.wm.nextPos.x, global.wm.nextPos.y, 0.22, "255 128 255 2");

		ShowLine(nextAgentPos.x, nextAgentPos.y, 0.2, nextBallPos.x, nextBallPos.y, 0.2, "255 120 0");

#endif

	return ang;

}




//返回的vector3 前两个量表示点的坐标，最后的量表示点的评价值

Vector3 Situation::getWhereToKickTo(double start, double end)
{
	Angle ang = getKickAngle();
	Vector3 p;
	Dist dist;
	double x,y;
#ifdef WIN32
	double dd=0.8;
	for(x=-50;x<50;x+=dd){
		for(y=-30;y<30;y+=dd){
			p = Vector3(x,y);
			Vector3 v3=EvaluatePassPoint(p);
			double v=v3.z;
			/*v = v*0.3;
			if(v>255)
				v=255;*/
			if(v<30) v=0;
			v=3*v;
			if(v>255) v=255;
			ShowRect(p.x-dd/8,p.y-dd/8,p.x+dd/8,p.y+dd/8,"%d %d 0 1",(int)(v),(int)(v),(int)(v));
//			Sleep(10);
		}
	}
#endif

	Vector3 target;
	double maxV=-1;
	double dTmp;

	if (start < 0) {
		start = 6;
		if(global.wm.ballPos.getDistToOurGoal() < 50)
			start = 4;
		if(global.wm.ballPos.getDistToOppGoal()<30)
			start = 2;
		if(global.wm.ballPos.getDistToOppGoal()<20)
			start = 0.5;
	}
	if (end < 0) {
		end = 25;
	}

	for(dist=start;dist<end;dist+=1){
		p = global.wm.ballPos+global.wm.ballVel+Vector3(dist,ang,0,POLAR);
		p.z=0;
#ifdef WIN32
		ShowCircle(p.x,p.y,0.111,"255 128 128 0");
#endif

		Vector3 vTemp=EvaluatePassPoint(p);
		dTmp = vTemp.z;
		if(dTmp>maxV){
			target = p;
			maxV = dTmp;
		}
	}

#ifdef WIN32
	ShowCircle(target.x,target.y,0.17,"255 255 128 1");
#endif

	if(maxV>30){
		target.z=maxV;          //把返回点的评价放在z里面
		return target;
	}

// step 2
	Vector3 bad = global.wm.ballPos+global.wm.ballVel+Vector3(40,ang,0,POLAR);

#ifdef WIN32	// add by zy
	ShowCircle(bad.x,bad.y,0.17,"255 0 255 1");
#endif

	bad.z=0;
	return bad;
}


Vector3 Situation::EvaluatePassPoint(Vector3 p)



{



//	if(p.x<15||p.x>SS->mFieldLength/2-12){

	if(p.x<15){

	int i;



	double distMinOur=100;



	int ourmin=global.wm.myNumber;

	
	int oppmin=1;


	double distMinOpp=100;



	double dTmp;



	Vector3 goalopp(X(100)-10,0);



	Vector3 goalour(-X(100)+10,0);



	AgentStatus a;

	

	Vector3 dest;



	Vector3 v;



	double vt,v_dist,v_pos,v_shootang,vp;//[1 , 0.4 , 0.2 , 0.3 , 0.1]权终

	

//in distance view

	for(i=0;i<11;i++){



		if(i+1==global.wm.myNumber&&global.wm.myNumber>5)

			continue;



		dTmp = (global.wm.ourPos[i]+global.wm.ourVel[i]/2- p).mod();//



		if((global.wm.ourPos[i] - goalopp).mod() < (p - goalopp).mod())



			dTmp += 1;



		if(dTmp<distMinOur){



			distMinOur = dTmp;

			ourmin=i;

			

		}



	}





	for(i=0;i<11;i++){

		

		dTmp = (global.wm.oppPos[i] +global.wm.oppVel[i]/2- p).mod();//



		if((global.wm.oppPos[i] - goalopp).mod() > (p - goalopp).mod())



			dTmp += 1;



		if(dTmp<distMinOpp){



			distMinOpp = dTmp;

			oppmin=i;

		}



	}





#ifdef WIN32



//		ShowCircle(p.x,p.y,ITCycMinOpp,"255 128 0 0");



//		ShowCircle(p.x,p.y,ITCycMinOur,"128 255 0 0");



#endif



	Angle agentToGoal=(global.wm.ourPos[ourmin]-Vector3(X(100),0)).ang();

	if(p.x<0) agentToGoal=(Vector3(-X(100),0)-global.wm.ourPos[ourmin]).ang();

	Angle agentToOpp=(global.wm.ourPos[ourmin]-global.wm.oppPos[oppmin]).ang();

	double k=200,a2=0.1;



	v_dist= k*exp(-a2*distMinOur)-k*exp(-a2*distMinOpp);



	if(v_dist<0)  v_dist=0;



	if(v_dist>150) v_dist=150;



	v_dist=v_dist/(37.5+v_dist*0.75);



	v_dist*=100;



	Angle ptoa=(global.wm.ourPos[ourmin]-p).getDirection();



	if(fabs(Normalize(ptoa-agentToGoal))>75) v_dist-=3;



	if(fabs(Normalize(ptoa-agentToGoal))>90)   v_dist-=3;



	if(fabs(Normalize(ptoa-agentToGoal))>135) v_dist-=3;



	if(fabs(Normalize(ptoa-agentToOpp))>100) v_dist+=3;

	

	if(fabs(Normalize(ptoa-agentToOpp))>135) v_dist+=3;



	if(fabs(Normalize(ptoa-agentToOpp))<45) v_dist-=3;

	

	//position and angle to the goal

	

	Dist distToGoalopp = (p - goalopp).mod();



	Dist distToGoalour = (p - goalour).mod();



	Angle angToGoalopp = (p - Vector3(X(100),0)).ang();



	Angle angToGoalour = (p - Vector3 (-X(100),0)).ang();



	double p_angopp,p_angour;

	

	if((p-Vector3(X(100),0)).mod()<10)  p_angopp=1;

	else p_angopp=10/(p-Vector3(X(100),0)).mod(); 



	if((p-Vector3(X(-100),0)).mod()<10)  p_angour=1;

	else p_angour=10/(p-Vector3(X(-100),0)).mod(); 

	

	if(p.x>0) 	v_shootang=0.04+0.03*p_angopp*(sinDeg(fabs(angToGoalopp)-90)+cosDeg(Normalize(angToGoalopp-agentToGoal)));



	if(p.x<0)  	v_shootang=0.06-0.06*p_angour*sinDeg(90-fabs(angToGoalour));



	v_pos=0.15+0.15*((distToGoalour-distToGoalopp)/(distToGoalour+distToGoalopp));



	vp=(v_pos+v_shootang)/0.004;



	vt=vp*0.4+v_dist*0.6;

	//vt=v_pos/0.003;



	v=Vector3(v_dist,vp,vt);



	if(p.isInField(0)==0)

		

		v=Vector3(0,0,0);



	Vector3 ball(global.wm.ballPos.x,global.wm.ballPos.y);



	Vector3 g1(X(100),SS->mGoalWidth/2.);

	Vector3 g2(X(100),-SS->mGoalWidth/2.);



	Angle ang1 = (g1-ball).getDirection();

	Angle ang2 = (g2-ball).getDirection();



	if(p.x>X(100)&&(p-ball).getDirection()>ang2&&(p-ball).getDirection()<ang1)

	{

		if(global.wm.ballPos.getDistToOppGoal()>25)

			v=Vector3(20,20,20);

		else{

			if(global.wm.ballPos.getDistToOppGoal()<16)

				v=Vector3(100,100,100);

			else

			{

				double vtmp=100-(global.wm.ballPos.getDistToOppGoal()-16)*80/9;

				v=Vector3(vtmp,vtmp,vtmp);

			}

		}

	}