interception.cpp

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

			else{
			//try to intercept with front side
				ballvel = ballvel.Rotate(-selfbodyfacing);
				Vector 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_IT, "2");
		}
	}
	else{
		min_dist = (float)sqrt(Sqr(fabs(ballrelpos.x) - maxdasheffect) + Sqr(ballrelpos.y));
		if (min_dist < kickable_area){
			if (testonly)
				return true;
			if (min_dist >= CP_desired_kickarea){
				dashpow = SP_max_power;
				DoLog(LOG_IT, "3");
			}
			else{
				dashpow = Max(float(fabs(ballrelpos.x) - sqrt(Sqr(CP_desired_kickarea) - Sqr(ballrelpos.y))) / action.Dashrate(), -SP_max_power);
				DoLog(LOG_IT, "4");
			}
		}
	}

	if (testonly){
		DoLog(LOG_IT, "maxdasheffect %.2f min_dist %.2f", maxdasheffect, min_dist);
	}

	if (dashpow == 2 * SP_max_power) return false;
	if (ballrelpos.x < 0) dashpow *= -1.0f;
	command.dash(dashpow);
	return true;
}

int Interception::Close_to_ball_line_intercept(Vector selfpos, Vector ballpos, Vector selfvel, Vector ballvel, 
											   float selfbodyfacing, float dist_player, float dist_ball, Command& command, bool testonly){
//Try just 3 cycles, last cycle is a adjusting cycle
	DoLog(LOG_IT, "bdfy %.2f ballrelpos (%.2f %.2f) selfvel (%.2f %.2f) ballvel (%.2f %.2f) ", selfbodyfacing, (ballpos - selfpos).x, (ballpos - selfpos).y,
		selfvel.x, selfvel.y, ballvel.x, ballvel.y);
	DoLog(LOG_IT, "distplayer %.2f  dist_ball %.2f testonly %d", dist_player, dist_ball, testonly);
	float dashpow;
	for(int i = 1; i <= 3; i ++){
		if (OneCycleIntercept(selfpos, ballpos, selfvel, ballvel, selfbodyfacing, command, testonly || i != 1))
			break;
		//do not move beyond the ball line
		
		dashpow = Min(SP_max_power, (dist_player - selfvel.mod()) / action.Dashrate());
		
		simulate_one_dash_step(selfpos, ballpos, selfvel, ballvel, selfbodyfacing, dashpow);
		dist_player -= selfvel.mod()/SP_player_decay ;
		dist_ball -= ballvel.mod() / SP_ball_decay;
	}

	if (i == 4) // I am not able to intercept it in three cycles
		return -1;

	if (testonly)
		return i;

	if (i != 1){
	//more delicate control can be added here
		if (i == 2){
		// to avoid collision
			dashpow += TwoCycleAvoidEnemy_Adjust(selfpos, ballpos, selfvel, ballvel, selfbodyfacing);
		}
		DoLog(LOG_IT, "Close to ball line dash pow %.2f %d %.2f", dashpow, i, dist_player);
		command.dash(dashpow);
	}
	return i;
}

int Interception::OnlyDashIntercept(int maxcycles, Vector selfpos, Vector ballpos, Vector selfvel, Vector ballvel, float selfbodyfacing, Command& command, bool testonly){
//Ok, see if dashes will bring me close to the ball
	if (maxcycles <= 0)
		return -1;

	if (OneCycleIntercept(selfpos, ballpos, selfvel, ballvel, selfbodyfacing, command, testonly))
		return 1;

	Vector intersection_pt;
	Ray ball_course(ballpos, ballvel), player_course(selfpos, selfbodyfacing);
	float dist_ball, dist_player;

	//selffacing is parallel to ball course, unable to get intersection point
	if (ball_course.InRightDir(selfbodyfacing) || ball_course.InOppositeDir(selfbodyfacing)){
		if (ballvel.mod2() > 0.3f){
			if ((ball_course.InRightDir(selfbodyfacing) && ball_course.InRightDir(selfpos))
				|| (ball_course.InOppositeDir(selfbodyfacing) && ball_course.InOppositeDir(selfpos))){
				return -1;
			}
		}
		for(int i = 2; i <= maxcycles; i ++){
			simulate_one_dash_step(selfpos, ballpos, selfvel, ballvel, selfbodyfacing);
			if (OneCycleIntercept(selfpos, ballpos, selfvel, ballvel, selfbodyfacing, command, testonly || i != 1))
				break;				
		}
		if (i <= maxcycles){
			if (!testonly){
				if (i == 2){
					command.dash(SP_max_power + TwoCycleAvoidEnemy_Adjust(selfpos, ballpos, selfvel, ballvel, selfbodyfacing));
				}
				else
					command.dash(SP_max_power);
			}
			return i;
		}
		return -1;
	}
	
	ball_course.intersection(player_course, intersection_pt);
	dist_ball = ballpos.dist(intersection_pt);
	dist_player = selfpos.dist(intersection_pt);
	
	/*DoLog(LOG_IT, "bdfy %.2f ballrelpos (%.2f %.2f) selfvel (%.2f %.2f) ballvel (%.2f %.2f) ", selfbodyfacing, (ballpos - selfpos).x, (ballpos - selfpos).y,
		selfvel.x, selfvel.y, ballvel.x, ballvel.y);
	DoLog(LOG_IT, "distplayer %.2f  dist_ball %.2f", dist_player, dist_ball);*/
// intercept with more than maxcycles is beyond the reach of this function
	if (dist_player > maxcycles * Self.max_speed + SP_kickable_area)
		return -1;
	if (dist_ball  > ballvel.mod() * (1 - Exp(ball.speed_decay, (float)maxcycles)) / (1 - ball.speed_decay) + SP_kickable_area)
		return -1;
	if (!player_course.InRightDir(intersection_pt) || !ball_course.InRightDir(intersection_pt)) // no hope
		return -1;
//simulate running num_cycles cycles
	int num_cycles = int((dist_player - SP_kickable_area) / Self.max_speed);
	for(int i =0; i < num_cycles; i++){
		simulate_one_dash_step(selfpos, ballpos, selfvel, ballvel, selfbodyfacing);
		dist_ball -= ballvel.mod() / SP_ball_decay;
		dist_player -= selfvel.mod() / SP_player_decay;
		if (dist_ball < -SP_kickable_area) // the ball is passing by
			return -1;
	}
	
	num_cycles = Close_to_ball_line_intercept(selfpos, ballpos, selfvel, ballvel, selfbodyfacing, dist_player, dist_ball, command, testonly || i != 0);
	if (num_cycles == -1) return -1;
	if (i > 0)
		motion.run(Self.max_speed, command);

	return i + num_cycles;
}

int Interception::TurnDashIntercept(int maxcycles, Vector selfpos, Vector ballpos, Vector selfvel, Vector ballvel, float selfbodyfacing, InterceptionInfo & inf, bool test){
	Vector predicted_selfpos, predicted_ballpos, predicted_selfvel, predicted_ballvel;
	int cycles;
	Command cmd;

	simulate_one_dash_step(selfpos, ballpos, selfvel, ballvel, 0, 0);
 
	predicted_selfpos = selfpos; predicted_ballpos = ballpos;
	predicted_selfvel = selfvel; predicted_ballvel = ballvel;

	float turnangle = 0, lastturnangle;
	for(int i = 1; i < maxcycles; i ++){
		lastturnangle = turnangle;
		turnangle = (predicted_ballpos + predicted_ballvel - predicted_selfpos - predicted_selfvel).Angle();
		if (i != 1 && fabs(NormalizeAngle(lastturnangle - turnangle)) < 8.0f)
			break;
		//DoLog(LOG_IT, "turn ang %.2f", turnangle);
		cycles = OnlyDashIntercept(maxcycles, selfpos, ballpos, selfvel, ballvel, turnangle, cmd, true);
		//DoLog(LOG_IT, "cycles %.2f", cycles);
		if (cycles != -1 && cycles <= i){
			DoLog(LOG_IT, "(close_ball) turn dash %d", cycles +1);
			inf.IT_is_closeball = true;
			inf.IT_angle = turnangle;
			inf.IT_cycles = float(cycles + 1);
			inf.IT_point = ballpos + ballvel * (1 - Exp(SP_ball_decay, (float)cycles)) / (1 - SP_ball_decay);
			inf.IT_valid = true;
			return cycles;
		}
		simulate_one_dash_step(predicted_selfpos, predicted_ballpos, predicted_selfvel, predicted_ballvel, 0, 0);
	}
	return -1;
}

float Interception::TwoCycleAvoidEnemy_Adjust(Vector selfpos, Vector ballpos, Vector selfvel, Vector ballvel, float selfbodyfacing){
	Vector ballrelpos = ballpos + ballvel - selfpos - selfvel;
	float t, t_dashpow = 0.0f;
	if (ballrelpos.mod2() < Sqr(CP_collision_threshold)){
		ballrelpos = ballrelpos.Rotate(- selfbodyfacing);
		t = ballrelpos.x - (float)sqrt(Sqr(CP_desired_kickarea) - Sqr(ballrelpos.y));
		t_dashpow = t / ( action.Dashrate() * (1 + SP_player_decay));
		DoLog(LOG_IT, " two cycle adjust %.2f pow %.2f", t, t_dashpow);
	}
	return t_dashpow;
}

void Interception::simulate_one_dash_step(Vector& selfpos, Vector& ballpos, Vector& selfvel, Vector& ballvel, float bodyfacing, float dashpow){
	selfvel += Polar2Vector(dashpow, bodyfacing) * action.Dashrate();
	if (selfvel.mod2() > Sqr(SP_player_speed_max)){
		selfvel = selfvel / selfvel.mod() * SP_player_speed_max;
	}
	selfpos += selfvel;
	ballpos += ballvel;
	
	selfvel *= SP_player_decay;
	ballvel *= SP_ball_decay;
}

IT_mode Interception::intercept(InterceptionInfo& inf){
	Command command;
	IT_mode it_mode = intercept(inf, command);
	mediator.enroll(command, Action_interception, PriorityA);
	return it_mode;
}

IT_mode Interception::intercept(InterceptionInfo& inf, Command& command){
	//intercept action
	command.Reset();
	if (inf.IT_is_closeball){
		if (fabs(inf.IT_angle) > 180.0f){
			command.dash(inf.IT_dashpow);
		}
		else{
			motion.turn_to(inf.IT_angle, command);
		}
		DoLog(LOG_IT, "closeballinterception");
		return IT_closeball;		
	}
	else{
		if (inf.IT_valid && fabs(NormalizeAngle(inf.IT_angle - Self.bodyfacing)) > 15 - 8*exp(-ball.distance * 0.15f)){
			motion.turn_to(inf.IT_angle, command);
			return IT_turn;
		}
		else{
			motion.run(inf.IT_rapidness, command);
			return IT_dash;
		}
	}
}

bool Interception::SmartInterception(){
	//first decide whether the player should intercept the ball
	//if he is in interception, decide whether he should cancel it
	Command command;

	//the sight information is sufficient to make interception decision
	if (ball.pos_conf < 0.75 || ball.speed_conf < 0.75) return false;
	
	//ball is in someone's control, don't be hook up
	if (!situation.BallFree) return false;

	DoLog(LOG_IT, "most promising IT player%d, cycle%.2f at (%.2f %.2f)",situation.most_promising_controller,
		GetPlayer(situation.most_promising_controller).IT_inf.IT_cycles, GetPlayer(situation.most_promising_controller).pos.x, GetPlayer(situation.most_promising_controller).pos.y);
	DoLog(LOG_IT, "My Cycles %.2f", Self.IT_inf.IT_cycles);
	
	// can not get the ball
	if (Self.IT_inf.IT_cycles == InfCycles ) return false;

	//if the ball is definitely out of pitch before i can get there, let it go
	if(!fieldinfo.WithInExpandedField(Self.IT_inf.IT_point)){
		DoLog(LOG_IT, "Out of pitch(%.2f,%.2f)",Self.IT_inf.IT_point.x,  Self.IT_inf.IT_point.y);
		return false;
	}

	float control;
//see if I can get ball from opponent
	int num = motion.Num_TheirVisiblePlayers();
	for(int i = 0; i < num; i ++){
		control = calculatecontrol(Self.IT_inf, TheirPlayer_CtrlOfBall(i).IT_inf);
		if(control >= 0.45f){
		//there is great chance
			break;
		}

		if(Self.aggressiveness > 0.5f){
			if (control >= 0.3f)
				break;
		}
		if (WouldIntercept(situation.TheirSide, TheirPlayers_Interception[i])){
				// there is no chance for me to get the ball from opponent
			DoLog(LOG_IT, "There is no chance to get ball from %d whose cycles is %.2f", TheirPlayers_Interception[i], TheirPlayer_CtrlOfBall(i).IT_inf.IT_cycles);
			return false;
		}
	}

	num = motion.Num_MyVisiblePlayers();

//check if there is need for me to intercept that ball
	for(i = 0; i < num; i ++){

		if (motion.MyPlayer_CtrlOfBall(i).Is_goalie 
			|| MyNumber == MyPlayers_Interception[i]) continue;

		control = calculatecontrol(Self.IT_inf, MyPlayer_CtrlOfBall(i).IT_inf, true);

		if (control > 0.55f){
		//the player with the best chance of getting the ball should go
			DoLog(LOG_IT, "I have better chance than %d", MyPlayers_Interception[i]);
			break;
		}
		else if (WouldIntercept(situation.MySide, MyPlayers_Interception[i])){
			if (control < 0.45f){
				DoLog(LOG_IT, "%d has a better chance whose cycle is %f", MyPlayers_Interception[i], MyPlayer_CtrlOfBall(i).IT_inf.IT_cycles);
				return false;
			}
			else if (!Is_IT_Priored(MyNumber, MyPlayers_Interception[i])){
			//is it dangerous
				DoLog(LOG_IT, "%d is equal in chance but it is priored whose cycle is %.2f", MyPlayers_Interception[i], MyPlayer_CtrlOfBall(i).IT_inf.IT_cycles);
				return false;
			}
			else{
				DoLog(LOG_IT, "I am priored than %d", MyPlayers_Interception[i]);
				break;
			}
		}
	}
	

	/* Interception Action */
	intercept(Self.IT_inf, command);
	
	if (Self.IT_inf.IT_cycles == 1.0f && Self.IT_inf.IT_is_closeball){
		mediator.IsLastCycleIntercept = true;
	}
	else{
		mediator.IsLastCycleIntercept = false;
	}
	mediator.enroll(command, Action_interception, 2 * PriorityA);
	return true;
}

bool Interception::Is_IT_Priored(UNum player1, UNum player2){
	if (!MyPlayer(player1).IsRoleKnown()) return false;
	if (!MyPlayer(player2).IsRoleKnown()) return true;
	Vector point1, point2, ballpos;
	point1 = fm.GetFormationPoint(MyPlayer(player1));
	point2 = fm.GetFormationPoint(MyPlayer(player2));

	ballpos = situation.NextControlBallPos();
	return ballpos.dist2(point1) < ballpos.dist2(point2);
}


bool Interception::WouldIntercept(char side, UNum No){
	if (GetPlayer(side, No).Is_goalie){
		return fieldinfo.their_penaltyarea.IsWithin(GetPlayer(side, No).IT_inf.IT_point);
	}
	return true;
}

float Interception::calculatecontrol(InterceptionInfo& myinf, InterceptionInfo& theirinf, bool is_teammate){
	if (myinf.IT_is_closeball && !is_teammate){
		return calculatecontrol(myinf.IT_cycles, (float)int(theirinf.IT_cycles) + 2.0f);
	}
	return calculatecontrol(myinf.IT_cycles, theirinf.IT_cycles);
}

float Interception::calculatecontrol(float my_cycles, float their_cycles){
	if(my_cycles == InfCycles){
		if (their_cycles == InfCycles)
			return 0.5f;
		else
			return 0.0f;
	}
	else if (their_cycles == InfCycles)
		return 1.0f;
	double input[2];
	double control;
	float cycles_gap = (float)fabs(my_cycles - their_cycles), min_cycles = Min(my_cycles, their_cycles);
	input[0] = fabs(my_cycles - their_cycles) /10;
	input[1] = fabs(Min(my_cycles, their_cycles)) / 10;
	ControlBallNet.SimulateNet(&input[0], &control);
	control = Max(control, 0.5);
	
	if (my_cycles <= their_cycles)
		return  (float)control;
	else
		return 1 - (float)control;
}


Player& Interception::MyPlayer_CtrlOfBall(int idx){
	return MyPlayer(MyPlayers_Interception[idx]);
}

Player& Interception::TheirPlayer_CtrlOfBall(int idx){
	return TheirPlayer(TheirPlayers_Interception[idx]);
}