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

bool ClearBall::Clear_Ball(Command& command){
	float clearangle=GetClearAngle();
	if (sensory.DirConf(clearangle)>0.85f)
	{
		if (Self.Is_goalie)
			kick.oakick(ball.max_speed *0.9f, clearangle, command, KK_fast); 
		else
		{
			float kicktospd = kick.smartkick(ball.max_speed,clearangle,command,KK_fast);
			if (kicktospd < 2.0f)
				return false;
		}
		lastclearangle = clearangle;
		return true;
	}
	else
		return false;
}

bool ClearBall::Clear_Ball(){
	Command command;
	bool success = Clear_Ball(command);
	if (success)
		mediator.enroll(command,Action_clear,PriorityA);
	return success;
}


float ClearBall::GetClearAngle()
{
	int upangle, lowangle,tempangle;
	if (Self.pos.y>SP_goal_width/2)
	{
		upangle = 0;
		lowangle = 90;
	}
	else if (Self.pos.y <-SP_goal_width/2)
	{
		upangle =-90;
		lowangle = 0;
	}
	else
	{
		upangle = -60;
		lowangle = 60;
	}
	float clearangle = 0,clearvalue = 0,tmpvalue = 0;
	tempangle = upangle;
	do{
		tmpvalue = clearsuccessvalue((float)tempangle) * usefulness((float)tempangle) * sensory.DirConf((float)tempangle);
		if (tmpvalue > clearvalue){
			clearangle = (float)tempangle;
			clearvalue = tmpvalue;
		}
		tempangle += (int)CP_goalie_clear_search_ang;
	}while(tempangle < lowangle);
	return clearangle;
}

/*
	evaluation function for clear angle
	the idea is from CMU .See Peter Stone's thesis for detail
*/
float ClearBall::usefulness(float ang)
{
	return 0.5f * (Sin(1.5f * (float)fabs(ang) + 45.0f) + 1);
}

float ClearBall::clearsuccessvalue(float ang)
{
	float value = 1;
	float w,d;
	Line angline;
	Vector projectpoint;
	Vector OPos;
	angline.LineFromRay(Self.pos,ang);
	for(int i = 0;i<motion.Num_TheirVisiblePlayers(); i++){ 
		OPos = motion.TheirPlayer_Close2Me(i).pos;
		if (Angle_between(NormalizeAngle((OPos - Self.pos).Angle()),NormalizeAngle(ang - CP_goalie_clear_ang),NormalizeAngle(ang + CP_goalie_clear_ang))){
			projectpoint = angline.ProjectPoint(OPos);
			w = OPos.dist(projectpoint);
			d = Self.pos.dist(projectpoint);
			value *= w/d;
		}
	}
	return value;
}

AngleDeg ClearBall::GetLastClearBallAngle(){
	return lastclearangle;
}