kickmetrics.cc

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

#include "KickMetrics.h"
#include "../Common/Common.h"
#include "../Globals.h"
#include "Trick.h"
#include "BallTricks.h"
#include "BasicTricks.h"
#include "ComplexTricks.h"
#include <math.h>

Trick* KickMetrics::DefaultMetric(double angle, double distance, double time, double accuracy) {
  angle = RAD_TO_DEG(angle);

  int maxKickCandidates = 5;
  int* kickCandidateIds = new int[maxKickCandidates];
  double* kickCandidateMetrics = new double[maxKickCandidates];
  for (int k = 0; k < maxKickCandidates; k++) {
    kickCandidateIds[k] = -1;
    kickCandidateMetrics[k] = 0;
  }

  for (unsigned int k = 0; k < kicks.size(); k++) {
    if ((kicks[k].features.angle > 0 && angle < 0) || (kicks[k].features.angle < 0 && angle > 0)) {
      continue;
    }
    double d = sqrt (5*SQUARE(kicks[k].features.angle-angle) + SQUARE(kicks[k].features.distance-distance));

    // add metric to ORDERED candidate list
    for (int i = 0; i < maxKickCandidates; i++) {
      if (kickCandidateIds[i] < 0) {
        kickCandidateIds[i] = kicks[k].id;
        kickCandidateMetrics[i] = d;
        continue;
      } else if (d < kickCandidateMetrics[i]) {
        for (int j = i+1; j < maxKickCandidates; j++) {
          kickCandidateIds[j] = kickCandidateIds[j-1];
          kickCandidateMetrics[j] = kickCandidateMetrics[j-1];
        }
        kickCandidateIds[i] = kicks[k].id;
        kickCandidateMetrics[i] = d;
        continue;
      }
    }    
  }

  int id = kickCandidateIds[0];
  if (id == -1) {
    return NULL;
  }

  // are we accurate 'enough' ? if no time, we have to do this too...
  if ( (ABS(kicks[id].features.angle - angle) < accuracy) || (time < 1)) {
    // yes. do kick.
    return new BasicTricks::Kick(id);
  }
  // not accurate enough and we have some time.. need turns !
  double turnAngle = kicks[id].features.angle - angle;

  return new ComplexTricks::ComplexKick(turnAngle, turnAngle);
}