goalie.cpp

robocup3d源化码 Nexus3D.tar.gz

/*
Copyright (C) 2005 Ferdowsi University
*/


#include "goalie.h"

Goalie::Goalie()
{

}
 
void Goalie::SmartGoalie()
{
	DoLog(LOG_GOALIE,"ball.vel=(%f,%f,%f) ball.length=%f",ball.vel[0],ball.vel[1],ball.vel[2]);
	if((ball.pos[0]>-20||ball.pos[1]>25||ball.pos[1]<-25)&&MyNumber==1){
		DoLog(LOG_GOALIE,"dont move");
		action_info.x = -FieldLength/2+0.5;
		action_info.y = 0;
		action_info.do_drive = true ;
		return;
	}
	if(ClearBall()){
		DoLog(LOG_GOALIE,"clear ball");
	}
	else if((ball.pos-self.pos).Length()<=2&&ball.vel[0]>-0.5)
		Intercept();
	else if(DefendShoot()){
		return;
	}
	else if(Interceptable()){
		Intercept();
	}
	else{
		DefenseMove();
	}
}

bool Goalie::DefenseMove()
{
	if(MyNumber==1){
		if(ball.pos[0]>-20||ball.pos[1]>25||ball.pos[1]<-25){
			DoLog(LOG_GOALIE,"dont move");
			action_info.x = -FieldLength/2+0.5;
			action_info.y = 0;
			action_info.do_drive = true ;
			return true;
		}
		if(ball.pos[0]>-15||ball.pos[1]>30||ball.pos[1]<-30){
			DoLog(LOG_GOALIE,"dont move1");
			action_info.x = -FieldLength/2+0.5;
			action_info.y = 0;
			action_info.do_drive = true ;
			return true;
		}
		DoLog(LOG_GOALIE,"defense move");
		Line line1(-(ball.pos[1]-3.66)/(ball.pos[0]+FieldLength/2),1,3.66);
		Line line2(-(ball.pos[1]+3.66)/(ball.pos[0]+FieldLength/2),1,-3.66);
		float m=GetSlopeOfMidAngle(line1.Angle(),line2.Angle());
		m*=-1;
		//DoLog(LOG_POS,"the slope is:%f",m);
		float y=0;
		DoLog(LOG_GOALIE,"ball.pos(%f,%f)",ball.pos[0],ball.pos[1]);
		if(ball.pos[0]<-FieldLength/2+0.5){
			DoLog(LOG_GOALIE,"corner pos");
			if(ball.pos[1]<-3.66) y=-3.6;
			if(ball.pos[1]>3.66) y=3.6;
		}
		else
			y=m*(-FieldLength/2+0.5-ball.pos[0])+ball.pos[1];
		if(y<-3.6) y=-3.6;
		else if(y>3.6) y=3.6;
		//GoTo(-52,y,0);
		DoLog(LOG_GOALIE,"new    x=%f",-FieldLength/2+0.5);
		action_info.x = -FieldLength/2+0.5;
		action_info.y = y;
		action_info.do_drive = true ;
	}
	else{
		float g_pos=0;
		if(ball.pos[0]<-FieldLength/2+0.5&&(ball.pos[1]>FieldWidth/2-3||ball.pos[1]<-FieldWidth/2+3)){
			if(MyNumber<4){
				DoLog(LOG_GOALIE,"dont move");
				action_info.x = -FieldLength/2+1;
				action_info.y = -3;
				action_info.do_drive = true ;
				return true;
			}
			else if(MyNumber<6){
				DoLog(LOG_GOALIE,"dont move");
				action_info.x = -FieldLength/2+1;
				action_info.y = 3;
				action_info.do_drive = true ;
				return true;
			}
		}	
		DoLog(LOG_GOALIE,"defense move");
		Line line1(-(ball.pos[1]-3.66)/(ball.pos[0]+FieldLength/2),1,3.66);
		Line line2(-(ball.pos[1]+3.66)/(ball.pos[0]+FieldLength/2),1,-3.66);
		float m=GetSlopeOfMidAngle(line1.Angle(),line2.Angle());
		m*=-1;
		//DoLog(LOG_POS,"the slope is:%f",m);
		float y=0;
		DoLog(LOG_GOALIE,"ball.pos(%f,%f)",ball.pos[0],ball.pos[1]);
		if(ball.pos[0]>-FieldLength/2+30) g_pos=-FieldLength/2+13+MyNumber;
		else if(ball.pos[0]>-FieldLength/2+20) g_pos=-FieldLength/2+7+MyNumber;
		else if(ball.pos[0]>-FieldLength/2+15) g_pos=-FieldLength/2+5+MyNumber;
		else g_pos=ball.pos[0]-(FieldLength/2+ball.pos[0])/3;
		if(ball.pos[0]<-FieldLength/2+0.5){
			DoLog(LOG_GOALIE,"corner pos");
			if(ball.pos[1]<-3.66) y=-3.6;
			if(ball.pos[1]>3.66) y=3.6;
		}
		else
			y=m*(g_pos-ball.pos[0])+ball.pos[1];
		if(y<-3.6) y=-3.6;
		else if(y>3.6) y=3.6;
		//GoTo(-52,y,0);
		DoLog(LOG_GOALIE,"new    x=%f",-FieldLength/2+0.5);
		action_info.x = g_pos;
		action_info.y = y;
		action_info.do_drive = true ;

	}
	
}

bool Goalie::DefendShoot()
{
	if(ball.pos[0]>-30||ball.pos[1]<-15||ball.pos[1]>15||(ball.distance<0.75&&ball.vel[0]>-0.2)) return false;
	float minoppdist2ball=200;
	int oppnum=-1;
	for(int i=1;i<=11;i++){
		if(minoppdist2ball>TheirPlayer[i].dist2ball){
			 minoppdist2ball=TheirPlayer[i].dist2ball;
			 oppnum=i;
		}
	}
	float m=0,y=0;
	float   g_pos=0;
	if(MyNumber==1){	
		if(minoppdist2ball<1.5&&ball.vel.Length()<1){
			if(TheirPlayer[oppnum].pos[0]<ball.pos[0]) return false;
			m=(ball.pos[1]-TheirPlayer[oppnum].pos[1])/(ball.pos[0]-TheirPlayer[oppnum].pos[0]);
			y=m*(-FieldLength/2+0.5-ball.pos[0])+ball.pos[1];
			if(y<-3.66||y>3.66) return false;
			if(y<-3) y=-3;
			else if(y>3) y=3;
			action_info.x = -FieldLength/2+0.5;
			action_info.y = y;
			action_info.do_drive = true ;
			DoLog(LOG_GOALIE,"defened shoot1");
			return true;
		}
		else if(ball.vel.Length()>3||(ball.vel.Length()>1.5&&ball.pos[0]<-FieldLength/2+7)){
			m=(ball.pos[1]-ball.last_info[(ball.index_pos+9)%5].pos[1])/(ball.pos[0]-ball.last_info[(ball.index_pos+9)%5].pos[0]);
			float x=self.pos[0];//-FieldLength/2+0.5
			y=m*(x-ball.pos[0])+ball.pos[1];
			if(y<-5||y>5) return false;
			if(y<-3.66) y=-3.66;
			else if(y>3.66) y=3.66;
			action_info.x = x;
			action_info.y = y;
			action_info.do_drive = true ;
			DoLog(LOG_GOALIE,"ball.last_info(%f,%f)   newpos(%f,%f)   ball.index_pos-1)%5= %d",ball.last_info[(ball.index_pos+9)%5].pos[0],ball.last_info[(ball.index_pos+9)%5].pos[1],action_info.x,action_info.y,(ball.index_pos+9)%5);
			DoLog(LOG_GOALIE,"defened shoot2");
			return true;
		}
	}
	else{
		if(ball.pos[0]>-FieldLength/2+30) g_pos=-FieldLength/2+13+MyNumber;
		else if(ball.pos[0]>-FieldLength/2+20) g_pos=-FieldLength/2+7+MyNumber;
		else if(ball.pos[0]>-FieldLength/2+15) g_pos=-FieldLength/2+5+MyNumber;
		else g_pos=ball.pos[0]-(FieldLength/2+ball.pos[0])/3;
		if(minoppdist2ball<1.5&&ball.vel.Length()<1){
			if(TheirPlayer[oppnum].pos[0]<ball.pos[0]) return false;
			m=(ball.pos[1]-TheirPlayer[oppnum].pos[1])/(ball.pos[0]-TheirPlayer[oppnum].pos[0]);
			y=m*(g_pos-ball.pos[0])+ball.pos[1];
			if(y<-6||y>6) return false;
			if(y<-4.5) y=-4.5;
			else if(y>4.5) y=4.5;
			action_info.x = g_pos;
			action_info.y = y;
			action_info.do_drive = true ;
			DoLog(LOG_GOALIE,"defened shoot1");
			return true;
		}
		else if(ball.vel.Length()>3||(ball.vel.Length()>1.5&&ball.pos[0]<-FieldLength/2+7)){
			m=(ball.pos[1]-ball.last_info[(ball.index_pos+9)%5].pos[1])/(ball.pos[0]-ball.last_info[(ball.index_pos+9)%5].pos[0]);
			float x=self.pos[0];//-FieldLength/2+0.5
			y=m*(x-ball.pos[0])+ball.pos[1];
			if(y<-5||y>5) return false;
			if(y<-3.66) y=-3.66;
			else if(y>3.66) y=3.66;
			action_info.x = x;
			action_info.y = y;
			action_info.do_drive = true ;
			DoLog(LOG_GOALIE,"ball.last_info(%f,%f)   newpos(%f,%f)   ball.index_pos-1)%5= %d",ball.last_info[(ball.index_pos+9)%5].pos[0],ball.last_info[(ball.index_pos+9)%5].pos[1],action_info.x,action_info.y,(ball.index_pos+9)%5);
			DoLog(LOG_GOALIE,"defened shoot2");
			return true;
		}
	}
	
	return false;
}
bool Goalie::Interceptable()
{
	float minoppdist2ball=200;
	if((fabs(ball.vel[0])>0.75||fabs(ball.vel[1])>0.75)&&ball.distance>0.75){
		DoLog(LOG_GOALIE,"using prediction in intercaptable");
		if(ball.vel[0]>1||ball.distance>20||(ball.vel[0]<-0.5&&ball.pos[0]<-FieldLength/2+0.5)) return false;
		for(int i=1;i<=11;i++){
			if(minoppdist2ball > TheirPlayer[i].dist2ball){
				minoppdist2ball = TheirPlayer[i].dist2ball;
			}
			if((self.pos-predicted_ball_pos[stop_index]).Length()-0.5 > TheirPlayer[i].dist2ball){
				return false;
			}
		}
	}
	else{
		if(ball.vel[0]>1||ball.distance>20||(ball.vel[0]<-0.5&&ball.pos[0]<-FieldLength/2+0.5)) return false;
		for(int i=1;i<=11;i++){
			if(minoppdist2ball > TheirPlayer[i].dist2ball){
				minoppdist2ball = TheirPlayer[i].dist2ball;
			}
			if(ball.distance-0.5 > TheirPlayer[i].dist2ball && ball.vel[0]>-0.2/*&&ball.distance>0.5*/){
				return false;
			}
			if(ball.distance-1 > TheirPlayer[i].dist2ball && ball.vel[0]>-0.5/*&&ball.distance>0.5*/){
				return false;
			}
			if(ball.distance-1.5 > TheirPlayer[i].dist2ball && ball.vel[0]>-1.0/*&&ball.distance>0.5*/){
				return false;
			}

		}
	}
	return true;
}
bool Goalie::Intercept()
{

	if((fabs(ball.vel[0])>0.75||fabs(ball.vel[1])>0.75)&&ball.distance>0.75){
		DoLog(LOG_GOALIE,"try intercept with prediction");
		float minKickDist = KickDistance;
		if ((self.pos-predicted_ball_pos[stop_index]).Length() > minKickDist){
			DoLog(LOG_GOALIE,"interception done");
			//GoTo(ball.pos.x,ball.pos.y,0);
			action_info.x = predicted_ball_pos[stop_index][0] ;
			action_info.y = predicted_ball_pos[stop_index][1] ;
			if(action_info.x<-FieldLength/2) action_info.x=-FieldLength/2+0.5;
			action_info.do_drive = true ;
			return true;
		}
	}
	else{
		DoLog(LOG_GOALIE,"try intercept");
		float minKickDist = KickDistance;
		if (ball.distance > minKickDist){
			DoLog(LOG_GOALIE,"interception done");
			//GoTo(ball.pos.x,ball.pos.y,0);
			action_info.x = ball.pos[0] ;
			action_info.y = ball.pos[1] ;
			action_info.do_drive = true ;
			return true;
		}
	}
	return false;


}

bool Goalie::ClearBall(){
	float minKickDist = KickDistance;
	DoLog(LOG_GOALIE,"ball.distance=%f         minKickDist=%f",ball.distance,minKickDist);
	if(ball.distance>1){
		 DoLog(LOG_GOALIE,"too far");
		 return false;
	}
	if(ball.vel.Length()>1){
	 DoLog(LOG_GOALIE,"very fast");
		 return false;
	}
	float m=(ball.pos[1]-self.pos[1])/(ball.pos[0]-self.pos[0]);
	float y=m*(-FieldLength/2-ball.pos[0])+ball.pos[1];

	float oppdist2ball=0,minoppdist2ball=200;
	int oppnum=-1;
	for(int i=1;i<=11;i++){
		if(minoppdist2ball > TheirPlayer[i].dist2ball){
			 minoppdist2ball = TheirPlayer[i].dist2ball;
			 oppnum=i;
		}
	}
	if(minoppdist2ball<minKickDist){
		if (ball.distance < minKickDist){
			//Kick(20,100);
			if(y>-3.66&&y<3.66){
				action_info.kick_angle=35;
				action_info.kick_direction=0;
				action_info.kick_power=100;
				action_info.do_kick=true;
				DoLog(LOG_GOALIE,"clear1");
				return true;
			}
			else{
				action_info.kick_angle=40;//25;
				action_info.kick_direction=0;
				action_info.kick_power=100;
				action_info.do_kick=true;
				DoLog(LOG_GOALIE,"clear2");
				return true;
			}
		}
		else{
			action_info.x = ball.pos[0]-minKickDist ;
			action_info.y = ball.pos[1] ;
			action_info.do_drive = true ;
			DoLog(LOG_GOALIE,"clear3");
			return true;
		}
	}
	else{
		if(minoppdist2ball-self.dist2ball>1){
			if(!AdjustKickPos.AdjustNeeded(0,25)){
				DoLog(LOG_GOALIE,"Kick");
				action_info.kick_angle=25;
				action_info.kick_direction=0;
				action_info.kick_power=100;
				action_info.do_kick=true;
				DoLog(LOG_GOALIE,"clear4");
				return true;
			}
			else {
				Vector3f Pos=AdjustKickPos.MoveAroundBall(0);
				DoLog(LOG_GOALIE,"adjust nedded    pos(%f,%f)",Pos[0],Pos[1]);
				action_info.x = Pos[0] ;
				action_info.y = Pos[1] ;
				action_info.do_drive = true ;
				DoLog(LOG_GOALIE,"clear5");
				return true;

			}
		}
		else{
			if (ball.distance < minKickDist){
				//Kick(20,100);
				if(y>-3.66&&y<3.66){
					action_info.kick_angle=35;
					action_info.kick_direction=0;
					action_info.kick_power=100;
					action_info.do_kick=true;
					DoLog(LOG_GOALIE,"clear6");
					return true;
				}
				else{
					action_info.kick_angle=25;
					action_info.kick_direction=0;
					action_info.kick_power=100;
					action_info.do_kick=true;
					DoLog(LOG_GOALIE,"clear7");
					return true;
				}
			}
			else{
				action_info.x = ball.pos[0]-minKickDist ;
				action_info.y = ball.pos[1] ;
				action_info.do_drive = true ;
				DoLog(LOG_GOALIE,"clear8");
				return true;
			}
		}

	}
/*	if (ball.distance < minKickDist){
		//Kick(20,100);
		action_info.kick_angle=20;
		action_info.kick_direction=0;
		action_info.kick_power=100;
		action_info.do_kick=true;
		return true;
	}*/
	return false;
}