worldmodel.cpp

robocup3d源化码 Nexus3D.tar.gz

{
    return mMyPos;
}

TTeamIndex WorldModel::GetMyTeam()
{
    return mTeamIndex;
}

float WorldModel::GetMinimalKickDistance()
{
    return mAgentRadius + mBallRadius + 0.04;
}

int WorldModel::GetTeamUnum()
{
    return mTeamUnum;
}


void WorldModel::SetTeamName(std::string name){
    mTeamName =   name;
}

string WorldModel::GetTeamName(){
    return mTeamName;
}

void WorldModel:: SetOppTeamName(std::string name)
{
    mOppTeamName = name;
}

string WorldModel::GetOppTeamName()
{
       return mOppTeamName;
}

float WorldModel::string2float ()
{
	int tmp,exp,sign;//tmp: number of digits after floating points.
	float output;
	sign=1;
	tmp=0;
	exp=0;
	output=0;
	while(input_string[parse_counter]==' ')
		parse_counter++;

	while(((input_string[parse_counter]>='0'&&input_string[parse_counter]<='9')||input_string[parse_counter]=='.'||input_string[parse_counter]=='-'))
	{
		if(input_string[parse_counter]=='-')
		{
			sign*=-1;
			parse_counter++;
			continue;
		}
		if(input_string[parse_counter]=='.')
		{
			tmp=1;
			parse_counter++;
			continue;
		}
		if(input_string[parse_counter]>='0'&& input_string[parse_counter]<='9')
		{
			exp+=tmp;
			output*=10;
			output+=input_string[parse_counter]-'0';
			parse_counter++;
		}
	}
	output*=pow(10.0,-exp);
 	output*=sign;
	return(output);
}

void WorldModel::parse_vision_object(int index)
{
	parse_counter++;
	while((input_string[parse_counter]<'0' || input_string[parse_counter]>'9')&&(input_string[parse_counter]!='-'))
		parse_counter++;
	static_objects_polar_relpos2me[index].distance=string2float();
	static_objects_polar_relpos2me[index].theta=string2float();
	static_objects_polar_relpos2me[index].phi=string2float();
	while(input_string[parse_counter]!=')')
		parse_counter++;
	parse_counter++;
	while(input_string[parse_counter]!=')')
		parse_counter++;
	parse_counter++;
	static_objects_polar_relpos2me[index].is_new=true;
}

void WorldModel::parse_flags()
{
	while(input_string[parse_counter]!='(')
		parse_counter++;
	while(input_string[parse_counter]!='1' && input_string[parse_counter]!='2')
		parse_counter++;
	if(input_string[parse_counter]=='1')
	{
		parse_counter+=2;
		if(input_string[parse_counter]=='l')
			parse_vision_object(VO_FLAG1L);
		if(input_string[parse_counter]=='r')
			parse_vision_object(VO_FLAG1R);
	}
	if(input_string[parse_counter]=='2')
	{
		parse_counter+=2;
		if(input_string[parse_counter]=='l')
			parse_vision_object(VO_FLAG2L);
		if(input_string[parse_counter]=='r')
			parse_vision_object(VO_FLAG2R);
	}
}

void WorldModel::parse_goals()
{
	while(input_string[parse_counter]!='(')
		parse_counter++;
	while(input_string[parse_counter]!='1'&&input_string[parse_counter]!='2')
		parse_counter++;
	if(input_string[parse_counter]=='1')
	{
		parse_counter+=2;
		if(input_string[parse_counter]=='l')
			parse_vision_object(VO_GOAL1L);
		if(input_string[parse_counter]=='r')
			parse_vision_object(VO_GOAL1R);
	}
	if(input_string[parse_counter]=='2')
	{
		parse_counter+=2;
		if(input_string[parse_counter]=='l')
			parse_vision_object(VO_GOAL2L);
		if(input_string[parse_counter]=='r')
			parse_vision_object(VO_GOAL2R);
	}
}

void WorldModel::parse_ball()
{
	while((input_string[parse_counter]<'0' || input_string[parse_counter]>'9')&&(input_string[parse_counter]!='-'))
		parse_counter++;
	static_objects_polar_relpos2me[VO_BALL].distance=string2float();
	static_objects_polar_relpos2me[VO_BALL].theta=string2float();
	static_objects_polar_relpos2me[VO_BALL].phi=string2float();
	while(input_string[parse_counter]!=')')
		parse_counter++;
	parse_counter++;
	while(input_string[parse_counter]!=')')
		parse_counter++;
	parse_counter++;

	static_objects_polar_relpos2me[VO_BALL].is_new=true;

}

void WorldModel::parse_player()
{
	int index=0;
	int team_id;
	int player_id;
	char team_name[50];

	while(input_string[parse_counter]!='m')//parses input string upto 'team' term.
		parse_counter++;
	parse_counter++;
	while(input_string[parse_counter]==' ')
		parse_counter++;
	while(input_string[parse_counter]!=')')
		team_name[index++]=input_string[parse_counter++];

	team_name[index]='\0';

	if(!strcmp(team_name, MyTeamName.c_str()))
		team_id=0; //our team
	else
	{
		if(!sw_their_team_name)
		{
			TheirTeamName = team_name;
			sw_their_team_name = true;
		}
		team_id=1; //oponent team
	}

	while((input_string[parse_counter]<'0'|| input_string[parse_counter]>'9')&&(input_string[parse_counter]!='-'))
		parse_counter++;

	player_id=(int)string2float();

	while((input_string[parse_counter]<'0'|| input_string[parse_counter]>'9')&&(input_string[parse_counter]!='-'))
		parse_counter++;

	players_polar_relpos2me[team_id][player_id].distance=(float)string2float();
	players_polar_relpos2me[team_id][player_id].theta=(float)string2float();
	players_polar_relpos2me[team_id][player_id].phi=(float)string2float();
	players_polar_relpos2me[team_id][player_id].is_new=true;


	//DoLog(LOG_PARSE,"team_id=%d, player_id=%d, (%.2f,%.2f,%.2f)",team_id,player_id,players_polar_relpos2me[team_id][player_id].distance,players_polar_relpos2me[team_id][player_id].theta,players_polar_relpos2me[team_id][player_id].phi);

	while(input_string[parse_counter]!=')')
		parse_counter++;
	parse_counter++;
	while(input_string[parse_counter]!=')')
		parse_counter++;
	parse_counter++;


}


void WorldModel::ResetObjects()
{
	int i;
	for(i=0; i<9; i++)
		static_objects_polar_relpos2me[i].is_new = false;
	for(i=1; i<12; i++)
	{
		players_polar_relpos2me[0][i].is_new = false;
		players_polar_relpos2me[1][i].is_new = false;
	}
}

void WorldModel::set_my_position()
{
	/* find the nearset flag to me*/
	float dist1 = 1000.0, dist2 = 1000.0, dist3 = 1000.0;
	int index1 = 1, index2 = 2, index3 = 3;
	for(int i=1;i<9;i++)
		if(static_objects_polar_relpos2me[i].is_new && static_objects_polar_relpos2me[i].distance < dist1)
		{
			index1 = i;
			dist1 = static_objects_polar_relpos2me[i].distance;
		}
	for(int i=1;i<9;i++)
		if(static_objects_polar_relpos2me[i].is_new && static_objects_polar_relpos2me[i].distance < dist2 && i != index1)
		{
			index2 = i;
			dist2 = static_objects_polar_relpos2me[i].distance;
		}
	for(int i=1;i<9;i++)
		if(static_objects_polar_relpos2me[i].is_new && static_objects_polar_relpos2me[i].distance < dist3 && i != index1 && i != index2)
		{
			index3 = i;
			dist3 = static_objects_polar_relpos2me[i].distance;
		}
	DoLog(LOG_PARSE,"object[%d].pos(%.2f, %.2f, %.2f), dist=%.2f",index1,object[index1].pos[0],object[index1].pos[1],object[index1].pos[2],dist1);
	DoLog(LOG_PARSE,"object[%d].pos(%.2f, %.2f, %.2f), dist=%.2f",index2,object[index2].pos[0],object[index2].pos[1],object[index2].pos[2],dist2);
	DoLog(LOG_PARSE,"object[%d].pos(%.2f, %.2f, %.2f), dist=%.2f",index3,object[index3].pos[0],object[index3].pos[1],object[index3].pos[2],dist3);
	/* calculate my Vector3fesian position */
	Vector3f pos1 = pol2vector(static_objects_polar_relpos2me[index1])*-1 + object[index1].pos;
	Vector3f pos2 = pol2vector(static_objects_polar_relpos2me[index2])*-1 + object[index2].pos;
	Vector3f pos3 = pol2vector(static_objects_polar_relpos2me[index3])*-1 + object[index3].pos;
	DoLog(LOG_PARSE,"pos1=(%.2f, %.2f, %.2f)",pos1[0],pos1[1],pos1[2]);
	DoLog(LOG_PARSE,"pos2=(%.2f, %.2f, %.2f)",pos2[0],pos2[1],pos2[2]);
	DoLog(LOG_PARSE,"pos2=(%.2f, %.2f, %.2f)",pos3[0],pos3[1],pos3[2]);
	//self.Update(pol2vector(static_objects_polar_relpos2me[index1])*-1 + object[index1].pos, static_objects_polar_relpos2me[VO_BALL].distance);
	self.Update((pos1+pos2+pos3)/3, static_objects_polar_relpos2me[VO_BALL].distance);
	DoLog(LOG_UPDATE,"is new %d",self.is_seen);
	DoLog(LOG_UPDATE,"Positions    self  x     %f y     %f z     %f",self.pos[0],self.pos[1],self.pos[2]);
	DoLog(LOG_UPDATE,"velacity     self  x     %f y     %f z     %f",self.vel[0],self.vel[1],self.vel[2]);
	DoLog(LOG_UPDATE,"accelaration self  x     %f y     %f z     %f",self.acc[0],self.acc[1],self.acc[2]);

	DoLog(LOG_PARSE,"self pos = (%.2f, %.2f, %.2f)",self.pos[0] ,self.pos[1] ,self.pos[2]);
}

void WorldModel::set_ball_position()
{
	DoLog(LOG_POS,"my pos = (%.2f, %.2f, %.2f)",self.pos[0] ,self.pos[1] ,self.pos[2]);

	if(static_objects_polar_relpos2me[VO_BALL].is_new)
	{
		ball.Update( self.pos + (pol2vector(static_objects_polar_relpos2me[VO_BALL])),static_objects_polar_relpos2me[VO_BALL].distance);
		DoLog(LOG_UPDATE,"is new %d",ball.is_seen);
		DoLog(LOG_UPDATE,"Positions    ball  x     %f y     %f z     %f",ball.pos[0],ball.pos[1],ball.pos[2]);
		DoLog(LOG_UPDATE,"velacity     ball  x     %f y     %f z     %f",ball.vel[0],ball.vel[1],ball.vel[2]);
		DoLog(LOG_UPDATE,"accelaration ball  x     %f y     %f z     %f",ball.acc[0],ball.acc[1],ball.acc[2]);
	}
	else
	{
		ball.Update();
		DoLog(LOG_UPDATE,"is new %d",ball.is_seen);
		DoLog(LOG_UPDATE,"Positions    ball  x     %f y     %f z     %f",ball.pos[0],ball.pos[1],ball.pos[2]);
		DoLog(LOG_UPDATE,"velacity     ball  x     %f y     %f z     %f",ball.vel[0],ball.vel[1],ball.vel[2]);
		DoLog(LOG_UPDATE,"accelaration ball  x     %f y     %f z     %f",ball.acc[0],ball.acc[1],ball.acc[2]);
	}
	DoLog(LOG_PARSE,"ball pos = (%.2f, %.2f, %.2f)",ball.pos[0] ,ball.pos[1] ,ball.pos[2]);
	MyPlayer[MyNumber].Update(self.pos, 0.0f);
}

void WorldModel::set_players_position()
{
	DoLog(LOG_UPDATE,"player infotmation ");
	for(int i=1;i<12;i++)
	{
		if(i != MyNumber)
		{
			if(players_polar_relpos2me[0][i].is_new)
			{
				MyPlayer [i].Update(self.pos + (pol2vector(players_polar_relpos2me[0][i])),players_polar_relpos2me[0][i].distance);
				DoLog(LOG_UPDATE,"is new %d",MyPlayer[i].is_seen);
				DoLog(LOG_UPDATE,"Positions    player Number[%d]  x     %f y     %f z     %f",i,MyPlayer[i].pos[0], MyPlayer[i].pos[1], MyPlayer[i].pos[2]);
				DoLog(LOG_UPDATE,"velacity     player Number[%d]  x     %f y     %f z     %f",i,MyPlayer[i].vel[0], MyPlayer[i].vel[1], MyPlayer[i].vel[2]);
				DoLog(LOG_UPDATE,"accelaration player Number[%d]  x     %f y     %f z     %f",i,MyPlayer[i].acc[0], MyPlayer[i].acc[1], MyPlayer[i].acc[2]);
			}
			else
			{
				MyPlayer [i].Update();
				DoLog(LOG_UPDATE,"is new %d",MyPlayer[i].is_seen);
				DoLog(LOG_UPDATE,"Positions    player Number[%d]  x     %f y     %f z     %f",i,MyPlayer [i].pos[0],MyPlayer [i].pos[1],MyPlayer [i].pos[2]);
				DoLog(LOG_UPDATE,"velacity     player Number[%d]  x     %f y     %f z     %f",i,MyPlayer [i].vel[0],MyPlayer [i].vel[1],MyPlayer [i].vel[2]);
				DoLog(LOG_UPDATE,"accelaration player Number[%d]  x     %f y     %f z     %f",i,MyPlayer [i].acc[0],MyPlayer [i].acc[1],MyPlayer [i].acc[2]);

			}
		}

		if(players_polar_relpos2me[1][i].is_new)
			TheirPlayer[i].Update(self.pos + (pol2vector(players_polar_relpos2me[1][i])),players_polar_relpos2me[1][i].distance);
		else
			TheirPlayer[i].Update();
		DoLog(LOG_PARSE,"my_player[%d].pos = (%.2f, %.2f, %.2f)",i,MyPlayer[i].pos[0] ,MyPlayer[i].pos[1] ,MyPlayer[i].pos[2]);
		DoLog(LOG_PARSE,"thier_player[%d].pos = (%.2f, %.2f, %.2f)",i,TheirPlayer[i].pos[0] ,TheirPlayer[i].pos[1] ,TheirPlayer[i].pos[2]);
	}
}

void WorldModel::parse_vision(const char * str)
{
	ResetObjects();
	strcpy(input_string,str);
	parse_counter=0;
	while(input_string[parse_counter]!='(')
		parse_counter++;
	while(input_string[parse_counter]!='V')//vision message
		parse_counter++;
	while(input_string[parse_counter]!='(')
		parse_counter++;
	parse_counter++;
	while(input_string[parse_counter]==' ')
		parse_counter++;
	while(input_string[parse_counter]!='\0')
	{
		 switch(input_string[parse_counter])
		{
			case 'F'://flag
				parse_flags();
				break;
			case 'G'://goal
				parse_goals();
				break;
			case 'B'://ball
				parse_ball();
				break;
			case 'P'://player
				parse_player();
				break;
		}
		while(input_string[parse_counter]!='('&&input_string[parse_counter]!='\0'){
			parse_counter++;
			//DoLog(LOG_PARSE,"\n %d",parse_counter );
		}
		if(input_string[parse_counter]!='\0'){
		 	 parse_counter++;
			 //(LOG_PARSE,"\n %d",parse_counter );
		}
	}

	set_my_position();
	set_ball_position();
	set_players_position();
}