goalie.cpp

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

			weights[i] = fieldinfo.GetDefensiveSensitivity(DEF_info[i].OppPos);
			sumweights += weights[i];
			DEF_info[i].DefendBase = (DEF_info[0].DefendBase * weights[i] + DEF_info[i].DefendBase * weights[0])
				 /(weights[0] + weights[i]);
		}
		else
			weights[i] = 0.0f;
	}
	if (sumweights ==0.0f)
		return GoalieRunto(DEF_info[0].DefendBase,0);
	else
	{
		NewDefPos = Vector(0,0);
		for (i=1;i<num_opp_considered;i++)
			NewDefPos += DEF_info[i].DefendBase * (weights[i]/sumweights);
		return GoalieRunto(NewDefPos,0);
	}
}

void Goalie::AdjustFacings(){
	AngleDeg bodyfacing;
	Vector OppPos;
	if (fieldinfo.NumNoneGoalieTeammatesInCone(ballpoint,Defendline.GetPoint(3.0f),
		Defendline.StrechAngle(fieldinfo.l_mygoalpost) * 2) <= 0
		&&!(CheckShootSpeed(ballpoint + Polar2Vector(2.0f,(ballpoint - fieldinfo.l_mygoalpost).Angle()),Self.pos)
		  &&CheckShootSpeed(ballpoint,Self.pos) 
		  && CheckShootSpeed(ballpoint + Polar2Vector(2.0f,(ballpoint - fieldinfo.r_mygoalpost).Angle()),Self.pos)))
		//prepare to dash out 
	{
		DoLog(LOG_GOALIE,"prep dash");
		Vector prpoint = Defendline.GetClosestPoint(Self.pos);
		if (Defendline.DistanceFromOrigin(prpoint) < CP_safecatch_width)
			prpoint = Defendline.GetPoint(CP_safecatch_width);
		else
			prpoint = Defendline.GetPoint(Defendline.DistanceFromOrigin(prpoint)*2/3);
	
		bodyfacing = motion.turn_to(prpoint);
		nextfacing = Self.bodyfacing + bodyfacing;
	}
	else
	{
		if (num_opp_considered <=1 )
			OppPos = ballpoint;
		else
			OppPos = fieldinfo.GetDefensiveSensitivity(ballpoint) > fieldinfo.GetDefensiveSensitivity(DEF_info[1].OppPos) ?
					ballpoint : DEF_info[1].OppPos;
		DoLog(LOG_GOALIE,"advise %.1f,%.1f",OppPos.x,OppPos.y);
		Line DOrgLine;
		Command command;
		DOrgLine.LineFromTwoPoints(OppPos,fieldinfo.mygoal);
		if(BallInNarrowAngle())
		{
			if (ball.pos.y > 0) bodyfacing = 90;
			else
				bodyfacing = - 90;
		}
		else
		{
			Vector defpoint = DOrgLine.ProjectPoint(Self.pos);
			if (!fieldinfo.WithInField(defpoint))
				defpoint = DOrgLine.intersection(bottomline);
			bodyfacing = (defpoint - Self.pos).Angle();
			if (fabs(NormalizeAngle(bodyfacing - (defpoint - fieldinfo.mygoal).Angle())) > 90)
			{
				bodyfacing = NormalizeAngle(bodyfacing + 180);
			}
		}
		bodyfacing = Self.bodyfacing + motion.turn_to(bodyfacing,command);
		// keep ball in focus
		if (fabs(NormalizeAngle(bodyfacing - ball.global_angle)) > SP_max_neck_angle)
			bodyfacing = fabs(NormalizeAngle(bodyfacing - ball.global_angle + SP_max_neck_angle))
						 > fabs(NormalizeAngle(bodyfacing - ball.global_angle - SP_max_neck_angle))
						 ? ball.global_angle + SP_max_neck_angle : ball.global_angle - SP_max_neck_angle;
			motion.turn_to(bodyfacing);
		nextfacing = bodyfacing;
	}
}

bool Goalie::GoalieRunto(Vector point ,int deviationdegree,float speed){
	Command command;
	DoLog(LOG_GOALIE,"target %.1f,%.1f",point.x,point.y);
	if (point.x < CP_safecatch_width - SP_semi_pitch_length)
	{
		point.x = CP_safecatch_width - SP_semi_pitch_length;
		DoLog(LOG_GOALIE,"Adjust posx %.1f",point.x);
	}	
	Vector gap = point - Self.PredictPos();
	float angle;
	
	if (deviationdegree == 0)
	{
		if (gap.mod() < ball.distance / 20)
		{
			DoLog(LOG_GOALIE,"Already reach");
			return false;
		}
			
		//perform a smooth motion
		if (speed > 0)
			speed = Min(speed,(gap + Self.global_vel).mod() * (1- SP_player_decay));
		else
			speed = Max(speed,-(gap + Self.global_vel).mod() * (1- SP_player_decay));
		
		if (speed < 0)
			angle = NormalizeAngle(gap.Angle() + 180);
		else
			angle = gap.Angle();

		//should dash backward to keep correct facing;	
		if (ball.pos_valid() &&(speed >0 && fabs(NormalizeAngle(gap.Angle() - ball.global_angle)) >90
			|| speed < 0 && fabs(NormalizeAngle(gap.Angle() - ball.global_angle)) < 90))
		{
			speed = - speed;
			angle = NormalizeAngle(angle + 180);
		}	 
	if (fabs(Self.pos.dist(point) * Sin(angle - Self.bodyfacing)) > SP_kickable_margin && 
		fabs(NormalizeAngle(angle -Self.bodyfacing)) > 15
		|| fabs(NormalizeAngle(angle - Self.bodyfacing)) > 90)
	{
			angle = motion.turn_to(angle, command);
			nextfacing = Self.bodyfacing + angle;
		}
		else{
			motion.run(speed, command);
			nextfacing = Self.bodyfacing;
		}
		mediator.enroll(command,Action_positioning_defense,PriorityA * 5.0f); 
		return true;
	}
	
	float controlmargin;
	if (deviationdegree ==2)
		controlmargin = CP_reliable_kickarea * 0.95f;
	else
		controlmargin = SP_catch_area_l;
	if (speed < 0)
			angle = NormalizeAngle(gap.Angle() + 180);
		else
			angle = NormalizeAngle(gap.Angle());

	if (fabs(Self.pos.dist(point) * Sin(angle - Self.bodyfacing)) > controlmargin
		|| fabs(NormalizeAngle(angle - Self.bodyfacing)) > 90)
	{
		angle = motion.turn_to(angle,command);
		nextfacing = Self.bodyfacing + angle;
	}
	else
	{
		motion.run(speed,command);
		nextfacing = Self.bodyfacing;
	}
		mediator.enroll(command,Action_interception,PriorityA);
	return true;
	
} 

void Goalie::Catchball(){
	action.seize(NormalizeAngle(ball.global_angle - Self.bodyfacing));
}

void Goalie::Kickaway(){
	kick.smartkick(SP_ball_speed_max, motion.GetClearAngle(),KK_fast); 
}

bool Goalie::IsShoot(){
	Ray ballray;
	if(ball.global_vel.x>=0) return false; 
	if (!situation.BallFree) 
	{
		DoLog(LOG_GOALIE,"Not ball free");
		return false;	
	}
	ballray.SetValues(ball.pos,ball.global_vel);
	Vector p;
	if(!fieldinfo.SideLines[SL_Left].RayIntersection(ballray,p))
	{
		DoLog(LOG_GOALIE,"No inters");
		return false;
	}
	if ( p == Vector(0,0) )
	{
		DoLog(LOG_GOALIE,"invalid inters");
		return false;
	}
	if ( (p.y > SP_goal_area_width /2) || (p.y < -SP_goal_area_width / 2) ) 
	{
		DoLog(LOG_GOALIE,"OUT  %.1f %.1f",p.x,p.y);
		return false;
	}
	bottomline.RayIntersection(ballray,p);
	float maxballdist=ball.speed/(1-SP_ball_decay);
	return bool(p.dist(ball.pos)<maxballdist);
}

void Goalie::AddPITinfo(PassiveITInfo& PassiveIT_info){
	PIT_info[num_PIT_infos]=PassiveIT_info;
	num_PIT_infos++;
	DoLog(LOG_GOALIE,"PIT cy %.1f, sp %.1f,wt %.1f,at %.1f %.1f",PassiveIT_info.IT_cycles,
		PassiveIT_info.IT_rapidness,PassiveIT_info.IT_waitcycles,PassiveIT_info.IT_point.x,PassiveIT_info.IT_point.y);
} 

bool Goalie::GetInterceptionInfo(Vector ballpos,Vector ballvel,Vector Selfpos,Vector Selfvel,AngleDeg facing){
	mostpromisingIT.turnangle = InvalidAngle;
	float mingap = SP_pitch_diameter;
	PassiveITInfo IT_info;
	int Turncycle;
	float angdif;
	float controlmargin;
	if (!fieldinfo.WithInField(ballpos)) return false;
	Vector orgballpos = ballpos;
	
	const int CP_stational_cycles = 6;
	Vector playerpos[CP_stational_cycles + 1];
	Vector playervel[CP_stational_cycles + 1];
	playerpos[0] = Selfpos;
	playervel[0] = Selfvel;
	int i;
	int reachballcycle;
	for (i=1;i <= CP_stational_cycles;i++)
	{
		playerpos[i] = playerpos[i-1] + playervel[i-1];
		playervel[i] = playervel[i] * SP_player_decay;
	}
	
	if (ballvel.mod() < 0.10f)
	{
		if (fieldinfo.my_penaltyarea.IsWithin(ballpos))
			controlmargin = SP_catch_area_l;
		else
			controlmargin = SP_kickable_area;
		controlmargin *= 0.95f;
		IT_info.IT_point= ballpos;
		IT_info.IT_waitcycles = 0;
		IT_info.turnangle = (ballpos - Selfpos).Angle();
		angdif = float(fabs(NormalizeAngle(IT_info.turnangle - facing)));
		Turncycle = 0;
		while (ballpos.dist(playerpos[Turncycle]) * Sin(angdif) > controlmargin){
				angdif -=action.Turnrate(playervel[Turncycle].mod()) * SP_max_moment;
				if (angdif <0) angdif =0;
				Turncycle ++;
			}
		IT_info.IT_rapidness = Min(SP_player_speed_max,(ballpos - Selfpos).mod());
		IT_info.IT_cycles =(ballpos - Selfpos).mod()  / SP_player_speed_max + Turncycle;
		AddPITinfo(IT_info);
		if (angdif >90 && ball.distance < 5 * SP_player_speed_max)
		{
			angdif = 180 - angdif;
			Turncycle = 0;
			while (ballpos.dist(playerpos[Turncycle]) * Sin(angdif) > controlmargin){
					angdif -=action.Turnrate(playervel[Turncycle].mod()) * SP_max_moment;
					if (angdif <0) angdif =0;
					Turncycle ++;
				}
			DoLog(LOG_GOALIE,"Turncycle %d",Turncycle);
			IT_info.IT_rapidness = Max(- SP_player_speed_max,- ballpos.dist(Selfpos));
			IT_info.IT_cycles = ballpos.dist(Selfpos) / SP_player_speed_max + Turncycle;		
			AddPITinfo(IT_info);
		}
		return true;
	}

	Ray ballcourse;
	ballcourse.SetValues(ballpos,ballvel);
	Vector closestpoint = ballcourse.GetClosestPoint(Selfpos);
	if (!fieldinfo.WithInField(closestpoint))
	ballcourse.intersection(fieldinfo.SideLines[SL_Left],closestpoint);
	float rundist = Selfpos.dist(closestpoint);
	float mincycle = (rundist - SP_catch_area_l)/ SP_player_speed_max;
	if (mincycle <0) mincycle = 0;
	Vector p1;
	while (ballrollingtime(ballvel.mod(),ballcourse.DistanceFromOrigin(closestpoint)) < mincycle)
	{
		rundist += SP_player_speed_max;
		mincycle +=1;
		if (ballcourse.CircleIntersect(rundist,Selfpos,p1,closestpoint) == 0)
			return false;
		if (!fieldinfo.WithInField(closestpoint))
			return false;

	}
	

	int ITCycle = int(mincycle);
	int FullDashCycle;
	bool canbackdash;
	int CP_back_dash_max_cycles = 15;
	Vector shadowpoint,dashtopoint,dashvel;
	DoLog(LOG_GOALIE,"Min by projectpoint %d",ITCycle);
	proceedball(ballpos,ballvel,ITCycle);
	do{
		FullDashCycle = Min(CP_stational_cycles,ITCycle);
		if (fieldinfo.my_penaltyarea.IsWithin(ballpos))
			controlmargin = SP_catch_area_l;
		else
			controlmargin = SP_kickable_area;
		IT_info.turnangle = (ballpos - playerpos[FullDashCycle]).Angle();
		
		if ((ballpos - playerpos[FullDashCycle]).mod() < controlmargin)
		{
			IT_info.IT_cycles = (float)ITCycle;
			IT_info.IT_point= ballpos;
			IT_info.IT_rapidness = Min((ballpos - playerpos[FullDashCycle]).mod(),SP_player_speed_max);
			IT_info.IT_waitcycles =float(ITCycle);
			AddPITinfo(IT_info);			
		}
		
		if (ITCycle > CP_back_dash_max_cycles)
			canbackdash = false;
		else canbackdash = true;
		// check if back dash will lost sight of ball
		if (canbackdash && ITCycle > 5)
		{
			shadowpoint = ballpos - ballvel * 6.0f;	    
			if ((shadowpoint - Selfpos).mod() > SP_player_speed_max * (ITCycle - 5) + 3.0f
				&&ballcourse.DistanceFromOrigin(ballcourse.GetClosestPoint(Selfpos)) < 5 * ballvel.mod())
			{
				
				dashtopoint = ballpos - Polar2Vector(5 * SP_player_speed_max + controlmargin,IT_info.turnangle);
				if (fabs(NormalizeAngle(IT_info.turnangle - (shadowpoint - dashtopoint).Angle())) > 45)
				canbackdash = false; 
			}
		}
		//check if turn and dashing forward can get the ball
		angdif = (float)fabs(NormalizeAngle(IT_info.turnangle - facing));
		if (angdif < 90) canbackdash = false;
		Turncycle = 0;
		while (ballpos.dist(playerpos[Turncycle]) * Sin(angdif) > controlmargin || (angdif > 90)){
			angdif -=action.Turnrate(playervel[Turncycle].mod()) * SP_max_moment;
			if (angdif <0) angdif =0;
			Turncycle ++;
		}
		reachballcycle = -1;
		if (Turncycle <= ITCycle)
		{
			dashtopoint = playerpos[Turncycle];
			dashvel = playervel[Turncycle];
			for (i = Turncycle; i < FullDashCycle;i++)
			{
				dashvel +=Polar2Vector(SP_max_power * action.Dashrate(),IT_info.turnangle);
				if (dashvel.mod() > SP_player_speed_max)
					dashvel =dashvel/dashvel.mod() * SP_player_speed_max;
				dashtopoint += dashvel;
				if (dashtopoint.dist(ballpos) < controlmargin)
					reachballcycle = i+1;
			}
			if ((dashtopoint.dist(ballpos) - controlmargin) /SP_player_speed_max 
				<= ITCycle - FullDashCycle)
			{
				IT_info.IT_cycles = (float)ITCycle;
				IT_info.IT_point= ballpos;
				if (angdif < 5 && ballvel.mod() > 1.4f)
					IT_info.IT_point+= Polar2Vector(SP_kickable_area /2,ballvel.Angle());
				IT_info.IT_rapidness = Min(SP_player_speed_max,(float)fabs(orgballpos.dist(Self.PredictPos(Turncycle)) * Cos(angdif)));
				if (reachballcycle != -1)
					IT_info.IT_waitcycles =float(ITCycle - reachballcycle);
				else
					IT_info.IT_waitcycles =ITCycle - FullDashCycle - Max((dashtopoint.dist(ballpos) - controlmargin) /SP_player_speed_max , 0.0f);
				AddPITinfo(IT_info);
			}
			else
			{