Modules/WalkingEngine/GT2005Parameters.cpp

Go to the documentation of this file.

/***************************************************************************
* @file GT2005Parameters.cpp
* class GT2005Parameters
* @author Denis Fisseler
****************************************************************************/

#include "GT2005Parameters.h"
#include "Platform/SystemCall.h"
#include "Tools/Streams/InStreams.h"
#include "Tools/Streams/OutStreams.h"

const int GT2005Parameters::Leg_FR = 0;
const int GT2005Parameters::Leg_FL = 1;
const int GT2005Parameters::Leg_HR = 2;
const int GT2005Parameters::Leg_HL = 3;

GT2005Parameters::GT2005Parameters()
{
  loadingFailed = false;
  setBasicParameters();
  setBasicPolygons();
  specialMode = false;
  framesSinceBoost = 0;
}

GT2005Parameters::~GT2005Parameters()
{
  for (int i = 0; i < 4; i++)
  {
    delete omniPoly[i];
    delete rotPoly[i];
    delete sforwardPoly[i];
    delete forwardPoly[i];
    delete dforwardPoly[i];
    delete sbackwardPoly[i];
    delete backwardPoly[i];
    delete dbackwardPoly[i];
    delete sidewardsPoly[i];
    delete boostPoly[i];
    delete grabPoly[i];
    delete rgrabPoly[i];
    delete evoPoly[i];
  }
}

void GT2005Parameters::setBasicParameters()
{
  stepLength = 64;
  counterRotation = 1234567890;

  // set foot phases
  footPhase[Leg_FR] = 0;
  footPhase[Leg_FL] = 0.5;
  footPhase[Leg_HR] = 0.5;
  footPhase[Leg_HL] = 0;

  // set foot zero positions
  footZeroPosition[Leg_FR].x = 50.3536; footZeroPosition[Leg_FR].y = -80.2702;  footZeroPosition[Leg_FR].z = -77.5836;
  footZeroPosition[Leg_FL].x = 50.3536; footZeroPosition[Leg_FL].y = 80.2702; footZeroPosition[Leg_FL].z = -77.5836;
  footZeroPosition[Leg_HR].x = -52.0932;  footZeroPosition[Leg_HR].y = -81.5432;  footZeroPosition[Leg_HR].z = -105.174;
  footZeroPosition[Leg_HL].x = -52.0932;  footZeroPosition[Leg_HL].y = 81.5432; footZeroPosition[Leg_HL].z = -105.174;
}

void GT2005Parameters::setBasicPolygons()
{
  int j;

  // set front shape
  Vector3<double> fPoints[4];
  fPoints[0].x = -24; fPoints[0].y = 0; fPoints[0].z = -6;
  fPoints[1].x = -25; fPoints[1].y = 0; fPoints[1].z = -1;
  fPoints[2].x = 23;  fPoints[2].y = 0; fPoints[2].z = 11;
  fPoints[3].x = 24;  fPoints[3].y = 0; fPoints[3].z = 6;

  // set hind shape
  Vector3<double> hPoints[4];
  hPoints[0].x = -25; hPoints[0].y = 0; hPoints[0].z = 0;
  hPoints[1].x = -25; hPoints[1].y = 0; hPoints[1].z = 24;
  hPoints[2].x = 25;  hPoints[2].y = 0; hPoints[2].z = 24;
  hPoints[3].x = 25;  hPoints[3].y = 0; hPoints[3].z = 0;
  //*/

  // n paar segmentlängen
  double fLen[4];
  fLen[0] = 0.06;
  fLen[1] = 0.39;
  fLen[2] = 0.06;
  fLen[3] = 0.49;

  double hLen[4];
  hLen[0] = 0.06;
  hLen[1] = 0.39;
  hLen[2] = 0.06;
  hLen[3] = 0.49;

  evoPoly[Leg_FR] = new GT2005Polygon(4, 64);
  evoPoly[Leg_FL] = new GT2005Polygon(4, 64);
  evoPoly[Leg_HR] = new GT2005Polygon(4, 64);
  evoPoly[Leg_HL] = new GT2005Polygon(4, 64);
  for (j = 0; j < 4; j++)
  {
    evoPoly[Leg_FR]->setSegment(j, fLen[j], fPoints[j]);
    evoPoly[Leg_FL]->setSegment(j, fLen[j], fPoints[j]);
    evoPoly[Leg_HR]->setSegment(j, hLen[j], hPoints[j]);
    evoPoly[Leg_HL]->setSegment(j, hLen[j], hPoints[j]);
  }
  activePoints = 4;
  
  // finalize polygons to calculate ground phases
  evoPoly[Leg_FR]->finalize();
  evoPoly[Leg_FL]->finalize();
  evoPoly[Leg_HR]->finalize();
  evoPoly[Leg_HL]->finalize();

  // hauptsache die teile sind erst mal initialisiert
  for (int i = 0; i < 4; i++)
  {
    sforwardPoly[i] = new GT2005Polygon(activePoints, 64);
    forwardPoly[i] = new GT2005Polygon(activePoints, 64);
    dforwardPoly[i] = new GT2005Polygon(activePoints, 64);
    sbackwardPoly[i] = new GT2005Polygon(activePoints, 64);
    backwardPoly[i] = new GT2005Polygon(activePoints, 64);
    dbackwardPoly[i] = new GT2005Polygon(activePoints, 64);
    sidewardsPoly[i] = new GT2005Polygon(activePoints, 64);
    boostPoly[i] = new GT2005Polygon(activePoints, 64);
    grabPoly[i] = new GT2005Polygon(activePoints, 64);
    rgrabPoly[i] = new GT2005Polygon(activePoints, 64);
  }

  omniPoly[Leg_FR] = evoPoly[Leg_FR]->copy();
  omniPoly[Leg_FL] = evoPoly[Leg_FL]->copy();
  omniPoly[Leg_HR] = evoPoly[Leg_HR]->copy();
  omniPoly[Leg_HL] = evoPoly[Leg_HL]->copy();

  // store a copy for rotation and do some optimisations
  rotPoly[Leg_FR] = evoPoly[Leg_FR]->copy();
  rotPoly[Leg_FL] = evoPoly[Leg_FL]->copy();
  rotPoly[Leg_HR] = evoPoly[Leg_HR]->copy();
  rotPoly[Leg_HL] = evoPoly[Leg_HL]->copy();
  rotPoly[Leg_FR]->stepLength = 42;
  rotPoly[Leg_FL]->stepLength = 42;
  rotPoly[Leg_HR]->stepLength = 42;
  rotPoly[Leg_HL]->stepLength = 42;

  // hier schätz ich mir mal nen guten Anfang für ne Evolution zusammen
  // dashfake
  fPoints[0].x = -25; fPoints[0].y = 0; fPoints[0].z = -6;
  fPoints[1].x = -25; fPoints[1].y = 0; fPoints[1].z = -1;
  fPoints[2].x = 25;  fPoints[2].y = 0; fPoints[2].z = 11;
  fPoints[3].x = 25;  fPoints[3].y = 0; fPoints[3].z = 6;
  hPoints[0].x = -15; hPoints[0].y = 0; hPoints[0].z = -30;
  hPoints[1].x = -15; hPoints[1].y = 0; hPoints[1].z = -5;
  hPoints[2].x = 35;  hPoints[2].y = 0; hPoints[2].z = -5;
  hPoints[3].x = 35;  hPoints[3].y = 0; hPoints[3].z = -30;

  for (j = 0; j < 4; j++)
  {
    evoPoly[Leg_FR]->setSegment(j, fLen[j], fPoints[j]);
    evoPoly[Leg_FL]->setSegment(j, fLen[j], fPoints[j]);
    evoPoly[Leg_HR]->setSegment(j, hLen[j], hPoints[j]);
    evoPoly[Leg_HL]->setSegment(j, hLen[j], hPoints[j]);
  }
  evoPoly[Leg_FR]->stepLength = 52;
  evoPoly[Leg_FL]->stepLength = 52;
  evoPoly[Leg_HR]->stepLength = 52;
  evoPoly[Leg_HL]->stepLength = 52;
  evoPoly[Leg_FR]->finalize();
  evoPoly[Leg_FL]->finalize();
  evoPoly[Leg_HR]->finalize();
  evoPoly[Leg_HL]->finalize();//*/

  setOptimizedPolys();//*/
}


void GT2005Parameters::setOptimizedPolys()
{
  //forwardPoly parameters
  forwardPoly[0]->numPoints = 4;
  forwardPoly[0]->rollAngle = 0;
  forwardPoly[0]->stepLength = 50;
  forwardPoly[0]->pPoints[0].point.x = -35.7702;
  forwardPoly[0]->pPoints[0].point.y = 15.9675;
  forwardPoly[0]->pPoints[0].point.z = -31.9739;
  forwardPoly[0]->pPoints[0].sLen = 0.104465;
  forwardPoly[0]->pPoints[1].point.x = -38.9055;
  forwardPoly[0]->pPoints[1].point.y = -31.4604;
  forwardPoly[0]->pPoints[1].point.z = -15.6947;
  forwardPoly[0]->pPoints[1].sLen = 0.247062;
  forwardPoly[0]->pPoints[2].point.x = 7.85936;
  forwardPoly[0]->pPoints[2].point.y = 16.92;
  forwardPoly[0]->pPoints[2].point.z = 11.9191;
  forwardPoly[0]->pPoints[2].sLen = 0.213031;
  forwardPoly[0]->pPoints[3].point.x = 29.6432;
  forwardPoly[0]->pPoints[3].point.y = 9.71364;
  forwardPoly[0]->pPoints[3].point.z = 32.4321;
  forwardPoly[0]->pPoints[3].sLen = 0.435442;
  forwardPoly[0]->numPoints = 4;
  forwardPoly[0]->rollAngle = 0;
  forwardPoly[1]->stepLength = 50;
  forwardPoly[1]->pPoints[0].point.x = -35.7702;
  forwardPoly[1]->pPoints[0].point.y = 15.9675;
  forwardPoly[1]->pPoints[0].point.z = -31.9739;
  forwardPoly[1]->pPoints[0].sLen = 0.104465;
  forwardPoly[1]->pPoints[1].point.x = -38.9055;
  forwardPoly[1]->pPoints[1].point.y = -31.4604;
  forwardPoly[1]->pPoints[1].point.z = -15.6947;
  forwardPoly[1]->pPoints[1].sLen = 0.247062;
  forwardPoly[1]->pPoints[2].point.x = 7.85936;
  forwardPoly[1]->pPoints[2].point.y = 16.92;
  forwardPoly[1]->pPoints[2].point.z = 11.9191;
  forwardPoly[1]->pPoints[2].sLen = 0.213031;
  forwardPoly[1]->pPoints[3].point.x = 29.6432;
  forwardPoly[1]->pPoints[3].point.y = 9.71364;
  forwardPoly[1]->pPoints[3].point.z = 32.4321;
  forwardPoly[1]->pPoints[3].sLen = 0.435442;
  forwardPoly[2]->numPoints = 4;
  forwardPoly[2]->rollAngle = 0;
  forwardPoly[2]->stepLength = 50;
  forwardPoly[2]->pPoints[0].point.x = -28.2799;
  forwardPoly[2]->pPoints[0].point.y = -7.11929;
  forwardPoly[2]->pPoints[0].point.z = -6.10541;
  forwardPoly[2]->pPoints[0].sLen = 0.121962;
  forwardPoly[2]->pPoints[1].point.x = -44.0651;
  forwardPoly[2]->pPoints[1].point.y = -14.2374;
  forwardPoly[2]->pPoints[1].point.z = 31.8775;
  forwardPoly[2]->pPoints[1].sLen = 0.449758;
  forwardPoly[2]->pPoints[2].point.x = 21.8429;
  forwardPoly[2]->pPoints[2].point.y = 5.48479;
  forwardPoly[2]->pPoints[2].point.z = 25.1939;
  forwardPoly[2]->pPoints[2].sLen = 0.0754086;
  forwardPoly[2]->pPoints[3].point.x = 12.793;
  forwardPoly[2]->pPoints[3].point.y = 7.26785;
  forwardPoly[2]->pPoints[3].point.z = -1.38549;
  forwardPoly[2]->pPoints[3].sLen = 0.352872;
  forwardPoly[3]->numPoints = 4;
  forwardPoly[3]->rollAngle = 0;
  forwardPoly[3]->stepLength = 50;
  forwardPoly[3]->pPoints[0].point.x = -28.2799;
  forwardPoly[3]->pPoints[0].point.y = -7.11929;
  forwardPoly[3]->pPoints[0].point.z = -6.10541;
  forwardPoly[3]->pPoints[0].sLen = 0.121962;
  forwardPoly[3]->pPoints[1].point.x = -44.0651;
  forwardPoly[3]->pPoints[1].point.y = -14.2374;
  forwardPoly[3]->pPoints[1].point.z = 31.8775;
  forwardPoly[3]->pPoints[1].sLen = 0.449758;
  forwardPoly[3]->pPoints[2].point.x = 21.8429;
  forwardPoly[3]->pPoints[2].point.y = 5.48479;
  forwardPoly[3]->pPoints[2].point.z = 25.1939;
  forwardPoly[3]->pPoints[2].sLen = 0.0754086;
  forwardPoly[3]->pPoints[3].point.x = 12.793;
  forwardPoly[3]->pPoints[3].point.y = 7.26785;
  forwardPoly[3]->pPoints[3].point.z = -1.38549;
  forwardPoly[3]->pPoints[3].sLen = 0.352872;
  forwardPoly[0]->finalize();
  forwardPoly[1]->finalize();
  forwardPoly[2]->finalize();
  forwardPoly[3]->finalize();

  //sforwardPoly parameters
  sforwardPoly[0]->numPoints = 4;
  sforwardPoly[0]->rollAngle = 0;
  sforwardPoly[0]->stepLength = 75;
  sforwardPoly[0]->pPoints[0].point.x = -30.0931;
  sforwardPoly[0]->pPoints[0].point.y = 2.08297;
  sforwardPoly[0]->pPoints[0].point.z = -18.8519;
  sforwardPoly[0]->pPoints[0].sLen = 0.107553;
  sforwardPoly[0]->pPoints[1].point.x = -27.0353;
  sforwardPoly[0]->pPoints[1].point.y = 0.823497;
  sforwardPoly[0]->pPoints[1].point.z = -5.40966;
  sforwardPoly[0]->pPoints[1].sLen = 0.377075;
  sforwardPoly[0]->pPoints[2].point.x = 23.0637;
  sforwardPoly[0]->pPoints[2].point.y = -1.33187;
  sforwardPoly[0]->pPoints[2].point.z = 1.05023;
  sforwardPoly[0]->pPoints[2].sLen = 0.0542556;
  sforwardPoly[0]->pPoints[3].point.x = 3.26128;
  sforwardPoly[0]->pPoints[3].point.y = -3.26234;
  sforwardPoly[0]->pPoints[3].point.z = -26.8779;
  sforwardPoly[0]->pPoints[3].sLen = 0.461116;
  sforwardPoly[1]->numPoints = 4;
  sforwardPoly[1]->rollAngle = 0;
  sforwardPoly[1]->stepLength = 75;
  sforwardPoly[1]->pPoints[0].point.x = -30.0931;
  sforwardPoly[1]->pPoints[0].point.y = -2.08297;
  sforwardPoly[1]->pPoints[0].point.z = -18.8519;
  sforwardPoly[1]->pPoints[0].sLen = 0.107553;
  sforwardPoly[1]->pPoints[1].point.x = -27.0353;
  sforwardPoly[1]->pPoints[1].point.y = -0.823497;
  sforwardPoly[1]->pPoints[1].point.z = -5.40966;
  sforwardPoly[1]->pPoints[1].sLen = 0.377075;
  sforwardPoly[1]->pPoints[2].point.x = 23.0637;
  sforwardPoly[1]->pPoints[2].point.y = 1.33187;
  sforwardPoly[1]->pPoints[2].point.z = 1.05023;
  sforwardPoly[1]->pPoints[2].sLen = 0.0542556;
  sforwardPoly[1]->pPoints[3].point.x = 3.26128;
  sforwardPoly[1]->pPoints[3].point.y = 3.26234;
  sforwardPoly[1]->pPoints[3].point.z = -26.8779;
  sforwardPoly[1]->pPoints[3].sLen = 0.461116;
  sforwardPoly[2]->numPoints = 4;
  sforwardPoly[2]->rollAngle = 0;
  sforwardPoly[2]->stepLength = 75;
  sforwardPoly[2]->pPoints[0].point.x = -23.7288;
  sforwardPoly[2]->pPoints[0].point.y = -9.6087;
  sforwardPoly[2]->pPoints[0].point.z = 3.06664;
  sforwardPoly[2]->pPoints[0].sLen = 0.0187699;
  sforwardPoly[2]->pPoints[1].point.x = -24.7325;
  sforwardPoly[2]->pPoints[1].point.y = -1.63567;
  sforwardPoly[2]->pPoints[1].point.z = 39.8304;
  sforwardPoly[2]->pPoints[1].sLen = 0.472549;
  sforwardPoly[2]->pPoints[2].point.x = 16.2482;
  sforwardPoly[2]->pPoints[2].point.y = -1.42779;
  sforwardPoly[2]->pPoints[2].point.z = 25.1732;
  sforwardPoly[2]->pPoints[2].sLen = 0.0312922;
  sforwardPoly[2]->pPoints[3].point.x = 23.5193;
  sforwardPoly[2]->pPoints[3].point.y = 3.41586;
  sforwardPoly[2]->pPoints[3].point.z = 3.02703;
  sforwardPoly[2]->pPoints[3].sLen = 0.477389;
  sforwardPoly[3]->numPoints = 4;
  sforwardPoly[3]->rollAngle = 0;
  sforwardPoly[3]->stepLength = 75;
  sforwardPoly[3]->pPoints[0].point.x = -23.7288;
  sforwardPoly[3]->pPoints[0].point.y = 9.6087;
  sforwardPoly[3]->pPoints[0].point.z = 3.06664;
  sforwardPoly[3]->pPoints[0].sLen = 0.0187699;
  sforwardPoly[3]->pPoints[1].point.x = -24.7325;
  sforwardPoly[3]->pPoints[1].point.y = 1.63567;
  sforwardPoly[3]->pPoints[1].point.z = 39.8304;
  sforwardPoly[3]->pPoints[1].sLen = 0.472549;
  sforwardPoly[3]->pPoints[2].point.x = 16.2482;
  sforwardPoly[3]->pPoints[2].point.y = 1.42779;
  sforwardPoly[3]->pPoints[2].point.z = 25.1732;
  sforwardPoly[3]->pPoints[2].sLen = 0.0312922;
  sforwardPoly[3]->pPoints[3].point.x = 23.5193;
  sforwardPoly[3]->pPoints[3].point.y = -3.41586;
  sforwardPoly[3]->pPoints[3].point.z = 3.02703;
  sforwardPoly[3]->pPoints[3].sLen = 0.477389;
  sforwardPoly[0]->finalize();
  sforwardPoly[1]->finalize();
  sforwardPoly[2]->finalize();
  sforwardPoly[3]->finalize();

  //dforwardPoly parameters
  dforwardPoly[0]->numPoints = 4;
  dforwardPoly[0]->rollAngle = 0;
  dforwardPoly[0]->stepLength = 51;
  dforwardPoly[0]->pPoints[0].point.x = -29.9474;
  dforwardPoly[0]->pPoints[0].point.y = -3.27726;
  dforwardPoly[0]->pPoints[0].point.z = -15.525;
  dforwardPoly[0]->pPoints[0].sLen = -0.039293;
  dforwardPoly[0]->pPoints[1].point.x = -27.6353;
  dforwardPoly[0]->pPoints[1].point.y = -2.28411;
  dforwardPoly[0]->pPoints[1].point.z = -2.09947;
  dforwardPoly[0]->pPoints[1].sLen = 0.273697;
  dforwardPoly[0]->pPoints[2].point.x = 5.70906;
  dforwardPoly[0]->pPoints[2].point.y = -9.44559;
  dforwardPoly[0]->pPoints[2].point.z = 12.5893;
  dforwardPoly[0]->pPoints[2].sLen = 0.240248;
  dforwardPoly[0]->pPoints[3].point.x = 25.9895;
  dforwardPoly[0]->pPoints[3].point.y = 2.41216;
  dforwardPoly[0]->pPoints[3].point.z = 3.08891;
  dforwardPoly[0]->pPoints[3].sLen = 0.525348;
  dforwardPoly[1]->numPoints = 4;
  dforwardPoly[1]->rollAngle = 0;
  dforwardPoly[1]->stepLength = 51;
  dforwardPoly[1]->pPoints[0].point.x = -29.9474;
  dforwardPoly[1]->pPoints[0].point.y = 3.27726;
  dforwardPoly[1]->pPoints[0].point.z = -15.525;
  dforwardPoly[1]->pPoints[0].sLen = -0.039293;
  dforwardPoly[1]->pPoints[1].point.x = -27.6353;
  dforwardPoly[1]->pPoints[1].point.y = 2.28411;
  dforwardPoly[1]->pPoints[1].point.z = -2.09947;
  dforwardPoly[1]->pPoints[1].sLen = 0.273697;
  dforwardPoly[1]->pPoints[2].point.x = 5.70906;
  dforwardPoly[1]->pPoints[2].point.y = 9.44559;
  dforwardPoly[1]->pPoints[2].point.z = 12.5893;
  dforwardPoly[1]->pPoints[2].sLen = 0.240248;
  dforwardPoly[1]->pPoints[3].point.x = 25.9895;
  dforwardPoly[1]->pPoints[3].point.y = -2.41216;
  dforwardPoly[1]->pPoints[3].point.z = 3.08891;
  dforwardPoly[1]->pPoints[3].sLen = 0.525348;
  dforwardPoly[2]->numPoints = 4;
  dforwardPoly[2]->rollAngle = 0;
  dforwardPoly[2]->stepLength = 51;
  dforwardPoly[2]->pPoints[0].point.x = -33.7782;
  dforwardPoly[2]->pPoints[0].point.y = 0.304222;
  dforwardPoly[2]->pPoints[0].point.z = -7.59602;
  dforwardPoly[2]->pPoints[0].sLen = 0.0432646;
  dforwardPoly[2]->pPoints[1].point.x = -37.4151;
  dforwardPoly[2]->pPoints[1].point.y = 0.0659411;
  dforwardPoly[2]->pPoints[1].point.z = 13.7733;
  dforwardPoly[2]->pPoints[1].sLen = 0.40272;
  dforwardPoly[2]->pPoints[2].point.x = 13.946;
  dforwardPoly[2]->pPoints[2].point.y = -11.4001;
  dforwardPoly[2]->pPoints[2].point.z = 25.7176;
  dforwardPoly[2]->pPoints[2].sLen = 0.0602455;
  dforwardPoly[2]->pPoints[3].point.x = 31.9201;
  dforwardPoly[2]->pPoints[3].point.y = -9.57183;
  dforwardPoly[2]->pPoints[3].point.z = -1.83454;
  dforwardPoly[2]->pPoints[3].sLen = 0.49377;
  dforwardPoly[3]->numPoints = 4;
  dforwardPoly[3]->rollAngle = 0;
  dforwardPoly[3]->stepLength = 51;
  dforwardPoly[3]->pPoints[0].point.x = -33.7782;
  dforwardPoly[3]->pPoints[0].point.y = -0.304222;
  dforwardPoly[3]->pPoints[0].point.z = -7.59602;
  dforwardPoly[3]->pPoints[0].sLen = 0.0432646;
  dforwardPoly[3]->pPoints[1].point.x = -37.4151;
  dforwardPoly[3]->pPoints[1].point.y = -0.0659411;
  dforwardPoly[3]->pPoints[1].point.z = 13.7733;
  dforwardPoly[3]->pPoints[1].sLen = 0.40272;
  dforwardPoly[3]->pPoints[2].point.x = 13.946;
  dforwardPoly[3]->pPoints[2].point.y = 11.4001;
  dforwardPoly[3]->pPoints[2].point.z = 25.7176;
  dforwardPoly[3]->pPoints[2].sLen = 0.0602455;
  dforwardPoly[3]->pPoints[3].point.x = 31.9201;
  dforwardPoly[3]->pPoints[3].point.y = 9.57183;
  dforwardPoly[3]->pPoints[3].point.z = -1.83454;
  dforwardPoly[3]->pPoints[3].sLen = 0.49377;
  dforwardPoly[0]->finalize();
  dforwardPoly[1]->finalize();
  dforwardPoly[2]->finalize();
  dforwardPoly[3]->finalize();

  //backwardPoly parameters
  backwardPoly[0]->numPoints = 4;
  backwardPoly[0]->rollAngle = 0;
  backwardPoly[0]->stepLength = 53;
  backwardPoly[0]->pPoints[0].point.x = -37.4699;
  backwardPoly[0]->pPoints[0].point.y = -6.69303;
  backwardPoly[0]->pPoints[0].point.z = 11.2389;
  backwardPoly[0]->pPoints[0].sLen = 0.209155;
  backwardPoly[0]->pPoints[1].point.x = -13.7743;
  backwardPoly[0]->pPoints[1].point.y = 33.8323;
  backwardPoly[0]->pPoints[1].point.z = 3.96052;
  backwardPoly[0]->pPoints[1].sLen = 0.326859;
  backwardPoly[0]->pPoints[2].point.x = 33.4119;
  backwardPoly[0]->pPoints[2].point.y = -6.3983;
  backwardPoly[0]->pPoints[2].point.z = 13.7076;
  backwardPoly[0]->pPoints[2].sLen = 0.0860842;
  backwardPoly[0]->pPoints[3].point.x = 19.7998;
  backwardPoly[0]->pPoints[3].point.y = -6.85233;
  backwardPoly[0]->pPoints[3].point.z = 2.52472;
  backwardPoly[0]->pPoints[3].sLen = 0.377902;
  backwardPoly[1]->numPoints = 4;
  backwardPoly[1]->rollAngle = 0;
  backwardPoly[1]->stepLength = 53;
  backwardPoly[1]->pPoints[0].point.x = -37.4699;
  backwardPoly[1]->pPoints[0].point.y = 6.69303;
  backwardPoly[1]->pPoints[0].point.z = 11.2389;
  backwardPoly[1]->pPoints[0].sLen = 0.209155;
  backwardPoly[1]->pPoints[1].point.x = -13.7743;
  backwardPoly[1]->pPoints[1].point.y = -33.8323;
  backwardPoly[1]->pPoints[1].point.z = 3.96052;
  backwardPoly[1]->pPoints[1].sLen = 0.326859;
  backwardPoly[1]->pPoints[2].point.x = 33.4119;
  backwardPoly[1]->pPoints[2].point.y = 6.3983;
  backwardPoly[1]->pPoints[2].point.z = 13.7076;
  backwardPoly[1]->pPoints[2].sLen = 0.0860842;
  backwardPoly[1]->pPoints[3].point.x = 19.7998;
  backwardPoly[1]->pPoints[3].point.y = 6.85233;
  backwardPoly[1]->pPoints[3].point.z = 2.52472;
  backwardPoly[1]->pPoints[3].sLen = 0.377902;
  backwardPoly[2]->numPoints = 4;
  backwardPoly[2]->rollAngle = 0;
  backwardPoly[2]->stepLength = 53;
  backwardPoly[2]->pPoints[0].point.x = -35.1536;
  backwardPoly[2]->pPoints[0].point.y = 33.4067;
  backwardPoly[2]->pPoints[0].point.z = -12.6396;
  backwardPoly[2]->pPoints[0].sLen = 0.172122;
  backwardPoly[2]->pPoints[1].point.x = -11.4648;
  backwardPoly[2]->pPoints[1].point.y = -5.42247;
  backwardPoly[2]->pPoints[1].point.z = 21.2323;
  backwardPoly[2]->pPoints[1].sLen = 0.317296;
  backwardPoly[2]->pPoints[2].point.x = 32.6411;
  backwardPoly[2]->pPoints[2].point.y = 3.09892;
  backwardPoly[2]->pPoints[2].point.z = 38.4976;
  backwardPoly[2]->pPoints[2].sLen = -0.132091;
  backwardPoly[2]->pPoints[3].point.x = 30.5193;
  backwardPoly[2]->pPoints[3].point.y = 4.43412;
  backwardPoly[2]->pPoints[3].point.z = 9.44578;
  backwardPoly[2]->pPoints[3].sLen = 0.642673;
  backwardPoly[3]->numPoints = 4;
  backwardPoly[3]->rollAngle = 0;
  backwardPoly[3]->stepLength = 53;
  backwardPoly[3]->pPoints[0].point.x = -35.1536;
  backwardPoly[3]->pPoints[0].point.y = -33.4067;
  backwardPoly[3]->pPoints[0].point.z = -12.6396;
  backwardPoly[3]->pPoints[0].sLen = 0.172122;
  backwardPoly[3]->pPoints[1].point.x = -11.4648;
  backwardPoly[3]->pPoints[1].point.y = 5.42247;
  backwardPoly[3]->pPoints[1].point.z = 21.2323;
  backwardPoly[3]->pPoints[1].sLen = 0.317296;
  backwardPoly[3]->pPoints[2].point.x = 32.6411;
  backwardPoly[3]->pPoints[2].point.y = -3.09892;
  backwardPoly[3]->pPoints[2].point.z = 38.4976;
  backwardPoly[3]->pPoints[2].sLen = -0.132091;
  backwardPoly[3]->pPoints[3].point.x = 30.5193;
  backwardPoly[3]->pPoints[3].point.y = -4.43412;
  backwardPoly[3]->pPoints[3].point.z = 9.44578;
  backwardPoly[3]->pPoints[3].sLen = 0.642673;
  backwardPoly[0]->finalize();
  backwardPoly[1]->finalize();
  backwardPoly[2]->finalize();
  backwardPoly[3]->finalize();

  //sbackwardPoly parameters
  sbackwardPoly[0]->numPoints = 4;
  sbackwardPoly[0]->rollAngle = 0;
  sbackwardPoly[0]->stepLength = 61;
  sbackwardPoly[0]->pPoints[0].point.x = -33.7375;
  sbackwardPoly[0]->pPoints[0].point.y = -6.70116;
  sbackwardPoly[0]->pPoints[0].point.z = 10.663;
  sbackwardPoly[0]->pPoints[0].sLen = 0.160136;
  sbackwardPoly[0]->pPoints[1].point.x = -21.0508;
  sbackwardPoly[0]->pPoints[1].point.y = 17.7523;
  sbackwardPoly[0]->pPoints[1].point.z = 8.24884;
  sbackwardPoly[0]->pPoints[1].sLen = 0.297181;
  sbackwardPoly[0]->pPoints[2].point.x = 35.8702;
  sbackwardPoly[0]->pPoints[2].point.y = -7.50427;
  sbackwardPoly[0]->pPoints[2].point.z = 25.9035;
  sbackwardPoly[0]->pPoints[2].sLen = 0.0713092;
  sbackwardPoly[0]->pPoints[3].point.x = 21.3798;
  sbackwardPoly[0]->pPoints[3].point.y = -4.7102;
  sbackwardPoly[0]->pPoints[3].point.z = 0.83056;
  sbackwardPoly[0]->pPoints[3].sLen = 0.471374;
  sbackwardPoly[1]->numPoints = 4;
  sbackwardPoly[1]->rollAngle = 0;
  sbackwardPoly[1]->stepLength = 61;
  sbackwardPoly[1]->pPoints[0].point.x = -33.7375;
  sbackwardPoly[1]->pPoints[0].point.y = 6.70116;
  sbackwardPoly[1]->pPoints[0].point.z = 10.663;
  sbackwardPoly[1]->pPoints[0].sLen = 0.160136;
  sbackwardPoly[1]->pPoints[1].point.x = -21.0508;
  sbackwardPoly[1]->pPoints[1].point.y = -17.7523;
  sbackwardPoly[1]->pPoints[1].point.z = 8.24884;
  sbackwardPoly[1]->pPoints[1].sLen = 0.297181;
  sbackwardPoly[1]->pPoints[2].point.x = 35.8702;
  sbackwardPoly[1]->pPoints[2].point.y = 7.50427;
  sbackwardPoly[1]->pPoints[2].point.z = 25.9035;
  sbackwardPoly[1]->pPoints[2].sLen = 0.0713092;
  sbackwardPoly[1]->pPoints[3].point.x = 21.3798;
  sbackwardPoly[1]->pPoints[3].point.y = 4.7102;
  sbackwardPoly[1]->pPoints[3].point.z = 0.83056;
  sbackwardPoly[1]->pPoints[3].sLen = 0.471374;
  sbackwardPoly[2]->numPoints = 4;
  sbackwardPoly[2]->rollAngle = 0;
  sbackwardPoly[2]->stepLength = 61;
  sbackwardPoly[2]->pPoints[0].point.x = -19.6924;
  sbackwardPoly[2]->pPoints[0].point.y = 5.31819;
  sbackwardPoly[2]->pPoints[0].point.z = -10.9156;
  sbackwardPoly[2]->pPoints[0].sLen = 0.102415;
  sbackwardPoly[2]->pPoints[1].point.x = -4.86939;
  sbackwardPoly[2]->pPoints[1].point.y = -4.86016;
  sbackwardPoly[2]->pPoints[1].point.z = 10.6604;
  sbackwardPoly[2]->pPoints[1].sLen = 0.324365;
  sbackwardPoly[2]->pPoints[2].point.x = 37.444;
  sbackwardPoly[2]->pPoints[2].point.y = 7.2039;
  sbackwardPoly[2]->pPoints[2].point.z = 45.862;
  sbackwardPoly[2]->pPoints[2].sLen = -0.104819;
  sbackwardPoly[2]->pPoints[3].point.x = 29.7827;
  sbackwardPoly[2]->pPoints[3].point.y = 0.697765;
  sbackwardPoly[2]->pPoints[3].point.z = 19.9519;
  sbackwardPoly[2]->pPoints[3].sLen = 0.678039;
  sbackwardPoly[3]->numPoints = 4;
  sbackwardPoly[3]->rollAngle = 0;
  sbackwardPoly[3]->stepLength = 61;
  sbackwardPoly[3]->pPoints[0].point.x = -19.6924;
  sbackwardPoly[3]->pPoints[0].point.y = -5.31819;
  sbackwardPoly[3]->pPoints[0].point.z = -10.9156;
  sbackwardPoly[3]->pPoints[0].sLen = 0.102415;
  sbackwardPoly[3]->pPoints[1].point.x = -4.86939;
  sbackwardPoly[3]->pPoints[1].point.y = 4.86016;
  sbackwardPoly[3]->pPoints[1].point.z = 10.6604;
  sbackwardPoly[3]->pPoints[1].sLen = 0.324365;
  sbackwardPoly[3]->pPoints[2].point.x = 37.444;
  sbackwardPoly[3]->pPoints[2].point.y = -7.2039;
  sbackwardPoly[3]->pPoints[2].point.z = 45.862;
  sbackwardPoly[3]->pPoints[2].sLen = -0.104819;
  sbackwardPoly[3]->pPoints[3].point.x = 29.7827;
  sbackwardPoly[3]->pPoints[3].point.y = -0.697765;
  sbackwardPoly[3]->pPoints[3].point.z = 19.9519;
  sbackwardPoly[3]->pPoints[3].sLen = 0.678039;
  sbackwardPoly[0]->finalize();
  sbackwardPoly[1]->finalize();
  sbackwardPoly[2]->finalize();
  sbackwardPoly[3]->finalize();

  //dbackwardPoly parameters
  dbackwardPoly[0]->numPoints = 4;
  dbackwardPoly[0]->rollAngle = 0;
  dbackwardPoly[0]->stepLength = 52;
  dbackwardPoly[0]->pPoints[0].point.x = -33.5813;
  dbackwardPoly[0]->pPoints[0].point.y = -0.1943;
  dbackwardPoly[0]->pPoints[0].point.z = 7.56177;
  dbackwardPoly[0]->pPoints[0].sLen = -0.00454608;
  dbackwardPoly[0]->pPoints[1].point.x = -15.7947;
  dbackwardPoly[0]->pPoints[1].point.y = 21.9034;
  dbackwardPoly[0]->pPoints[1].point.z = 0.786908;
  dbackwardPoly[0]->pPoints[1].sLen = 0.364702;
  dbackwardPoly[0]->pPoints[2].point.x = 38.1074;
  dbackwardPoly[0]->pPoints[2].point.y = -4.01859;
  dbackwardPoly[0]->pPoints[2].point.z = 21.7376;
  dbackwardPoly[0]->pPoints[2].sLen = 0.187669;
  dbackwardPoly[0]->pPoints[3].point.x = 24.8112;
  dbackwardPoly[0]->pPoints[3].point.y = 2.65664;
  dbackwardPoly[0]->pPoints[3].point.z = 10.205;
  dbackwardPoly[0]->pPoints[3].sLen = 0.452174;
  dbackwardPoly[1]->numPoints = 4;
  dbackwardPoly[1]->rollAngle = 0;
  dbackwardPoly[1]->stepLength = 52;
  dbackwardPoly[1]->pPoints[0].point.x = -33.5813;
  dbackwardPoly[1]->pPoints[0].point.y = 0.1943;
  dbackwardPoly[1]->pPoints[0].point.z = 7.56177;
  dbackwardPoly[1]->pPoints[0].sLen = -0.00454608;
  dbackwardPoly[1]->pPoints[1].point.x = -15.7947;
  dbackwardPoly[1]->pPoints[1].point.y = -21.9034;
  dbackwardPoly[1]->pPoints[1].point.z = 0.786908;
  dbackwardPoly[1]->pPoints[1].sLen = 0.364702;
  dbackwardPoly[1]->pPoints[2].point.x = 38.1074;
  dbackwardPoly[1]->pPoints[2].point.y = 4.01859;
  dbackwardPoly[1]->pPoints[2].point.z = 21.7376;
  dbackwardPoly[1]->pPoints[2].sLen = 0.187669;
  dbackwardPoly[1]->pPoints[3].point.x = 24.8112;
  dbackwardPoly[1]->pPoints[3].point.y = -2.65664;
  dbackwardPoly[1]->pPoints[3].point.z = 10.205;
  dbackwardPoly[1]->pPoints[3].sLen = 0.452174;
  dbackwardPoly[2]->numPoints = 4;
  dbackwardPoly[2]->rollAngle = 0;
  dbackwardPoly[2]->stepLength = 52;
  dbackwardPoly[2]->pPoints[0].point.x = -27.5297;
  dbackwardPoly[2]->pPoints[0].point.y = 11.0379;
  dbackwardPoly[2]->pPoints[0].point.z = -6.243;
  dbackwardPoly[2]->pPoints[0].sLen = 0.152211;
  dbackwardPoly[2]->pPoints[1].point.x = -2.84302;
  dbackwardPoly[2]->pPoints[1].point.y = -4.34425;
  dbackwardPoly[2]->pPoints[1].point.z = 11.8847;
  dbackwardPoly[2]->pPoints[1].sLen = 0.265556;
  dbackwardPoly[2]->pPoints[2].point.x = 41.8341;
  dbackwardPoly[2]->pPoints[2].point.y = 10.6103;
  dbackwardPoly[2]->pPoints[2].point.z = 46.5946;
  dbackwardPoly[2]->pPoints[2].sLen = 0.0330728;
  dbackwardPoly[2]->pPoints[3].point.x = 34.0414;
  dbackwardPoly[2]->pPoints[3].point.y = -0.346163;
  dbackwardPoly[2]->pPoints[3].point.z = 16.6259;
  dbackwardPoly[2]->pPoints[3].sLen = 0.549161;
  dbackwardPoly[3]->numPoints = 4;
  dbackwardPoly[3]->rollAngle = 0;
  dbackwardPoly[3]->stepLength = 52;
  dbackwardPoly[3]->pPoints[0].point.x = -27.5297;
  dbackwardPoly[3]->pPoints[0].point.y = -11.0379;
  dbackwardPoly[3]->pPoints[0].point.z = -6.243;
  dbackwardPoly[3]->pPoints[0].sLen = 0.152211;
  dbackwardPoly[3]->pPoints[1].point.x = -2.84302;
  dbackwardPoly[3]->pPoints[1].point.y = 4.34425;
  dbackwardPoly[3]->pPoints[1].point.z = 11.8847;
  dbackwardPoly[3]->pPoints[1].sLen = 0.265556;
  dbackwardPoly[3]->pPoints[2].point.x = 41.8341;
  dbackwardPoly[3]->pPoints[2].point.y = -10.6103;
  dbackwardPoly[3]->pPoints[2].point.z = 46.5946;
  dbackwardPoly[3]->pPoints[2].sLen = 0.0330728;
  dbackwardPoly[3]->pPoints[3].point.x = 34.0414;
  dbackwardPoly[3]->pPoints[3].point.y = 0.346163;
  dbackwardPoly[3]->pPoints[3].point.z = 16.6259;
  dbackwardPoly[3]->pPoints[3].sLen = 0.549161;
  dbackwardPoly[0]->finalize();
  dbackwardPoly[1]->finalize();
  dbackwardPoly[2]->finalize();
  dbackwardPoly[3]->finalize();

  //sidewardsPoly parameters
  sidewardsPoly[0]->numPoints = 4;
  sidewardsPoly[0]->rollAngle = 0;
  sidewardsPoly[0]->stepLength = 56;
  sidewardsPoly[0]->pPoints[0].point.x = -15.3573;
  sidewardsPoly[0]->pPoints[0].point.y = -7.10737;
  sidewardsPoly[0]->pPoints[0].point.z = -11.1891;
  sidewardsPoly[0]->pPoints[0].sLen = 0.0420935;
  sidewardsPoly[0]->pPoints[1].point.x = -9.09527;
  sidewardsPoly[0]->pPoints[1].point.y = 3.34993;
  sidewardsPoly[0]->pPoints[1].point.z = -4.0212;
  sidewardsPoly[0]->pPoints[1].sLen = 0.462489;
  sidewardsPoly[0]->pPoints[2].point.x = 22.3719;
  sidewardsPoly[0]->pPoints[2].point.y = -4.84692;
  sidewardsPoly[0]->pPoints[2].point.z = 8.82873;
  sidewardsPoly[0]->pPoints[2].sLen = -0.0516716;
  sidewardsPoly[0]->pPoints[3].point.x = 20.7936;
  sidewardsPoly[0]->pPoints[3].point.y = 22.176;
  sidewardsPoly[0]->pPoints[3].point.z = 3.78541;
  sidewardsPoly[0]->pPoints[3].sLen = 0.547089;
  sidewardsPoly[1]->numPoints = 4;
  sidewardsPoly[1]->rollAngle = 0;
  sidewardsPoly[1]->stepLength = 56;
  sidewardsPoly[1]->pPoints[0].point.x = -15.3573;
  sidewardsPoly[1]->pPoints[0].point.y = 7.10737;
  sidewardsPoly[1]->pPoints[0].point.z = -11.1891;
  sidewardsPoly[1]->pPoints[0].sLen = 0.0420935;
  sidewardsPoly[1]->pPoints[1].point.x = -9.09527;
  sidewardsPoly[1]->pPoints[1].point.y = -3.34993;
  sidewardsPoly[1]->pPoints[1].point.z = -4.0212;
  sidewardsPoly[1]->pPoints[1].sLen = 0.462489;
  sidewardsPoly[1]->pPoints[2].point.x = 22.3719;
  sidewardsPoly[1]->pPoints[2].point.y = 4.84692;
  sidewardsPoly[1]->pPoints[2].point.z = 8.82873;
  sidewardsPoly[1]->pPoints[2].sLen = -0.0516716;
  sidewardsPoly[1]->pPoints[3].point.x = 20.7936;
  sidewardsPoly[1]->pPoints[3].point.y = -22.176;
  sidewardsPoly[1]->pPoints[3].point.z = 3.78541;
  sidewardsPoly[1]->pPoints[3].sLen = 0.547089;
  sidewardsPoly[2]->numPoints = 4;
  sidewardsPoly[2]->rollAngle = 0;
  sidewardsPoly[2]->stepLength = 56;
  sidewardsPoly[2]->pPoints[0].point.x = -25.3751;
  sidewardsPoly[2]->pPoints[0].point.y = -14.5053;
  sidewardsPoly[2]->pPoints[0].point.z = -0.76603;
  sidewardsPoly[2]->pPoints[0].sLen = -0.0382207;
  sidewardsPoly[2]->pPoints[1].point.x = -24.1757;
  sidewardsPoly[2]->pPoints[1].point.y = 1.42974;
  sidewardsPoly[2]->pPoints[1].point.z = 28.0637;
  sidewardsPoly[2]->pPoints[1].sLen = 0.406898;
  sidewardsPoly[2]->pPoints[2].point.x = 7.94299;
  sidewardsPoly[2]->pPoints[2].point.y = -5.45393;
  sidewardsPoly[2]->pPoints[2].point.z = 26.9968;
  sidewardsPoly[2]->pPoints[2].sLen = 0.115272;
  sidewardsPoly[2]->pPoints[3].point.x = 3.20244;
  sidewardsPoly[2]->pPoints[3].point.y = -19.7213;
  sidewardsPoly[2]->pPoints[3].point.z = 1.12852;
  sidewardsPoly[2]->pPoints[3].sLen = 0.516051;
  sidewardsPoly[3]->numPoints = 4;
  sidewardsPoly[3]->rollAngle = 0;
  sidewardsPoly[3]->stepLength = 56;
  sidewardsPoly[3]->pPoints[0].point.x = -25.3751;
  sidewardsPoly[3]->pPoints[0].point.y = 14.5053;
  sidewardsPoly[3]->pPoints[0].point.z = -0.76603;
  sidewardsPoly[3]->pPoints[0].sLen = -0.0382207;
  sidewardsPoly[3]->pPoints[1].point.x = -24.1757;
  sidewardsPoly[3]->pPoints[1].point.y = -1.42974;
  sidewardsPoly[3]->pPoints[1].point.z = 28.0637;
  sidewardsPoly[3]->pPoints[1].sLen = 0.406898;
  sidewardsPoly[3]->pPoints[2].point.x = 7.94299;
  sidewardsPoly[3]->pPoints[2].point.y = 5.45393;
  sidewardsPoly[3]->pPoints[2].point.z = 26.9968;
  sidewardsPoly[3]->pPoints[2].sLen = 0.115272;
  sidewardsPoly[3]->pPoints[3].point.x = 3.20244;
  sidewardsPoly[3]->pPoints[3].point.y = 19.7213;
  sidewardsPoly[3]->pPoints[3].point.z = 1.12852;
  sidewardsPoly[3]->pPoints[3].sLen = 0.516051;
  sidewardsPoly[0]->finalize();
  sidewardsPoly[1]->finalize();
  sidewardsPoly[2]->finalize();
  sidewardsPoly[3]->finalize();

  //boostPoly parameters
  boostPoly[0]->numPoints = 4;
  boostPoly[0]->rollAngle = 0;
  boostPoly[0]->stepLength = 58;
  boostPoly[0]->pPoints[0].point.x = -54.1131;
  boostPoly[0]->pPoints[0].point.y = 3.85926;
  boostPoly[0]->pPoints[0].point.z = -49.6204;
  boostPoly[0]->pPoints[0].sLen = 0.0817619;
  boostPoly[0]->pPoints[1].point.x = -40.627;
  boostPoly[0]->pPoints[1].point.y = 8.24308;
  boostPoly[0]->pPoints[1].point.z = -23.3423;
  boostPoly[0]->pPoints[1].sLen = 0.288787;
  boostPoly[0]->pPoints[2].point.x = 5.69701;
  boostPoly[0]->pPoints[2].point.y = 26.2382;
  boostPoly[0]->pPoints[2].point.z = -11.3584;
  boostPoly[0]->pPoints[2].sLen = 0.0623604;
  boostPoly[0]->pPoints[3].point.x = -0.820145;
  boostPoly[0]->pPoints[3].point.y = 5.34335;
  boostPoly[0]->pPoints[3].point.z = -57.6659;
  boostPoly[0]->pPoints[3].sLen = 0.567091;
  boostPoly[1]->numPoints = 4;
  boostPoly[1]->rollAngle = 0;
  boostPoly[1]->stepLength = 58;
  boostPoly[1]->pPoints[0].point.x = -54.1131;
  boostPoly[1]->pPoints[0].point.y = -3.85926;
  boostPoly[1]->pPoints[0].point.z = -49.6204;
  boostPoly[1]->pPoints[0].sLen = 0.0817619;
  boostPoly[1]->pPoints[1].point.x = -40.627;
  boostPoly[1]->pPoints[1].point.y = -8.24308;
  boostPoly[1]->pPoints[1].point.z = -23.3423;
  boostPoly[1]->pPoints[1].sLen = 0.288787;
  boostPoly[1]->pPoints[2].point.x = 5.69701;
  boostPoly[1]->pPoints[2].point.y = -26.2382;
  boostPoly[1]->pPoints[2].point.z = -11.3584;
  boostPoly[1]->pPoints[2].sLen = 0.0623604;
  boostPoly[1]->pPoints[3].point.x = -0.820145;
  boostPoly[1]->pPoints[3].point.y = -5.34335;
  boostPoly[1]->pPoints[3].point.z = -57.6659;
  boostPoly[1]->pPoints[3].sLen = 0.567091;
  boostPoly[2]->numPoints = 4;
  boostPoly[2]->rollAngle = 0;
  boostPoly[2]->stepLength = 58;
  boostPoly[2]->pPoints[0].point.x = -26.0478;
  boostPoly[2]->pPoints[0].point.y = -7.82422;
  boostPoly[2]->pPoints[0].point.z = -16.88;
  boostPoly[2]->pPoints[0].sLen = 0.0654221;
  boostPoly[2]->pPoints[1].point.x = -27.2295;
  boostPoly[2]->pPoints[1].point.y = 7.91244;
  boostPoly[2]->pPoints[1].point.z = -22.6058;
  boostPoly[2]->pPoints[1].sLen = 0.39671;
  boostPoly[2]->pPoints[2].point.x = 31.8959;
  boostPoly[2]->pPoints[2].point.y = -1.54275;
  boostPoly[2]->pPoints[2].point.z = -11.4453;
  boostPoly[2]->pPoints[2].sLen = -0.0241281;
  boostPoly[2]->pPoints[3].point.x = 37.7492;
  boostPoly[2]->pPoints[3].point.y = 38.3235;
  boostPoly[2]->pPoints[3].point.z = -25.741;
  boostPoly[2]->pPoints[3].sLen = 0.561996;
  boostPoly[3]->numPoints = 4;
  boostPoly[3]->rollAngle = 0;
  boostPoly[3]->stepLength = 58;
  boostPoly[3]->pPoints[0].point.x = -26.0478;
  boostPoly[3]->pPoints[0].point.y = 7.82422;
  boostPoly[3]->pPoints[0].point.z = -16.88;
  boostPoly[3]->pPoints[0].sLen = 0.0654221;
  boostPoly[3]->pPoints[1].point.x = -27.2295;
  boostPoly[3]->pPoints[1].point.y = -7.91244;
  boostPoly[3]->pPoints[1].point.z = -22.6058;
  boostPoly[3]->pPoints[1].sLen = 0.39671;
  boostPoly[3]->pPoints[2].point.x = 31.8959;
  boostPoly[3]->pPoints[2].point.y = 1.54275;
  boostPoly[3]->pPoints[2].point.z = -11.4453;
  boostPoly[3]->pPoints[2].sLen = -0.0241281;
  boostPoly[3]->pPoints[3].point.x = 37.7492;
  boostPoly[3]->pPoints[3].point.y = -38.3235;
  boostPoly[3]->pPoints[3].point.z = -25.741;
  boostPoly[3]->pPoints[3].sLen = 0.561996;
  boostPoly[0]->finalize();
  boostPoly[1]->finalize();
  boostPoly[2]->finalize();
  boostPoly[3]->finalize();

  //grabPoly parameters
  grabPoly[0]->numPoints = 4;
  grabPoly[0]->rollAngle = 0;
  grabPoly[0]->stepLength = 42;
  grabPoly[0]->pPoints[0].point.x = 2.5;
  grabPoly[0]->pPoints[0].point.y = 0;
  grabPoly[0]->pPoints[0].point.z = -25;
  grabPoly[0]->pPoints[0].sLen = 0.06;
  grabPoly[0]->pPoints[1].point.x = 2.5;
  grabPoly[0]->pPoints[1].point.y = 0;
  grabPoly[0]->pPoints[1].point.z = -10;
  grabPoly[0]->pPoints[1].sLen = 0.39;
  grabPoly[0]->pPoints[2].point.x = 12.5;
  grabPoly[0]->pPoints[2].point.y = 0;
  grabPoly[0]->pPoints[2].point.z = -10;
  grabPoly[0]->pPoints[2].sLen = 0.06;
  grabPoly[0]->pPoints[3].point.x = 12.5;
  grabPoly[0]->pPoints[3].point.y = 0;
  grabPoly[0]->pPoints[3].point.z = -25;
  grabPoly[0]->pPoints[3].sLen = 0.49;
  grabPoly[1]->numPoints = 4;
  grabPoly[1]->rollAngle = 0;
  grabPoly[1]->stepLength = 42;
  grabPoly[1]->pPoints[0].point.x = 2.5;
  grabPoly[1]->pPoints[0].point.y = 0;
  grabPoly[1]->pPoints[0].point.z = -25;
  grabPoly[1]->pPoints[0].sLen = 0.06;
  grabPoly[1]->pPoints[1].point.x = 2.5;
  grabPoly[1]->pPoints[1].point.y = 0;
  grabPoly[1]->pPoints[1].point.z = -10;
  grabPoly[1]->pPoints[1].sLen = 0.39;
  grabPoly[1]->pPoints[2].point.x = 12.5;
  grabPoly[1]->pPoints[2].point.y = 0;
  grabPoly[1]->pPoints[2].point.z = -10;
  grabPoly[1]->pPoints[2].sLen = 0.06;
  grabPoly[1]->pPoints[3].point.x = 12.5;
  grabPoly[1]->pPoints[3].point.y = 0;
  grabPoly[1]->pPoints[3].point.z = -25;
  grabPoly[1]->pPoints[3].sLen = 0.49;
  grabPoly[2]->numPoints = 4;
  grabPoly[2]->rollAngle = 0;
  grabPoly[2]->stepLength = 42;
  grabPoly[2]->pPoints[0].point.x = -25;
  grabPoly[2]->pPoints[0].point.y = 0;
  grabPoly[2]->pPoints[0].point.z = 0;
  grabPoly[2]->pPoints[0].sLen = 0.06;
  grabPoly[2]->pPoints[1].point.x = -25;
  grabPoly[2]->pPoints[1].point.y = 0;
  grabPoly[2]->pPoints[1].point.z = 25;
  grabPoly[2]->pPoints[1].sLen = 0.39;
  grabPoly[2]->pPoints[2].point.x = 25;
  grabPoly[2]->pPoints[2].point.y = 0;
  grabPoly[2]->pPoints[2].point.z = 25;
  grabPoly[2]->pPoints[2].sLen = 0.06;
  grabPoly[2]->pPoints[3].point.x = 25;
  grabPoly[2]->pPoints[3].point.y = 0;
  grabPoly[2]->pPoints[3].point.z = 0;
  grabPoly[2]->pPoints[3].sLen = 0.49;
  grabPoly[3]->numPoints = 4;
  grabPoly[3]->rollAngle = 0;
  grabPoly[3]->stepLength = 42;
  grabPoly[3]->pPoints[0].point.x = -25;
  grabPoly[3]->pPoints[0].point.y = 0;
  grabPoly[3]->pPoints[0].point.z = 0;
  grabPoly[3]->pPoints[0].sLen = 0.06;
  grabPoly[3]->pPoints[1].point.x = -25;
  grabPoly[3]->pPoints[1].point.y = 0;
  grabPoly[3]->pPoints[1].point.z = 25;
  grabPoly[3]->pPoints[1].sLen = 0.39;
  grabPoly[3]->pPoints[2].point.x = 25;
  grabPoly[3]->pPoints[2].point.y = 0;
  grabPoly[3]->pPoints[2].point.z = 25;
  grabPoly[3]->pPoints[2].sLen = 0.06;
  grabPoly[3]->pPoints[3].point.x = 25;
  grabPoly[3]->pPoints[3].point.y = 0;
  grabPoly[3]->pPoints[3].point.z = 0;
  grabPoly[3]->pPoints[3].sLen = 0.49;
  grabPoly[0]->finalize();
  grabPoly[1]->finalize();
  grabPoly[2]->finalize();
  grabPoly[3]->finalize();

  //rgrabPoly parameters
  rgrabPoly[0]->numPoints = 4;
  rgrabPoly[0]->rollAngle = 0;
  rgrabPoly[0]->stepLength = 42;
  rgrabPoly[0]->pPoints[0].point.x = -4;
  rgrabPoly[0]->pPoints[0].point.y = 0;
  rgrabPoly[0]->pPoints[0].point.z = -6;
  rgrabPoly[0]->pPoints[0].sLen = 0.06;
  rgrabPoly[0]->pPoints[1].point.x = -5;
  rgrabPoly[0]->pPoints[1].point.y = 0;
  rgrabPoly[0]->pPoints[1].point.z = -1;
  rgrabPoly[0]->pPoints[1].sLen = 0.39;
  rgrabPoly[0]->pPoints[2].point.x = 43;
  rgrabPoly[0]->pPoints[2].point.y = 0;
  rgrabPoly[0]->pPoints[2].point.z = 11;
  rgrabPoly[0]->pPoints[2].sLen = 0.06;
  rgrabPoly[0]->pPoints[3].point.x = 44;
  rgrabPoly[0]->pPoints[3].point.y = 0;
  rgrabPoly[0]->pPoints[3].point.z = 6;
  rgrabPoly[0]->pPoints[3].sLen = 0.49;
  rgrabPoly[1]->numPoints = 4;
  rgrabPoly[1]->rollAngle = 0;
  rgrabPoly[1]->stepLength = 42;
  rgrabPoly[1]->pPoints[0].point.x = -4;
  rgrabPoly[1]->pPoints[0].point.y = 0;
  rgrabPoly[1]->pPoints[0].point.z = -6;
  rgrabPoly[1]->pPoints[0].sLen = 0.06;
  rgrabPoly[1]->pPoints[1].point.x = -5;
  rgrabPoly[1]->pPoints[1].point.y = 0;
  rgrabPoly[1]->pPoints[1].point.z = -1;
  rgrabPoly[1]->pPoints[1].sLen = 0.39;
  rgrabPoly[1]->pPoints[2].point.x = 43;
  rgrabPoly[1]->pPoints[2].point.y = 0;
  rgrabPoly[1]->pPoints[2].point.z = 11;
  rgrabPoly[1]->pPoints[2].sLen = 0.06;
  rgrabPoly[1]->pPoints[3].point.x = 44;
  rgrabPoly[1]->pPoints[3].point.y = 0;
  rgrabPoly[1]->pPoints[3].point.z = 6;
  rgrabPoly[1]->pPoints[3].sLen = 0.49;
  rgrabPoly[2]->numPoints = 4;
  rgrabPoly[2]->rollAngle = 0;
  rgrabPoly[2]->stepLength = 42;
  rgrabPoly[2]->pPoints[0].point.x = -25;
  rgrabPoly[2]->pPoints[0].point.y = 0;
  rgrabPoly[2]->pPoints[0].point.z = 0;
  rgrabPoly[2]->pPoints[0].sLen = 0.06;
  rgrabPoly[2]->pPoints[1].point.x = -25;
  rgrabPoly[2]->pPoints[1].point.y = 0;
  rgrabPoly[2]->pPoints[1].point.z = 24;
  rgrabPoly[2]->pPoints[1].sLen = 0.39;
  rgrabPoly[2]->pPoints[2].point.x = 25;
  rgrabPoly[2]->pPoints[2].point.y = 0;
  rgrabPoly[2]->pPoints[2].point.z = 24;
  rgrabPoly[2]->pPoints[2].sLen = 0.06;
  rgrabPoly[2]->pPoints[3].point.x = 25;
  rgrabPoly[2]->pPoints[3].point.y = 0;
  rgrabPoly[2]->pPoints[3].point.z = 0;
  rgrabPoly[2]->pPoints[3].sLen = 0.49;
  rgrabPoly[3]->numPoints = 4;
  rgrabPoly[3]->rollAngle = 0;
  rgrabPoly[3]->stepLength = 42;
  rgrabPoly[3]->pPoints[0].point.x = -25;
  rgrabPoly[3]->pPoints[0].point.y = 0;
  rgrabPoly[3]->pPoints[0].point.z = 0;
  rgrabPoly[3]->pPoints[0].sLen = 0.06;
  rgrabPoly[3]->pPoints[1].point.x = -25;
  rgrabPoly[3]->pPoints[1].point.y = 0;
  rgrabPoly[3]->pPoints[1].point.z = 24;
  rgrabPoly[3]->pPoints[1].sLen = 0.39;
  rgrabPoly[3]->pPoints[2].point.x = 25;
  rgrabPoly[3]->pPoints[2].point.y = 0;
  rgrabPoly[3]->pPoints[2].point.z = 24;
  rgrabPoly[3]->pPoints[2].sLen = 0.06;
  rgrabPoly[3]->pPoints[3].point.x = 25;
  rgrabPoly[3]->pPoints[3].point.y = 0;
  rgrabPoly[3]->pPoints[3].point.z = 0;
  rgrabPoly[3]->pPoints[3].sLen = 0.49;
  rgrabPoly[0]->finalize();
  rgrabPoly[1]->finalize();
  rgrabPoly[2]->finalize();
  rgrabPoly[3]->finalize();
}

GT2005Polygon* GT2005Parameters::calculateCurrentPoly(int leg, Pose2D mRequest, WalkRequest::WalkType walkType)
{
  /*/ remember last time in boost mode
  if (walkType == WalkRequest::boost)
  {
    framesSinceBoost = SystemCall::getCurrentSystemTime();
  }
  unsigned long bTime = SystemCall::getCurrentSystemTime() - framesSinceBoost;
  // boost mode shutdown sequence
  if ((bTime > 10) && (bTime < 2000))
  {
    mRequest.translation.x = min(mRequest.translation.x, 200.0 + (100.0 * (1.0 - (bTime / 1990.0))));
  }//*/

  if (activePoints > 0)
  {
    if (specialMode)
    {
      GT2005Polygon* temp = rotPoly[leg]->copy();
      return temp;
    }
    else
    {
      GT2005Polygon* temp;
      //temp = evoPoly[leg]->copy();
      // reactivate for normal mode
      switch (walkType)
      {
        case WalkRequest::normal:
        case WalkRequest::boost:
        {
          if (mRequest.translation.x < 0)
          {
            if (mRequest.translation.x < -200) // mix fast backward
            {
              double a = (mRequest.translation.x + 200) / -100.0;
              temp = sbackwardPoly[leg]->interpolateShape(a, backwardPoly[leg]);
            }
            else // mix normal/smooth backward
            {
              double a = mRequest.translation.x / -200.0;
              temp = omniPoly[leg]->interpolateShape(a, sbackwardPoly[leg]);
            }
          }
          else
          {
            if (mRequest.translation.x > 200) // mix fast forward
            {
              if (walkType == WalkRequest::boost)
              {
                double a = (mRequest.translation.x - 200) / 100.0;
                temp = sforwardPoly[leg]->interpolateShape(a, boostPoly[leg]);
              }
              else
              {
                double a = (mRequest.translation.x - 200) / 100.0;
                temp = sforwardPoly[leg]->interpolateShape(a, forwardPoly[leg]);
              }
            }
            else // mix normal/smooth forward
            {
              double a = mRequest.translation.x / 200.0;
              temp = omniPoly[leg]->interpolateShape(a, sforwardPoly[leg]);
            }
          }
          // mix sidestepping
          if (fabs(mRequest.translation.y) > 20)
          {
            GT2005Polygon* temp2;
            GT2005Polygon* temp3;
            double a;
            // calculate best sidestepping polygon
            if (mRequest.translation.x < 0)
            {
              a = mRequest.translation.x / -300.0;
              temp2 = sidewardsPoly[leg]->interpolateShape(a, dbackwardPoly[leg]);
            }
            else
            {
              a = mRequest.translation.x / 300.0;
              temp2 = sidewardsPoly[leg]->interpolateShape(a, dforwardPoly[leg]);
            }
            // mix it up
            temp3 = temp->copy();
            delete temp;
            a = (fabs(mRequest.translation.y) - 20) / 280.0;
            temp = temp3->interpolateShape(a, temp2);
            delete temp2;
            delete temp3;
          }

          // mix rotation
          if (fabs(mRequest.rotation) > 0.2)
          {
            GT2005Polygon* temp2;
            temp2 = temp->copy();
            delete temp;
            double a = min((fabs(mRequest.rotation) / 5.236), 1.0); //7.8
            temp = temp2->interpolateShape(a, rotPoly[leg]);
            delete temp2;
          }
          temp->finalize(); // this is required for accurate ground phase approximation
          break;
        }
        case WalkRequest::walkWithBall:
        {
          temp = grabPoly[leg]->copy();
          // mix rotation
          if (fabs(mRequest.rotation) > 0.2)
          {
            GT2005Polygon* temp2;
            temp2 = temp->copy();
            delete temp;
            double a = min((fabs(mRequest.rotation) / 5.236), 1.0);
            temp = temp2->interpolateShape(a, rgrabPoly[leg]);
            delete temp2;
          }
          temp->finalize(); // this is required for accurate ground phase approximation
          break;
        }
        default:
        {
          temp = evoPoly[leg]->copy();
          break;
        }
      }
      //*/

      return temp;
    }
  }
  return NULL;
}

bool GT2005Parameters::load(char* filename)
{
  char name[128];
  if (filename != 0)
  {
    strcpy(name, filename);
  }
  InBinaryFile file(name);
  if (file.exists())
  {
    file >> *this;
    return true;
  }
  return false;
}

void GT2005Parameters::save(char* filename)
{
  char name[128];
  if (filename != 0)
  {
    strcpy(name, filename);
  }
  OutBinaryFile file(name);
  file << *this;
}

In& operator>>(In& stream, GT2005Parameters& Parameters)
{

  stream >> Parameters.activePoints;
  stream >> Parameters.stepLength;
  stream >> Parameters.counterRotation;

  for (int i = 0; i < 4; i++)
  {
    stream >> Parameters.footPhase[i];
    stream >> Parameters.footZeroPosition[i];
    stream >> *Parameters.omniPoly[i];
    stream >> *Parameters.rotPoly[i];
    stream >> *Parameters.sforwardPoly[i];
    stream >> *Parameters.forwardPoly[i];
    stream >> *Parameters.dforwardPoly[i];
    stream >> *Parameters.sbackwardPoly[i];
    stream >> *Parameters.backwardPoly[i];
    stream >> *Parameters.dbackwardPoly[i];
    stream >> *Parameters.sidewardsPoly[i];
    stream >> *Parameters.boostPoly[i];
    stream >> *Parameters.grabPoly[i];
    stream >> *Parameters.rgrabPoly[i];
    stream >> *Parameters.evoPoly[i];
  }
  return stream;
}
 
Out& operator<<(Out& stream, const GT2005Parameters& Parameters)
{
  stream << Parameters.activePoints;
  stream << Parameters.stepLength;
  stream << Parameters.counterRotation;
  for (int i = 0; i < 4; i++)
  {
    stream << Parameters.footPhase[i];
    stream << Parameters.footZeroPosition[i];
    stream << *Parameters.omniPoly[i];
    stream << *Parameters.rotPoly[i];
    stream << *Parameters.sforwardPoly[i];
    stream << *Parameters.forwardPoly[i];
    stream << *Parameters.dforwardPoly[i];
    stream << *Parameters.sbackwardPoly[i];
    stream << *Parameters.backwardPoly[i];
    stream << *Parameters.dbackwardPoly[i];
    stream << *Parameters.sidewardsPoly[i];
    stream << *Parameters.boostPoly[i];
    stream << *Parameters.grabPoly[i];
    stream << *Parameters.rgrabPoly[i];
    stream << *Parameters.evoPoly[i];
  }
  return stream;
}

GTWalkParametersIndividual::GTWalkParametersIndividual()
{
  fitness = 0;
  calculateTauTau0();
  //initalize strategyParameters
  for (int i = 0; i < 2; i++)
  {
    for (int j = 0; j < 32; j += 4)
    {
      strategyParameters[i][j] = 4.5; // x axis 2.5
      strategyParameters[i][j + 1] = 4; // y axis 2
      strategyParameters[i][j + 2] = 4.5; // z axis 2.5
      strategyParameters[i][j + 3] = 0.08;  // segment length 0.04
    }
  }
  strategyParameterStepLength = 5;
  bMessage = 0;
}

GTWalkParametersIndividual& GTWalkParametersIndividual::operator=(const GTWalkParametersIndividual& other)
{
  int i, j;

  bMessage = other.bMessage;
  fitness = other.fitness;
  for (i = 0; i < 2; i++)
  {
    for (j = 0; j < 32; j++)
    {
      strategyParameters[i][j] = other.strategyParameters[i][j];
    }
  }
  strategyParameterStepLength = other.strategyParameterStepLength;

  for (i = 0; i < 4; i++)
  {
    delete walkingParameters.evoPoly[i];
    walkingParameters.evoPoly[i] = other.walkingParameters.evoPoly[i]->copy();
  }

  return *this;
}

void GTWalkParametersIndividual::mutate()
{
  double randGTau0 = exp(randG(tau0));
  int i;

  //mutate strategy parameters
  strategyParameterStepLength = strategyParameterStepLength * randGTau0 * exp(randG(tau));
  for (i = 0; i < 2; i++)
  {
    for (int j = 0; j < 32; j++)
    {
      strategyParameters[i][j] = strategyParameters[i][j] * randGTau0 * exp(randG(tau));
    }
  }
        
  //mutate objectParameters
  int tmpStepLength = (int)(randG(strategyParameterStepLength));
  walkingParameters.evoPoly[GT2005Parameters::Leg_FR]->stepLength += tmpStepLength;
  walkingParameters.evoPoly[GT2005Parameters::Leg_FL]->stepLength += tmpStepLength;
  walkingParameters.evoPoly[GT2005Parameters::Leg_HR]->stepLength += tmpStepLength;
  walkingParameters.evoPoly[GT2005Parameters::Leg_HL]->stepLength += tmpStepLength;
  Vector3<double> polyPoint;
  double segment;
  for (i = 0; i < walkingParameters.evoPoly[GT2005Parameters::Leg_FR]->numPoints; i++)
  {
    polyPoint = walkingParameters.evoPoly[GT2005Parameters::Leg_FR]->getPointCoord(i);
    polyPoint.x += randG(strategyParameters[0][i * 4]);
    polyPoint.y += randG(strategyParameters[0][i * 4 + 1]);
    polyPoint.z += randG(strategyParameters[0][i * 4 + 2]);
    segment = walkingParameters.evoPoly[GT2005Parameters::Leg_FR]->getSegmentLength(i);
    segment += randG(strategyParameters[0][i * 4 + 3]);
    walkingParameters.evoPoly[GT2005Parameters::Leg_FR]->setSegment(i, segment, polyPoint);
    //mirror point for other side
    polyPoint.y = -polyPoint.y;
    walkingParameters.evoPoly[GT2005Parameters::Leg_FL]->setSegment(i, segment, polyPoint);
  }
  for (i = 0; i < walkingParameters.evoPoly[GT2005Parameters::Leg_HR]->numPoints; i++)
  {
    polyPoint = walkingParameters.evoPoly[GT2005Parameters::Leg_HR]->getPointCoord(i);
    polyPoint.x += randG(strategyParameters[1][i * 4]);
    polyPoint.y += randG(strategyParameters[1][i * 4 + 1]);
    polyPoint.z += randG(strategyParameters[1][i * 4 + 2]);
    segment = walkingParameters.evoPoly[GT2005Parameters::Leg_HR]->getSegmentLength(i);
    segment += randG(strategyParameters[1][i * 4 + 3]);
    walkingParameters.evoPoly[GT2005Parameters::Leg_HR]->setSegment(i, segment, polyPoint);
    //mirror point for other side
    polyPoint.y = -polyPoint.y;
    walkingParameters.evoPoly[GT2005Parameters::Leg_HL]->setSegment(i, segment, polyPoint);
  }

  //hier noch sicherstellen, dass die segmente zusammen 1 ergeben
}

void GTWalkParametersIndividual::crossOver(GTWalkParametersIndividual* parent2)
{
  //cross over strategy parameters
  strategyParameterStepLength = (strategyParameterStepLength + parent2->strategyParameterStepLength) / 2;
  for (int i = 0; i < 2; i++)
  {
    for (int j = 0; j < 32; j++)
    {
      strategyParameters[i][j] = (strategyParameters[i][j] + parent2->strategyParameters[i][j]) / 2;
    }
  }
    
  //using intermediate recombination
  //walkingParameters.stepLength = (int)((walkingParameters.stepLength + parent2->walkingParameters.stepLength) / 2);

  //blend the two polys by 50% and store the result in the own evoPoly set
  walkingParameters.evoPoly[GT2005Parameters::Leg_FR]->interpolateShapeP(0.5, parent2->walkingParameters.evoPoly[GT2005Parameters::Leg_FR]);
  walkingParameters.evoPoly[GT2005Parameters::Leg_FL]->interpolateShapeP(0.5, parent2->walkingParameters.evoPoly[GT2005Parameters::Leg_FL]);
  walkingParameters.evoPoly[GT2005Parameters::Leg_HR]->interpolateShapeP(0.5, parent2->walkingParameters.evoPoly[GT2005Parameters::Leg_HR]);
  walkingParameters.evoPoly[GT2005Parameters::Leg_HL]->interpolateShapeP(0.5, parent2->walkingParameters.evoPoly[GT2005Parameters::Leg_HL]);
}

void GTWalkParametersIndividual::crossingOverOf(GTWalkParametersIndividual* parent1, GTWalkParametersIndividual* parent2)
{
  //cross over strategy parameters
  strategyParameterStepLength = (parent1->strategyParameterStepLength + parent2->strategyParameterStepLength) / 2;
  for (int i = 0; i < 2; i++)
  {
    for (int j = 0; j < 32; j++)
    {
      strategyParameters[i][j] = (parent1->strategyParameters[i][j] + parent2->strategyParameters[i][j]) / 2;
    }
  }
    
  //using intermediate recombination
  //walkingParameters.stepLength = (int)((parent1->walkingParameters.stepLength+parent2->walkingParameters.stepLength) / 2);

  //copy Konstruktor benutzen
  delete walkingParameters.evoPoly[GT2005Parameters::Leg_FR];
  delete walkingParameters.evoPoly[GT2005Parameters::Leg_FL];
  delete walkingParameters.evoPoly[GT2005Parameters::Leg_HR];
  delete walkingParameters.evoPoly[GT2005Parameters::Leg_HL];

  walkingParameters.evoPoly[GT2005Parameters::Leg_FR] = parent1->walkingParameters.evoPoly[GT2005Parameters::Leg_FR]->interpolateShape(0.5, parent2->walkingParameters.evoPoly[GT2005Parameters::Leg_FR]);
  walkingParameters.evoPoly[GT2005Parameters::Leg_FL] = parent1->walkingParameters.evoPoly[GT2005Parameters::Leg_FL]->interpolateShape(0.5, parent2->walkingParameters.evoPoly[GT2005Parameters::Leg_FL]);
  walkingParameters.evoPoly[GT2005Parameters::Leg_HR] = parent1->walkingParameters.evoPoly[GT2005Parameters::Leg_HR]->interpolateShape(0.5, parent2->walkingParameters.evoPoly[GT2005Parameters::Leg_HR]);
  walkingParameters.evoPoly[GT2005Parameters::Leg_HL] = parent1->walkingParameters.evoPoly[GT2005Parameters::Leg_HL]->interpolateShape(0.5, parent2->walkingParameters.evoPoly[GT2005Parameters::Leg_HL]);
}

void GTWalkParametersIndividual::calculateTauTau0()
{
  int n;
  n = walkingParameters.evoPoly[GT2005Parameters::Leg_FR]->numPoints * 4 * 2 + 1;
  tau = 1 / (sqrt((double)(2 * sqrt((double)n))));
  tau0 = 1 / (sqrt((double)(2 * n)));
}

double GTWalkParametersIndividual::randG(const double mutationStrength)
{
  return mutationStrength * sqrt(-2.0 * log(random())) * sin(pi2 * random());
}

In& operator>>(In& stream, GTWalkParametersIndividual& individualParameters)
{
  int i, j;

  stream >> individualParameters.bMessage;
  stream >> individualParameters.fitness;
  for (i = 0; i < 2; i++)
  {
    for (j = 0; j < 32; j++)
    {
      stream >> individualParameters.strategyParameters[i][j];
    }
  }
  stream >> individualParameters.strategyParameterStepLength;

  for (i = 0; i < 4; i++)
  {
    stream >> *individualParameters.walkingParameters.evoPoly[i];
  }
  return stream;
}
 
Out& operator<<(Out& stream, const GTWalkParametersIndividual& individualParameters)
{
  int i, j;

  stream << individualParameters.bMessage;
  stream << individualParameters.fitness;
  for (i = 0; i < 2; i++)
  {
    for (j = 0; j < 32; j++)
    {
      stream << individualParameters.strategyParameters[i][j];
    }
  }
  stream << individualParameters.strategyParameterStepLength;

  for (i = 0; i < 4; i++)
  {
    stream << *individualParameters.walkingParameters.evoPoly[i];
  }
  return stream;
}

---