situation.cpp

浙江大学 RoboCup3D 2006 源代码

	if(myPos.y>0)
		goal=goal1;
	else
		goal=goal2;
/*	
	if(maxv>40)
		return best;
	else if((myPos-goal).mod()<15)
		return goal;
	else
		return p_nearest;
*/
	Angle ahead=(goal-global.wm.nextPos).ang();
	Vector3 p0(global.wm.myPos.x+cos(ahead/360*2*3.14)*4,global.wm.myPos.y+sin(ahead/360*2*3.14)*4,0);

	#ifdef WIN32
		ShowCircle(p0,1,"255 255 255 2");
	#endif


	if((myPos-goal).mod()<15&&(best-goal).mod()>13||(global.wm.myPos-goal).mod()<10)
		return goal;
	else if(isDribbleSafe()&&(global.wm.myPos-goal).mod()<20)
		return	p0; 
	else if(maxv>40)
		return best;
	return p_nearest;



}



Angle Situation::getNextPanAng()
	
{
	
	Vector3 ball2me = global.wm.ballPos + global.wm.ballVel - global.wm.nextPos;

	Vector3 myPos=global.wm.nextPos;

	Angle angball2me= Normalize(ball2me.ang());	

	Angle nextPanAng=angball2me;

	Angle mypanang = Normalize(global.wm.myPanAngle);

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

	if(global.wm.myNumber==1){/////////////yzp0527
		
		Vector3 flag1(-X(105),-Y(100),0);

		Vector3 flag2(-X(105),Y(100),0);

		Vector3 flag(0,0,0);

		if(global.wm.ballPos.y>global.wm.myPos.y){//////yzp0531

			flag=flag2;

			Angle flag2me=Normalize((flag-myPos).ang());

			Angle minPanAng=flag2me-90;

			if(nextPanAng<minPanAng)
				nextPanAng=minPanAng;
				
		}else{

			flag=flag1;
			
			Angle flag2me=Normalize((flag-myPos).ang());

			Angle maxPanAng=flag2me+90;

			if(nextPanAng>maxPanAng)
				nextPanAng=maxPanAng;
		}
		
	}
	else if(!global.wm.ballPos.isInField(10)&&global.wm.ballPos.isInField(-10)
&&       ////////////yzp0529
shouldIIntercept()){

	Vector3 flag_all[8]={Vector3(-X(100),SS->mGoalWidth/2,0),
		Vector3(-X(100),-SS->mGoalWidth/2,0),
		Vector3(X(100),SS->mGoalWidth/2,0),
		Vector3(X(100),-SS->mGoalWidth/2,0),
		Vector3(-X(100),-Y(100),0),
		Vector3(-X(100),Y(100),0),
		Vector3(X(100),-Y(100),0),
		Vector3(X(100),Y(100),0),
		};
	Angle ang=-90;
	Angle ang2flag=0;
	int n=0;
	int max=0;
	Angle temp=0;
	Angle ang_margin=10;
	if((global.wm.ballPos-global.wm.myPos).mod()<5)
		ang_margin=30;
	
		for(ang;ang<=90;ang+=20){
			temp=Normalize(mypanang+ang);
			if(fabs(temp-angball2me)>90-ang_margin&&fabs(temp-angball2me-360)>90-ang_margin&&fabs(temp-angball2me+360)>90-ang_margin)
				continue;
			
			for(int i=0;i<8;i++){
				ang2flag=Normalize((flag_all[i]-myPos).ang());
				
				if(fabs(temp-ang2flag)<85||fabs(temp-ang2flag-360)<85||fabs(temp-ang2flag+360)<85)
					n++;	
			}
			if(max<n){
				max=n;

				nextPanAng=temp;
			}
			n=0;
		}
		
	}
	else{

	bool forward=false;
	bool back=false;
	bool left=false;
	bool right=false;
		
	Vector3 goalFL(X(95),SS->mGoalWidth/2,0);
	Vector3 goalFR(X(95),-SS->mGoalWidth/2,0);
	Vector3 goalBL(X(95),SS->mGoalWidth/2,0);
	Vector3 goalBR(X(95),-SS->mGoalWidth/2,0);
	Vector3 flagBR(-X(105),-Y(100),0);
	Vector3 flagBL(-X(105),Y(100),0);
	Vector3 flagFR(X(105),-Y(100),0);
	Vector3 flagFL(X(105),Y(100),0);
	Vector3 goal(0,0,0);

	if(fabs(ball.x)<=X(105)&&fabs(ball.x)>=X(95)){

		Vector3 goalFL(X(100),SS->mGoalWidth/2,0);
		Vector3 goalFR(X(100),-SS->mGoalWidth/2,0);
		Vector3 goalBL(X(100),SS->mGoalWidth/2,0);
		Vector3 goalBR(X(100),-SS->mGoalWidth/2,0);
		Vector3 flagBR(-X(100),-Y(100),0);
		Vector3 flagBL(-X(100),Y(100),0);
		Vector3 flagFR(X(100),-Y(100),0);
		Vector3 flagFL(X(100),Y(100),0);
	}
		

	if(ball.x>=myPos.x)
		forward=true;
	else
		back=true;
	
	if(ball.y>=myPos.y)
		left=true;
	else
		right=true;

	if(myPos.x>0){
		if(forward&&left)
			goal=goalFR;
		else if(forward&&right)
			goal=goalFL;
		else if(back&&left)
			goal=flagFL;
		else if(back&&right)
			goal=flagFR;
		}
	else{
		if(forward&&left)
			goal=flagBL;
		else if(forward&&right)
			goal=flagBR;
		else if(back&&left)
			goal=goalBR;
		else if(back&&right)
			goal=goalBL;
	}

	Angle goal2me=Normalize((goal-myPos).ang());

	Angle limPanAng;



	if(myPos.x>0){
		if(forward&&left)
			limPanAng=goal2me+90;
		else if(forward&&right)
			limPanAng=goal2me-90;
		else if(back&&left)
			limPanAng=goal2me+90;
		else if(back&&right)
			limPanAng=goal2me-90;
		}
	else{
		if(forward&&left)
			limPanAng=goal2me-90;
		else if(forward&&right)
			limPanAng=goal2me+90;
		else if(back&&left)
			limPanAng=goal2me-90;
		else if(back&&right)
			limPanAng=goal2me+90;
	}

	bool min;
	if(myPos.x>0){
		if(forward&&left)
			min=false;
		else if(forward&&right)
			min=true;
		else if(back&&left)
			min=false;
		else if(back&&right)
			min=true;
		}
	else{
		if(forward&&left)
			min=true;
		else if(forward&&right)
			min=false;
		else if(back&&left)
			min=true;
		else if(back&&right)
			min=false;
	}

	if(min){
		if(nextPanAng<limPanAng)
			nextPanAng=limPanAng;
		}
	else{
		if(nextPanAng>limPanAng)
			nextPanAng=limPanAng;
		}
	}
	
	return nextPanAng;

}


void Situation::getAngInc(Angle & panAngInc, Angle & tiltAngInc)
{

	Vector3 ball2me = global.wm.ballPos + global.wm.ballVel - global.wm.nextPos;
       	
	Angle nextPanAng=getNextPanAng();

	Angle mypanang = global.wm.myPanAngle;	


	if(global.wm.noball>=2){
		panAngInc=90;
	}
	else
		panAngInc = Normalize(nextPanAng- mypanang);

	
	if(panAngInc > 90)
		panAngInc = 90;
	if(panAngInc < -90)
		panAngInc = -90;



	Angle tiltAng_ball2me = asinDeg(ball2me.z / ball2me.mod());

	Angle mytiltang = global.wm.myTiltAngle;

	tiltAngInc = tiltAng_ball2me - mytiltang;

	tiltAngInc = Normalize(tiltAngInc);

/*
	if(tiltAngInc > 10)
		tiltAngInc = 10;
	else if(tiltAngInc < -10)
		tiltAngInc = -10;
*/
	if(tiltAngInc<=10&&tiltAngInc>=-10 )
		return;
	else if(tiltAngInc > 10)
		tiltAngInc = 10;
	else if(tiltAngInc < -10)
		tiltAngInc = -10;
	else
		tiltAngInc=0;
		

}



void Situation::getShootVel(Angle & theta, double & power,Angle kick_ang)//////////////yzp0529
{
	
	Vector3 ball=global.wm.nextBallPos;

	Vector3 myPos=global.wm.nextPos;

	bool goaliedef=false;

	bool flipshoot=false;

	bool ingoaliectrl=false;

	Angle shootAng=kick_ang;



	Angle ang2Goalie=Normalize((global.wm.oppPos[0]-ball).ang());

	Angle t_ang=fabs(shootAng-ang2Goalie);

	double dist2Goalie=(global.wm.oppPos[0]-ball).mod();


	#ifdef WIN32
		ShowCircle(global.wm.oppPos[0],1.02,"0 0 0 2");
	#endif
	
	double margin=1;

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

	if(dist2Goalie>15)
		margin=2;
	else if(dist2Goalie>12)
		margin=1.8;
	else if(dist2Goalie>8)
		margin=1.5;
	else
		margin=1.2;

	if(dist2Goalie<5&&t_ang<20)
		goaliedef=true;

	if(dist2Goalie*sin(t_ang/180*PI)<=margin)
		ingoaliectrl=true;

	if(dist2goal-dist2Goalie>3)
		flipshoot=true;

	if(goaliedef){

		if(dist2goal>10){
			theta = 50;
	       	power=100;
			}
		else{

			if(flipshoot){
				theta = 50;
	       		power=80;
				}
			else{
				if(dist2Goalie<3){
				theta=30;
				power=80;
				}
				else{
				theta=30;
				power=100;
				}
					
			}
			
			}

		}
	else if(!ingoaliectrl){

		theta = 10;
	       power=100;

		}
	else if(flipshoot){
		if(dist2goal<6){
			theta=50;
			power=80;
			}
		else if(dist2goal<=11){
			theta=30;
			power=80;
			}
		else if(dist2goal>11){
			theta=35;
			power=100;
			}

		}
	else{ 

		if(dist2goal<12&&dist2goal>6){
			theta=24;
			power=100;
			}
		else if(dist2goal<=6){
			theta=20;
			power=100;
			}
		else{
			theta=30;
			power=100;
			}
		}
}


BallStatus Situation::getBallStatusAfterCollisionWithMe(const int nCycT1, const Vector3 ballPosT1, const Vector3 ballVelT1)
{
	Vector3 ballPosAfterCollision;
	Vector3 ballVelAfterCollision;
	Vector3 ballPos;
	Vector3 ballVel;
	BallStatus ball;
	Angle angleBallAgent;
	Vector3 ballVelBeforeCollision;
	int nCycT1Left;
	nCycT1Left = 20 - nCycT1;

	Vector3 addAgentVel;
	addAgentVel =  ((global.wm.nextPos - global.wm.myPos)/20).rotate2d(180);
	ballVelBeforeCollision.x = addAgentVel.x + ballVelT1.x / ss.B_decay_t1;
	ballVelBeforeCollision.y = addAgentVel.y + ballVelT1.y / ss.B_decay_t1;
	
	//碰撞之后的状态
	Vector3 ballPos0;
	ballPos0 = ballPosT1;
	angleBallAgent = (ballPos0-(global.wm.myPos + ((global.wm.nextPos - global.wm.myPos)/20)*nCycT1)).ang();
	ballVelBeforeCollision.rotate2d(-Normalize(angleBallAgent-90));
	ballVelAfterCollision.x = ballVelBeforeCollision.x * 5 / 7;
	ballVelAfterCollision.y = ballVelBeforeCollision.y * (-ss.e);	
	ballVelAfterCollision.rotate2d(Normalize(angleBallAgent-90));
	ballPosAfterCollision = ballPosT1;
	#ifdef WIN32
		ShowLine((global.wm.myPos + ((global.wm.nextPos - global.wm.myPos)/20)*nCycT1),5,Normalize(angleBallAgent-90),"255 255 0"); 
		ShowLine((global.wm.myPos + ((global.wm.nextPos - global.wm.myPos)/20)*nCycT1),5,angleBallAgent,"255 255 0"); 
		Vector3 tt;
		tt.x = ballPosAfterCollision.x + ballVelAfterCollision.x;
		tt.y = ballPosAfterCollision.y + ballVelAfterCollision.y;
		tt.z = ss.ballSize;
		ShowCircle(tt,0.111,"255 255 255 2");
	#endif

	//碰撞之后到0.2s时单位为0.01的末状态
	double sum1;
	sum1 = getSumGeomSeries(1, ss.B_decay_t1, nCycT1Left);
	ballPos.x = ballPosAfterCollision.x + sum1*ballVelAfterCollision.x;
	ballPos.y = ballPosAfterCollision.y + sum1*ballVelAfterCollision.y;
	ballPos.z = ss.ballSize;
	ballVel.x = pow(ballVelAfterCollision.x, nCycT1Left);
	ballVel.y = pow(ballVelAfterCollision.y, nCycT1Left);

	//速度改为单位为0.2的状态
	ballVel = t1Tot20(ballVel);
	ballVel = 0;

	//将球的状态赋给StatusBall型变量ball
	ball.pos = ballPos;
	ball.vel= ballVel;
	#ifdef WIN32
		ShowCircle(ball.pos,0.111,"0 255 0 2");
	#endif

	return ball; 
}

bool Situation::isBallInCollisionWithMe(int & nCycT1, Vector3 &ballPosT1, Vector3 &ballVelT1)
{
	Circle ball;
	Circle agent;
	Vector3 agentSpeedT1;
	Vector3 tmp;

	agentSpeedT1= (global.wm.nextPos - global.wm.myPos)/20;
	nCycT1 = 0;
	ballVelT1 = t20Tot1(global.wm.ballVel);
	ballPosT1 = global.wm.ballPos;
	
	for(int i=0; i<20; i++){
		ballPosT1 = ballPosT1 + ballVelT1;
		ballVelT1.x = ballVelT1.x* ss.B_decay_t1;
		ballVelT1.y = ballVelT1.y* ss.B_decay_t1;
		ballVelT1.z = ballVelT1.z*ss.B_decay_t1 -ss.g*square_t1;
		nCycT1 = i+1;
		tmp = ballPosT1;
		if(tmp.z < 0.25)
			tmp.z = ss.playerSize;
		
		ball.setCenter(tmp);
		ball.setRadius(ss.ballSize);
		agent.setCenter(global.wm.myPos + agentSpeedT1*(i+1));
		agent.setRadius(ss.playerSize);

#ifdef WIN32
		ShowCircle(tmp,0.111,"255 0 0 2");
		ShowCircle(global.wm.myPos + agentSpeedT1*(i+1),0.22,"255 0 0 2");
#endif
		
		if(isBallOverlapAgent(ball, agent) && tmp.z==ss.playerSize)
			return 1;
	}
	
	return 0;
}

bool Situation::isBallInCollisionWithMe()
{
	int nCyc;
	Vector3 ballPos;
	Vector3 ballVel;
	if(isBallInCollisionWithMe(nCyc,ballPos,ballVel))
		return 1;
	else
		return 0;	
}

BallStatus Situation::getBallStatusAfterCollisionWithMe()
{
	int nCyc;
	Vector3 ballPos;
	Vector3 ballVel;
	BallStatus ballstatus;
	if(isBallInCollisionWithMe(nCyc,ballPos,ballVel))
	{
		return getBallStatusAfterCollisionWithMe(nCyc, ballPos, ballVel);
	}
	else
	{
		ballstatus.pos = global.wm.ballPos;
		ballstatus.vel = global.wm.ballVel;
		return ballstatus;
	}
}

bool Situation::isBallOverlapAgent(Circle ball, Circle agent)
{
    double x0, y0, r0;
    double x1, y1, r1;

    x0 = ball.getCenter( ).x;
    y0 = ball.getCenter( ).y;
    r0 = ball.getRadius( );
    x1 = agent.getCenter( ).x;
    y1 = agent.getCenter( ).y;
    r1 = agent.getRadius( );

    double      d, dx, dy,  a;

    dx = x1 - x0;
    dy = y1 - y0;
    d = sqrt(dx*dx + dy*dy);

    a = (r0*r0 + d*d - r1*r1) / (2.0 * d);

    double      arg = r0*r0 - a*a;

    return (arg < 0.0) ? 0 : 1;
}

BallStatus Situation::getBallStatusAfterCollisionWithField(const int nCycT1, const Vector3 ballPosT1, const Vector3 ballVelT1)
{
	BallStatus ballstatus;
	Vector3 ballVel;
	Vector3 ballPos;
	Vector3 ballVelBeforeCollision;
	Vector3 ballPosAfterCollision;
	Vector3 ballVelAfterCollision;
	int nCycT1Left;
	nCycT1Left = 20 - nCycT1;
	
	ballVelBeforeCollision.x = ballVelT1.x / ss.B_decay_t1;
	ballVelBeforeCollision.y = ballVelT1.y / ss.B_decay_t1;
	ballVelBeforeCollision.z = (ballVelT1.z + ss.g * square_t1) / ss.B_decay_t1;
	
	ballVelAfterCollision.x = ballVelBeforeCollision.x * 1.2246;
	ballVelAfterCollision.y = ballVelBeforeCollision.y * 1.2246;
	ballVelAfterCollision.z = ballVelBeforeCollision.z * (-ss.e);
	ballPosAfterCollision = ballPosT1;

	#ifdef WIN32
		Vector3 tt;
		tt.x = ballPosAfterCollision.x + ballVelAfterCollision.x;
		tt.y = ballPosAfterCollision.y + ballVelAfterCollision.y;
		tt.z = ballPosAfterCollision.z + ballVelAfterCollision.z;
		ShowSphere(tt.x,tt.y,tt.z,0.111,"255 255 255");
	#endif

	Vector3 tmpPos;
	Vector3 tmpVel;
	tmpPos = ballPosAfterCollision;
	tmpVel = ballVelAfterCollision;
	for(int j = 0; j < nCycT1Left; j++){
		tmpPos = tmpPos + tmpVel;
		tmpVel.x = tmpVel.x * ss.B_decay_t1;
		tmpVel.y = tmpVel.y * ss.B_decay_t1;
		tmpVel.z = tmpVel.z * ss.B_decay_t1 - ss.g*square_t1;
	}
	ballPos = tmpPos;
	ballVel = tmpVel;
	
	ballVel= t1Tot20(ballVel);


	ballstatus.pos = ballPos;
	ballstatus.vel= ballVel;
	#ifdef WIN32
		ShowSphere(ballstatus.pos.x ,ballstatus.pos.y ,ballstatus.pos.z ,0.111,"0 255 0");
	#endif

	return ballstatus; 	
}


bool Situation::isBallInCollisionWithField(int & nCycT1, Vector3 &ballPosT1, Vector3 &ballVelT1)
{
	nCycT1 = 0;
	ballVelT1 = t20Tot1(global.wm.ballVel);
	ballPosT1 = global.wm.ballPos;
	for(int i=0; i<20; i++){
		ballPosT1 = ballPosT1 + ballVelT1;
		ballVelT1.x = ballVelT1.x * ss.B_decay_t1;
		ballVelT1.y = ballVelT1.y * ss.B_decay_t1;
		ballVelT1.z = ballVelT1.z * ss.B_decay_t1 - ss.g * square_t1;
		nCycT1 = i+1;
		if(ballPosT1.z < 0.111)
			return 1;
	}

	return 0;
}


Vector3 Situation::t20Tot1(const Vector3 V20)
{
	Vector3 V1;
	double sum1,sum2;
	int j;
	double temp;
	sum1 = getSumGeomSeries(1, ss.B_decay_t1, 20);
	sum2 = 0;
	for(j=0; j<=18; j++)
	{
		temp = (20 - j - 1) * pow(ss.B_decay_t1, j);
		sum2 = sum2 + temp;
	}
	V1.x = V20.x/sum1;
	V1.y = V20.y/sum1;
	V1.z = (V20.z+sum2*ss.g*square_t1)/sum1;

	return V1;
}

Vector3 Situation::t1Tot20(const Vector3 V1)
{
	Vector3 V20;
	double sum1,sum2;
	int j;
	double temp;
	sum1 = getSumGeomSeries(1, ss.B_decay_t1, 20);
	sum2 =0;
	for(j=0; j<=18; j++)
	{
		temp = (20 - j - 1) * pow(ss.B_decay_t1, j);
		sum2 = sum2 + temp;
	}
	V20.x = sum1*V1.x;
	V20.y = sum1*V1.y;
	V20.z = sum1*V1.z - sum2*ss.g*square_t1;

	double m = sum2*ss.g*square_t1;
	V20.x = V20.x /pow(ss.B_decay_t1, 20);
	V20.y = V20.y /pow(ss.B_decay_t1, 20);
	V20.z = (V20.z + m + sum1*sum1*ss.g*square_t1)/pow(ss.B_decay_t1, 20) - m;

	return V20;
}


Vector3 Situation::EvaluatePosVal(Vector3 p)
{
	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++){

		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;

		}

	}

	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(f