positioning.cpp

robocup3d源化码 Nexus3D.tar.gz

/*
Copyright (C) 2005 Ferdowsi University
*/


#include "positioning.h"

Positioning::Positioning() //:  Soccer()
{
  
}

bool Positioning::Position()
{
	Vector3f newpos(0,0,0);
	float minteammatedist2ball=100;
	if((fabs(ball.vel[0])>0.75||fabs(ball.vel[1])>0.75)&&ball.distance>0.75){
		DoLog(LOG_GOALIE,"using prediction in position");
		for(int i=1;i<=11;i++){
			if((MyPlayer[i].pos-predicted_ball_pos[stop_index]).Length()<minteammatedist2ball&&i!=MyNumber){
				minteammatedist2ball=(MyPlayer[i].pos-predicted_ball_pos[stop_index]).Length();
			}
		}
		if(((self.home_pos-predicted_ball_pos[stop_index]).Length()>15)||((self.pos-predicted_ball_pos[stop_index]).Length()>minteammatedist2ball&&((self.home_pos-predicted_ball_pos[stop_index]).Length()>3))){
   			float minoppdist=100;
			int player_num=1;
			bool itssafe=true;
			float dist2me;
			int jj;

			int oppnum_list[13]={0};
			int ch[12];
			int p_no=0;
			int k,i,j;
			float min1;
			for(jj=0;jj<12;jj++)
			{
				ch[jj]=1;
				oppnum_list[12]=0;

			}
			/*for(i=0;i<10;i++)
			{
				min1=1000;
				for(j=1;j<=11;j++)
				{
					if( ch[j]==1 && (self.pos-TheirPlayer[j].pos).Length()<min1 )
					{
						min1=(self.pos-TheirPlayer[j].pos).Length();
						p_no=j;
					}
				}
				oppnum_list[i]=p_no;
				ch[p_no]=0;
			}*/
			for(i=0;i<=10;i++)
			{
				min1=1000;
				for(j=1;j<=11;j++)
				{
					if( ch[j]==1 && TheirPlayer[j].pos[0]<min1 )
					{
						min1=TheirPlayer[j].pos[0];
						p_no=j;
					}
				}
				oppnum_list[i]=p_no;
				ch[p_no]=0;
			}
			for(k=0;k<=10;k++) DoLog(LOG_GOALIE,"range:: %d   ",oppnum_list[k]);
			for(i=0;i<11&&itssafe;i++){
				if((self.home_pos-TheirPlayer[oppnum_list[i]].pos).Length()>self.move_radius) continue;
				//if((self.home_pos-TheirPlayer[oppnum_list[i]].pos).Length()<minoppdist){
					newpos=TheirPlayer[oppnum_list[i]].pos;
					minoppdist=(self.home_pos-TheirPlayer[oppnum_list[i]].pos).Length();
					 player_num=oppnum_list[i];
				//}
				DoLog(LOG_GOALIE,"player_number to mark is  %d   ",player_num);
				itssafe=false;
				dist2me=(self.pos-newpos).Length();
				for(int i=2;i<=11;i++){
					if((newpos-MyPlayer[i].pos).Length()<dist2me&&i!=MyNumber){
						for(jj=1;jj<=11;jj++){
							if(jj!=player_num&&((MyPlayer[i].pos-TheirPlayer[jj].pos).Length()<(newpos-MyPlayer[i].pos).Length())) break;
						}
						if(jj>11){
							 if(MyNumber<6&&i>5&&MyPlayer[i].pos[0]<self.pos[0]){
							 	DoLog(LOG_GOALIE,"its not safe.change with player %f",i);
							 }
							 else{
							 	itssafe=true;
							 	minoppdist=100;
								DoLog(LOG_GOALIE,"its safe for myplayer[%d]",i);
							 }


						}
					}
				}
			}

			/*bool sw_1;
			for(int i=1;i<=11;i++){
				if(TheirPlayer[i].pos[0]>self.pos[0]) continue;
				sw_1=false;
				for(int j=2;j<=11&&!sw_1;j++){
					sw_1=true;
					if((MyPlayer[j].pos-TheirPlayer[i].pos).Length()>TheirPlayer[i].distance) continue;
					for(int k=1;k<=11&&sw_1;k++){
						if(k!=i&&(MyPlayer[j].pos-TheirPlayer[k].pos).Length()<1.5){
							sw_1=false;
							DoLog(LOG_GOALIE,"I am beasy at theirplayer[%f]",k);
						}

					}
					if(sw_1) DoLog(LOG_GOALIE,"myplayer[%f] can get theirplayer[%f]",j,i);
				}
				if(!sw_1){
					newpos=TheirPlayer[i].pos;
					itssafe=false;
					DoLog(LOG_GOALIE,"go back to mark a theirplayer[%f] in (%f,%f)",i,TheirPlayer[i].pos[0],TheirPlayer[i].pos[1]);
				}
			}*/
			//
			if((self.home_pos-newpos).Length()<MyPlayer[MyNumber].move_radius && !itssafe){
				DoLog(LOG_GOALIE,"goto1");
				if(BallIsOurs() && predicted_ball_pos[stop_index][0]>0 && self.pos[0]>0){
					/*if(self.pos[1]<-3){
						 newpos[0]=newpos[0]+3;
						 newpos[1]=newpos[1]-3;
					}
					else if(self.pos[1]>3){
						 newpos[0]=newpos[0]+3;
						 newpos[1]=newpos[1]+3;
					}*/
					newpos=self.home_pos;
				}//

				//bihind the theirplayer;
				if(self.pos[0]<-30) newpos[0]-=0.22;
				//if(self.pos[0]>-30&&MyNumber<6) newpos[0]-=7;
				//else if(self.pos[0]>-40&&MyNumber<6) newpos[0]-=2;
				//else if(MyNumber<6)  newpos[0]-=0.5;
				/*offensiveplayers go to their home_pos*/if(self.pos[0]>5) newpos=self.home_pos;
				action_info.x = newpos[0];
				action_info.y = newpos[1];
				action_info.do_drive = true ;
			}
			else{
				bool is_goalie2=false;
				int i;
				for(i=2;i<=5;i++){
					if(i!=MyNumber){
						for(int j=1;j<12;j++) if((TheirPlayer[j].pos-MyPlayer[i].pos).Length()<10) break;
					}
					if(j==12&&MyPlayer[i].pos[0]<self.pos[0])   is_goalie2=true;
				} 
				
				if(MyNumber<6&&!is_goalie2&&predicted_ball_pos[stop_index][0]<-15){
					DoLog(LOG_GOALIE,"play as goalie 2");
					return false;/*goalie2*/
				}
				else{
					action_info.x = self.home_pos[0];
					action_info.y = self.home_pos[1];
					action_info.do_drive = true ;
					DoLog(LOG_GOALIE,"goto 2");
				}	
			}

		}
	}
	else{
		for(int i=1;i<=11;i++){
			if(MyPlayer[i].dist2ball<minteammatedist2ball&&i!=MyNumber){
				minteammatedist2ball=MyPlayer[i].dist2ball;
			}
		}
		if(ball.distance>15||(ball.distance>minteammatedist2ball&&ball.distance>3)){
   			float minoppdist=100;
			int player_num=1;
			bool itssafe=true;
			float dist2me;
			int jj;

			int oppnum_list[13]={0};
			int ch[12];
			int p_no=0;
			int k,i,j;
			float min1;
			for(jj=0;jj<12;jj++)
			{
				ch[jj]=1;
				oppnum_list[12]=0;

			}
			/*for(i=0;i<10;i++)
			{
				min1=1000;
				for(j=1;j<=11;j++)
				{
					if( ch[j]==1 && (self.pos-TheirPlayer[j].pos).Length()<min1 )
					{
						min1=(self.pos-TheirPlayer[j].pos).Length();
						p_no=j;
					}
				}
				oppnum_list[i]=p_no;
				ch[p_no]=0;
			}*/
			for(i=0;i<=10;i++)
			{
				min1=1000;
				for(j=1;j<=11;j++)
				{
					if( ch[j]==1 && TheirPlayer[j].pos[0]<min1 )
					{
						min1=TheirPlayer[j].pos[0];
						p_no=j;
					}
				}
				oppnum_list[i]=p_no;
				ch[p_no]=0;
			}
			for(k=0;k<=10;k++) DoLog(LOG_GOALIE,"range:: %d   ",oppnum_list[k]);
			for(i=0;i<11&&itssafe;i++){
				if((self.home_pos-TheirPlayer[oppnum_list[i]].pos).Length()>self.move_radius) continue;
				//if((self.home_pos-TheirPlayer[oppnum_list[i]].pos).Length()<minoppdist){
					newpos=TheirPlayer[oppnum_list[i]].pos;
					minoppdist=(self.home_pos-TheirPlayer[oppnum_list[i]].pos).Length();
					 player_num=oppnum_list[i];
				//}
				DoLog(LOG_GOALIE,"player_number to mark is  %d   ",player_num);
				itssafe=false;
				dist2me=(self.pos-newpos).Length();
				for(int i=2;i<=11;i++){
					if((newpos-MyPlayer[i].pos).Length()<dist2me&&i!=MyNumber){
						for(jj=1;jj<=11;jj++){
							if(jj!=player_num&&((MyPlayer[i].pos-TheirPlayer[jj].pos).Length()<(newpos-MyPlayer[i].pos).Length())) break;
						}
						if(jj>11){
							 if(MyNumber<6&&i>5&&MyPlayer[i].pos[0]<self.pos[0]){
							 	DoLog(LOG_GOALIE,"its not safe.change with player %f",i);
							 }
							 else{
							 	itssafe=true;
							 	minoppdist=100;
								DoLog(LOG_GOALIE,"its safe for myplayer[%d]",i);
							 }


						}
					}
				}
			}

			/*bool sw_1;
			for(int i=1;i<=11;i++){
				if(TheirPlayer[i].pos[0]>self.pos[0]) continue;
				sw_1=false;
				for(int j=2;j<=11&&!sw_1;j++){
					sw_1=true;
					if((MyPlayer[j].pos-TheirPlayer[i].pos).Length()>TheirPlayer[i].distance) continue;
					for(int k=1;k<=11&&sw_1;k++){
						if(k!=i&&(MyPlayer[j].pos-TheirPlayer[k].pos).Length()<1.5){
							sw_1=false;
							DoLog(LOG_GOALIE,"I am beasy at theirplayer[%f]",k);
						}

					}
					if(sw_1) DoLog(LOG_GOALIE,"myplayer[%f] can get theirplayer[%f]",j,i);
				}
				if(!sw_1){
					newpos=TheirPlayer[i].pos;
					itssafe=false;
					DoLog(LOG_GOALIE,"go back to mark a theirplayer[%f] in (%f,%f)",i,TheirPlayer[i].pos[0],TheirPlayer[i].pos[1]);
				}
			}*/
			//
			if((self.home_pos-newpos).Length()<MyPlayer[MyNumber].move_radius && !itssafe){
				DoLog(LOG_GOALIE,"goto1");
				if(MyNumber<6&&BallIsOurs() && ball.pos[0]>0 && self.pos[0]>0){
					/*if(self.pos[1]<-3){
						 newpos[0]=newpos[0]+3;
						 newpos[1]=newpos[1]-3;
					}
					else if(self.pos[1]>3){
						 newpos[0]=newpos[0]+3;
						 newpos[1]=newpos[1]+3;
					}*/
					newpos=self.home_pos;
				}//
				/*test*/if(MyNumber>5&&MyNumber<9&&BallIsOurs() && ball.pos[0]>-10 && self.pos[0]>-10){
					/*if(self.pos[1]<-3){
						 newpos[0]=newpos[0]+3;
						 newpos[1]=newpos[1]-3;
					}
					else if(self.pos[1]>3){
						 newpos[0]=newpos[0]+3;
						 newpos[1]=newpos[1]+3;
					}*/
					newpos=self.home_pos;
				}
				if(MyNumber>8&&BallIsOurs() && ball.pos[0]>-15 && self.pos[0]>-20){
					/*if(self.pos[1]<-3){
						 newpos[0]=newpos[0]+3;
						 newpos[1]=newpos[1]-3;
					}
					else if(self.pos[1]>3){
						 newpos[0]=newpos[0]+3;
						 newpos[1]=newpos[1]+3;
					}*/
					newpos=self.home_pos;
				}

				//bihind the theirplayer;
				if(self.pos[0]<-30) newpos[0]-=0.22;
				//if(self.pos[0]>-30&&MyNumber<6) newpos[0]-=7;
				//else if(self.pos[0]>-40&&MyNumber<6) newpos[0]-=2;
				//else if(MyNumber<6)  newpos[0]-=0.5;
				/*offensiveplayers go to their home_pos*/if(self.pos[0]>5) newpos=self.home_pos;
				action_info.x = newpos[0];
				action_info.y = newpos[1];
				action_info.do_drive = true ;
			}
			else{
				bool is_goalie2=false;
				int i;
				for(i=2;i<=5;i++){
					if(i!=MyNumber){
						for(int j=1;j<12;j++) if((TheirPlayer[j].pos-MyPlayer[i].pos).Length()<10) break;
					}
					if(j==12&&MyPlayer[i].pos[0]<self.pos[0])   is_goalie2=true;
				} 
				if(MyNumber<6&&ball.pos[0]<-15&&!is_goalie2){
					return false;
					DoLog(LOG_GOALIE,"play as goalie 2");
				}
				else{
					DoLog(LOG_GOALIE,"goto2");
					action_info.x = self.home_pos[0];
					action_info.y = self.home_pos[1];
					action_info.do_drive = true ;
				}	
				
			}

		}
	}
	return true;
}

bool Positioning::BallIsOurs(){
	if((fabs(ball.vel[0])>0.75||fabs(ball.vel[1])>0.75)&&ball.distance>0.75){
		DoLog(LOG_GOALIE,"using prediction in ballisours");
		for(int i=1;i<=11;i++)
		{
			if( fabs(TheirPlayer[i].pos[0] - predicted_ball_pos[stop_index][0] ) < 1 && fabs(TheirPlayer[i].pos[1] - predicted_ball_pos[stop_index][1]) < 1 &&  fabs(TheirPlayer[i].pos[2] - predicted_ball_pos[stop_index][2]) < 1)
			return false;
		}
		for(int i=1;i<=11;i++)
		{
			if( fabs(MyPlayer[i].pos[0] - predicted_ball_pos[stop_index][0] ) < 0.1 && fabs(MyPlayer[i].pos[1] - predicted_ball_pos[stop_index][1]) < 0.1 &&  fabs(MyPlayer[i].pos[2] - predicted_ball_pos[stop_index][2]) < 0.1)
			return true;
		}
	}
	else{
		for(int i=1;i<=11;i++)
		{
			if( fabs(TheirPlayer[i].pos[0] - ball.pos[0] ) < 1 && fabs(TheirPlayer[i].pos[1] - ball.pos[1]) < 1 &&  fabs(TheirPlayer[i].pos[2] - ball.pos[2]) < 1)
			return false;
		}
		for(int i=1;i<=11;i++)
		{
			if( fabs(MyPlayer[i].pos[0] - ball.pos[0] ) < 0.1 && fabs(MyPlayer[i].pos[1] - ball.pos[1]) < 0.1 &&  fabs(MyPlayer[i].pos[2] - ball.pos[2]) < 0.1)
			return true;
		}
	}
}