kick.cpp

2002年

#include "kick.h"
#include "worldmodel.h"
#include "skill.h"
#include "log.h"
#include "agent.h"
#include "utils.h"
/**************	My OA	****************/
void MyOA::Initvpos(char* filename){
	int i,j,k,l;
	for(i=0; i<KICKDIS; i++){
		for(j=0; j<KICKAGL; j++){
			oapos[i][j].idx_dis = i;
			oapos[i][j].idx_agl = j;
			oapos[i][j].enabled = true;
			oapos[i][j].pos = Polar2Vector(ServerParam::kickable_margin*(i+0.5f)/KICKDIS + ServerParam::ball_size + ServerParam::player_size,360.0f*j/KICKAGL);
		}
	}

	FILE* fp;
	if(filename!=NULL) fp = fopen(filename,"r");
	if(fp == NULL){
		for(i=0; i<KICKDIS; i++){
			for(j=0; j<KICKAGL; j++){
				for(k=0; k<KICKDIS; k++){
					for(l=0; l<KICKAGL; l++){
						oapos[i][j].pts[k*KICKAGL+l] = &oapos[k][l];
					}
				}
			}
		}	
		return;
	}
	fscanf(fp,"%f",&Tvel.x);
	fscanf(fp,"%f",&Tvel.y);

	Vector vel;
	float maxvel = Sqr(ServerParam::ball_speed_max);
	float minv = -1.0f;
	float tmp;
	for(i=0; i<KICKDIS; i++){
		for(j=0; j<KICKAGL; j++){
			for(k=0; k<KICKDIS; k++){
				for(l=0; l<KICKAGL; l++){
					oapos[i][j].pts[k*KICKAGL+l] = &oapos[k][l];
					fscanf(fp,"%f",&tmp);
					oapos[i][j].qvalue[k*KICKAGL+l] = tmp;
				}
			}
			//排序
			for(k = 0; k<KICKDIS*KICKAGL-1; k++){
				int max = k;
				float maxv = oapos[i][j].qvalue[max];
				for(l = k+1; l<KICKDIS*KICKAGL; l++){
					if(oapos[i][j].qvalue[l] > maxv){
						max = l;
						maxv = oapos[i][j].qvalue[l];
					}
				}
				if(max != k){
					Swap(oapos[i][j].qvalue[k],oapos[i][j].qvalue[max]);
					Swap(oapos[i][j].pts[k],oapos[i][j].pts[max]);
				}
			}
			oapos[i][j].pts[KICKDIS*KICKAGL] = NULL;
		}
	}

	if(fp!=NULL) fclose(fp);
} 

void MyOA::SetInitState(Vector pos, Vector vel, Vector pvel, float bodyfacing){
	s_pos = pos;
	s_vel = vel;
	p_vel[0] = pvel;
	int i;
	for(i=1; i<maxdepth; i++) p_vel[i] = p_vel[i-1] * Self.player_decay;
	bdfacing = bodyfacing;
	//float fvalue = Agent::motion.GetStateV(pos,vel);
	float hestimate = 0;
	int idx1= getdisidx(pos.mod());
	int idx2 = getaglidx(pos.Angle());

	kstate s(getkickpos(idx1,idx2));
	s.depth = 0;
	SetInitial(s);
	DoLog(LOG_OAKICK, "setinitial");
}

void MyOA::Extend(Node& sel_s){
	if(sel_s.d.depth >= maxdepth) return;

	Vector pos, vel;
	GetNode_PosVel(sel_s, pos, vel);

	int counts = 0;
	kpos* p_next_pos;
	while((p_next_pos = sel_s.d.go_next()) != NULL)
	{
		counts ++;
		if(!p_next_pos->enabled) continue;

		Vector n_pos = p_next_pos->pos+p_vel[sel_s.d.depth];
		if(!Canreachpos(pos, vel, bdfacing, n_pos))        continue;

		kstate nstate(p_next_pos, sel_s.d.pos);
		nstate.depth = sel_s.d.depth + 1;

		MyOA::Node node(nstate, sel_s.c_idx);

		if(IsVisited(node)) continue;
		
		TryNode(node);
		break;
	}

	DoLog(LOG_OAKICK,"extend counts %d",counts);
} 

void MyOA::ExtendSel(){

	DoLog(LOG_OAKICK,"oa extend 1");
	if(!p_sel->IsRoot()){
		Extend(GetPnode(*p_sel)); 
	};

	DoLog(LOG_OAKICK,"oa extend 2");
	Extend(*p_sel);
}

/*Get current ball's pos and vel.  
To decrease error : using the original data,
				and compensate for player's move
*/
void MyOA::GetNode_PosVel(const Node& node, Vector& pos, Vector& vel) const{
	if(node.d.IsRoot()){
		pos = s_pos;
		vel = s_vel;
	}else{
		pos = node.d.pos->pos;

		Vector lpos;
		if(GetPnode(node).d.IsRoot()){
			lpos = s_pos;
		}else{
			lpos = node.d.p_pos->pos;
		}

		vel = (pos + p_vel[node.d.depth-1] - lpos) * ServerParam::ball_decay;
	}
}

bool MyOA::IsTarget(){
	Vector pos,vel;
	GetNode_PosVel(*p_sel, pos, vel);
	return Canreachvel(pos,vel,bdfacing,dvel);
}

void MyOA::Enableall(){
	int i;
	for(i=0; i<KICKDIS; i++){
		int j;
		for(j=0; j<KICKAGL; j++){
			oapos[i][j].enabled = true;
		}
	}
} 

Vector MyOA::Getnextpos(){
	if(!IsValidSel()) return Vector(0, 0);
	if(p_sel->IsRoot()) return Vector(0, 0);
	
	Node *tmp1 = p_sel;
	Node *tmp2 = & GetPnode(*p_sel);
	while(!tmp2->IsRoot()){
		tmp1 = tmp2;
		tmp2 = & GetPnode(*tmp1);
	}
	return tmp1->d.pos->pos;
}

Vector MyOA::Getfirstkick(){
	Vector npos = Getnextpos();
	return Getnextpos() + p_vel[0] - s_pos - s_vel;
}

bool MyOA::Canreachvel(Vector& bpos, Vector& bvel, float bdfacing, Vector& next_bvel, float* effort){
	float dist_ball = bpos.mod() - Self.player_size - ServerParam::ball_size;
	if(dist_ball > Self.kickable_margin) return false;
	float dir_diff = (float)fabs(Deg2Rad(NormalizeAngle(bpos.Angle() - bdfacing)));
	float eff_power = ServerParam::max_power * ServerParam::kick_power_rate * 
		(1 - 0.25f*dir_diff/PI - 0.25f*dist_ball/Self.kickable_margin);//to be modified

	float gap = (next_bvel- bvel).mod() ;
	if(effort!=NULL) *effort = gap / eff_power ;
	if(gap > eff_power)	return false;
	else return true;
}

bool MyOA::Canreachpos(Vector& bpos, Vector& bvel, float bdfacing, Vector& next_bpos, float* effort){
	Vector tvel = next_bpos - bpos;
	if(tvel.mod2() > Sqr(ServerParam::ball_speed_max)) return false;
	return Canreachvel(bpos,bvel,bdfacing,tvel,effort);
}

int MyOA::getdisidx(float balldis){
	return Constrain(int((balldis-ServerParam::ball_size-ServerParam::player_size)/ServerParam::kickable_margin*KICKDIS), 0, KICKDIS-1);
}

int MyOA::getaglidx(float ballagl){
	return round(NormalizeAngle(ballagl,0)/360.0f*KICKAGL) % KICKAGL;
}

int MyOA::Getneedkicks(){
	if(!IsValidSel()) return 0;

	return p_sel->d.depth;
}

/***********	Kick	********************/
Kick::Kick(){
	speedinc[0] = 0.0f;
	maxinc_angle[0] = 0.0f;
	speedinc[1] = 0.0078f;
	maxinc_angle[1] = 5.7296f;
	speedinc[2] = 0.0316f;
	maxinc_angle[2] = 11.4592f;
	speedinc[3] = 0.0718f;
	maxinc_angle[3] = 17.7617f;
	speedinc[4] = 0.1293f;
	maxinc_angle[4] = 24.0642f;
	speedinc[5] = 0.2051f;
	maxinc_angle[5] = 30.9397f;
	speedinc[6] = 0.3000f;
	maxinc_angle[6] = 38.3882f;
	speedinc[7] = 0.4143f;
	maxinc_angle[7] = 46.4096f;
	speedinc[8] =  0.5475f;
	maxinc_angle[8] = 55.0039f;
	speedinc[9] = 0.6974f;
	maxinc_angle[9] = 64.7442f;
	speedinc[10] = 0.8601f;
	maxinc_angle[10] = 75.0575f;
	speedinc[11] = 1.0274f;
	maxinc_angle[11] = 83.0789f;
	speedinc[12] = 1.1455f;
	maxinc_angle[12] = 83.0789f;
	speedinc[13] = 1.1928f;
	maxinc_angle[13] = 83.0789f;
	speedinc[14] = 1.1677f;
	maxinc_angle[14] = 83.0789f;
	speedinc[15] = 1.0711f;
	maxinc_angle[15] = 83.0789f;
	speedinc[16] = 0.9060f;
	maxinc_angle[16] = 83.0789f;
	speedinc[17] = 0.6774f;
	maxinc_angle[17] = 83.0789f;
	speedinc[18] = 0.3951f;
	maxinc_angle[18] = 83.0789f;
		
	{
		/*debug section
		Self.bodyfacing = 0;
		ball.rel_pos = Vector ( -0.4f, 0.3f);
		ball.global_angle = ball.rel_pos.Angle();
		ball.distance = ball.rel_pos.mod();
		ball.global_vel = Vector ( -0.3f, 0.7f);
		Self.global_vel = Vector ( 0.1f, 0.5f);
		Self.effort = 1.0f;
		Skill::kick.Enableall();
		Skill::kick.oakick(2.7f, 0 );   
		*/
	}
}
/* Function smartkick  kick to desire angle with desire speed*/
float Kick::smartkick(float kickspeed, AngleDeg kickangle, Command& command, KK_Mode mode){
	command.Reset();
	if (!ball.kickable()) return 0.0f;

	Vector dvel;
	if (ball.speed_valid())
		dvel = Polar2Vector(kickspeed, kickangle) - ball.global_vel;
	else
		dvel = Polar2Vector(kickspeed, kickangle);

	float angle = NormalizeAngle(dvel.Angle() - Self.bodyfacing);
	float power = dvel.mod() / Skill::action.Kickrate();

	if (power > ServerParam::max_power || power < ServerParam::min_power){
		/* try to kick harder than allowed*/
		if (!ball.speed_valid() || ball.speed < FLOAT_EPS){
			pose_limitation(power, ServerParam::min_power, ServerParam::max_power);
		}
		else{
			/* try to kick to the right direction, the kickspeed may not be desire, but
			the angle is correct. And we do our best with the kickspeed.
			It is a ray intersection problem. The idea is from CMU.*/
			Vector solution1, solution2, gap;
			Ray vel(ball.pos, kickangle);
			int num_of_solutions = vel.CircleIntersect(ServerParam::max_power * Skill::action.Kickrate(), 
				ball.PredictPos(1), solution1, solution2);
			if (num_of_solutions == 0){
				/* can not kick to the right trajectory. just try to stop it*/
				power = ball.speed / Skill::action.Kickrate();
				pose_limitation(power, ServerParam::min_power, ServerParam::max_power);
				angle = NormalizeAngle(ball.vel_angle - Self.bodyfacing + 180);
				command.kick(power, angle);
				my_error("No solution. So stop it!");
			}
			else{
				if (mode == KK_fast)
					gap = solution2 - ball.PredictPos(1);
				else
					gap = solution1 - ball.PredictPos(1);
				power = gap.mod() / Skill::action.Kickrate();
				angle = NormalizeAngle(gap.Angle() - Self.bodyfacing);
				pose_limitation(power, ServerParam::min_power, ServerParam::max_power);
			}
		}
	}

	Vector kickvel;
	if (ball.speed_valid()){
		kickvel = ball.global_vel + Polar2Vector(power * Skill::action.Kickrate(), angle + Self.bodyfacing);
	}
	else{
		kickvel = Polar2Vector(power * Skill::action.Kickrate(), angle + Self.bodyfacing);
	}
	command.kick(power, angle);
	Vector kickeffect = Polar2Vector(power, angle + Self.bodyfacing) * Skill::action.Kickrate();
	return kickvel.mod();
}

float Kick::smartkick(float speed, AngleDeg ang, KK_Mode mode){
	Command command;
	float kickspeed;
	kickspeed = smartkick(speed, ang, command, mode);
	Agent::mediator.enroll(command, Action_other, PriorityA);
	return kickspeed;
}

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

KT_Res Kick::turn_ball(TurnDir dir, AngleDeg turn_angle, Command& command, float turnball_radius, KK_Mode kk_mode){
	if(!ball.kickable()){
		return KT_lostball;
	}
	else {
		float kickspeed, kickangle;
		if (ball.distance < ClientParam::turnball_radius_threshold) {
			// the ball is too close
			kickspeed = ClientParam::kickballaway_speed;
			kickangle = ball.global_angle;

			CorrectForMyVel(kickspeed, kickangle);
			smartkick(kickspeed, kickangle, command, kk_mode);
			return KT_kickaway;
		}
		else {