kick.cpp

RoboCup仿真组世界冠军源代码

		}
		else {
			Vector vel;
			vel = Polar2Vector((ball.distance + turnball_radius) /2, dir * turn_angle) - 
				 Polar2Vector(ball.distance , 0);
			kickspeed = vel.mod();
			kickangle = NormalizeAngle(ball.global_angle + vel.Angle());
	
			if (!CanKickOut(ball.pos - Self.pos, kickspeed, kickangle)){
				// the ball course is too close to the player
				kickangle = NormalizeAngle(180.0f + ball.global_angle - dir * (float)ASin(CP_turnball_radius_threshold/ ball.distance));
				kickspeed = (float)(sqrt(Sqr(ball.distance) - Sqr(CP_turnball_radius_threshold))
					+ sqrt(Sqr(turnball_radius) - Sqr(CP_turnball_radius_threshold)));
			}

			CorrectForMyVel(kickspeed, kickangle);

			float candospeed = smartkick(kickspeed, kickangle, command, kk_mode);
/*			if ((Polar2Vector(candospeed, kickangle) - Self.global_vel).mod2() > Sqr(SP_kickable_area) && kk_mode != KK_fast){
				motion.smartkick(0.0f, 0.0f, command);
				return KT_stopball;
			}
			else{*/
				return KT_success;
			//}
		}
	}
}

KT_Res Kick::turn_ball(AngleDeg turn_angle, float turnball_radius, KK_Mode kk_mode){
	Command command;
	KT_Res res = turn_ball(turn_angle, command, turnball_radius, kk_mode);
	mediator.enroll(command, Action_turn_ball, PriorityA);
	return res;
}

KT_Res Kick::turn_ball(AngleDeg turn_angle, Command& command, float turnball_radius, KK_Mode kk_mode){
	if (turn_angle < 0)
		return turn_ball(Turn_cw, turn_angle, command, turnball_radius);
	else
		return turn_ball(Turn_cw, turn_angle, command, turnball_radius);
}

void Kick::CorrectForMyVel(float& kickspeed, float& kickangle){
	Vector kickvel = Polar2Vector(kickspeed, kickangle) + Self.global_vel;
	kickspeed = kickvel.mod();
	kickangle = kickvel.Angle();
}

bool Kick::stopball(){
	Command command;
	bool success = stopball(command);
	mediator.enroll(command, Action_other, PriorityA);
	return success;
}

bool Kick::stopball(Command& command){
	if (!ball.kickable()) return false;
	Vector gap = ball.pos - Self.PredictPos(1);
	gap.Normalize();
	gap = gap * CP_desired_kickarea + Self.PredictPos(1) - ball.pos;
	smartkick(gap.mod(), gap.Angle(), command);
	if((command.GetPower() > 0.1f * SP_max_power) || ball.distance > 0.9f * CP_reliable_kickarea){

	}else
		command.stay(); 
	return true;
}

bool Kick::CanKickOut(Vector ballrelpos, float speed, float angle){
	ballrelpos += Polar2Vector(speed, angle);
	return bool(ballrelpos.mod2() >= Sqr(CP_turnball_radius_threshold));
}

KT_Res Kick::wisekick(float kickspeed, AngleDeg kickangle, KK_Mode kk_mode){
	Command command;
	KT_Res success = wisekick(kickspeed, kickangle, command, kk_mode);
	mediator.enroll(command, Action_dribble, PriorityA);
	return success;
}

KT_Res Kick::wisekick(float kickspeed, AngleDeg kickangle, Command& command, KK_Mode kk_mode){
	command.Reset();
	if (!ball.kickable()) return KT_lostball;
	float candospeed;
	if (kk_mode == KK_fast)
		candospeed = smartkick(SP_ball_speed_max, kickangle, command);
	else
		candospeed = smartkick(kickspeed, kickangle, command);
	if (candospeed > CP_kick_threshold_factor * kickspeed || candospeed >= 1.90f){
		if ((ball.pos + Polar2Vector(kickspeed, kickangle)).dist2(Self.PredictPos())
			< Sqr(SP_player_size + SP_ball_size)){
			//can not kick out, so turn the ball
			DoLog(11,"Adjust 30");
			if (NormalizeAngle(ball.global_angle - kickangle) < 0){
				turn_ball(30.0f, command);
				return KT_adjustball;
			}
			else{
				turn_ball(-30.0f, command);
				return KT_adjustball;
			}
		}
		else{
			// can kick out
			return KT_success;
		}
	}
	else{
		//see if an acceleration helps
		//first see the range of angle i can turn the ball to, it is a two circle intersection
		//problem
		Vector ballpos = ball.pos - Self.pos;
		Vector ballpred = ballpos + ball.global_vel;

		ballpos = ballpos.Rotate(-kickangle); // Rotate to relative coords
		ballpred = ballpred.Rotate(-kickangle); // Rotate to relative coords
		float ballangle = ballpos.Angle();
		float ball_on_left = bool(ballangle <= 0.0f) ? 1.0f : -1.0f;
		ballangle = (float)fabs(ballangle);
		float circle2_angle = ballpred.Angle();
		if (ball_on_left == 1.0f)
			circle2_angle = - circle2_angle;
		float dist_between_circles = ballpred.mod();
		
		float maxkickeffect = action.Max_KickEffect();

		float upangle = 0.0f, lowangle = 0.0f;

		if (dist_between_circles <= CP_turnball_radius + maxkickeffect
			&& dist_between_circles > CP_turnball_radius - maxkickeffect){
			lowangle = 0.0f;
			upangle = 360.0f;

			if (dist_between_circles > maxkickeffect - CP_turnball_radius){
				float tmpangle = ACos((Sqr(dist_between_circles) + Sqr(CP_turnball_radius) - Sqr(maxkickeffect))
					/ (2 * CP_turnball_radius * dist_between_circles));
				lowangle = NormalizeAngle(ballangle - circle2_angle - tmpangle, 0);
				upangle = NormalizeAngle(ballangle - circle2_angle + tmpangle, 0);
			}
		}
		float maxincangle = getmaxincangle(ballangle);

		if (!Angle_between(maxincangle, lowangle, upangle)){
			maxincangle = NormalizeAngle(getspeedinc(ballangle, upangle) > getspeedinc(ballangle, lowangle)
				? upangle : lowangle);
		}
		
		float predictkickeffect = (float) action.CalcKickRate(ball.global_angle + ball_on_left * maxincangle - Self.bodyfacing, CP_turnball_radius) * SP_max_power;


		float max_accelerated_speed = getspeedinc(ballangle, (float)fabs(maxincangle)) + predictkickeffect;
		if (max_accelerated_speed >= kickspeed || max_accelerated_speed > 1.95f){
				// a kick can get enough acceleration
			turn_ball(ball_on_left * maxincangle, command, CP_powerkick_radius, KK_fast);
			return KT_accelerate;
		}

		float speedincneeded = kickspeed - maxkickeffect;
		if(speedincneeded < 0.05f){
			turn_ball(0.0f, command, CP_powerkick_radius);
		}
		else if (speedincneeded <= 0.1f){
			if (ballangle < 40.0f)
				turn_ball(-ball_on_left * (40.0f - ballangle), command, CP_powerkick_radius);
			else
				turn_ball(0.0f, command);
		}
		else if (speedincneeded <= 0.3f){
			if (ballangle < 65.0f)
				turn_ball(-ball_on_left * (65.0f - ballangle), command, CP_powerkick_radius);
			else
				turn_ball(0.0f, command, CP_powerkick_radius);
		}
		else if (speedincneeded <= 0.6f){
			if (ballangle < 130.0f){
				if (ballangle < 90.0f)
					turn_ball(-ball_on_left * (90.0f - ballangle), command, CP_powerkick_radius);
				else
					turn_ball(0.0f, command, CP_powerkick_radius);
			}
			else{
				if (ballangle > 170.0f)
					turn_ball(-ball_on_left * (170.0f - ballangle), command, CP_powerkick_radius);
				else
					turn_ball(0.0f, command, CP_powerkick_radius);
			}
		}
		else{
			//if (ballangle < 130.0f){
				//if(ballangle < 135.0f)
					turn_ball(-ball_on_left * (135.0f - ballangle < 80 ? 135.0f - ballangle : 80.0f), command, CP_powerkick_radius);
				//else
				//	turn_ball(0.0f, command, CP_powerkick_radius);
			/*}
			else{
				if (ballangle > 150.0f)
					turn_ball(-ball_on_left * (150.0f - ballangle), command, CP_powerkick_radius);
				else
					turn_ball(0.0f, command, CP_powerkick_radius);
			}*/
		}
	}
	return KT_adjustball;
}


float Kick::getmaxincangle(AngleDeg startangle){
	startangle = (float)fabs(NormalizeAngle(startangle)) / 10;
	return (maxinc_angle[int(startangle)] * (1 + int(startangle) - startangle) +
		maxinc_angle[int(startangle) + 1] * (startangle - int(startangle)));
}

AngleDeg Kick::getspeedinc(AngleDeg startangle, AngleDeg turnangle){
	float d = 2 * CP_turnball_radius * Sin(turnangle / 2);
	float u = Sqr(CP_mnmx_kickeffect) - Sqr(d * Cos(startangle - turnangle / 2));
	if (u < 0) return 0.0f;
	return float(sqrt(u) + d * fabs(Sin(startangle - turnangle / 2))) - CP_mnmx_kickeffect;
}


void Kick::SetOainf(){
	oainf_time = situation.CurrentTime; 
	oainf_Miny = - SP_semi_pitch_width - Self.pos.y;
	oainf_Maxy = SP_semi_pitch_width - Self.pos.y;
	oainf_Maxx = SP_semi_pitch_length - Self.pos.x;
	oainf_Minx = - SP_semi_pitch_length - Self.pos.x;
	if(oainf_Miny > -SP_kickable_area || oainf_Maxy < SP_kickable_area
		|| oainf_Minx > -SP_kickable_area || oainf_Maxx < SP_kickable_area){
		line_limit = true;
		DoLog(LOG_OAKICK,"oa limited line(%.1f, %.1f, %.1f, %.1f)", oainf_Miny, oainf_Maxy, oainf_Minx, oainf_Maxx );
	}else
		line_limit = false;

	int num = Max(3, motion.Num_TheirVisiblePlayers());
	for(int i=0; i<num; i++){
		if(motion.TheirPlayer_Close2Me(i).distance > SP_feel_distance || !motion.TheirPlayer_Close2Me(i).pos_valid()) break;
		float maxradius = SP_kickable_area + SP_player_speed_max + 0.6f * (situation.CurrentTime - motion.TheirPlayer_Close2Me(i).original_postime);
		float dis = motion.TheirPlayer_Close2Me(i).distance - maxradius;
		if(dis > SP_kickable_area )	break;
		oainf_pos[i] = motion.TheirPlayer_Close2Me(i).rel_pos;
		if(dis > 0)
			oainf_radius[i] = maxradius;
		else
			oainf_radius[i] = Max(SP_kickable_area, motion.TheirPlayer_Close2Me(i).distance);
		DoLog(LOG_OAKICK,"oa limited pos(%.1f, %.1f), radius %.1f", oainf_pos[i].x, oainf_pos[i].y, oainf_radius[i]);
	}
	pos_limit= i;

	if(!line_limit && pos_limit ==0){
		DoLog(LOG_OAKICK,"kick no limited");
		if(!oainf_allenabled){
			Enableall();
			oainf_allenabled = true;
		}
		return;
	}
	oainf_allenabled = false;
}

void Kick::Oa_limitpos(float rel_angle){
	for(int j=0; j<KICKAGL; j++){
		for(int i=0; i<KICKDIS; i++){
			Vector pos = KickOA1_5.getkickpos(i,j)->pos.Rotate(rel_angle) + Self.global_vel;
			if(line_limit){
				if(pos.x > oainf_Maxx || pos.x < oainf_Minx || pos.y > oainf_Maxy || pos.y < oainf_Miny){
					Disablepos(i,j);
					//DoLog(LOG_OAKICK,"disable (%.2f %.2f)", pos.x,pos.y);
					continue;
				}
			}
			for(int k=0; k<pos_limit; k++){
				if(pos.dist(oainf_pos[k]) <= oainf_radius[k]){
					Disablepos(i,j);
					break;
					//DoLog(LOG_OAKICK,"disable (%.2f %.2f)", pos.x,pos.y);
				}
			}
			if(k == pos_limit) Enablepos(i,j);
		}
	}
}

void Kick::Enablepos(int disidx, int aglidx){
	KickOAlast.Enable(disidx,aglidx);
	KickOA2_5.Enable(disidx,aglidx);
	KickOA2_0.Enable(disidx,aglidx);
	KickOA1_5.Enable(disidx,aglidx);
}

void Kick::Disablepos(int disidx, int aglidx){
	KickOAlast.Disable(disidx,aglidx);
	KickOA2_5.Disable(disidx,aglidx);
	KickOA2_0.Disable(disidx,aglidx);
	KickOA1_5.Disable(disidx,aglidx);
}

void Kick::Enableall(){
	KickOAlast.Enableall();
	KickOA2_5.Enableall();
	KickOA2_0.Enableall();
	KickOA1_5.Enableall();
} 

KT_Res Kick::oakick(float kickspeed, AngleDeg kickangle, KK_Mode kk_mode){
	Command command;
	KT_Res success = oakick(kickspeed, kickangle, command, kk_mode);
	mediator.enroll(command, Action_dribble, PriorityA);
	return success;
}

KT_Res Kick::oakick(float kickspeed, AngleDeg kickangle, Command& command, KK_Mode kk_mode){	
	Vector bpos = ball.rel_pos.Rotate(-kickangle);
	Vector bvel = ball.global_vel.Rotate(-kickangle);
	Vector pvel = Self.global_vel.Rotate(-kickangle);
	
	float bdfacing = NormalizeAngle(Self.bodyfacing - kickangle);
	float difficult = kickspeed + (float) fabs(bdfacing) / 180.0f * 0.25f * 2.7f;
	MyOA* useoa;
	if(difficult > 2.5f)
		useoa = &KickOAlast;
	else if(kickspeed > 2.0f)
		useoa = &KickOA2_5;
	else if(kickspeed > 1.5f)
		useoa = &KickOA2_0;
	else
		useoa = &KickOA1_5;
	useoa->SetInitState(bpos, bvel, pvel, bdfacing);
	useoa->SetDesirevel(Polar2Vector(kickspeed,0));
	if(useoa->IsTargetState()){//only one kick
		if (kk_mode == KK_fast){
			smartkick(SP_ball_speed_max, kickangle, command, kk_mode);
		}
		else
			smartkick(kickspeed,kickangle,command,kk_mode);
		return KT_success;
	} 

	if(oainf_time != situation.CurrentTime) SetOainf(); 
	if(!oainf_allenabled) Oa_limitpos(kickangle);

	DoLog(LOG_OAKICK,"start oa find");
	bool find = useoa->Findoptimalway();
	if(!find){//impossible
		DoLog(LOG_OAKICK, "oa find fail");
		return wisekick(kickspeed,kickangle,command,kk_mode);
	}else{
		DoLog(LOG_OAKICK,"find success");
		Vector kick = useoa->Getfirstkick();
		float power = kick.mod() / action.Kickrate();
		float angle = NormalizeAngle(kick.Angle() + kickangle - Self.bodyfacing);
		pose_limitation(power, SP_min_power, SP_max_power);
		command.kick(power,angle);

		Vector kickeffect = ball.global_vel + Polar2Vector(power * action.Kickrate(), NormalizeAngle(angle+ Self.bodyfacing));
		Vector nextbpos = ball.rel_pos + kickeffect;
		Vector oabpos = useoa->Getnextpos();
		oabpos = oabpos.Rotate(kickangle);
		DoLog(LOG_OAKICK, "oa find after %d, oapos(%.2f, %.2f), kick to(%.2f %.2f)", useoa->Getsearchcounts(), oabpos.x, oabpos.y, nextbpos.x, nextbpos.y);
		if(useoa->Getneedkicks() == 1) return KT_accelerate;
		else return KT_adjustball;
	}
}