goingthroughspeed.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 "goingthroughspeed.h"
#include "global.h"

/** Goingthroughspeed, the basic judge pass module that can trust**/
#define MINERROR 0.05f

float GT_speed_utils::calc_peakpoint(Vector pos){
//use Newton Method to calc the peak point
	double t = pos.x, tmp1, tmp2, tmp3, tmp4, f, _f;
	int cycles = 0;
	if (pos.y == 0){
	//if y == 0 the function give wrong result
		pos.y = 0.0001f;
	}

	tmp4 = log(SP_ball_decay);
	do{
		tmp1 = sqrt(Sqr(t -pos.x) + Sqr(pos.y));
		tmp2 = (t - pos.x) / tmp1;
		tmp3 =  pow(SP_ball_decay, tmp1);
		f = 1.0 - tmp3 * ( 1.0 - tmp4 * t * tmp2);
		_f =  tmp4 * ((f - 1.0) * tmp2 +  tmp3 * ((2 * t -pos.x ) * tmp1 - t * (t - pos.x) * tmp2) / Sqr(tmp1));
		if (f < 0 && _f > 0){
			t = (t + pos.x) /2;
		}
		else{
			t = t - f/_f;
		}
		cycles ++;
	}while(fabs(f) > MINERROR && (_f < 0  || f < 0 )&& cycles < 5);
	if (fabs(f) > MINERROR)
		return SP_pitch_diameter;
	return float(t);
}

float GT_speed_utils::calc_spottingspeed(Vector pos, float t){
	
//why plus 1 here? --- the player see the ball one cycle after the ball is kicked out
	float cycles = Max((pos.dist(Vector(t, 0)) - CP_kickarea_compensation) / SP_player_speed_max, 0.0f) + 1;
	return Min(float(t * (1 - SP_ball_decay) /(1 - pow(SP_ball_decay, cycles))), CP_impossible_speed);
}

float GT_speed_utils::calc_goingthroughspeed(Vector pos, float t){
	float peakpoint = calc_peakpoint(pos);
	if (t > peakpoint){
		return Max(calc_spottingspeed(pos, t), calc_spottingspeed(pos, peakpoint));
	}
	return calc_spottingspeed(pos, t);
}

float GT_speed_utils::calc_goalie_spottingspeed(Vector pos, float t){
//why plus 1 here? --- the player see the ball one cycle after the ball is kicked out
	float cycles = Max((pos.dist(Vector(t, 0)) - CP_catcharea_compensation) / SP_player_speed_max, 0.0f) + 0.01f;
	return Min(float(t * (1 - SP_ball_decay) /(1 - pow(SP_ball_decay, cycles))), CP_impossible_speed);
}

float GT_speed_utils::calc_goalie_goingthroughspeed(Vector pos, float t){
	float peakpoint = calc_peakpoint(pos);
	if (t > peakpoint){
		return Max(calc_goalie_spottingspeed(pos, t), calc_goalie_spottingspeed(pos, peakpoint));
	}
	return calc_goalie_spottingspeed(pos, t);
}

bool GT_speed_utils::calc_interception_solution(Vector startpoint, float ballspeed, Vector& inf_sup){
	float t_inf, t_sup, t_closest, t_test, cycles, t_ball, test_ballspeed, conf;
	int run_cycles;
	t_inf = 0.0f; t_sup = InfCycles;
	t_test = t_closest = Max(0.0f, startpoint.x);
	run_cycles = 0;
	bool is_before_closest_pt;
	while(t_sup > t_inf + 2.0f && run_cycles <= 8){
		run_cycles ++;
		cycles = (float)sqrt(Sqr(t_test - startpoint.x) + Sqr(startpoint.y)) / SP_player_speed_max;
		conf = Exp(SP_ball_decay, cycles);
		t_ball = ballspeed * (1 - conf) / (1 - SP_ball_decay);
		if (t_test == t_closest){
			is_before_closest_pt = bool(t_test >= t_ball);
		}

		if (is_before_closest_pt){
			if (t_test > t_ball){
				t_sup = t_test;
				t_inf = t_ball;
				t_test = (t_inf + t_sup) /2;
			}
			else{
				t_inf = t_test;
				t_sup = Min(t_ball, t_closest);
				t_test = (t_inf + t_sup) /2;
			}
		}
		else{
			if (t_test > t_ball){
				t_sup = Max(t_ball, t_closest);
				t_test = (t_inf + t_sup) /2;
			}
			else{
				t_inf = t_ball;
				if (t_sup != InfCycles){
					t_test = (t_sup + t_inf) /2;
				}
				else{
					test_ballspeed = ballspeed * conf;
					t_test = t_ball + 3.0f * test_ballspeed;
				}
			}
		}
	}
	inf_sup.x = t_inf;
	inf_sup.y = t_sup;
	return t_inf < t_sup && t_sup != InfCycles && run_cycles <= 8;
}