kick.cc

该文件是包含了机器人足球比赛中的整个系统的代码

    } else if (c == '}' || c == '{' || c == '/') {
      if (c == '{') {
        temp = "";
      }
      if (c == '}') {
        kicks.push_back(k);
        prev = NULL;
        k.frames.clear();
        temp = "";
      }
      while (filebuffer[filePos] !='\n') {filePos++;} // go to end of line..
    } else if (c != '\n') temp = temp + c;
  }
  free(filebuffer);
}
*/


void TheStrut::XMLGetNextTag(char* fileBuffer, int* index, int length, char* nextTag) {
  int i = *index;
  while (fileBuffer[i] != '<' && i < length) {
    i++;
  }
  if (i == length) { 
    printf("XMLConfig: No tag found!");
    return; 
  }
  i++;
  int j=0;
  while (fileBuffer[i] != '>') {
    nextTag[j]=fileBuffer[i];
    j++;
    i++;
  }
  i++;
  nextTag[j]=(char)0;
  *index = i;
}

void TheStrut::XMLVerifyNextTag(char* fileBuffer, int* index, int length, char* nextTag) {
  char* nt = (char*)malloc(256);
  int i = *index;
  while (fileBuffer[i] != '<' && i < length) {
    i++;
  }
  if (i == length) { 
    printf("XMLConfig: No tag found to verify!");
    free(nt);
    return; 
  }
  i++;
  int j=0;
  while (fileBuffer[i] != '>') {
    nt[j]=fileBuffer[i];
    j++;
    i++;
  }
  i++;
  nt[j]=(char)0;
  *index = i;
  if (stricmp(nt,nextTag)!=0) {
    printf("XMLConfig: Verify \"%s\" tag failed! Found \"%s\" instead.",nextTag,nt);
  }
  free(nt);
}


void TheStrut::XMLGetStringEntry(char* fileBuffer, int* index, int length, char* stringEntry) {
  int i = *index;
  int j = 0;
  while (fileBuffer[i] != '<' && i < length) {
    stringEntry[j] = fileBuffer[i];
    i++;
    j++;
  }
  if (i == length) {
    printf("XMLConfig: No string found!");
    return;
  }
  stringEntry[j]=(char)0;
  *index = i;
}
void TheStrut::XMLGetIntEntry(char* fileBuffer, int* index, int length, int* ret) {
  int i = *index;
  char* stringEntry = (char*)malloc(256);
  int j = 0;
  while (fileBuffer[i] != '<' && i < length) {
    stringEntry[j] = fileBuffer[i];
    i++;
    j++;
  }
  if (i == length) {
    printf("XMLConfig: No int found!");
    return;
  }
  stringEntry[j]=(char)0;
  *index = i;
  *ret = atoi(stringEntry);
  free(stringEntry);
}

void TheStrut::XMLGetDoubleEntry(char* fileBuffer, int* index, int length, double* ret) {
  int i = *index;
  char* stringEntry = (char*)malloc(256);
  int j = 0;
  while (fileBuffer[i] != '<' && i < length) {
    stringEntry[j] = fileBuffer[i];
    i++;
    j++;
  }
  if (i == length) {
    printf("XMLConfig: No double found!");
    return;
  }
  stringEntry[j]=(char)0;
  *index = i;
  *ret = atof(stringEntry);
  free(stringEntry);
}

void TheStrut::XMLParseKicksFile(const char* filename) {
  printf("XMLConfig: Loading %s.\n", filename);
  FILE* f = fopen(filename,"rb");
  if (f == NULL) {
    printf("XMLConfig: Unable to open file %s.\n",filename);
    return;
  }
  fseek(f,0,SEEK_END);
  long filesize = ftell(f);
  rewind(f);
  char* fileBuffer = (char*) malloc(filesize);
  if (fileBuffer == NULL) {
    printf("XMLConfig: Unable to malloc %d bytes required to parse %s.\n",(int)filesize,filename);
    return;
  }
  fread(fileBuffer,1,filesize,f);
  fclose(f);


  for (unsigned int z=0; z < kicks.size(); z++) {
    for (unsigned int y=0; y < kicks[z].frames.size(); y++) {
      double* a = kicks[z].frames[y];
      delete a;
    }    
    kicks[z].frames.clear();
  }
  kicks.clear();



  Kick k;


  int index = 0;
  char* tag = (char*)malloc(256);

  double* prev = NULL;

  XMLGetNextTag(fileBuffer, &index, filesize, tag);
  if (stricmp(tag,"kicks")==0) {
  } else {
    printf("XMLConfig: No <Kicks> tag found\n");
    return;
  }
  XMLGetNextTag(fileBuffer, &index, filesize,tag);
  while (stricmp(tag,"/kicks")!=0) {
    XMLGetNextTag(fileBuffer, &index, filesize, tag);
    prev=NULL;
//    printf("New Kick!\n");
    while (stricmp(tag,"/kick")!=0) {
      if (stricmp(tag,"Name")==0) {
        char* name = (char*)malloc(256);
        XMLGetStringEntry(fileBuffer,&index,filesize,name);
//        printf("name: %s\n",name);
        k.name = name;
        //free(name);
        XMLVerifyNextTag(fileBuffer,&index,filesize,"/name");
      } else if (stricmp(tag,"id")==0) {
        int id;
        XMLGetIntEntry(fileBuffer, &index,filesize,&id);
//        printf("id: %d\n",id);
        k.id = id;
        XMLVerifyNextTag(fileBuffer,&index,filesize,"/id");
      } else if (stricmp(tag,"angles")==0) {
        // fixme do stuff here
        double* a = new double[16];
        char* angleStr = (char*)malloc(512);
        XMLGetStringEntry(fileBuffer,&index,filesize,angleStr);
        istringstream input(angleStr);
        free(angleStr);
        input >> a[0] >> a[1] >> a[2] >> a[3] >> a[4] >> a[5] >> a[6] >> a[7] >> a[8] >> a[9] >> a[10] >> a[11] >> a[12] >> a[13] >> a[14] >> a[15];

        if (prev != NULL) {
          // interpolate. this takes lots of memory. Somewhere in the vicinity of 1000kb (?) for our current kicks.xml
          // basically, this allows us to easily change the speed of the transition between two joint frames in a kick.
          double jointDeltas[15];
          double numFrames = (double)(a[15]);
          for (int j = 0; j < NUM_HEAD_JOINTS+NUM_BODY_JOINTS; j++) {
            jointDeltas[j] = ((double)(a[j]-prev[j]))/numFrames;
          }
          for (int f = 1; f < numFrames; f++) {
            double* joints = new double[16];
            for (int j = 0; j < NUM_HEAD_JOINTS+NUM_BODY_JOINTS; j++) { 
              joints[j] = (double)(prev[j]+jointDeltas[j]*(double)(f));
            }
            k.frames.push_back(joints);
          }
        }
        k.frames.push_back(a);
        prev = a;

        XMLVerifyNextTag(fileBuffer,&index,filesize,"/angles");
      } else if (stricmp(tag,"features")==0) {
        XMLGetNextTag(fileBuffer, &index,filesize, tag);
        while (stricmp(tag,"/features")!=0) {
          if (stricmp(tag,"distance")==0) {
            double distance;
            XMLGetDoubleEntry(fileBuffer,&index,filesize,&distance);
//            printf("distance: %f\n",distance);
            k.features.distance=distance;

            XMLVerifyNextTag(fileBuffer,&index,filesize,"/distance");
          } else if (stricmp(tag,"angle")==0) {
            double angle;
            XMLGetDoubleEntry(fileBuffer,&index,filesize,&angle);
//            printf("angle: %f\n",angle);
            k.features.angle = angle;
            XMLVerifyNextTag(fileBuffer,&index,filesize,"/angle");
          } else if (stricmp(tag,"reliability")==0) {
            double reliability;
            XMLGetDoubleEntry(fileBuffer,&index,filesize,&reliability);
//            printf("reliability: %f\n",reliability);
            k.features.reliability=reliability;
            XMLVerifyNextTag(fileBuffer,&index,filesize,"/reliability");
          } else {
            printf("XMLParse: Unrecognised tag within <features> - \"%s\"\n",tag);
            XMLGetNextTag(fileBuffer,&index,filesize,tag);
          }
          XMLGetNextTag(fileBuffer,&index,filesize,tag);
        }
      } else {
        printf("XMLParse: Unrecognised tag within <kick> - \"%s\"\n",tag);
        XMLGetNextTag(fileBuffer,&index,filesize,tag);
      }
      XMLGetNextTag(fileBuffer,&index,filesize,tag);
    }
    kicks.push_back(k);
    k.frames.clear();
    XMLGetNextTag(fileBuffer, &index, filesize, tag);
  }
  free(tag);
  free(fileBuffer);
}