goalie.cpp

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

				if (ITCycle >0)
				if (dashtopoint.dist(ballpos)/ITCycle < mingap)
				{
					mingap = dashtopoint.dist(ballpos)/ITCycle;
					mostpromisingIT.turnangle = IT_info.turnangle;
					mostpromisingIT.IT_rapidness = SP_player_speed_max;
					mostpromisingIT.IT_point= ballpos;
				}
			}

		}
		//check if turn and dash backward can get the ball
		if (canbackdash)
		{
			angdif = float(180 - fabs(NormalizeAngle(IT_info.turnangle - facing)));
			Turncycle = 0;
			while (ballpos.dist(playerpos[Turncycle]) * Sin(angdif) > controlmargin || angdif > 90 ){
				angdif -=action.Turnrate(playervel[Turncycle].mod()) * SP_max_moment;
				if (angdif <0) angdif =0;
				Turncycle ++;
			}
			if (Turncycle <= ITCycle)
			{
				dashtopoint = playerpos[Turncycle];
				dashvel = playervel[Turncycle];
				reachballcycle = -1;
				for (i = Turncycle; i < FullDashCycle;i++)
				{
					dashvel +=Polar2Vector(SP_max_power * action.Dashrate(),IT_info.turnangle);
					if (dashvel.mod() > SP_player_speed_max)
						dashvel =dashvel/dashvel.mod() * SP_player_speed_max;
					dashtopoint += dashvel;
					if (dashtopoint.dist(ballpos)<controlmargin)
						reachballcycle = i+1;
				}
				if ((dashtopoint.dist(ballpos) - controlmargin) /SP_player_speed_max 
					<= ITCycle - FullDashCycle)
				{
					IT_info.IT_cycles = (float)ITCycle;
					IT_info.IT_point= ballpos;
					if (angdif < 5 && ballvel.mod() > 1.4f)
						IT_info.IT_point+= Polar2Vector(SP_kickable_area /2,ballvel.Angle());
					IT_info.IT_rapidness = Max( -SP_player_speed_max,- (float)fabs(orgballpos.dist(Self.PredictPos(Turncycle)) * Cos (angdif)));
					if (reachballcycle != -1)
						IT_info.IT_waitcycles =float(ITCycle - reachballcycle);
					else
						IT_info.IT_waitcycles =ITCycle - FullDashCycle - Max((dashtopoint.dist(ballpos) - controlmargin) /SP_player_speed_max,0.0f);
					AddPITinfo(IT_info);
				}
				else
				{
					if (ITCycle > 0)
					if (dashtopoint.dist(ballpos)/ITCycle < mingap)
					{
						mingap = dashtopoint.dist(ballpos)/ITCycle;
						mostpromisingIT.turnangle = IT_info.turnangle;
						mostpromisingIT.IT_rapidness = SP_player_speed_max;
						mostpromisingIT.IT_point= ballpos;
					}
				}
			}
		}
		ballpos +=ballvel;
		ballvel *=SP_ball_decay;
		ITCycle ++;
	}while (fieldinfo.WithInField(ballpos) && (ballvel.mod() > 0.05f * ITCycle));
	if (num_PIT_infos ==0 && Self.IT_inf.IT_cycles >ITCycle && Self.IT_inf.IT_cycles < InfCycles
		&& fieldinfo.WithInField(Self.IT_inf.IT_point))
	{
		IT_info.IT_cycles = Self.IT_inf.IT_cycles;
		IT_info.IT_rapidness = Self.IT_inf.IT_rapidness;
		IT_info.turnangle = Self.IT_inf.IT_angle;
		IT_info.IT_point= Self.IT_inf.IT_point;
		IT_info.IT_waitcycles = -1;//by net
		AddPITinfo(IT_info);

	}
	return (num_PIT_infos > 0);
}


bool Goalie::OneCycleIT(){ 
	if (!fieldinfo.my_penaltyarea.IsWithin(ball.PredictPos(1)) && motion.TheirPlayer_Close2Ball(0).balldist > 5.0f)
		return false;
	Vector ballrelpos = ball.PredictPos(1) - Self.PredictPos() ;
	float dashpow = 2 * SP_max_power;
	ballrelpos = ballrelpos.Rotate( - Self.bodyfacing);
	if (fabs(ballrelpos.y) > SP_catch_area_l) return false;
	Vector ballvel = ball.global_vel;
	Vector next_ball_pos;
	float maxdasheffect = action.Max_DashEffect();
	if (maxdasheffect > fabs(ballrelpos.x)){
		if (fabs(ballrelpos.y) >= CP_desired_kickarea){
			dashpow = float(fabs(ballrelpos.x) / action.Dashrate());
			DoLog(LOG_GOALIE, " One Cyc IT1");
		}
		else{
		//There are two solutions, choose the one that is more robust
			float solution[2], dist[2];
			int final_solution = 0;
			solution[0] = float(fabs(ballrelpos.x) - sqrt(Sqr(CP_desired_kickarea) - Sqr(ballrelpos.y)));
			solution[1] = 2 * (float)fabs(ballrelpos.x) - solution[0];
			pose_limitation(solution[0], -action.Max_DashEffect(), action.Max_DashEffect());
			solution[1] = Min(solution[1], action.Max_DashEffect());
			dist[0] = float(sqrt(Sqr(solution[0] - fabs(ballrelpos.x)) + Sqr(ballrelpos.y)));
			dist[1] = float(sqrt(Sqr(solution[1] - fabs(ballrelpos.x)) + Sqr(ballrelpos.y)));
			if (dist[0] < CP_collision_threshold || dist[1] < CP_collision_threshold){
				//if one solution leads to a collision, choose the bigger one
				if (dist[0] > dist[1])
					final_solution = 0;
				else
					final_solution = 1;
			}
			else{
			//try to intercept with front side
				ballvel.Rotate(-Self.bodyfacing);
				next_ball_pos = ballvel + ballrelpos;
				if (Vector(solution[0], 0).dist2(next_ball_pos) > Vector(solution[1], 0).dist2(next_ball_pos))
					final_solution = 1;
				else
					final_solution =0;
			}
			
			dashpow = solution[final_solution] / action.Dashrate();
			DoLog(LOG_GOALIE, "One cyc IT2");
		}
	}
	else{
		float min_dist = (float)sqrt(Sqr(fabs(ballrelpos.x) - maxdasheffect) + Sqr(ballrelpos.y));
		float GoalieIT_area = fieldinfo.my_penaltyarea.IsWithin(Self.PredictPos(SP_max_power)) ? SP_catch_area_l : SP_kickable_area;

		if (min_dist < GoalieIT_area){
			if (min_dist >= CP_desired_kickarea){
				dashpow = SP_max_power;
			}
			else{
				dashpow = Max(float(fabs(ballrelpos.x) - sqrt(Sqr(CP_desired_kickarea) - Sqr(ballrelpos.y))) / action.Dashrate(), -SP_max_power);
			}
			DoLog(LOG_GOALIE,"One cyc IT 3");
		}
	}

	if (dashpow == 2 * SP_max_power) return false;
	if (ballrelpos.x < 0) dashpow *= -1.0f;
	Command command;
	command.dash(dashpow);
	mediator.enroll(command,Action_interception,2 * PriorityA);
	return true;
	
}


bool Goalie::GoalieIntercept(){
	int i;
	bool haveintercept = false;
	if (OneCycleIT()) 
	{
		DoLog(LOG_GOALIE,"One Cycle and get ball");
		haveintercept = true;
	}
	if (!ball.pos_valid()) return  false;
	DoLog(LOG_GOALIE,"Try intercept");
	int bestPIT = -1;
	if (num_PIT_infos == 0)
	{
		if (!IsShoot()|| IfOppkickmore()) 
		{
			DoLog(LOG_GOALIE,"Not final shooting");
			return false;
		}
		DoLog(LOG_GOALIE,"IS Shoot so Try");
		minITcycles = 0;
		if (mostpromisingIT.turnangle != InvalidAngle)
		{
			GoalieRunto(mostpromisingIT.IT_point,1);
			haveintercept = true;
		}
		else
		{
			Vector crosspoint;
			Ray ballray;
			ballray.SetValues(ball.pos,ball.global_vel);
			if	(!bottomline.RayIntersection(ballray,crosspoint))
			{
				haveintercept = GoalieRunto(ball.PredictPos(1),1);
				return haveintercept;
			}
			GoalieRunto(crosspoint,1);
			haveintercept = true;
		}		
	}
	else
	{
		float oppmincycles;
		if (motion.Num_TheirVisiblePlayers() == 0)
			oppmincycles = InfCycles;
		else
		oppmincycles= (situation.BallFree || !IsTheirPlayer(situation.ballcontroller)) ? TheirPlayer(motion.TheirPlayers_Interception[0]).IT_inf.IT_cycles : 3.0f;
		DoLog(LOG_GOALIE,"Their cycles %.1f",oppmincycles);
		int dev_degree;
		while (num_PIT_infos > 0 && PIT_info[num_PIT_infos -1].IT_cycles > 2 && (PIT_info[num_PIT_infos -1].IT_cycles > oppmincycles
			||PIT_info[num_PIT_infos -1].IT_cycles > oppmincycles * 0.9f && !fieldinfo.my_penaltyarea.IsWithin(PIT_info[num_PIT_infos -1].IT_point)))
			num_PIT_infos --;

		float mycycles;
		if (!situation.BallFree)
		{
			if (IsMyPlayer(situation.ballcontroller))
				mycycles = 0.0f;
			else
				mycycles = 3.0f;
		}
		else
		if (motion.MyPlayers_Interception[0] ==FP_goalie_number)
		{
			if (motion.Num_MyVisiblePlayers() <1)
				mycycles = InfCycles;
			else
				mycycles = motion.MyPlayer_CtrlOfBall(1).IT_inf.IT_cycles;
		}
		else
			mycycles = motion.MyPlayer_CtrlOfBall(0).IT_inf.IT_cycles;
		
		while (num_PIT_infos > 0 &&  (PIT_info[num_PIT_infos -1].IT_cycles > mycycles + 2.0f
			||PIT_info[num_PIT_infos -1].IT_cycles > mycycles  * 0.9f && !fieldinfo.my_penaltyarea.IsWithin(PIT_info[num_PIT_infos -1].IT_point)))
			num_PIT_infos --;
		if (motion.calculatecontrol(mycycles,oppmincycles) > 0.6f) 
		{
			while (num_PIT_infos >0 &&!fieldinfo.my_penaltyarea.IsWithin(PIT_info[num_PIT_infos -1].IT_point))
				num_PIT_infos --;
		}
		
		if (num_PIT_infos == 0)
			return false;
		
		if (PIT_info[0].IT_cycles > oppmincycles -2)
		{
			if (fieldinfo.my_penaltyarea.IsWithin(PIT_info[0].IT_point))
				dev_degree = 1;
			else
				dev_degree = 2;
			bestPIT = 0;
			GoalieRunto(PIT_info[0].IT_point,dev_degree,PIT_info[0].IT_rapidness);
			haveintercept = true;
			DoLog(LOG_GOALIE,"My cycle %.1f",PIT_info[0].IT_cycles);
		}
		else
		{
			while (num_PIT_infos > 0 && PIT_info[num_PIT_infos -1].IT_cycles > oppmincycles -2)
				num_PIT_infos --;
			int  first_considered_PIT = 0;
			if (fieldinfo.my_penaltyarea.IsWithin(PIT_info[num_PIT_infos -1].IT_point))
				while (!fieldinfo.my_penaltyarea.IsWithin(PIT_info[first_considered_PIT].IT_point) &&first_considered_PIT < num_PIT_infos -1)
						first_considered_PIT ++;
			bestPIT = first_considered_PIT;
			for (i = first_considered_PIT ;i <num_PIT_infos; i++)
				if (PIT_info[i].IT_waitcycles >=1.0f && PIT_info[i].IT_rapidness >0 &&
					PIT_info[i].IT_point.x > -SP_semi_pitch_length - CP_safecatch_width)
				{
					bestPIT =i;
					break;
				}
			if (bestPIT == first_considered_PIT)
			for (i =first_considered_PIT ;i <num_PIT_infos; i++)
				if (PIT_info[i].IT_waitcycles >=1.0f &&
					PIT_info[i].IT_point.x > -SP_semi_pitch_length - CP_safecatch_width)
				{
					bestPIT =i;
					break;
				}
			if (fieldinfo.my_penaltyarea.IsWithin(PIT_info[bestPIT].IT_point))
				dev_degree = 1;
			else
				dev_degree = 2;
			if (!GoalieRunto(PIT_info[bestPIT].IT_point,dev_degree,PIT_info[bestPIT].IT_rapidness))
				motion.turn_to(ball.PredictPos(1));
			haveintercept = true;
			DoLog(LOG_GOALIE,"best %d My cycle  %.1f",bestPIT,PIT_info[bestPIT].IT_cycles);
		}
	}
	VIEWWIDTH myviewwidth;
	float ballfreecycles = Min(minITcycles,TheirPlayer(motion.TheirPlayers_Interception[0]).IT_inf.IT_cycles);
	
	if (ballfreecycles < 4)
		myviewwidth = VW_Narrow;
	else if (ballfreecycles < 10 )
		myviewwidth = VW_Normal;
	else
		myviewwidth = VW_Wide;
	if (bestPIT != -1)
	{
		if (PIT_info[bestPIT].IT_waitcycles < 5 && myviewwidth == VW_Wide)
			myviewwidth = VW_Normal;
	}
	
	return haveintercept;
}

Vector Goalie::GetGuardPoint(){

	return Vector(10.0f - SP_semi_pitch_length ,0);
} 

float Goalie::ballrollingtime(float orgspeed,float dist){
	if (orgspeed < 0.15f) return InfCycles;
	if (dist >orgspeed / (1-SP_ball_decay)) return InfCycles;
	return float (log(1.0f - (dist/orgspeed)*(1.0f - SP_ball_decay))/log(SP_ball_decay));
}

float Goalie::ballspeedrequired(float dist,int time){
	return float(dist * (1 - SP_ball_decay) /(1 - Exp(SP_ball_decay,(float)time)));
}

void Goalie::CalcToleranceTime(DefendInfo &definf){
	float cycles = 0;
	if (situation.BallFree) 
		cycles += situation.min_IT_cycles;
	definf.TolerantTime = cycles;
	if (ballpoint.dist(definf.OppPos) > CP_different_distance)
	definf.TolerantTime += ballrollingtime(SP_ball_speed_max + SP_player_speed_max,ballpoint.dist(definf.OppPos));
}

float Goalie::dist_to_goal(Vector pos){
	if (fabs(pos.y) <= SP_goal_width /2)
		return (pos.x + SP_semi_pitch_length);
	if (pos.y > 0) return pos.dist(fieldinfo.r_mygoalpost);
	else return pos.dist(fieldinfo.l_mygoalpost);
}

void Goalie::proceedball(Vector &ballpos,Vector &ballvel,int cycles){
	for (int i=0;i < cycles;i++)
	{
		ballpos += ballvel;
		ballvel *=SP_ball_decay;
	}
}

bool Goalie::BallInNarrowAngle(){ 
	if (fabs(ballpoint.y) < SP_goal_width) return false;
	Vector farpost = ballpoint.y >0 ? fieldinfo.l_mygoalpost :fieldinfo.r_mygoalpost;	
	return fabs(SP_goal_width * Cos((ballpoint - farpost).Angle())) <= 2.5 * SP_catch_area_l;
}

bool Goalie::CatchWait(){
	
	if (!situation.BallFree) return false;
	if (ball.distance < SP_catch_area_l * 0.85f) return false;
	if (!fieldinfo.WithInField(ball.PredictPos(1)) ||ball.PredictPos(1).x < CP_safecatch_width - SP_semi_pitch_length)
		return false;
	Vector Mydashend1 = Self.PredictPos(SP_max_power);
	Vector Mydashend2 = Self.PredictPos( -SP_min_power);
	Line Mydashline;
	Mydashline.LineFromTwoPoints(Mydashend1,Mydashend2);
	Vector pointpr = Mydashline.ProjectPoint(ball.PredictPos(1));
	float mindis;
	if (Mydashline.InBetween(pointpr,Mydashend1,Mydashend2))
		mindis = pointpr.dist(ball.PredictPos(1));
	else
		mindis = Min(ball.PredictPos(1).dist(Mydashend1),ball.PredictPos(2).dist(Mydashend2));
	return mindis < ball.distance * 0.9;
}