interception.cpp

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

/*
    Copyright (C) 2001  Tsinghuaeolus

    Authors : ChenJiang, YaoJinyi, CaiYunpeng, Lishi

    This library is free software; you can redistribute it and/or
    modify it under the terms of the GNU Lesser General Public
    License as published by the Free Software Foundation; either
    version 2.1 of the License, or (at your option) any later version.

    This library is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
    Lesser General Public License for more details.

	If you make any changes or have any comments we would appreciate a 
	message to [e-mail].
*/

#include "Interception.h"
#include "strategy.h"
#include "global.h"

/***********************   Interception   *****************************************/
Interception::Interception(){
}

void Interception::SetInterceptionInfo(){
	int i, j, num_my, num_their;

	num_my = motion.Num_MyVisiblePlayers();
	num_their = motion.Num_TheirVisiblePlayers();

	for(i = 0; i < num_my; i ++){
		MyPlayers_Interception[i] = motion.MyVisiblePlayers[i];
		getinterceptioninfo(MyPlayer(MyPlayers_Interception[i]).pos,
			ball.pos, MyPlayer(MyPlayers_Interception[i]).global_vel, ball.global_vel, 
			MyPlayer(MyPlayers_Interception[i]).bodyfacing, MyPlayer(MyPlayers_Interception[i]).IT_inf);
	}

	for(i =0; i < num_their; i ++){
		TheirPlayers_Interception[i] = motion.TheirVisiblePlayers[i];
		getinterceptioninfo(TheirPlayer(TheirPlayers_Interception[i]).pos,
		ball.pos, TheirPlayer(TheirPlayers_Interception[i]).global_vel, ball.global_vel, 
		TheirPlayer(TheirPlayers_Interception[i]).bodyfacing, TheirPlayer(TheirPlayers_Interception[i]).IT_inf);
	}

	getinterceptioninfo(Self.pos, ball.pos, Self.global_vel, ball.global_vel, Self.bodyfacing, Self.IT_inf, false);
	
/* sort players by their ability to intercept the ball */
	for(i = 0; i < num_my - 1; i++)
		for ( j = i + 1; j < num_my; j ++){
			if (MyPlayer(MyPlayers_Interception[j]).IT_inf.IT_cycles < MyPlayer(MyPlayers_Interception[i]).IT_inf.IT_cycles){
				Swap(MyPlayers_Interception[j], MyPlayers_Interception[i]);
			}
		}
	
	for(i = 0; i < num_their - 1; i++)
		for ( j = i + 1; j < num_their; j ++){
			if (TheirPlayer(TheirPlayers_Interception[j]).IT_inf.IT_cycles < TheirPlayer(TheirPlayers_Interception[i]).IT_inf.IT_cycles){
				Swap(TheirPlayers_Interception[j], TheirPlayers_Interception[i]);
			}
		}

	situation.most_promising_controller = -1;

	if (num_my > 0){
		my_min_cycles = MyPlayer(MyPlayers_Interception[0]).IT_inf.IT_cycles;
	}
	else{
		my_min_cycles = InfCycles;
	}

	if(num_their > 0){
		their_min_cycles = TheirPlayer(TheirPlayers_Interception[0]).IT_inf.IT_cycles;
	}
	else{
		their_min_cycles = InfCycles;
	}
	
	situation.min_IT_cycles = InfCycles;

	if (their_min_cycles < situation.min_IT_cycles){
		if(TheirPlayers_Interception[0] <= SP_team_size){
			situation.most_promising_controller = SP_team_size + TheirPlayers_Interception[0];
		}
		else{
			situation.most_promising_controller = TheirPlayers_Interception[0];
		}

		situation.next_interception_point = TheirPlayer(TheirPlayers_Interception[0]).IT_inf.IT_point;
		situation.min_IT_cycles = TheirPlayer(TheirPlayers_Interception[0]).IT_inf.IT_cycles;
	}

	if (my_min_cycles < situation.min_IT_cycles){
		situation.most_promising_controller = MyPlayers_Interception[0];
		situation.next_interception_point = MyPlayer(MyPlayers_Interception[0]).IT_inf.IT_point;
		situation.min_IT_cycles = MyPlayer(MyPlayers_Interception[0]).IT_inf.IT_cycles;
	}

	if (Self.IT_inf.IT_cycles < situation.min_IT_cycles){
		situation.most_promising_controller = MyNumber;
		situation.next_interception_point = Self.IT_inf.IT_point;
		situation.min_IT_cycles = Self.IT_inf.IT_cycles;
	}

	situation.Team_Control = calculatecontrol(Min(Self.IT_inf.IT_cycles, my_min_cycles), their_min_cycles);
}

/*	get interception information*/

void Interception::getinterceptioninfo(const Player& player, InterceptionInfo& inf, bool testonly){
	getinterceptioninfo(player.pos, ball.pos, player.global_vel, ball.global_vel, player.bodyfacing, inf, true);
}

void Interception::getinterceptioninfo(Vector startpoint, Vector ballpos, Vector selfvel, Vector ballvel, float selffacing, InterceptionInfo& inf, bool testonly){
	getinterceptioninfo_new(startpoint, ballpos, selfvel, ballvel, selffacing, inf, testonly);
	return;
}

IT_test_res Interception::interception_test(Ray& ballcourse, Ray& playercourse, float ballspeed, float selffacing, float& distball, InterceptionInfo & inf){
	Vector intersection_pt;
	float distplayer, min_cycles, stretch_angle, dist, conf, speed;

	ballcourse.intersection(playercourse, intersection_pt);
	distball = ballcourse.DistanceFromOrigin(intersection_pt);
	distplayer = playercourse.DistanceFromOrigin(intersection_pt);
	if (!ballcourse.InRightDir(intersection_pt) || !playercourse.InRightDir(intersection_pt)){
		return IT_test_beyond;
	}
	
	stretch_angle = (float)fabs(NormalizeAngle(ballcourse.Dir() - playercourse.Dir()));

	min_cycles = (float)Max(int(distplayer / SP_player_speed_max), 0);
	conf = Exp(SP_ball_decay, min_cycles);
	distball -= ballspeed * (1 - conf) / (1 - SP_ball_decay);
	distplayer -= SP_player_speed_max * min_cycles;
	if (distball < 0){
	//go beyond
		dist = (float)sqrt(Sqr(distball) + Sqr(distplayer) + 2 * fabs(distball) * distplayer * Cos(stretch_angle));
		if (dist > CP_reliable_kickarea)
			return IT_test_beyond;
	}
	else{
	//see if it's too far away to wait for
		speed = ballspeed * conf;
		if (distball > (1 + SP_ball_decay) * speed + CP_reliable_kickarea)
			return IT_test_not_desired;
		dist = (float)sqrt(Sqr(distball) + Sqr(distplayer) - 2 * fabs(distball) * distplayer * Cos(stretch_angle));
		if (dist > CP_reliable_kickarea){
			if (distball - speed < -CP_reliable_kickarea){
				//go beyond this cycle
				return IT_test_beyond;
			}
		}
	}
	inf.IT_angle = selffacing;
	inf.IT_cycles = min_cycles;
	inf.IT_point = intersection_pt;
	inf.IT_is_closeball = false;
	inf.IT_rapidness = SP_player_speed_max;
	inf.IT_valid = true;
	return IT_test_ok;
}

inline void Interception::InfCycleProcess(float& cycles){
	if (fabs(cycles) > InfCycles || cycles < 0)
		cycles = InfCycles;
}

void Interception::getinterceptioninfo_new(Vector startpoint, Vector ballpos, Vector selfvel, Vector ballvel, float selffacing, InterceptionInfo& inf, bool testonly){
	IT_test_res it_test_res_noturn; 
	Ray ballcourse, playercourse;
	Vector gap;
	float distball_noturn, ballspeed, angle, ballangle;
	InterceptionInfo inf_noturn;

	if (!testonly){
	//test closeball interception
		if (getcloseballinterceptioninfo(startpoint, ballpos, selfvel, ballvel, selffacing, inf, testonly))
			return;
	}
	inf.IT_cycles = InfCycles;
	inf.IT_valid = false;
	inf.IT_rapidness = Self.max_speed;
	inf.IT_is_closeball = false;

	ballcourse.SetValues(ballpos, ballvel);
	playercourse.SetValues(startpoint, selffacing);
	ballspeed = ballvel.mod();
	ballangle = ballvel.Angle();
	gap = ballpos - startpoint;
	angle = gap.Angle();
	
	if (ballspeed < 0.15f){
		inf.IT_angle = angle;
		inf.IT_point = ballpos;
		inf.IT_cycles = float(int(gap.mod() / inf.IT_rapidness));
		InfCycleProcess(inf.IT_cycles);
		inf.IT_valid = true;
		return;
	}

	if (ballcourse.InRightDir(startpoint)){
		inf.IT_angle = angle;
		inf.IT_cycles = 0;
		inf.IT_point = ballpos;
		int cycles = 1, counter = 0; // initialize to be > 0
		float dist = gap.mod(), conf;
		while(dist > CP_reliable_kickarea && counter < 20){
			cycles = Max(int(dist / (ballvel.mod() + Self.max_speed)), 1);
			inf.IT_cycles += cycles;
			conf = Exp(ball.speed_decay, float(cycles));
			dist -= Self.max_speed * cycles + ballvel.mod() * (1 - conf) / ( 1- ball.speed_decay);
			inf.IT_point += ballvel * (1 - conf) / ( 1- ball.speed_decay);
			ballvel *= conf;
			counter ++;
		}
		InfCycleProcess(inf.IT_cycles);
		inf.IT_valid = true;
		return;
	}
	else if (ballcourse.InOppositeDir(startpoint)){
		inf.IT_angle = angle;
		inf.IT_cycles = 0;
		inf.IT_point = ballpos;
		int cycles, counter = 0;
		float dist = gap.mod(), conf;
		while(dist > CP_reliable_kickarea && counter < 20){
			cycles = Max(int(dist / Self.max_speed), 1);
			inf.IT_cycles += cycles;
			conf = Exp(ball.speed_decay, float(cycles));
			dist -= SP_player_speed_max * cycles - ballvel.mod() * (1 - conf) / ( 1- ball.speed_decay);
			inf.IT_point += ballvel * (1 - conf) / ( 1- ball.speed_decay);
			ballvel *= conf;
			counter ++;
		}
		InfCycleProcess(inf.IT_cycles);
		inf.IT_valid = true;
		return;
	}

	if (!testonly){
		if ((it_test_res_noturn = interception_test(ballcourse, playercourse, ballspeed, selffacing, distball_noturn, inf_noturn)) == IT_test_ok){
			DoLog(LOG_IT, "test ok");
			inf = inf_noturn;
			return;
		}
	}
	
	//should turn to intercept	
	ballpos += ballvel;
	ballvel *= SP_ball_decay;
	ballspeed *= SP_ball_decay;
	startpoint -= ballpos;
	startpoint = startpoint.Rotate(- ballangle);
	Vector inf_sup;
	float t;
	
	if (calc_interception_solution(startpoint, ballspeed, inf_sup)){
		t = inf_sup.y;
	}
	else{
		t = InfCycles;
		if (inf_sup.x > inf_sup.y){
			DoLog(LOG_IT, "Bug why inf > sup (%.2f %.2f)", inf_sup.x, inf_sup.y);
		}
		if (inf_sup.y == InfCycles){
			DoLog(LOG_IT, "see if really cannot get the ball(%.2f %.2f)", inf_sup.x, inf_sup.y);
		}
	}
	if (t == InfCycles){
		inf.IT_cycles = InfCycles;
		inf.IT_valid = true;
	}
	else{
		if (t < 0){
			DoLog(LOG_IT, "bug why t < 0");
		}
		inf.IT_angle = NormalizeAngle((Vector(t, 0) - startpoint).Angle() + ballangle);
		if (!testonly)
			DoLog(LOG_IT, "ballangle %.2f inf angle %.2f", ballangle, inf.IT_angle);
		ballcourse.SetValues(ballpos, ballangle);
		ballcourse.Normalize();
		inf.IT_point = ballcourse.GetPoint(t);
		inf.IT_cycles = (float)sqrt(Sqr(startpoint.x - t) + Sqr(startpoint.y)) / SP_player_speed_max;
		inf.IT_valid = true;
	}
}

bool Interception::getcloseballinterceptioninfo(Vector selfpos, Vector ballpos, Vector selfvel, Vector ballvel, float selffacing, InterceptionInfo &inf, bool testonly){
//more sophisticated design can be added here, compare onlydashintercept with turn intercept

	if (!ball.pos_valid()) return false;
	Command command;
	float cycles = (float)OnlyDashIntercept(5, selfpos, ballpos, selfvel, ballvel, selffacing, command, testonly);
	if (cycles != -1){
	//Only dashs will do
	//patches
		if (cycles == 2 && fabs(last_cycle_test_res.y) > 0.75){
			InterceptionInfo t_inf;
			int turn_cycles = TurnDashIntercept(3, selfpos, ballpos, selfvel, ballvel, selffacing, t_inf);
			if (turn_cycles > 0 && turn_cycles <= 3 && fabs(last_cycle_test_res.y) < 0.8){
				inf = t_inf;
				DoLog(LOG_IT, "patches applyed---turn dash");
				return true;
			}
		}

		DoLog(LOG_IT, "(close_ball) Only dash %.2f", cycles);
		inf.IT_is_closeball = true;
		inf.IT_dashpow = command.GetPower();
		inf.IT_angle = 360.0f; // disable turn
		inf.IT_valid = true;
		inf.IT_cycles = cycles;
		inf.IT_point = ballpos +ballvel * (1 - Exp(SP_ball_decay, cycles)) / (1 - SP_ball_decay);
		return true;
	}
//dash backwards
	cycles = (float)OnlyDashIntercept(3, selfpos, ballpos, selfvel, ballvel, selffacing + 180, command, testonly);
	if ( cycles != -1){
	//Only dashs will do
		DoLog(LOG_IT, "(close_ball) backward Only dash %.2f", cycles);
		inf.IT_is_closeball = true;
		inf.IT_dashpow = -command.GetPower();
		inf.IT_angle = 360.0f; // disable turn
		inf.IT_valid = true;
		inf.IT_cycles = cycles;
		inf.IT_point = ballpos +ballvel * (1 - Exp(SP_ball_decay, cycles)) / (1 - SP_ball_decay);
		return true;
	}
//if turn angle changes little, please
	cycles = (float)TurnDashIntercept(5, selfpos, ballpos, selfvel, ballvel, selffacing, inf);
	if (cycles != -1){
		return true;
	}
	
	DoLog(LOG_IT, "No CloseBall");
	return false;
}

bool Interception::OneCycleIntercept(Vector selfpos, Vector ballpos, Vector selfvel, Vector ballvel, float selfbodyfacing, Command& command, bool testonly){
	float dashpow = 2 * SP_max_power, min_dist, maxdasheffect, kickable_area;

	Vector ballrelpos = ballpos + ballvel- selfpos - selfvel;
	
	ballrelpos = ballrelpos.Rotate( - selfbodyfacing);
	
	if (testonly)
		kickable_area = CP_reliable_kickarea;
	else
		kickable_area = SP_kickable_area;

	if (fabs(ballrelpos.y) > kickable_area) return false;
	maxdasheffect = action.Max_DashEffect();
	if (ballrelpos.mod2() > Sqr(kickable_area + maxdasheffect)) return false;

	DoLog(LOG_IT, "(LastCycle) (%.2f %.2f) testonly %d", ballrelpos.x, ballrelpos.y, testonly);
	last_cycle_test_res = ballrelpos;
	
	if (maxdasheffect > fabs(ballrelpos.x)){
		if (testonly)
			return true;
		
		if (fabs(ballrelpos.y) >= CP_desired_kickarea){
			dashpow = float(fabs(ballrelpos.x) / action.Dashrate());
			DoLog(LOG_IT, "1");
		}
		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], -maxdasheffect, maxdasheffect);
			solution[1] = Min(solution[1], maxdasheffect);
			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;
			}