dribble.cpp

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

	bool success = DribbleAvoidEnemy(draccount,command);
	mediator.enroll(command, Action_dribble, PriorityA);
	return success;
}


bool Dribble::DribbleAvoidEnemy(DribbleAccount& draccount,Command& command){ 
	draccount.draction = DR_NoAction;
	int num =fieldinfo.NumOpponentsWithin(3.0f);
	if (num == 1 && !Self.IsBack())
	if (!ball.kickable())
		return false;
	AngleDeg avang = AvoidEnemyAngle();
	AngleDeg angle = GetBallAngle(avang + 90,false);
	if (angle == InvalidAngle)
	{
		angle = GetBallAngle(avang - 90,false);
		if (angle == InvalidAngle)
			return false;
		else
			angle = NormalizeAngle(avang - 90 + angle);
	}
	else
	{
		angle = NormalizeAngle(avang + 90 + angle);
	}
	if (KickAdjust(0,angle,0.0f,draccount,command)) 
	{
		if (fieldinfo.WithInField(draccount.NextBallPos))
			return true;
		else
		{
			draccount.draction = DR_NoAction;
			return false;
		}
		
	}
	else
		return false;
}

/* 
	selection of ball holding angle to perform adversarial dribble
	the idea is from CMU but slightly modified
*/
AngleDeg Dribble::AvoidEnemyAngle(){ 
	const int CP_ang_divs = 24;
	int i,j;
	float anglepriority[CP_ang_divs];
	float weight;
	float dist;
	AngleDeg enemyangle;
	AngleDeg bestangle = InvalidAngle;
	int num = motion.Num_TheirVisiblePlayers();
	if (num == 0){
		DoLog(LOG_DRIBBLE, "no visible opp");
		return InvalidAngle;
	}
	for (i = 0;i < CP_ang_divs;i++)
		anglepriority[i] = 0;
	for(i = 0; i < num; i ++){
		dist = motion.TheirPlayer_Close2Me(i).distance;
		if (dist > 5.0f)
		{
			if (i ==0){
				DoLog(LOG_DRIBBLE, "closest opp %d %.2f", motion.TheirPlayers_Close2Me[i],  dist );
				return InvalidAngle;
			}
			break;
		}
		weight = DRWeightByDistance(dist);
		if (motion.TheirPlayer_Close2Me(i).Is_goalie)
			weight *=2.0f;
		enemyangle = motion.TheirPlayer_Close2Me(i).global_angle;
		for (j = 0; j < CP_ang_divs; j++)
			anglepriority[j] += weight * HighPassFilter(0,180,AngleDif(enemyangle,j * 360.0f/CP_ang_divs));
	}
	weight = -1;
	for (i = 0; i < CP_ang_divs; i++){
		if (anglepriority[i] > weight)
		{
			bestangle = NormalizeAngle((float)i * 360.0f / CP_ang_divs);
			weight = anglepriority[i];
		}
	}
	return bestangle;
}

// a second order function
float Dribble::DRWeightByDistance(float distance){
	float weight = CP_drweight_conf_a * Sqr(distance) + CP_drweight_conf_b * distance + CP_drweight_conf_c;
	pose_limitation(weight, 0, 1);
	return weight;
}

bool Dribble::CanDribble(AngleDeg angle){ 
	return GetBallAngle(angle) != InvalidAngle;
} 

float Dribble::GetDribbleRadius(AngleDeg ball_angle) 
{
	float desired_sidegap =0.6f;
	return Min(CP_reliable_kickarea * 0.9f,float(fabs(desired_sidegap / Sin(ball_angle))));
}

AngleDeg Dribble::GetBallAngle(AngleDeg dribble_angle,bool dribbleforward){
	const int CP_DR_ang_divs = 10;
	float angcandribble[CP_DR_ang_divs];
	int bestsel = -CP_DR_ang_divs;
	float maxpr = -1.0f;
	float Max_angle = 200.0f;
	Vector ballpos[CP_DR_ang_divs];
	Vector myprpos = dribbleforward ? Self.PredictPos() + Polar2Vector(SP_max_power * action.Dashrate(),dribble_angle) : Self.PredictPos();
	AngleDeg orgballang = (ball.PredictPos(1) - myprpos).Angle();
	AngleDeg angdif;
	AngleDeg avoidang = AvoidEnemyAngle();
	float buf_factor = Self.GetDefsensitivity();
	float ballang;
	int i,j,k,kl,ku;
	for (i=0;i<CP_DR_ang_divs;i++)
	{
		ballang = (i - CP_DR_ang_divs/2 + 0.5f) * Max_angle/(CP_DR_ang_divs-1);
		ballpos[i] = myprpos + Polar2Vector(GetDribbleRadius(ballang),dribble_angle + ballang);
		angcandribble[i] = fieldinfo.WithInField(ballpos[i]) ? float(fabs(2.0f * i - (CP_DR_ang_divs -1))/(CP_DR_ang_divs - 1)) : 0;
		if (angcandribble[i] > 0.1f)
		{
						
			angdif= AngleDif(ballang + dribble_angle,orgballang); 
			if (angdif < 80)
				angcandribble[i] +=0.20f;
			if (avoidang != InvalidAngle)
			{
				angdif = AngleDif(ballang + dribble_angle,avoidang);
				if (angdif < 60)
					angcandribble[i] +=0.15f;			
			}
		}
	}
	int nums = motion.Num_TheirVisiblePlayers();
	for (i=0;i<CP_DR_ang_divs;i++)
	{
		if (angcandribble[i] < 0.1f) continue;
		for (j=0;j < nums; j++)
		{
			if (motion.TheirPlayer_Close2Me(j).distance > 5.0f) break;
			if (Covered(ballpos[i],motion.TheirPlayers_Close2Me[j],situation.CurrentTime + 1,buf_factor))
			{
				if (dribbleforward)
				{
					if (i >= CP_DR_ang_divs /2)
					{
						kl =i;
						ku = CP_DR_ang_divs -1;
					}
					else
					{
						kl = 0;
						ku = i;
					}
				}
				else
				{
					kl = i;
					ku = i;
				}
				for (k= kl;k<=ku;k++)
				{
					if (angcandribble[k] < 0.1f) continue;
					angcandribble[k] = 0.0f;
				}
				break;
			}
		}
		if (angcandribble[i] > 0.1f && angcandribble[i] > maxpr)
		{
			maxpr = angcandribble[i];
			bestsel = i - CP_DR_ang_divs/2;
		}
	}
	if (bestsel == -CP_DR_ang_divs) return InvalidAngle;
	else return (bestsel + 0.5f) * Max_angle/(CP_DR_ang_divs-1);
}

bool Dribble::Covered(Vector pos,UNum opp,Time time, float buf_factor){
	if (!TheirPlayer(opp).IsLastSeen()) return false; 
	if (TheirPlayer(opp).distance > 5.0f) return false;
	Vector OPos = TheirPlayer(opp).pos;
	float despersion = TheirPlayer(opp).max_speed;
	Line dashline;
	dashline.LineFromTwoPoints(pos,OPos);
	if (opp != fieldinfo.theirgoalie)
	if (dashline.dist(Self.pos) < 2 * SP_player_size && dashline.InBetween(Self.pos,pos,OPos)) 
	{
		despersion = 0;
	}
	if (TheirPlayer(opp).Is_goalie)
		despersion +=(SP_catch_area_l - SP_kickable_area);
	float avoid_buffer = 0.1f;
	return OPos.dist(pos) < despersion + SP_kickable_area + avoid_buffer * buf_factor + 0.1f;
}

bool Dribble::SmartDribble(){
	Command command;
	float angle;

	num_dribble=0;
	if(ball.kickable()) {
		num_dribble = CP_totaldirs;
		return true;
	}
	
	if(!ball.pos_valid())	return false;
	float SP_dribble_area=2.5f;
	if(ball.distance>SP_dribble_area) return false;

	if(motion.Num_MyVisiblePlayers()>0){
		if(motion.TheirPlayer_Close2Me(0).distance<Self.distance){
			return false;
		}
	} 
	InterceptionInfo IT_inf;

	AngleDeg directangle=(fieldinfo.theirgoal-Self.pos).Angle();
	for(int i = 0; i < 8; i ++){
		angle = NormalizeAngle(directangle+float(i) / 8 * 360);
		if(ball.kickable() || CanDribble(angle)){
			dribbleangle[num_dribble]=angle;
			num_dribble++;
		}
	}
	if (num_dribble>0) return true;
	else return false;
}

bool Dribble::holdball(Command& command){	
	DribbleAccount draccount;
	if(motion.TheirPlayer_Close2Ball(0).balldist < 3.0f){
		if (KickAdjust(0, AvoidEnemyAngle(),0, draccount,command))
		{
			DoLog(LOG_DRIBBLE,"Try avoid %.1f",motion.AvoidEnemyAngle());
			return (command.GetPower() > 0.1f * SP_max_power || ball.distance > 0.9f * CP_reliable_kickarea);
		}

	}
	
	if(kick.stopball(command)){
		DoLog(LOG_DRIBBLE, "stopball");
		return true;
	}
	return false;
}

void Dribble::DoDribble(AngleDeg dribble_angle, float priority){
	Command command;
	DribbleAccount draccount; 
	DoLog(LOG_DRIBBLE, "Try dr ang %.0f", dribble_angle);
	float ballang = GetBallAngle(dribble_angle);

	Vector predictball = ball.PredictPos(1);
	if (motion.k_StepDribble(dribble_angle, ballang, 1.0f, draccount,command))
	{
		if (fieldinfo.WithInField(draccount.NextBallPos) && fieldinfo.WithInField(draccount.RetainPos))
		{
			mediator.enroll(command, Action_dribble, priority);
			predictball = draccount.NextBallPos;
			DoLog(LOG_DRIBBLE,"ball angle %.1f to %.1f %.1f",ballang,predictball.x,predictball.y);
		}
		else 
		{
			draccount.draction = DR_NoAction;
			predictball = ball.PredictPos(1);
		}
	}else 
		DoLog(LOG_DRIBBLE,"No angle dribbleable");
	
	bool avoidneeded = false;
	for (int i = 0;i < motion.Num_TheirVisiblePlayers(); i++)
	{
		if (motion.TheirPlayer_Close2Me(i).distance > 5.0f) break;
		if (motion.Covered(predictball,motion.TheirPlayers_Close2Me[i],situation.CurrentTime + 1,Self.GetDefsensitivity()))
		{
			avoidneeded = true;
			break;
		}
	}
	if (avoidneeded){
		if(motion.DribbleAvoidEnemy(draccount,command)){
			mediator.enroll(command, Action_avoidenemy, priority + 0.001f);
		}
		else
		{
			DoLog(LOG_DRIBBLE, "Avoid failed");
			draccount.draction = DR_NoAction;
		}
	}

	if(draccount.draction == DR_NoAction){
		if(motion.holdball(command)){
			mediator.enroll(command, Action_holdball, priority);
			DoLog(LOG_DRIBBLE, "hold %.2f", priority);
		}
	}
}