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

/********************  Shoot   *******************************/
//根据允许误差范围得到失败几率
//假设kick方向在[-CP_shootangle_tolerance,CP_shootangle_tolerance]内均匀分布
float Shoot::Getprob_post(float tolerance){
	return 0.5f * Max(0.0f, 1.0f - tolerance / CP_shootangle_tolerance);
}

bool Shoot::GetShootInfo(){
	//to see if it's in the shot distance CP_max_shoot_distance
	float shoot_angle, lastfindangle;
	Ray ballcourse;

	num_shootinfo = 0; // Clear Buffer

	shoot_angle = leftangle;
	spreadangle = 0;

	bool continuum = false, newinfo;
	do{
		newinfo = GetShootInfo(shoot_angle);
		if(newinfo){
			if(continuum) 
				spreadangle += shoot_angle - lastfindangle;

			continuum = true;
			lastfindangle = shoot_angle;

			if(fabs(shoot_angle - leftangle) < CP_shootangle_tolerance || fabs(rightangle - shoot_angle) < CP_shootangle_tolerance)
				shoot_angle += CP_min_shootangle;
			else
				shoot_angle += CP_shoot_search_angle;
		}else{
			shoot_angle += CP_shoot_search_angle;
			continuum = false;
		}		
	}while(shoot_angle < rightangle);

	return num_shootinfo > 0;
}

bool Shoot::GetShootInfo(float shoot_angle){
	float shootspeed, min_shoot_speed;
	int i, num_goaliefail;
	Ray ballcourse;

	ballcourse.SetValues(ballpos, shoot_angle);
	float t = ballcourse.intersection(fieldinfo.SideLines[SL_Right]);
	if (t < 0 || fabs(t) > SP_pitch_length) 
		return false;//it is impossible to shoot
	min_shoot_speed = t * (1 - SP_ball_decay);
	num_goaliefail = 0;
	
	if(goalie_exist){
		shootspeed = calc_goalie_goingthroughspeed(goaliepos.Rotate(-shoot_angle) , t);
		if (shootspeed >= SP_ball_speed_max)
			return false;
		min_shoot_speed = Max(min_shoot_speed, shootspeed);
		for(int i = 0; i< num_checkpos; i++){
			shootspeed = calc_goalie_goingthroughspeed(checkpos[i].Rotate(-shoot_angle) , t);
			if (shootspeed >= SP_ball_speed_max) num_goaliefail ++;
		}
	}
	
	for (i = 0; i < num_consider_opp; i++){
		shootspeed = calc_goingthroughspeed(Opp_relpos[i].Rotate(-shoot_angle) , t);
		min_shoot_speed = Max(shootspeed, min_shoot_speed);
		if(min_shoot_speed >= SP_ball_speed_max)
			return false;
	}
	
	if (min_shoot_speed < SP_ball_speed_max){
		AddShootInfo(min_shoot_speed, shoot_angle, CP_shoot_avoidgoalie * (float)num_goaliefail / (num_checkpos + 1));
		return true;
	}	
	return false;
} 

void Shoot::AddShootInfo(float min_shootspeed, float shootangle, float goaliefail){
	shootinfo[num_shootinfo].shootangle = shootangle;
	shootinfo[num_shootinfo].prob_postfail = Getprob_post(shootangle - leftangle)
		+ Getprob_post(rightangle - shootangle);
	shootinfo[num_shootinfo].min_shootspeed = min_shootspeed;
	shootinfo[num_shootinfo].prob_goaliefail = goaliefail;

	DoLog(LOG_SHOOT, "Shoot %.2f %.2f %.2f %.2f", shootangle, min_shootspeed, shootinfo[num_shootinfo].prob_postfail, goaliefail);
	num_shootinfo ++;
}

bool Shoot::SmartShoot(){
	if(!ball.shootable()) return false;

	Initballpos(ball.pos);
	if(fieldinfo.IsValidtheirgoalie()){
		Setgoaliepos(TheirPlayer(fieldinfo.theirgoalie).rel_pos_2_ball);  
		int time = situation.CurrentTime -  TheirPlayer(fieldinfo.theirgoalie).original_postime ;
		time = Max(1, Min(time, 3));
		Setgoalieradius(1.0f * time);
	}

	for(int i = 0; i < motion.Num_TheirVisiblePlayers(); i++){
		if(motion.TheirPlayer_InFront(i).Is_goalie) continue;
		
		if(i > 3 && motion.TheirPlayer_InFront(i).rel_pos_2_ball.x < - 5) break;
		Addopponent(motion.TheirPlayer_InFront(i).rel_pos_2_ball);
	}

	GetShootInfo();
	return bool(num_shootinfo > 0);
}

void Shoot::Initballpos(Vector pos, bool Reverse){
	goalie_exist = false;
	num_consider_opp = 0;
	reverse = Reverse;	
	ballpos.x = Reverse ? -pos.x : pos.x;
	ballpos.y = pos.y;
	num_checkpos = 0;
	
	leftangle = (fieldinfo.l_theirgoalpost - ballpos).Angle();
	rightangle = (fieldinfo.r_theirgoalpost - ballpos).Angle();
}

void Shoot::Setgoaliepos(Vector relpos){
	goalie_exist = true;
	goaliepos.x = reverse ? -relpos.x : relpos.x;
	goaliepos.y = relpos.y;
	
	DoLog(LOG_SHOOT, "goalie relpos(%.2f %.2f)", goaliepos.x, goaliepos.y);
}

void Shoot::Addopponent(Vector relpos){
	if(num_consider_opp < SP_team_size){
		Opp_relpos[num_consider_opp].x = reverse ? -relpos.x : relpos.x;
		Opp_relpos[num_consider_opp].y = relpos.y;
		num_consider_opp++;
	}
}

void Shoot::Setgoalieradius(float radius){
	if(!goalie_exist) return;
	checkpos[0] = Supposegoalie(ballpos + goaliepos, radius) - ballpos;
	num_checkpos = 1;
	return;

	checkpos[0].x = goaliepos.x;
	checkpos[0].y = goaliepos.y - radius;
	checkpos[1].x = goaliepos.x;
	checkpos[1].y = goaliepos.y + radius;
	num_checkpos = 2;
}

bool Shoot::GetBestShoot(ShootInfo& info){
	if(num_shootinfo <= 0) return false;
	
	int bestinfo = -1;
	float maxpriority = 0, priority;
	for(int i = 0; i< num_shootinfo; i++){
		priority = 0.1f + (1.0f - shootinfo[i].prob_postfail) * (1.0f - shootinfo[i].prob_goaliefail)
			- 0.01f * shootinfo[i].min_shootspeed;
		if(priority > maxpriority){
			maxpriority = priority;
			bestinfo = i;
		}
	}
	if(bestinfo > -1){
		info = shootinfo[bestinfo];
		return true;
	}else
		return false;
}

Vector  Shoot::Supposegoalie(Vector pos, float radius){
	Vector tpos;
	float midangle = 0.5f * (leftangle + rightangle);
	Ray ballcourse;
	ballcourse.SetValues(ballpos, midangle);
	float t = ballcourse.intersection(fieldinfo.SideLines[SL_Right]);
	tpos = ballpos + Polar2Vector(0.66f * t, midangle);	

	if(reverse) pos.x = -pos.x;
	if(pos.dist(tpos) > radius){
		tpos =   pos + Polar2Vector(radius, (tpos - pos).Angle());
	}

	if(reverse) tpos.x = -tpos.x;
	DoLog(LOG_SHOOT, " suppose goalie pos(%.2f %.2f)", tpos.x, tpos.y);
	return tpos;
}

float Shoot::GuessShoot(Vector pos, int cycles){
	if(!fieldinfo.WithInShootableArea(pos)) return 0;
	
	motion.Initballpos(pos);
	Vector goaliepos;
	if(fieldinfo.IsValidtheirgoalie()){
		goaliepos = motion.Supposegoalie(TheirPlayer(fieldinfo.theirgoalie).pos, 1.0f *  cycles);
		motion.Setgoaliepos(goaliepos - pos);
	}else if(motion.Num_TheirVisiblePlayers() > 0){
		goaliepos = motion.Supposegoalie(motion.TheirPlayer_InFront(0).pos, 1.0f *  cycles);
		motion.Setgoaliepos(goaliepos - pos);
	} 
	motion.GetShootInfo();
	return motion.GetShootWidth();
}

KT_Res Shoot::shoot(ShootInfo& shootinf, Command& command){
	command.Reset();
	return kick.oakick(shootinf.MinShootSpeed(), shootinf.ShootAngle(), command, KK_fast);
}