kick.cpp

RoboCup仿真组世界冠军源代码

/*
    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 "kick.h"
#include "global.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(SP_kickable_margin*(i+0.5f)/KICKDIS + SP_ball_size + SP_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].nextp[k*KICKAGL+l] = &oapos[k][l];
					}
				}
			}
		}	
		return;
	}
	fscanf(fp,"%f",&Tvel.x);
	fscanf(fp,"%f",&Tvel.y);

	Vector vel;
	float maxvel = Sqr(SP_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].nextp[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].nextp[k],oapos[i][j].nextp[max]);
				}
			}
			oapos[i][j].nextp[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;
	for(int i=1; i<maxdepth; i++) p_vel[i] = p_vel[i-1] * SP_player_decay;
	bdfacing = bodyfacing;
	//float fvalue = 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,hestimate);
	DoLog(LOG_OAKICK, "setinitial");
}

void MyOA::Extend(OAnode<kstate>* pnode){
	if(pnode == NULL) return;
	if(pnode->data.depth >= maxdepth) return;

	kstate* pstate = &pnode->data;
	OApos* npos;	
	float hvalue,cost;
	Vector pos,vel,pmove;
	if(pnode->Getwaypre() == NULL){
		pos = s_pos;
		vel = s_vel;
	}else{
		pos = pnode->data.pos->pos;
		Vector lpos;
		if(pnode->Getwaypre()->Getwaypre() == NULL) lpos = s_pos;
		else lpos = pnode->Getwaypre()->data.pos->pos;
		vel = (pos + p_vel[pnode->data.depth-1] - lpos) * SP_ball_decay;
	}
	int counts = 0;
	while((npos = pstate->Subnode()) != NULL && (counts++)< 100){
		pstate->subnode ++;
		if(!npos->enabled) {
			DoLog(LOG_OAKICK,"skip (%.2f %.2f) %d", npos->pos.x, npos->pos.y, counts);
			continue;
		}
		if(!Canreachpos(pos, vel, bdfacing, npos->pos+p_vel[pnode->data.depth]))	continue;
		kstate nstate(npos);
		if(closelist.FindNode(nstate) != NULL) continue;
		//enqueue cstate's valid nextsubnode
		OAnode<kstate>* ptmp = openlist.FindNode(nstate);
		hvalue = pstate->pos->qvalue[pstate->subnode-1];
		cost = pnode->Getcost() + stepcost;
		if(ptmp == NULL){
			kstate s(npos);
			OAnode<kstate>* node = new OAnode<kstate>(nstate);
			node->Sethvalue(hvalue);
			node->Setcost(cost);
			node->Setwaypre(pnode);
			node->data.depth = pnode->data.depth + 1;
			openlist.enqueue(node);
			break;
		}else if(ptmp->Gethvalue() < hvalue){
			ptmp->Sethvalue(hvalue);
			OAnode<kstate>* ptmp2 = ptmp->Getwaypre(); 
			ptmp->Setwaypre(pnode);
			ptmp->data.depth = ptmp2->data.depth + 1;
			//extend another pstate's sub node
			pnode = ptmp2;
			pstate = &pnode->data ;
		}
	}
	DoLog(LOG_OAKICK,"extend counts %d",counts);
} 

void MyOA::Extendcurnode(){
	if(curnode == NULL) return;
	DoLog(LOG_OAKICK,"oa extend 1");
	Extend(curnode->Getwaypre());
	DoLog(LOG_OAKICK,"oa extend 2");
	Extend(curnode);
}

bool MyOA::IsTargetState(){
	Vector pos,vel;
	if(curnode->Getwaypre() == NULL){
		pos = s_pos;
		vel = s_vel;
	}else{
		Vector lpos;
		pos = curnode->data.pos->pos;
		if(curnode->Getwaypre()->Getwaypre() == NULL){
			lpos = s_pos;
		}else
			lpos = curnode->Getwaypre()->data.pos->pos;
		vel = (pos + p_vel[curnode->data.depth-1] - lpos) * SP_ball_decay;
	}
	return Canreachvel(pos,vel,bdfacing,dvel);
}

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

Vector MyOA::Getnextpos(){
	OAnode<kstate>* tmp1 = curnode;
	OAnode<kstate>* tmp2 = tmp1->Getwaypre(); 
	while(tmp2 != NULL){
		if(tmp2->Getwaypre() == NULL){
			return tmp1->data.pos->pos;
		}
		tmp1 = tmp2;
		tmp2 = tmp1->Getwaypre();
	}
	return Vector(0,0);
}

Vector MyOA::Getfirstkick(){
	OAnode<kstate>* tmp1 = curnode;
	OAnode<kstate>* tmp2 = tmp1->Getwaypre(); 
	while(tmp2 != NULL){
		if(tmp2->Getwaypre() == NULL){
			return tmp1->data.pos->pos + p_vel[0] - s_pos - s_vel;
		}
		tmp1 = tmp2;
		tmp2 = tmp1->Getwaypre();
	}
	return Vector(-s_vel.x, -s_vel.y);
}

bool MyOA::Canreachvel(Vector& bpos, Vector& bvel, float bdfacing, Vector& next_bvel, float* effort){
	float dist_ball = bpos.mod() - SP_player_size - SP_ball_size;
	if(dist_ball > SP_kickable_margin) return false;
	float dir_diff = (float)fabs(Deg2Rad(NormalizeAngle(bpos.Angle() - bdfacing)));
	float eff_power = SP_max_power * SP_kick_power_rate * 
		(1 - 0.25f*dir_diff/PI - 0.25f*dist_ball/SP_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(SP_ball_speed_max)) return false;
	return Canreachvel(bpos,bvel,bdfacing,tvel,effort);
}

/***********	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;
}
/* 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() / action.Kickrate();

	if (power > SP_max_power || power < SP_min_power){
		/* try to kick harder than allowed*/
		if (!ball.speed_valid() || ball.speed < FLOAT_EPS){
			pose_limitation(power, SP_min_power, SP_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(SP_max_power * 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 / action.Kickrate();
				pose_limitation(power, SP_min_power, SP_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() / action.Kickrate();
				angle = NormalizeAngle(gap.Angle() - Self.bodyfacing);
				pose_limitation(power, SP_min_power, SP_max_power);
			}
		}
	}

	Vector kickvel;
	if (ball.speed_valid()){
		kickvel = ball.global_vel + Polar2Vector(power * action.Kickrate(), angle + Self.bodyfacing);
	}
	else{
		kickvel = Polar2Vector(power * action.Kickrate(), angle + Self.bodyfacing);
	}
	command.kick(power, angle);
	Vector kickeffect = Polar2Vector(power, angle + Self.bodyfacing) * 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);
	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);
	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 < CP_turnball_radius_threshold) {
			// the ball is too close
			kickspeed = CP_kickballaway_speed;
			kickangle = ball.global_angle;

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