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

/**************   Dribble **********************************/
Dribble::Dribble(){
}

bool Dribble::k_StepDribble(AngleDeg dribble_angle, AngleDeg ball_angle, float rapidness){
	Command command;
	DribbleAccount dribbleacount;
	bool success = k_StepDribble(dribble_angle, ball_angle, rapidness, dribbleacount, command);
	mediator.enroll(command, Action_dribble, 2 * PriorityA);
	return success;
}

bool Dribble::k_StepDribble(AngleDeg dribble_angle, AngleDeg ball_angle, float rapidness, DribbleAccount& draccount){
	Command command;
	bool success = k_StepDribble(dribble_angle, ball_angle, rapidness, draccount, command);
	mediator.enroll(command, Action_dribble, 2 * PriorityA);
	return success;
}


bool Dribble::k_StepDribble(AngleDeg dribble_angle, AngleDeg ball_angle, float rapidness, DribbleAccount& draccount, Command& command){
	draccount.draction = DR_NoAction;
	if (ball_angle == InvalidAngle) return false; 
	if (fabs(NormalizeAngle(Self.bodyfacing - dribble_angle)) > 15){
		int Turncycle = action.Turnrate() * SP_max_moment > 
				fabs(NormalizeAngle(dribble_angle - Self.bodyfacing)) ? 1 : 2;	
		Vector ballrel = ball.pos - Self.pos;
		float predballangle = NormalizeAngle(ballrel.Angle() - dribble_angle);
		int intrcycles = TryIntercept(dribble_angle,ball.PredictPos(Turncycle),ball.global_vel * Exp(SP_ball_decay,(float)Turncycle),
			Self.PredictPos(Turncycle),Self.global_vel * Exp(SP_player_decay,(float)Turncycle),rapidness,1);
		if (intrcycles != -1 && ball.PredictPos(1).dist(Self.PredictPos()) > CP_turnball_radius_threshold)
			//no need to hold ball, so turn to target direction
		{
			DoLog(LOG_DRIBBLE,"Turn");
			motion.turn_to(dribble_angle,command);
			draccount.NextBallPos = ball.PredictPos(1);
			draccount.NextCmd = command;
			draccount.draction = DR_Turn;
			draccount.RetainCycle = intrcycles + Turncycle;
			draccount.RetainPos = ball.PredictPos(intrcycles + Turncycle);
			draccount.IsCriticalAction = true;
			return true;
		}
		else
			return TurnKick(dribble_angle,ball_angle,rapidness,draccount,command,Turncycle);
	}
	else
	{
		if (BallInTrace(Self.bodyfacing,ball.pos,ball.global_vel,Self.pos,Self.global_vel,ball_angle,rapidness))
		{
			DoLog(LOG_DRIBBLE,"dash");
			command.dash(SP_max_power * rapidness);
			draccount.NextBallPos = ball.PredictPos(1);
			draccount.NextCmd = command;
			draccount.draction = DR_Dash;
			draccount.RetainCycle = TryIntercept(Self.bodyfacing,ball.pos,ball.global_vel,Self.pos,Self.global_vel,rapidness);
			draccount.RetainPos = ball.PredictPos(draccount.RetainCycle);
			draccount.IsCriticalAction = false;
			return true;
		}
		else
		{
			if (!ball.kickable()) return false;
			bool canchangetarget;
			int dashstep = 1;
			Vector dashedvel = Self.global_vel * Self.speed_decay + Polar2Vector(action.Max_DashEffect() * rapidness,Self.bodyfacing);
			if (dashedvel.mod() > SP_player_speed_max)
				dashedvel = dashedvel / dashedvel.mod() * SP_player_speed_max;
			Vector PredDashedPos = Self.PredictPos() + dashedvel;
			Vector targetpos = PredDashedPos + Polar2Vector(GetDribbleRadius(ball_angle),ball_angle + Self.bodyfacing);
			AngleDeg avoidcollangle = ball_angle > 0 ? 90.0f : -90.0f;
			Vector avoidcollisionpos = PredDashedPos + Polar2Vector(CP_turnball_radius_threshold *1.2f,avoidcollangle + Self.bodyfacing);
			Vector predminballvel = avoidcollisionpos - ball.pos;
			Vector vel_2_facing = predminballvel.Rotate(-Self.bodyfacing);
			if (vel_2_facing.x < 0)
			{
				vel_2_facing.x = 0; 
				predminballvel = vel_2_facing.Rotate(Self.bodyfacing);
				avoidcollisionpos = ball.pos + predminballvel;
			}
			canchangetarget = (BallInTrace(Self.bodyfacing,avoidcollisionpos,predminballvel * ball.speed_decay,Self.PredictPos(),
				Self.global_vel * Self.speed_decay,ball_angle,rapidness) ||avoidcollisionpos.dist(Self.PredictPos()) < CP_reliable_kickarea - 0.1f * (predminballvel - ball.global_vel).mod())
				&& ((predminballvel - ball.global_vel).mod() <= action.Max_KickEffect());
			Vector kickrel =(targetpos - ball.pos) / (1 + SP_ball_decay);
			Vector savekickrel = kickrel;
			float vdist = (ball.pos + kickrel).dist(Self.PredictPos());
			while ( vdist < CP_reliable_kickarea * 0.95f && dashstep < 3)
			{
				dashstep++;
				dashedvel *= SP_player_decay;
				dashedvel +=Polar2Vector(action.Max_DashEffect() * rapidness,Self.bodyfacing);
				if (dashedvel.mod() > SP_player_speed_max)
				dashedvel = dashedvel / dashedvel.mod() * SP_player_speed_max;
				targetpos +=dashedvel;
				kickrel = (targetpos - ball.pos) * 2/((dashstep + 1) * (2 - (1-SP_ball_decay) * dashstep));
				vdist = (ball.pos + kickrel).dist(Self.PredictPos());
				if (vdist < CP_reliable_kickarea * 0.95f)
					savekickrel = kickrel;
				else
					dashstep--;
			}
			kickrel = savekickrel;
			float kicktospd = kick.smartkick(kickrel.mod(),kickrel.Angle(),command);
			kickrel = kickrel/kickrel.mod() * kicktospd;
			

			if (!BallInTrace(Self.bodyfacing,ball.pos + kickrel,kickrel * ball.speed_decay,Self.PredictPos(),
				Self.global_vel * Self.speed_decay,ball_angle,rapidness) || 
				(ball.pos + kickrel).dist(Self.PredictPos()) < CP_turnball_radius_threshold)
			{
				if (canchangetarget)
				{
					DoLog(LOG_DRIBBLE,"Change target for coll");
					kickrel = avoidcollisionpos - ball.pos;
					kicktospd = kick.smartkick(kickrel.mod(),kickrel.Angle(),command);
					kickrel = kickrel/kickrel.mod() * kicktospd;
				}
				else
					// adjust to desired relative angle
					return KickAdjust(Self.bodyfacing,ball_angle,rapidness,draccount,command);
			}

			DoLog(LOG_DRIBBLE,"kick forward cyc %d",dashstep+1);
			draccount.NextBallPos = ball.pos + kickrel;
			draccount.NextCmd = command;
			draccount.draction = DR_KickForward;
			if (Self.PredictPos().dist(draccount.NextBallPos) < CP_reliable_kickarea)
				{
					draccount.RetainCycle =1;
					draccount.RetainPos = draccount.NextBallPos;
				}
				else
				{
					draccount.RetainCycle = 2;
					draccount.RetainPos = targetpos;
				}
			draccount.IsCriticalAction = false;
			return true;
		}
	}
	return false;
}

bool Dribble::TurnKick(AngleDeg dribble_angle, AngleDeg ball_angle, float rapidness, DribbleAccount& draccount, Command& command,int Turncycle){
	if (!ball.kickable()) return false;
	Vector turndashpos = Self.PredictPos(Turncycle) + Polar2Vector(SP_max_power * rapidness * action.Dashrate(),dribble_angle);
	if (ball_angle > 90) ball_angle =90.0f;
	if (ball_angle < -90) ball_angle = -90.0f;
	Vector kickrel = (turndashpos + Polar2Vector(GetDribbleRadius(ball_angle),dribble_angle + ball_angle) - ball.pos) / (1.0f + Turncycle);
	
	if ((ball.pos + kickrel).dist(Self.PredictPos()) < CP_turnball_radius_threshold)
		return KickAdjust(dribble_angle,ball_angle,rapidness,draccount,command);
			
	DoLog(LOG_DRIBBLE,"Kick before turn");
	kick.smartkick(kickrel.mod(),kickrel.Angle(),command);
	draccount.NextBallPos = ball.pos + kickrel;
	draccount.NextCmd = command;
	draccount.draction = DR_KickTurn;
	draccount.RetainCycle = Turncycle + 1;
	draccount.RetainPos = turndashpos + Polar2Vector(CP_turnball_radius_threshold,dribble_angle + ball_angle);
	draccount.IsCriticalAction = false;
	return true;
}

bool Dribble::KickAdjust(AngleDeg dribble_angle,AngleDeg ball_angle,float rapidness,DribbleAccount& draccount,Command& command){
	if (ball_angle == InvalidAngle)
		return false;
	Vector	kickrel = Self.PredictPos() + Polar2Vector(CP_desired_kickarea,ball_angle + dribble_angle);
   	kickrel  -= ball.pos;
	float kicktospd = action.Kickrate() * SP_max_power;
	if (kicktospd  >= (ball.global_vel - kickrel).mod())
	{
		DoLog(LOG_DRIBBLE,"Direct adjust");
		kick.smartkick(kickrel.mod(),kickrel.Angle(),command);
		draccount.NextBallPos = ball.pos + kickrel;
		draccount.NextCmd = command;
		draccount.draction = DR_KickAdjust;
		draccount.RetainCycle = 1;
		draccount.RetainPos = draccount.NextBallPos;
		draccount.IsCriticalAction = ((ball.global_vel - kickrel).mod() > 1.8f);
		return true;
	}
	else
	{
	//adjust to most close relative angle, see dribble.doc for geometry analyze
		Vector ballrel = ball.PredictPos(1) - Self.PredictPos(); 
		float movdis = (Sqr(kicktospd) - Sqr(CP_desired_kickarea) - Sqr(ballrel.mod())) / (2* ballrel.mod());
		Line sectionline;
		sectionline.LineFromRay(Self.PredictPos() + Polar2Vector(movdis,ballrel.Angle() + 180),ballrel.Angle() + 90);
		Vector target1,target2;
		int num_is = sectionline.CircleIntersect(CP_desired_kickarea,Self.PredictPos(),target1,target2);
		if (num_is == 0)
		{
			DoLog(LOG_BUG," too strong or weak ??");
			return false;
		}
		float Angle1 = NormalizeAngle((target1 - Self.PredictPos()).Angle() - dribble_angle - ball_angle);
		float Angle2 = NormalizeAngle((target2 - Self.PredictPos()).Angle() - dribble_angle - ball_angle);
		if (fabs(Angle1) > 15)
			kickrel = target2 - ball.pos;
		else if (fabs(Angle2) > 15)
			kickrel = target1 - ball.pos;
		else
		{
			Angle1 = NormalizeAngle((target1 - Self.PredictPos()).Angle() - dribble_angle);
			Angle2 = NormalizeAngle((target2 - Self.PredictPos()).Angle() - dribble_angle);
			if (fabs(Angle1) < fabs(Angle2))
				kickrel = target1 - ball.pos;
			else
				kickrel = target2 - ball.pos;
		}
		DoLog(LOG_DRIBBLE,"Try Adjust");
		kick.smartkick(kickrel.mod(),kickrel.Angle(),command);
		draccount.NextBallPos = ball.pos + kickrel;
		draccount.NextCmd = command;
		draccount.draction = DR_KickAdjust;
		draccount.RetainCycle = 1;
		draccount.RetainPos = draccount.NextBallPos;
		draccount.IsCriticalAction = true;
		return true;
	}
		
}

bool Dribble::BallInTrace(AngleDeg bodyfacing,Vector ballpos, Vector ballvel,Vector playerpos,Vector playervel,AngleDeg ball_angle,float rapidness, int IT_cycle){
	playervel += Polar2Vector(action.Max_DashEffect(),bodyfacing);
	if (playervel.mod() > Self.max_speed)
		playervel = playervel / playervel.mod() * Self.max_speed;
	
	Vector nextplayerpos = playerpos + playervel;
	Vector ballnextrelpos = ballpos + ballvel - nextplayerpos;
	AngleDeg predballangle = NormalizeAngle((ballnextrelpos).Angle() - bodyfacing);
	if (fabs(NormalizeAngle(predballangle - ball_angle)) > 15 
		&& (fabs(predballangle) > fabs(ball_angle) || predballangle * ball_angle < 0))
	{
		DoLog(LOG_DRIBBLE,"ball falling off");
		return false; //ball falling off
	}
	if (fabs(predballangle) > 90 && ballnextrelpos.mod() > CP_reliable_kickarea)
	{
		DoLog(LOG_DRIBBLE,"ball dropped");
		return false; //ball dropped
	}
	if (ballnextrelpos.mod() < CP_turnball_radius_threshold)
		//||(ballpos - nextplayerpos).mod() < SP_ball_size + SP_player_size) //collision 
	{
		DoLog(LOG_DRIBBLE,"to coll");
		return false;
	}
	Command command;
	if (TryIntercept(bodyfacing,ballpos + ballvel,ballvel * SP_ball_decay,nextplayerpos, playervel * Self.speed_decay,rapidness,IT_cycle) == -1)
	{
		DoLog(LOG_DRIBBLE,"lose control");
		return false; //lose control
	}
	return true;
}



int Dribble::TryIntercept(AngleDeg dribble_angle,Vector ballpos,Vector ballvel,Vector selfpos, Vector selfvel,float rapidness, int recovercycle){ 
	int cycles = 0;
	float errorgap =float(fabs(ballvel.Rotate( -dribble_angle).y) * 0.1f ); 
	do{
		if (ballpos.dist(selfpos) < CP_reliable_kickarea && 
			fabs(Sin((ballpos - selfpos).Angle() - dribble_angle) * ballpos.dist(selfpos)) < 0.8f - errorgap)
		return cycles;
		ballpos += ballvel;
		ballvel *= SP_ball_decay;
		selfvel += Polar2Vector(action.Dashrate() * SP_max_power * rapidness,dribble_angle);
		if (selfvel.mod() > SP_player_speed_max)
			selfvel = selfvel / selfvel.mod() * SP_player_speed_max;
		selfpos += selfvel;
		selfvel *= SP_player_decay;
		cycles++;
	}while (cycles <= recovercycle);
	return -1;

}

bool Dribble::DribbleAvoidEnemy(DribbleAccount& draccount){
	Command command;