param.cpp~

底层robocup3d源码。 suse１０．０ kdevelop 开发.

#include "param.h"
#include <sstream>
#include <iostream>
namespace MSpace {
Param::Param()
{
     CycleSent=0;
     CycleReceive=0;
     PlayMode="BeforeKickOff";
     FieldLength=110.0,FieldWidth= 75.0,FieldHeight= 40.0;
     GoalWidth= 7.32,GoalDepth= 2.0,GoalHeight= 0.5;
     BorderSize= 10.0,AgentMass= 75.0,AgentRadius= 0.22,AgentMaxSpeed= 10.0;
     BallMass=0.45,Battery=100.0,Temperature=23.0;
     CycleSent=0;CycleReceive=0;
     Time=0;BallRadius=0.111;
     IsKnowNumber=false;
     Reaction=false;
     PlayerNumber=-1;
     UseThinkTime=0;
     IsLeft=false;
     pan=0;tilt=0;
     HearDegree=0;
     HearTime=0;
     TeamName="lwp";
     for(int i=0;i<8;i++)
	     FGCycle[i]=0;
     for(int j=0;j<11;j++)
     {
	     MateCycle[j]=0;
	     OpponentCycle[j]=0;
     }
     BallCycle=0;
}
void  Param::prase(const char *message,int MessageLength)
{
    if(message[0]=='S'||message[0]=='I')
    {
        if(message[0]=='S')Reaction=true;
        else  Reaction=false;
        int i=1;
        CycleSent=GetFloat(message,i);
        CycleReceive=GetFloat(message,i);
        std::string str;
        while(i<MessageLength)
        {
           str=GetString(message,i);
           if(str=="Flag")
           {
               GetString(message,i);
               std::string str2=GetString(message,i);
               GetString(message,i);
	       if(str2=="1_l"){GetPol(FG_Pol[0],message,i);FGCycle[0]=CycleSent;}
	       else if(str2=="1_r"){GetPol(FG_Pol[2],message,i);FGCycle[2]=CycleSent;}
	       else if(str2=="2_l"){GetPol(FG_Pol[1],message,i);FGCycle[1]=CycleSent;}
	       else if(str2=="2_r"){GetPol(FG_Pol[3],message,i);FGCycle[3]=CycleSent;}
           }
           else if(str=="Goal")
           {
               GetString(message,i);
               std::string str2=GetString(message,i);
               GetString(message,i);
	       if(str2=="1_l"){GetPol(FG_Pol[4],message,i);FGCycle[4]=CycleSent;}
	       else if(str2=="1_r"){GetPol(FG_Pol[6],message,i);FGCycle[6]=CycleSent;}
	       else if(str2=="2_l"){GetPol(FG_Pol[5],message,i);FGCycle[5]=CycleSent;}
	       else if(str2=="2_r"){GetPol(FG_Pol[7],message,i);FGCycle[7]=CycleSent;}
           }
           else if(str=="Ball")
           {
              GetString(message,i);
              GetPol(ball,message,i);
	      BallCycle=CycleSent;
           }
           else if(str=="Player")
           {
               GetString(message,i);
               std::string str2=GetString(message,i);
               GetString(message,i);
               int num=GetFloat(message,i);
               if(num>=1&&num<=11)
               {
                   GetString(message,i);
		   if(str2==TeamName)
		   {
			   GetPol(Player_teammate[num-1],message,i);MateCycle[num-1]=CycleSent;
		   }
		   else {
			   GetPol(Player_opponent[num-1],message,i);OpponentCycle[num-1]=CycleSent;
		        }
               }
           }
           else if(str=="time")
           {
              double temp=GetFloat(message,i);
              if(temp>0)Time=temp;
           }
           else if(str=="playmode")
           {
              PlayMode=GetString(message,i);
           }
           else  if(str=="battery")
           {
              Battery=GetFloat(message,i);
           }
           else if(str=="temp")
           {
              Temperature=GetFloat(message,i);
           }
	   else if(str=="pan_tilt")
	   {
		   pan=GetFloat(message,i);
		   tilt=GetFloat(message,i);
	   }
	   else if(str=="hear")
	   {
		   HearTime=GetFloat(message,i);
		   HearDegree=GetFloat(message,i);
		   HearWord=GetString(message,i);
		   if(HearDegree==0&&HearWord=="self")
	                   HearWord=GetString(message,i);	
	   }
        }
    }
    else if(message[0]=='T')
    {
        int i=1;
        CycleSent=GetFloat(message,i);
	CycleReceive=CycleSent;
        Reaction=true;
    }
    else  if(message[0]=='K')
    {
         int i=1;
         UseThinkTime=GetFloat(message,i);
         Reaction=false;
    }
    else if(message[0]=='O')
    {
        int i=1;
        CycleSent=GetFloat(message,i);
        CycleReceive=CycleSent+10;
        Reaction=false;
    }
    else  Reaction=false;
}
void  Param::prase(const std::string& message,int MessageLength)
{
     prase(message.c_str(),MessageLength);
}
void  Param::GetInit(const char *message,int MessageLength)
{
     std::string str=message;
     int i=str.find("GameState");
     if(i<str.length())
     {  i=i+10;
        std::string str2;
        while(i<MessageLength)
        {
           str2=GetString(message,i);
           if(str2=="time")Time=GetFloat(message,i);
           else if(str2=="playmode")
           {
              PlayMode=GetString(message,i);
              break;
           }
           else if(str2=="unum")
           {
              PlayerNumber=GetFloat(message,i);
              IsKnowNumber=true;
           }
           else if(str2=="team")
           {
              if(GetString(message,i)=="left")IsLeft=true;
              else IsLeft=false;
           }
           else if(str2=="FieldLength")FieldLength=GetFloat(message,i);
           else if(str2=="FieldWidth")FieldWidth=GetFloat(message,i);
           else if(str2=="BallMass")BallMass=GetFloat(message,i);
        }
        
     }
}
std::string  Param::GetString(const char *message,int &i)
{
    while(message[i]==' '||message[i]==')'||message[i]=='(')i++;
    std::string str="";
    while(!(message[i]==' '||message[i]==')'||message[i]=='(')&&i<strlen(message))
    {
       str=str+message[i];
       i++;
    }
    return  str;
}
bool Param::GetCycle(const char *message)
{
   int i=1;
   CycleSent=GetFloat(message,i);
   CycleReceive=CycleSent+10;
   return true;
}
double Param::GetFloat(const char *message,int& i)
{
     while(message[i]==' '||message[i]==')'||message[i]=='(')i++;
     double plus;
     if(message[i]=='-'){plus=-1.0;++i;}
     else plus=1.0;
     double tempf=0.0;
     double  jinzhi=1.0;
     bool  xiaoshu=false;
      while((message[i]<='9'&&message[i]>='0')||message[i]=='.')
        {
           if(message[i]!='.')
           {
              if(!xiaoshu)tempf=tempf*10.0+(double)(message[i]-'0');
              else
              {
                  jinzhi=jinzhi/10.0;
                  tempf=tempf+jinzhi*((double)(message[i]-'0'));
              }
           }
           else xiaoshu=true;
           i++;
        }
      return tempf*plus;
}
void Param::GetPol(Pol &p,const char *message,int& i)
{
    p.Px=GetFloat(message,i);
    p.Py=GetFloat(message,i);
    p.Pz=GetFloat(message,i);
    if(p.Pz==0.0)
    {
	    int j=i;
	    std::string str=GetString(message,j);
	    if(str=="nan")GetString(message,i);
    }
}
bool  Param::GetPlayMode(const char *message)
{
     std::string str=message;
     int i=str.find("playmode");
     if(i<str.length())
     {  i=i+8;
        PlayMode=GetString(message,i);
        return true;
     }
     else return false;
}
bool  Param::GetNumber(const char *message)
{
    std::string str=message;
     int i=str.find("unum");
     if(i<str.length())
     {  i=i+4;
        PlayerNumber=GetFloat(message,i);
        IsKnowNumber=true;
        return true;
     }
     else return false;
}
Pol  Param::GetFlag(std::string FalgName,std::string FlagNumber,const char *message)
{
   std::string str2=FalgName+" (id "+FlagNumber;
   std::string str=message;
     int i=str.find(str2);
     Pol p;
     if(i<str.length())
     {  i=i+18;
        p.Px=GetFloat(message,i);
        p.Py=GetFloat(message,i);
        p.Pz=GetFloat(message,i);
     }
     return p;
}
double  Param::GetState(std::string State,const char *message)
{
     std::string str=message;
     int i=str.find(State);
     if(i<str.length())
     {
        i=i+State.length();
        return GetFloat(message,i);
     }
     else return -1.0;
}
bool  Param::GetBall(const char *message)
{
     std::string str2="Ball (pol";
     std::string str=message;
     int i=str.find(str2);
     if(i<str.length())
     {  i=i+str2.length();
        ball.Px=GetFloat(message,i);
        ball.Py=GetFloat(message,i);
        ball.Pz=GetFloat(message,i);
        return true;
     }
     else  return false;
}
bool  Param::GetPlayer(std::string team_name,int Number,const char *message)
{      
     std::string str2;
     if(Number<=0||Number>11)return false;
     else if(0<Number&&Number<10)
     {
         char ch='0'+Number;
         str2="team "+team_name+") (id "+ch;
     }
     else if(Number==10)str2="team "+team_name+") (id "+"10";
     else str2="team "+team_name+") (id "+"11";
     std::string str=message;
     int i=str.find(str2);
     if(i<str.length())
     {  i=i+str2.length()+7;
        if(team_name==TeamName)
        {
           Player_teammate[Number-1].Px=GetFloat(message,i);
           Player_teammate[Number-1].Py=GetFloat(message,i);
           Player_teammate[Number-1].Pz=GetFloat(message,i);
        }
        else 
        {
           Player_opponent[Number-1].Px=GetFloat(message,i);
           Player_opponent[Number-1].Py=GetFloat(message,i);
           Player_opponent[Number-1].Pz=GetFloat(message,i);
        }
        return true;
     }
     else  return false;
}
Param::~Param()
{
         
}


}