strategy.cpp

robocup源代码2001年清华机器人源代码

		//filter out players that do not intend to get ball								
		ofpos=fm.GetOffensivePoint(FP_attackness[Idx(i)],FP_leftness[Idx(i)],ball.pos);
		player_i_pos_dis=ofpos.dist(ball.pos);
		player_i_cur_dis=MyPlayer(i).pos.dist(ball.pos);
		if (player_i_pos_dis>1.2*min_pos_dis) continue;
		if (min_pos_dis>1.2*player_i_pos_dis)
		{
			select=i;
			min_pos_dis=player_i_pos_dis;
			min_cur_dis=player_i_cur_dis;
		}
		else
		{
			if (player_i_cur_dis>1.2*min_cur_dis) continue;
			if (min_cur_dis>1.2*player_i_cur_dis||i>select)
			{
				select=i;
				min_pos_dis=player_i_pos_dis;
				min_cur_dis=player_i_cur_dis;
			}
		}
	}
	return select;
}


bool PlaceKick::CanExecute(){
	float passpriority;
	int i,bestpass;
	if (!(situation.IsSetPlayState(SPST_Executing)||RecentSeen()))
	{
		DoLog(18,"Cannot Exec 1");
		return false;
	}
	//Wait for best chance to pass
	canpass=motion.SmartPass();
	float maxpriority=0.5f;
	DoLog(18,"num pass %.0f",(float)motion.num_passinfo);
	for(i = 0; i < motion.num_passinfo; i++){
		if (motion.passinfo[i].PassSpeed()<SP_kickable_margin*0.7f)
			continue;
		passpriority =motion.GetPassPriority(motion.passinfo[i]);
		if(passpriority>maxpriority){
			maxpriority=passpriority;
			bestpass=i;
		}
	}
	if( maxpriority >0.5f ){
		motion.logpass=motion.passinfo[bestpass];
		DoLog(18,"Can pass");
		canpass = true;
	}
	else
		canpass = false;
	if (canpass) return true;
	if (Counter>CP_setplay_wait_time||situation.IsSetPlayState(SPST_Executing)) return true; 
		else 
		{
			return false;
		}

}

bool PlaceKick::ExecutorGo(){
	if (ball.kickable())
		return false;
	Vector Execpos=ball.pos+Polar2Vector(CP_reliable_kickarea-SP_ball_size,GetFaceAngle());
	bool success = motion.smartgoto(Execpos,CP_setplay_speed);
	if (!success){
		float runspeed = ball.kickable() ? Execpos.dist(Self.pos)*(1 - SP_player_decay):Execpos.dist(Self.pos);

		if (Execpos.dist(Self.pos)>0.1f || !ball.kickable())
		{
			if (fabs(NormalizeAngle((Execpos-Self.pos).Angle()-Self.bodyfacing))>15)
				motion.turn_to(Execpos);
			else
				motion.run(runspeed);
			return true;
		}
		else
		{
			DoLog(18,"Already get ball");
			return false;
		}
	}
	else
		return true;
}

AngleDeg PlaceKick::GetFaceAngle(){
	Vector pos0(Self.pos.x,0),pos1,pos2;
	AngleDeg ang0 = 0,ang1,ang2;
	float w0,w1,w2;

	if (Self.pos.y>0)
	{
		pos1 = Vector(Self.pos.x,SP_pitch_width/4);
		ang1 = (fieldinfo.l_theirgoalpost-pos1).Angle();
		pos2 = Vector(Self.pos.x,SP_pitch_width/2);
		ang2 = -90;
	}
	else{
		pos1 = Vector(Self.pos.x,-SP_pitch_width/4);
		ang1 = (fieldinfo.r_theirgoalpost-pos1).Angle();
		pos2 = Vector(Self.pos.x,-SP_pitch_width/2);
		ang2 = 90;
	}
	w0 =1.0f/( (pos0 - Self.pos).mod2()+0.01f);
    w1 =1.0f/( (pos1 - Self.pos).mod2()+0.01f);
    w2 =1.0f/( (pos2 - Self.pos).mod2()+0.01f);
    return (w0*ang0+w1*ang1+w2*ang2)/(w0+w1+w2);
}

void PlaceKick::Execute(){
	Command command;
	//move to the ball
	if (ExecutorGo()) 
	{
		DoLog(18,"Going to ball");
		return;
	}
	
	if (CanExecute())
	{
		situation.ChangeSetPlay(SPST_Executing);
		if (canpass)
		{
			DoLog(18,"Pass");
			KT_Res passok = motion.pass(motion.logpass);
		}
		else
		{
			DoLog(18,"Dribble");
			motion.k_StepDribble(GetFaceAngle(), 90.0f, 1.0f); 
		}
	}
	else
	{
		DoLog(18,"Cannot execute");
		my_error("Cannot execute");
	}
}


void PlaceKick::going(){
	UNum i;
	if (!ball.pos_valid())
		return;
	else
	{		
		switch (situation.PlaceKickState) {
		case SPST_Initialize:
			Counter=0;
			for (i=1;i<=SP_team_size;i++)
				if (MyPlayer(i).pos_valid())
					Trackmark[i]=MyPlayer(i).pos;
				else 
					Trackmark[i]=Vector(SP_pitch_length,SP_pitch_width);
			for (i=1;i<=SP_team_size;i++)
				HeadingToBall[i]=1.0f;
			situation.ChangeSetPlay(SPST_Prego);
		case SPST_Prego:	
			
			Executor=JudgeExecutor();
			DoLog(18,"Executor %.0f",(float)Executor);
			motion.B_evaluate();	//initialization
			
			if (Executor==MyNumber)
			{
				if (!ExecutorGo())
				{
					situation.ChangeSetPlay(SPST_ExecWait);
					Counter = Max(2 * CP_setplay_wait_time - (SP_drop_ball_time + situation.ModeChangedTime - situation.CurrentTime),0);
				}
			}
			
			else
			{
				motion.Offense();
						
				if (IsMyPlayer(situation.ballcontroller)&&(situation.ballcontroller==MyNumber||
					MyPlayer(situation.ballcontroller).balldist<0.9*ball.distance)
					&&(situation.IsGoalieKick || fieldinfo.mygoalie != situation.ballcontroller))
				{
					situation.ChangeSetPlay(SPST_ExecWait);
					Counter = Max(2 * CP_setplay_wait_time - (SP_drop_ball_time + situation.ModeChangedTime - situation.CurrentTime),0);
				}
			}
			
			break;
		case SPST_ExecWait:
		case SPST_Executing:
			
			my_error("On Execute %d",Executor);
			DoLog(18,"On Execute %.0f",(float)Executor);
			if (Executor==MyNumber)
				Execute();			
			else
				motion.Offense();	
			break;
		case SPST_AfterExec:
			DoLog(18,"(SP)Should Not get here!!");
			my_error("Should not get here!!");
			break;
		default:
			break;
		}
		Counter++;
		DoLog(18,"(SP) Counter %.0f",(float)Counter);

	}
}

GoaliePlaceKick::GoaliePlaceKick(){
MovePoint[0]=Vector(-47,-15);
MovePoint[1]=Vector(-47,15);
MovePoint[2]=Vector(-40,-10);
MovePoint[3]=Vector(-40,10);
MovePoint[4]=Vector(-43.5,-15);
MovePoint[5]=Vector(-43.5,15);
movetime = -1;
}

float GoaliePlaceKick::CalcAngle(Vector ATpos,Vector DFpos){
	/*以ATPOS点所面向的角度与守门员－球门张角之比
		作为危险度的估算量，符合
		1。守门员远离球门移动，函数单调增
		2。进攻队员远离球门移动，函数单调减
		3。守门员对进攻队员封堵角度越好，函数值越小
	*/
	AngleDeg dfangle=float(fabs(Ray(DFpos,(fieldinfo.l_mygoalpost-DFpos).Angle()).StrechAngle(fieldinfo.r_mygoalpost)));
	if (ATpos.dist(DFpos)<9) 
		ATpos=DFpos+Polar2Vector(9,(ATpos-DFpos).Angle());
	Ray lcourse(ATpos,(fieldinfo.l_mygoalpost-ATpos).Angle());
	Ray rcourse(ATpos,(fieldinfo.r_mygoalpost-ATpos).Angle());
	AngleDeg atangle1=float(fabs(lcourse.StrechAngle(DFpos)));
	AngleDeg atangle2=float(fabs(rcourse.StrechAngle(DFpos)));
	if (atangle1+atangle2>dfangle) return 1;
	else return (atangle1+atangle2)/dfangle;
}

float GoaliePlaceKick::CalcPass(Vector pos){
	PassInfo currinfo;
	Ray ballcourse;
	float bestpr,currpr;
	float factpasspr,factintrdange,factreceivedange,factcontrol;
	float banang,bangoalcross,banstrive,banclose,bancriticarea;

	Vector dangerpos;
	PassInfo potninfo;
	bestpr=-1.0f;
	//Ray goalieray(fieldinfo.mygoal,(pos-fieldinfo.mygoal).Angle());
	DoLog(18,"Numpasses %.0f",(float)motion.num_passinfo);
	for (UNum i =0;i<motion.num_passinfo;i++)
	{
		currinfo=motion.passinfo[i];
		banang = LowPassFilter(85,95,(float)fabs(currinfo.PassAngle()));
		if (fabs((pos-fieldinfo.mygoal).Angle())< 10)
			bangoalcross = 1.0f;
		else
		bangoalcross = pos.y<0 ? LowPassFilter(-5,5,currinfo.PassAngle()):HighPassFilter(-5,5,currinfo.PassAngle());
		banclose = HighPassFilter(CP_closeinterception_max_cycles-1.0f,(float)CP_closeinterception_max_cycles,currinfo.MinReceiverCycles())
					*HighPassFilter(CP_goalie_kick_min_vel-0.1f,CP_goalie_kick_min_vel,currinfo.PassSpeed());
		bancriticarea = LowPassFilter(0.85f,0.9f,fieldinfo.GetDefensiveSensitivity(currinfo.ReceiveBallPoint()));
		//banstrive = HighPassFilter(currinfo.MinReceiverCycles(),currinfo.MinReceiverCycles()+1.0f,currinfo.MinOpponentCycles());
		banstrive = 1.0f;
		factcontrol=(float)(1-Sqr(currinfo.MinReceiverCycles()/currinfo.MinOpponentCycles()));
		ballcourse.SetValues(pos,currinfo.PassAngle());
		dangerpos=ballcourse.GetClosestPoint(currinfo.MostDangerOpponentPos());
										//中途被截的危险性
		factpasspr=motion.GetPassPriority(currinfo);//传球评价
		if (factpasspr < 0) factpasspr = 0;
		if (ballcourse.OnRay(dangerpos) && currinfo.MostDangerOpponent()!=-1)
				factintrdange=2*CalcAngle(dangerpos,pos)*(float)sqrt(1-fieldinfo.GetDefensiveSensitivity(dangerpos));
			else 
				factintrdange=0;
		factreceivedange=2*CalcAngle(currinfo.ReceiveBallPoint(),pos)*fieldinfo.GetDefensiveSensitivity(currinfo.ReceiveBallPoint());
										//在得球后丢球的危险性
		currpr=0.3f*factpasspr+0.2f*(1 - factintrdange)+0.3f*(1 - factreceivedange)+0.2f*factcontrol;
		
		currpr *= banang * bangoalcross * banstrive * banclose * bancriticarea;

		if (bestpr<currpr)
		{
			bestpr=currpr;
			motion.logpass=currinfo;
		}

	}
	return bestpr;

}

float GoaliePlaceKick::GetControl(int mynum,int oppnum)
{
	if (oppnum==0) return 1.0f;
	float factop,factmine;
	if (oppnum==1) factop=0.2f;
	else if (oppnum==2) factop=0.5f;
	else factop=0.8f;

	if (mynum==1) factmine=0.1f;
	else if (oppnum==2) factmine=0.3f;
	else factmine=0.4f;

	if (factmine>factop) return 1.0f;
	else
		return 1.0f+factmine-factop;

}

float GoaliePlaceKick::CalcPoint(Vector pos){
	if (fieldinfo.my_goaltenderbox.IsWithin(pos))
		return 0;
	AngleDeg direction=NormalizeAngle((pos-fieldinfo.mygoal).Angle());
	int N_N_Teammates=fieldinfo.NumTeammatesInCone(pos,direction,15,150);
	int N_M_Teammates=fieldinfo.NumTeammatesInCone(pos,direction,30,150)-N_N_Teammates;
	int N_F_Teammates=fieldinfo.NumTeammatesInCone(pos,direction,40,150)-N_M_Teammates-N_N_Teammates;
	int N_N_Opponents=fieldinfo.NumOpponentsInCone(pos,direction,15,150);
	int N_M_Opponents=fieldinfo.NumOpponentsInCone(pos,direction,30,150)-N_N_Opponents;
	int N_F_Opponents=fieldinfo.NumOpponentsInCone(pos,direction,40,150)-N_M_Opponents-N_N_Opponents;
	return GetControl(N_N_Teammates,N_N_Opponents)*0.5f+GetControl(N_M_Teammates,N_M_Opponents)*0.3f
		+GetControl(N_F_Teammates,N_F_Opponents)*0.2f;
}

AngleDeg GoaliePlaceKick::MyFaceAngle(Vector pos){
	return 0;
	AngleDeg res = NormalizeAngle((pos-fieldinfo.mygoal).Angle());
	if (res>45) res = 45;
	if (res<-45) res = -45;
	return res;
}

bool GoaliePlaceKick::ShouldMove(){
	UNum j,bestpnt=0;
	float calcpr,movepr,nativepr;
	if (situation.SightTime <= movetime)
		return false;
	if (situation.playmode!=PM_My_Free_Kick||movecount == 0)
		return false;
	else
	{
		if (passpriority>0.3f) return false;
		if (!RecentSeen())
		return false;

		nativepr = CalcPoint(Self.pos);
		DoLog(18,"(GSP)native (%f %f) calcvalue %f",Self.pos.x,Self.pos.y,nativepr);
		movepr=0.0f;
		for (j=0;j<CP_NumGoalieMovePoint;j++)
		{
			calcpr=CalcPoint(MovePoint[j]);
			DoLog(18,"(GSP)point (%f %f) calcvalue %f",MovePoint[j].x,MovePoint[j].y,calcpr);
			if (movepr<calcpr)
			{	bestpnt=j;
				movepr=calcpr;
			}
		}

		if ((movecount==SP_goalie_max_moves && movepr>1.1*nativepr || movepr>1.2*nativepr
			||deadtime>=CP_setplay_wait_time-3)&&MovePoint[bestpnt].dist(Self.pos)>4)
		{
			DoLog(18,"(GSP)Move %.0f to (%f %f) ",3.0f-movecount,MovePoint[bestpnt].x,MovePoint[bestpnt].y);
			action.move(MovePoint[bestpnt]);
			movetime = situation.CurrentTime;
			deadtime = Max(2 * CP_setplay_wait_time - (SP_drop_ball_time + situation.ModeChangedTime - situation.CurrentTime),0);
			movecount--;
			return true;
		}
		else
		{
			return false;
		}
	}
}

bool GoaliePlaceKick::CanKickOut(){
	if (motion.Num_MyVisiblePlayers() > 0 && motion.MyPlayer_Close2Me(0).distance < 6.0f)
	{
		DoLog(18,"Teammate too close ");
		return false;
	}
	if (!(RecentSeen()||situation.IsSetPlayState(SPST_Executing))||(situation.SightTime <= movetime))
		{
			DoLog(18,"No Start without sight %d",situation.SightTime);
			return false;
		}
	if ((action.cancatch(situation.CurrentTime) || situation.playmode == PM_My_Goal_Kick) && passpriority>0.2f)
		return true;
	else 
		return false;

}

bool GoaliePlaceKick::KickOut(){
	DoLog(18,"Pass to angle %.2f receiver %.0f speed %.2f",motion.logpass.PassAngle(),
		float(motion.logpass.Receiver()),motion.logpass.PassSpeed());
	KT_Res passok = motion.pass(motion.logpass);
	motion.GoalieJustKicked = true;
	situation.ChangeSetPlay(SPST_Executing);
	mediator.SetViewWidth(VW_Normal);
	return true;
}

 void GoaliePlaceKick::going(){
	if (!ball.pos_valid())
		return;
	UNum i;
	float mvspeed=0;
	if (situation.IsGoalieKick)
	{				
		if (situation.IsSetPlayState(SPST_Initialize))
			deadtime = 0;
		if (ball.kickable())
		{	
		    if (!situation.IsSetPlayState(SPST_Executing))
				situation.ChangeSetPlay(SPST_ExecWait);
						
			motion.B_evaluate();
			motion.SmartPass();
			passpriority=CalcPass(Self.pos);
			DoLog(18,"pass priority %f",passpriority);

			if (!ShouldMove()) 
			{			
				DoLog(18,"(GSP)No move required");
				if (fabs(NormalizeAngle(MyFaceAngle(Self.pos) - Self.bodyfacing)) > 15.0f  
					&& !situation.IsSetPlayState(SPST_Executing))  //additional of visual
					motion.turn_to(MyFaceAngle(Self.pos));
				else
				if(CanKickOut())
				{
					DoLog(18,"(GSP)Good Chance to start");
					KickOut();
				}
				else
				{  
					/*没有传球机会时，看场面的变化，如果己方球员仍在移动，则
					可能拉出传球空档，等待，否则开大脚破坏*/
					for (i=0;i<motion.Num_MyVisiblePlayers();i++)
						mvspeed+=MyPlayer(motion.MyVisiblePlayers[i]).speed;
					mvspeed/=motion.Num_MyVisiblePlayers();
					if (mvspeed>0.1)  deadtime=Max(2 * CP_setplay_wait_time - (SP_drop_ball_time + situation.ModeChangedTime - situation.CurrentTime),0); 
					else deadtime++;
					DoLog(18,"Teammate Unresponsed %.0f",(float)deadtime);
					if (deadtime>CP_setplay_wait_time&&RecentSeen()
						//ball.speed_valid()&&ball.speed>0.1f||
					    ||situation.IsSetPlayState(SPST_Executing)) 
					{
						if (motion.Clear_Ball())
						{
							motion.GoalieJustKicked = true;
							situation.ChangeSetPlay(SPST_Executing);
							deadtime=Max(2 * CP_setplay_wait_time - (SP_drop_ball_time + situation.ModeChangedTime - situation.CurrentTime),0); 
							DoLog(18,"Clear Ball");
						}
						else
						{
							if (situation.IsSetPlayState(SPST_Executing))
								kick.stopball();
							DoLog(18,"Clear Unconfirmed");
						}
					}
					else
					{
						DoLog(18,"(GSP)Hehe,Wait a minute.");
					}
				}
			}
		}
		else
		{
			if (situation.IsSetPlayState(SPST_Executing))
				motion.SmartInterception();
			else
			{	
				situation.ChangeSetPlay(SPST_Prego);
				motion.smartgoto(ball.pos+Vector(0.7f,0),CP_setplay_speed);
				
			}
		}
	}
	else
	{
		situation.ChangeSetPlay(SPST_ExecWait);
		DoLog(18,"(GSP) none of my business");
		motion.GoalieRunto(motion.GetGuardPoint(),0,0.4f);
	}

}