goalie.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 "goalie.h"
#include "bpn.h"
#include "types.h"
#include "Command.h"
#include "Strategy.h"
#include "global.h"
#include "Utils.h"

/**************************goalie***************************/
Goalie::Goalie(){

}

bool Goalie::Smartgoalie(){
	num_PIT_infos = 0;
	bottomline.LineFromTwoPoints(Vector(CP_safecatch_width - SP_semi_pitch_length,0),Vector(CP_safecatch_width - SP_semi_pitch_length,1));

	if (ball.distance>SP_catch_area_l
		||motion.TheirPlayer_Close2Ball(0).pos.dist(ball.pos)<7
		||(!ball.kickable()&&ball.speed_valid()&&
				(ball.global_vel.x<0||ball.speed<0.9f*CP_goalie_kick_min_vel)))
		GoalieJustKicked = false;
	if(!Ifcare()){
		DoLog(LOG_GOALIE,"Ball far away");
		Dawdle();
		return true;
	}
	
	
	if(ball.catchable()){
		if (!GoalieJustKicked)
		{
			if (!CatchWait())
			{
				Catchball();
				mediator.SetViewWidth(VW_Narrow); 
				return true;
			}
			else
			{
				if (OneCycleIT())
					return true;
				else
				{
					DoLog(LOG_BUG,"wait but no interception!");
					return false;
				}
			}
		}
		else DoLog(LOG_GOALIE,"Should not catch");
	}
	
	
	if(ball.kickable()){
		Kickaway();
		return true;
	}

	if (ball.pos_valid() &&ball.speed_valid() && GetInterceptionInfo(ball.pos,ball.global_vel,Self.pos,Self.global_vel,Self.bodyfacing))
		minITcycles = PIT_info[0].IT_cycles;
	else
		minITcycles = InfCycles;
	DoLog(LOG_GOALIE,"num_PIT %d",num_PIT_infos);
	bool intercept = false;	
	if (true){
		intercept =	GoalieIntercept();
	}
	if (!intercept) DefendMove();
	return true;
} 

bool Goalie::GoalieDefense(){
	GoalieJustKicked = false;
	if(!Ifcare()){
		Dawdle();
		return true;
	}
	bottomline.LineFromTwoPoints(Vector(CP_safecatch_width - SP_semi_pitch_length,0),Vector(CP_safecatch_width - SP_semi_pitch_length,1));
	
	DefendMove();
	
	return true;
}

bool Goalie::Ifcare(){
	return (ball.pos.x < 40 - SP_semi_pitch_length || ball.distance < 30);
} 
bool Goalie::IfOut(){
	if(!ball.pos_valid()) return false;
	if (IsMyPlayer(situation.ballcontroller) && ball.distance>3.5f)
		return false;
	bool controller=bool(situation.most_promising_controller==FP_goalie_number);
	float Teammatecyclegap;
	if(controller){
		Teammatecyclegap = motion.MyPlayer_CtrlOfBall(1).IT_inf.IT_cycles - motion.TheirPlayer_CtrlOfBall(0).IT_inf.IT_cycles;		
	}else{
		Teammatecyclegap = motion.MyPlayer_CtrlOfBall(0).IT_inf.IT_cycles - motion.TheirPlayer_CtrlOfBall(0).IT_inf.IT_cycles;
	}

	if(minITcycles <3) return true;
	bool withforbiddenarea;
	withforbiddenarea = (num_PIT_infos == 0) ? fieldinfo.my_penaltyarea.IsWithin(Self.IT_inf.IT_point) : fieldinfo.my_penaltyarea.IsWithin(PIT_info[0].IT_point);
	if(minITcycles<motion.TheirPlayer_CtrlOfBall(0).IT_inf.IT_cycles ){
		if(Teammatecyclegap>=0 || withforbiddenarea)
			return true;
		else
		{
			DoLog(LOG_GOALIE,"Not out intercepting");
			return false;
		}
	}else
		return false;	
}
bool Goalie::IfOppkickmore(){
	//ball is under control of opponent
	Vector p;
	Ray ballray(ball.pos,ball.vel_angle);
	if (!fieldinfo.SideLines[SL_Left].RayIntersection(ballray,p))
		return true;
	return bool( ballrollingtime(ball.speed,p.dist(ball.pos)) > motion.TheirPlayer_CtrlOfBall(0).IT_inf.IT_cycles);
} 
void Goalie::Dawdle(){
	GoalieRunto(GetGuardPoint(),0,CP_setplay_speed);
}

void Goalie::DefendMove(){
	bool shouldrun = false;
	if (!ball.pos_valid())
		return;
	
	if (!situation.BallFree || !ball.speed_valid())
		ballpoint = ball.pos;
	else
	// need further consideration
		ballpoint = ball.PredictPos((int)situation.min_IT_cycles);

	if (ballpoint.x < -SP_semi_pitch_length)
		ballpoint.x = -SP_semi_pitch_length;
	closerpost = ballpoint.y < 0 ?	fieldinfo.l_mygoalpost : fieldinfo.r_mygoalpost;
	DEF_info[0].OppPos = ballpoint;
	CalcToleranceTime(DEF_info[0]);
	DoLog(LOG_GOALIE, "Tolarence %.1f",DEF_info[0].TolerantTime);
	num_opp_considered = 1;
		
	Defendline.SetValues(ballpoint,MidAngle2((fieldinfo.l_mygoalpost - ballpoint).Angle(), 
			(fieldinfo.r_mygoalpost - ballpoint).Angle()));
	DoLog(LOG_GOALIE,"Defend Ray (%.1f %.1f) %.1f",ballpoint.x,ballpoint.y,MidAngle2((fieldinfo.l_mygoalpost - ballpoint).Angle(),(fieldinfo.r_mygoalpost - ballpoint).Angle())); 

	if (!DefendShootOK()){
		DoLog(LOG_GOALIE,"Def Shoot");
		shouldrun = DefendShoot();
	}
	else
	{
		DoLog(LOG_GOALIE,"Def Pass");
		shouldrun = DefendPass();
	}
	if (!shouldrun)
		AdjustFacings();
}


float Goalie::GetGothroughSpeed(Ray ballcourse,Vector Playerpos, Line pline){
	//assume that the player is always before the target line
	Vector secpoint;
	Vector origin = ballcourse.GetPoint(0.0f);
	if (!ballcourse.intersection(pline,secpoint))
		return CP_impossible_speed;
	float t = ballcourse.DistanceFromOrigin(secpoint);
	Vector rpos = Polar2Vector(secpoint.dist(origin),ballcourse.StrechAngle(secpoint));
	if (fieldinfo.my_penaltyarea.IsWithin(Playerpos))
		return calc_goalie_goingthroughspeed(rpos,t);
	else
		return calc_goingthroughspeed(rpos,t);
}

bool Goalie::CheckShootSpeed(Vector BallPoint,Vector DefPos ){
	float langle = (fieldinfo.l_mygoalpost -BallPoint).Angle();
	float rangle = (fieldinfo.r_mygoalpost -BallPoint).Angle();
	L_through_spd = GetGothroughSpeed(Ray(BallPoint + Polar2Vector(1.5f,langle),langle),DefPos,fieldinfo.SideLines[SL_Left]);
	R_through_spd = GetGothroughSpeed(Ray(BallPoint + Polar2Vector(1.5f,rangle),rangle),DefPos,fieldinfo.SideLines[SL_Left]);
	return (L_through_spd > SP_ball_speed_max + 0.15f && R_through_spd > SP_ball_speed_max + 0.15f);

}

bool Goalie::DefendShootOK(){
	 return CheckShootSpeed(ballpoint,Self.pos) && CheckShootSpeed(ballpoint,Self.pos);
}

bool Goalie::DefendShoot(){
	Vector defendpoint;
		fieldinfo.my_goaldefendbox.RayIntersection(Defendline,defendpoint);
	if (defendpoint.dist(Self.pos) < 3.0f)
	{
		Vector ppoint;
		if (Defendline.intersection(Line(Ray(Self.PredictPos(),Self.bodyfacing)),ppoint))
			defendpoint = ppoint;
	}
	if (defendpoint.dist(ballpoint)< CP_safecatch_width)
		defendpoint = Defendline.GetPoint(CP_safecatch_width);
	if (!fieldinfo.my_penaltyarea.IsWithin(defendpoint))
		fieldinfo.my_penaltyarea.RayIntersection(Defendline,defendpoint);	
	Vector defpoints;
	int mincycles = (int)InfCycles;
	int thiscycles;
	float pointgap = Defendline.intersection(bottomline)/8;
	float angledif;
	Vector gap;
	for (int i=0;i<8;i++)
	{
		defpoints = Defendline.GetPoint(pointgap *(i+1));
		if(fieldinfo.my_penaltyarea.IsWithin(defpoints) && CheckShootSpeed(ballpoint,defpoints))
		{
			gap =defpoints - Self.pos;
			if (gap.mod() < ball.distance/20) continue;
			angledif =(float) fabs(NormalizeAngle(gap.Angle() - Self.bodyfacing));
			if (angledif > 90) angledif = 180 - angledif;
			if (fabs(gap.mod() * angledif) < SP_kickable_margin)
				thiscycles = 0;
			else if (angledif < action.Turnrate(Self.speed))
				thiscycles = 1;
			else
				thiscycles = 2;
			thiscycles += int(gap.mod()/SP_player_speed_max - ballrollingtime(SP_player_speed_max,pointgap * (i+1))); 
			if (thiscycles  < mincycles)
			{
				mincycles = thiscycles;
				defendpoint = defpoints;
			}
		}			
	}
	if (mincycles < DEF_info[0].TolerantTime + 2)
	{
		DoLog(LOG_GOALIE,"Safe Def");
		return GoalieRunto(defendpoint,0);
	}
	else
	{
		DoLog(LOG_GOALIE,"strive Def");
		if (fieldinfo.NumNoneGoalieTeammatesInCone(ballpoint,Defendline.GetPoint(Self.pos.dist(ballpoint)/3), Defendline.StrechAngle(fieldinfo.l_mygoalpost) *2) >= 1
			||Line(Defendline).dist(Self.pos) > SP_catch_area_w)
		{
			if (GoalieRunto(defendpoint,0))
			return true;
		}
		
		Vector prpoint = Defendline.GetClosestPoint(Self.pos);
		if (Defendline.DistanceFromOrigin(prpoint) < CP_safecatch_width)
			defendpoint = Defendline.GetPoint(CP_safecatch_width);
		else
			defendpoint = Defendline.GetPoint(Defendline.DistanceFromOrigin(prpoint)*2/3);
		if (!fieldinfo.my_penaltyarea.IsWithin(defendpoint))
				if	(!fieldinfo.my_penaltyarea.RayIntersection(Defendline,defendpoint))
					DoLog(LOG_BUG,"Why no intersection ?");
		DoLog(LOG_GOALIE,"OUT Dashing");
		return GoalieRunto(defendpoint,0);
		
	}
}

bool Goalie::DefendPass(){
	int i;
	Vector NewDefPos;
	
	float shoottimel = ballrollingtime(SP_ball_speed_max,ballpoint.dist(fieldinfo.l_mygoalpost));
	float shoottimer = ballrollingtime(SP_ball_speed_max,ballpoint.dist(fieldinfo.r_mygoalpost));
	float shoottime;
	if (BallInNarrowAngle())
	{
		shoottime = Min(shoottimel,shoottimer);
		float tarkang = (ballpoint - closerpost).Angle() + 90;
		if (fabs(tarkang) > 90) tarkang = NormalizeAngle(tarkang + 180);
		DEF_info[0].DefendBase = closerpost + Polar2Vector(Min(SP_goal_area_length+1.0f,SP_player_speed_max * (shoottime - 4)),tarkang);
		DEF_info[0].TolerantTime += shoottime/2;
		DoLog(LOG_GOALIE,"narrow ang new tol %.1f",DEF_info[0].TolerantTime);
	}
	else
	{
		shoottime =Min(shoottimel - Self.pos.dist(fieldinfo.l_mygoalpost) / SP_player_speed_max,
						shoottimer - Self.pos.dist(fieldinfo.r_mygoalpost) / SP_player_speed_max);
		DEF_info[0].TolerantTime += shoottime/2;
		if (DEF_info[0].TolerantTime == 0 )
		{
			DoLog(LOG_GOALIE,"No Time For pass def");
			return false;
		}
		if (!fieldinfo.my_goaldefendbox.RayIntersection(Defendline,DEF_info[0].DefendBase))
		DEF_info[0].DefendBase = Self.pos;
	}
	DoLog(LOG_GOALIE,"Basic Def at %.1f %.1f",DEF_info[0].DefendBase.x,DEF_info[0].DefendBase.y);
	Ray ConsiderLine;
	Ray ballray;
	Vector Ppos;
	Line pLine;
	for (i = 0; i < motion.Num_TheirVisiblePlayers();i ++)
	{
		if (num_opp_considered >= 5) break;
		if (motion.TheirPlayers_Danger[i] == motion.TheirPlayers_Interception[0]
			||situation.IsControllerOrFutureController(situation.TheirSide, motion.TheirPlayers_Danger[i])) continue;
		Ppos =TheirPlayer(motion.TheirPlayers_Danger[i]).pos;
		if (TheirPlayer(motion.TheirPlayers_Danger[i]).GetDefsensitivity() < 0.89f) break;
		//if i can intercept the ball, get ready to intercept;
		ballray.SetValues(ballpoint,(Ppos-ballpoint).Angle());
		Line pLine(Ray((Self.pos + Ppos)/2,(Self.pos - Ppos).Angle() + 90));
		if (pLine.HalfPlaneTest(Self.pos) == pLine.HalfPlaneTest(ballpoint) && GetGothroughSpeed(ballray,Self.pos,pLine) > SP_ball_speed_max
			&& fieldinfo.my_penaltyarea.IsWithin(Ppos))

		{
			DEF_info[num_opp_considered].OppPos = Ppos;
			NewDefPos = ballray.GetClosestPoint(Self.pos);
			DEF_info[num_opp_considered].DefendBase = DEF_info[0].DefendBase + 
				Polar2Vector(SP_player_speed_max * Min(DEF_info[0].TolerantTime/3,3.5f),(NewDefPos - Self.pos).Angle());
			DEF_info[num_opp_considered++].TolerantTime = Max(0.0f,ballrollingtime(SP_ball_speed_max,ballray.DistanceFromOrigin(NewDefPos))
				  - NewDefPos.dist(Self.pos) /SP_player_speed_max);
			DoLog(LOG_GOALIE,"Block # %u (%.1f,%.1f) at (%.1f,%.1f)",motion.TheirPlayers_Danger[i],Ppos.x,Ppos.y,DEF_info[num_opp_considered].DefendBase.x,DEF_info[num_opp_considered].DefendBase.y);
			continue;
		};
		//get to the balanced defend point
		ConsiderLine.SetValues(Ppos,MidAngle2((fieldinfo.l_mygoalpost - Ppos).Angle(),(fieldinfo.r_mygoalpost - Ppos).Angle())); 
			DEF_info[num_opp_considered].OppPos = Ppos;
			if (!fieldinfo.my_goaldefendbox.RayIntersection(ConsiderLine,NewDefPos))
			{
				DoLog(LOG_BUG,"why no intersection 2");
				continue;
			}
			CalcToleranceTime(DEF_info[num_opp_considered]);
			DEF_info[num_opp_considered ++].DefendBase = NewDefPos;
			DoLog(LOG_GOALIE,"Def # %u (%.1f,%.1f)",motion.TheirPlayers_Danger[i],Ppos.x,Ppos.y);
			
		

	}
	if (num_opp_considered ==1)
		return GoalieRunto(DEF_info[0].DefendBase,0);
	float weights[5];
	float sumweights = 0.0f;
	weights[0] = fieldinfo.GetDefensiveSensitivity(ballpoint);
	for (i=1;i<num_opp_considered;i++)
	{
		if (DEF_info[i].DefendBase.dist(DEF_info[0].DefendBase)
			< (DEF_info[i].TolerantTime + DEF_info[0].TolerantTime) * SP_player_speed_max)
		{