Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Modules/WalkingEngine/GT2005Parameters.cpp

Go to the documentation of this file.

    00001 /***************************************************************************
    00002 * @file GT2005Parameters.cpp
    00003 * class GT2005Parameters
    00004 * @author Denis Fisseler
    00005 ****************************************************************************/
    00006
    00007 #include "GT2005Parameters.h"
    00008 #include "Platform/SystemCall.h"
    00009 #include "Tools/Streams/InStreams.h"
    00010 #include "Tools/Streams/OutStreams.h"
    00011
    00012 const int GT2005Parameters::Leg_FR = 0;
    00013 const int GT2005Parameters::Leg_FL = 1;
    00014 const int GT2005Parameters::Leg_HR = 2;
    00015 const int GT2005Parameters::Leg_HL = 3;
    00016
    00017 GT2005Parameters::GT2005Parameters()
    00018 {
    00019   loadingFailed = false;
    00020   setBasicParameters();
    00021   setBasicPolygons();
    00022   specialMode = false;
    00023   framesSinceBoost = 0;
    00024 }
    00025
    00026 GT2005Parameters::~GT2005Parameters()
    00027 {
    00028   for (int i = 0; i < 4; i++)
    00029   {
    00030     delete omniPoly[i];
    00031     delete rotPoly[i];
    00032     delete sforwardPoly[i];
    00033     delete forwardPoly[i];
    00034     delete dforwardPoly[i];
    00035     delete sbackwardPoly[i];
    00036     delete backwardPoly[i];
    00037     delete dbackwardPoly[i];
    00038     delete sidewardsPoly[i];
    00039     delete boostPoly[i];
    00040     delete grabPoly[i];
    00041     delete rgrabPoly[i];
    00042     delete evoPoly[i];
    00043   }
    00044 }
    00045
    00046 void GT2005Parameters::setBasicParameters()
    00047 {
    00048   stepLength = 64;
    00049   counterRotation = 1234567890;
    00050
    00051   // set foot phases
    00052   footPhase[Leg_FR] = 0;
    00053   footPhase[Leg_FL] = 0.5;
    00054   footPhase[Leg_HR] = 0.5;
    00055   footPhase[Leg_HL] = 0;
    00056
    00057   // set foot zero positions
    00058   footZeroPosition[Leg_FR].x = 50.3536; footZeroPosition[Leg_FR].y = -80.2702;  footZeroPosition[Leg_FR].z = -77.5836;
    00059   footZeroPosition[Leg_FL].x = 50.3536; footZeroPosition[Leg_FL].y = 80.2702; footZeroPosition[Leg_FL].z = -77.5836;
    00060   footZeroPosition[Leg_HR].x = -52.0932;  footZeroPosition[Leg_HR].y = -81.5432;  footZeroPosition[Leg_HR].z = -105.174;
    00061   footZeroPosition[Leg_HL].x = -52.0932;  footZeroPosition[Leg_HL].y = 81.5432; footZeroPosition[Leg_HL].z = -105.174;
    00062 }
    00063
    00064 void GT2005Parameters::setBasicPolygons()
    00065 {
    00066   int j;
    00067
    00068   // set front shape
    00069   Vector3<double> fPoints[4];
    00070   fPoints[0].x = -24; fPoints[0].y = 0; fPoints[0].z = -6;
    00071   fPoints[1].x = -25; fPoints[1].y = 0; fPoints[1].z = -1;
    00072   fPoints[2].x = 23;  fPoints[2].y = 0; fPoints[2].z = 11;
    00073   fPoints[3].x = 24;  fPoints[3].y = 0; fPoints[3].z = 6;
    00074
    00075   // set hind shape
    00076   Vector3<double> hPoints[4];
    00077   hPoints[0].x = -25; hPoints[0].y = 0; hPoints[0].z = 0;
    00078   hPoints[1].x = -25; hPoints[1].y = 0; hPoints[1].z = 24;
    00079   hPoints[2].x = 25;  hPoints[2].y = 0; hPoints[2].z = 24;
    00080   hPoints[3].x = 25;  hPoints[3].y = 0; hPoints[3].z = 0;
    00081   //*/
    00082
    00083   // n paar segmentlängen
    00084   double fLen[4];
    00085   fLen[0] = 0.06;
    00086   fLen[1] = 0.39;
    00087   fLen[2] = 0.06;
    00088   fLen[3] = 0.49;
    00089
    00090   double hLen[4];
    00091   hLen[0] = 0.06;
    00092   hLen[1] = 0.39;
    00093   hLen[2] = 0.06;
    00094   hLen[3] = 0.49;
    00095
    00096   evoPoly[Leg_FR] = new GT2005Polygon(4, 64);
    00097   evoPoly[Leg_FL] = new GT2005Polygon(4, 64);
    00098   evoPoly[Leg_HR] = new GT2005Polygon(4, 64);
    00099   evoPoly[Leg_HL] = new GT2005Polygon(4, 64);
    00100   for (j = 0; j < 4; j++)
    00101   {
    00102     evoPoly[Leg_FR]->setSegment(j, fLen[j], fPoints[j]);
    00103     evoPoly[Leg_FL]->setSegment(j, fLen[j], fPoints[j]);
    00104     evoPoly[Leg_HR]->setSegment(j, hLen[j], hPoints[j]);
    00105     evoPoly[Leg_HL]->setSegment(j, hLen[j], hPoints[j]);
    00106   }
    00107   activePoints = 4;
    00108
    00109   // finalize polygons to calculate ground phases
    00110   evoPoly[Leg_FR]->finalize();
    00111   evoPoly[Leg_FL]->finalize();
    00112   evoPoly[Leg_HR]->finalize();
    00113   evoPoly[Leg_HL]->finalize();
    00114
    00115   // hauptsache die teile sind erst mal initialisiert
    00116   for (int i = 0; i < 4; i++)
    00117   {
    00118     sforwardPoly[i] = new GT2005Polygon(activePoints, 64);
    00119     forwardPoly[i] = new GT2005Polygon(activePoints, 64);
    00120     dforwardPoly[i] = new GT2005Polygon(activePoints, 64);
    00121     sbackwardPoly[i] = new GT2005Polygon(activePoints, 64);
    00122     backwardPoly[i] = new GT2005Polygon(activePoints, 64);
    00123     dbackwardPoly[i] = new GT2005Polygon(activePoints, 64);
    00124     sidewardsPoly[i] = new GT2005Polygon(activePoints, 64);
    00125     boostPoly[i] = new GT2005Polygon(activePoints, 64);
    00126     grabPoly[i] = new GT2005Polygon(activePoints, 64);
    00127     rgrabPoly[i] = new GT2005Polygon(activePoints, 64);
    00128   }
    00129
    00130   omniPoly[Leg_FR] = evoPoly[Leg_FR]->copy();
    00131   omniPoly[Leg_FL] = evoPoly[Leg_FL]->copy();
    00132   omniPoly[Leg_HR] = evoPoly[Leg_HR]->copy();
    00133   omniPoly[Leg_HL] = evoPoly[Leg_HL]->copy();
    00134
    00135   // store a copy for rotation and do some optimisations
    00136   rotPoly[Leg_FR] = evoPoly[Leg_FR]->copy();
    00137   rotPoly[Leg_FL] = evoPoly[Leg_FL]->copy();
    00138   rotPoly[Leg_HR] = evoPoly[Leg_HR]->copy();
    00139   rotPoly[Leg_HL] = evoPoly[Leg_HL]->copy();
    00140   rotPoly[Leg_FR]->stepLength = 42;
    00141   rotPoly[Leg_FL]->stepLength = 42;
    00142   rotPoly[Leg_HR]->stepLength = 42;
    00143   rotPoly[Leg_HL]->stepLength = 42;
    00144
    00145   // hier schätz ich mir mal nen guten Anfang für ne Evolution zusammen
    00146   // dashfake
    00147   fPoints[0].x = -25; fPoints[0].y = 0; fPoints[0].z = -6;
    00148   fPoints[1].x = -25; fPoints[1].y = 0; fPoints[1].z = -1;
    00149   fPoints[2].x = 25;  fPoints[2].y = 0; fPoints[2].z = 11;
    00150   fPoints[3].x = 25;  fPoints[3].y = 0; fPoints[3].z = 6;
    00151   hPoints[0].x = -15; hPoints[0].y = 0; hPoints[0].z = -30;
    00152   hPoints[1].x = -15; hPoints[1].y = 0; hPoints[1].z = -5;
    00153   hPoints[2].x = 35;  hPoints[2].y = 0; hPoints[2].z = -5;
    00154   hPoints[3].x = 35;  hPoints[3].y = 0; hPoints[3].z = -30;
    00155
    00156   for (j = 0; j < 4; j++)
    00157   {
    00158     evoPoly[Leg_FR]->setSegment(j, fLen[j], fPoints[j]);
    00159     evoPoly[Leg_FL]->setSegment(j, fLen[j], fPoints[j]);
    00160     evoPoly[Leg_HR]->setSegment(j, hLen[j], hPoints[j]);
    00161     evoPoly[Leg_HL]->setSegment(j, hLen[j], hPoints[j]);
    00162   }
    00163   evoPoly[Leg_FR]->stepLength = 52;
    00164   evoPoly[Leg_FL]->stepLength = 52;
    00165   evoPoly[Leg_HR]->stepLength = 52;
    00166   evoPoly[Leg_HL]->stepLength = 52;
    00167   evoPoly[Leg_FR]->finalize();
    00168   evoPoly[Leg_FL]->finalize();
    00169   evoPoly[Leg_HR]->finalize();
    00170   evoPoly[Leg_HL]->finalize();//*/
    00171
    00172   setOptimizedPolys();//*/
    00173 }
    00174
    00175
    00176 void GT2005Parameters::setOptimizedPolys()
    00177 {
    00178   //forwardPoly parameters
    00179   forwardPoly[0]->numPoints = 4;
    00180   forwardPoly[0]->rollAngle = 0;
    00181   forwardPoly[0]->stepLength = 50;
    00182   forwardPoly[0]->pPoints[0].point.x = -35.7702;
    00183   forwardPoly[0]->pPoints[0].point.y = 15.9675;
    00184   forwardPoly[0]->pPoints[0].point.z = -31.9739;
    00185   forwardPoly[0]->pPoints[0].sLen = 0.104465;
    00186   forwardPoly[0]->pPoints[1].point.x = -38.9055;
    00187   forwardPoly[0]->pPoints[1].point.y = -31.4604;
    00188   forwardPoly[0]->pPoints[1].point.z = -15.6947;
    00189   forwardPoly[0]->pPoints[1].sLen = 0.247062;
    00190   forwardPoly[0]->pPoints[2].point.x = 7.85936;
    00191   forwardPoly[0]->pPoints[2].point.y = 16.92;
    00192   forwardPoly[0]->pPoints[2].point.z = 11.9191;
    00193   forwardPoly[0]->pPoints[2].sLen = 0.213031;
    00194   forwardPoly[0]->pPoints[3].point.x = 29.6432;
    00195   forwardPoly[0]->pPoints[3].point.y = 9.71364;
    00196   forwardPoly[0]->pPoints[3].point.z = 32.4321;
    00197   forwardPoly[0]->pPoints[3].sLen = 0.435442;
    00198   forwardPoly[0]->numPoints = 4;
    00199   forwardPoly[0]->rollAngle = 0;
    00200   forwardPoly[1]->stepLength = 50;
    00201   forwardPoly[1]->pPoints[0].point.x = -35.7702;
    00202   forwardPoly[1]->pPoints[0].point.y = 15.9675;
    00203   forwardPoly[1]->pPoints[0].point.z = -31.9739;
    00204   forwardPoly[1]->pPoints[0].sLen = 0.104465;
    00205   forwardPoly[1]->pPoints[1].point.x = -38.9055;
    00206   forwardPoly[1]->pPoints[1].point.y = -31.4604;
    00207   forwardPoly[1]->pPoints[1].point.z = -15.6947;
    00208   forwardPoly[1]->pPoints[1].sLen = 0.247062;
    00209   forwardPoly[1]->pPoints[2].point.x = 7.85936;
    00210   forwardPoly[1]->pPoints[2].point.y = 16.92;
    00211   forwardPoly[1]->pPoints[2].point.z = 11.9191;
    00212   forwardPoly[1]->pPoints[2].sLen = 0.213031;
    00213   forwardPoly[1]->pPoints[3].point.x = 29.6432;
    00214   forwardPoly[1]->pPoints[3].point.y = 9.71364;
    00215   forwardPoly[1]->pPoints[3].point.z = 32.4321;
    00216   forwardPoly[1]->pPoints[3].sLen = 0.435442;
    00217   forwardPoly[2]->numPoints = 4;
    00218   forwardPoly[2]->rollAngle = 0;
    00219   forwardPoly[2]->stepLength = 50;
    00220   forwardPoly[2]->pPoints[0].point.x = -28.2799;
    00221   forwardPoly[2]->pPoints[0].point.y = -7.11929;
    00222   forwardPoly[2]->pPoints[0].point.z = -6.10541;
    00223   forwardPoly[2]->pPoints[0].sLen = 0.121962;
    00224   forwardPoly[2]->pPoints[1].point.x = -44.0651;
    00225   forwardPoly[2]->pPoints[1].point.y = -14.2374;
    00226   forwardPoly[2]->pPoints[1].point.z = 31.8775;
    00227   forwardPoly[2]->pPoints[1].sLen = 0.449758;
    00228   forwardPoly[2]->pPoints[2].point.x = 21.8429;
    00229   forwardPoly[2]->pPoints[2].point.y = 5.48479;
    00230   forwardPoly[2]->pPoints[2].point.z = 25.1939;
    00231   forwardPoly[2]->pPoints[2].sLen = 0.0754086;
    00232   forwardPoly[2]->pPoints[3].point.x = 12.793;
    00233   forwardPoly[2]->pPoints[3].point.y = 7.26785;
    00234   forwardPoly[2]->pPoints[3].point.z = -1.38549;
    00235   forwardPoly[2]->pPoints[3].sLen = 0.352872;
    00236   forwardPoly[3]->numPoints = 4;
    00237   forwardPoly[3]->rollAngle = 0;
    00238   forwardPoly[3]->stepLength = 50;
    00239   forwardPoly[3]->pPoints[0].point.x = -28.2799;
    00240   forwardPoly[3]->pPoints[0].point.y = -7.11929;
    00241   forwardPoly[3]->pPoints[0].point.z = -6.10541;
    00242   forwardPoly[3]->pPoints[0].sLen = 0.121962;
    00243   forwardPoly[3]->pPoints[1].point.x = -44.0651;
    00244   forwardPoly[3]->pPoints[1].point.y = -14.2374;
    00245   forwardPoly[3]->pPoints[1].point.z = 31.8775;
    00246   forwardPoly[3]->pPoints[1].sLen = 0.449758;
    00247   forwardPoly[3]->pPoints[2].point.x = 21.8429;
    00248   forwardPoly[3]->pPoints[2].point.y = 5.48479;
    00249   forwardPoly[3]->pPoints[2].point.z = 25.1939;
    00250   forwardPoly[3]->pPoints[2].sLen = 0.0754086;
    00251   forwardPoly[3]->pPoints[3].point.x = 12.793;
    00252   forwardPoly[3]->pPoints[3].point.y = 7.26785;
    00253   forwardPoly[3]->pPoints[3].point.z = -1.38549;
    00254   forwardPoly[3]->pPoints[3].sLen = 0.352872;
    00255   forwardPoly[0]->finalize();
    00256   forwardPoly[1]->finalize();
    00257   forwardPoly[2]->finalize();
    00258   forwardPoly[3]->finalize();
    00259
    00260   //sforwardPoly parameters
    00261   sforwardPoly[0]->numPoints = 4;
    00262   sforwardPoly[0]->rollAngle = 0;
    00263   sforwardPoly[0]->stepLength = 75;
    00264   sforwardPoly[0]->pPoints[0].point.x = -30.0931;
    00265   sforwardPoly[0]->pPoints[0].point.y = 2.08297;
    00266   sforwardPoly[0]->pPoints[0].point.z = -18.8519;
    00267   sforwardPoly[0]->pPoints[0].sLen = 0.107553;
    00268   sforwardPoly[0]->pPoints[1].point.x = -27.0353;
    00269   sforwardPoly[0]->pPoints[1].point.y = 0.823497;
    00270   sforwardPoly[0]->pPoints[1].point.z = -5.40966;
    00271   sforwardPoly[0]->pPoints[1].sLen = 0.377075;
    00272   sforwardPoly[0]->pPoints[2].point.x = 23.0637;
    00273   sforwardPoly[0]->pPoints[2].point.y = -1.33187;
    00274   sforwardPoly[0]->pPoints[2].point.z = 1.05023;
    00275   sforwardPoly[0]->pPoints[2].sLen = 0.0542556;
    00276   sforwardPoly[0]->pPoints[3].point.x = 3.26128;
    00277   sforwardPoly[0]->pPoints[3].point.y = -3.26234;
    00278   sforwardPoly[0]->pPoints[3].point.z = -26.8779;
    00279   sforwardPoly[0]->pPoints[3].sLen = 0.461116;
    00280   sforwardPoly[1]->numPoints = 4;
    00281   sforwardPoly[1]->rollAngle = 0;
    00282   sforwardPoly[1]->stepLength = 75;
    00283   sforwardPoly[1]->pPoints[0].point.x = -30.0931;
    00284   sforwardPoly[1]->pPoints[0].point.y = -2.08297;
    00285   sforwardPoly[1]->pPoints[0].point.z = -18.8519;
    00286   sforwardPoly[1]->pPoints[0].sLen = 0.107553;
    00287   sforwardPoly[1]->pPoints[1].point.x = -27.0353;
    00288   sforwardPoly[1]->pPoints[1].point.y = -0.823497;
    00289   sforwardPoly[1]->pPoints[1].point.z = -5.40966;
    00290   sforwardPoly[1]->pPoints[1].sLen = 0.377075;
    00291   sforwardPoly[1]->pPoints[2].point.x = 23.0637;
    00292   sforwardPoly[1]->pPoints[2].point.y = 1.33187;
    00293   sforwardPoly[1]->pPoints[2].point.z = 1.05023;
    00294   sforwardPoly[1]->pPoints[2].sLen = 0.0542556;
    00295   sforwardPoly[1]->pPoints[3].point.x = 3.26128;
    00296   sforwardPoly[1]->pPoints[3].point.y = 3.26234;
    00297   sforwardPoly[1]->pPoints[3].point.z = -26.8779;
    00298   sforwardPoly[1]->pPoints[3].sLen = 0.461116;
    00299   sforwardPoly[2]->numPoints = 4;
    00300   sforwardPoly[2]->rollAngle = 0;
    00301   sforwardPoly[2]->stepLength = 75;
    00302   sforwardPoly[2]->pPoints[0].point.x = -23.7288;
    00303   sforwardPoly[2]->pPoints[0].point.y = -9.6087;
    00304   sforwardPoly[2]->pPoints[0].point.z = 3.06664;
    00305   sforwardPoly[2]->pPoints[0].sLen = 0.0187699;
    00306   sforwardPoly[2]->pPoints[1].point.x = -24.7325;
    00307   sforwardPoly[2]->pPoints[1].point.y = -1.63567;
    00308   sforwardPoly[2]->pPoints[1].point.z = 39.8304;
    00309   sforwardPoly[2]->pPoints[1].sLen = 0.472549;
    00310   sforwardPoly[2]->pPoints[2].point.x = 16.2482;
    00311   sforwardPoly[2]->pPoints[2].point.y = -1.42779;
    00312   sforwardPoly[2]->pPoints[2].point.z = 25.1732;
    00313   sforwardPoly[2]->pPoints[2].sLen = 0.0312922;
    00314   sforwardPoly[2]->pPoints[3].point.x = 23.5193;
    00315   sforwardPoly[2]->pPoints[3].point.y = 3.41586;
    00316   sforwardPoly[2]->pPoints[3].point.z = 3.02703;
    00317   sforwardPoly[2]->pPoints[3].sLen = 0.477389;
    00318   sforwardPoly[3]->numPoints = 4;
    00319   sforwardPoly[3]->rollAngle = 0;
    00320   sforwardPoly[3]->stepLength = 75;
    00321   sforwardPoly[3]->pPoints[0].point.x = -23.7288;
    00322   sforwardPoly[3]->pPoints[0].point.y = 9.6087;
    00323   sforwardPoly[3]->pPoints[0].point.z = 3.06664;
    00324   sforwardPoly[3]->pPoints[0].sLen = 0.0187699;
    00325   sforwardPoly[3]->pPoints[1].point.x = -24.7325;
    00326   sforwardPoly[3]->pPoints[1].point.y = 1.63567;
    00327   sforwardPoly[3]->pPoints[1].point.z = 39.8304;
    00328   sforwardPoly[3]->pPoints[1].sLen = 0.472549;
    00329   sforwardPoly[3]->pPoints[2].point.x = 16.2482;
    00330   sforwardPoly[3]->pPoints[2].point.y = 1.42779;
    00331   sforwardPoly[3]->pPoints[2].point.z = 25.1732;
    00332   sforwardPoly[3]->pPoints[2].sLen = 0.0312922;
    00333   sforwardPoly[3]->pPoints[3].point.x = 23.5193;
    00334   sforwardPoly[3]->pPoints[3].point.y = -3.41586;
    00335   sforwardPoly[3]->pPoints[3].point.z = 3.02703;
    00336   sforwardPoly[3]->pPoints[3].sLen = 0.477389;
    00337   sforwardPoly[0]->finalize();
    00338   sforwardPoly[1]->finalize();
    00339   sforwardPoly[2]->finalize();
    00340   sforwardPoly[3]->finalize();
    00341
    00342   //dforwardPoly parameters
    00343   dforwardPoly[0]->numPoints = 4;
    00344   dforwardPoly[0]->rollAngle = 0;
    00345   dforwardPoly[0]->stepLength = 51;
    00346   dforwardPoly[0]->pPoints[0].point.x = -29.9474;
    00347   dforwardPoly[0]->pPoints[0].point.y = -3.27726;
    00348   dforwardPoly[0]->pPoints[0].point.z = -15.525;
    00349   dforwardPoly[0]->pPoints[0].sLen = -0.039293;
    00350   dforwardPoly[0]->pPoints[1].point.x = -27.6353;
    00351   dforwardPoly[0]->pPoints[1].point.y = -2.28411;
    00352   dforwardPoly[0]->pPoints[1].point.z = -2.09947;
    00353   dforwardPoly[0]->pPoints[1].sLen = 0.273697;
    00354   dforwardPoly[0]->pPoints[2].point.x = 5.70906;
    00355   dforwardPoly[0]->pPoints[2].point.y = -9.44559;
    00356   dforwardPoly[0]->pPoints[2].point.z = 12.5893;
    00357   dforwardPoly[0]->pPoints[2].sLen = 0.240248;
    00358   dforwardPoly[0]->pPoints[3].point.x = 25.9895;
    00359   dforwardPoly[0]->pPoints[3].point.y = 2.41216;
    00360   dforwardPoly[0]->pPoints[3].point.z = 3.08891;
    00361   dforwardPoly[0]->pPoints[3].sLen = 0.525348;
    00362   dforwardPoly[1]->numPoints = 4;
    00363   dforwardPoly[1]->rollAngle = 0;
    00364   dforwardPoly[1]->stepLength = 51;
    00365   dforwardPoly[1]->pPoints[0].point.x = -29.9474;
    00366   dforwardPoly[1]->pPoints[0].point.y = 3.27726;
    00367   dforwardPoly[1]->pPoints[0].point.z = -15.525;
    00368   dforwardPoly[1]->pPoints[0].sLen = -0.039293;
    00369   dforwardPoly[1]->pPoints[1].point.x = -27.6353;
    00370   dforwardPoly[1]->pPoints[1].point.y = 2.28411;
    00371   dforwardPoly[1]->pPoints[1].point.z = -2.09947;
    00372   dforwardPoly[1]->pPoints[1].sLen = 0.273697;
    00373   dforwardPoly[1]->pPoints[2].point.x = 5.70906;
    00374   dforwardPoly[1]->pPoints[2].point.y = 9.44559;
    00375   dforwardPoly[1]->pPoints[2].point.z = 12.5893;
    00376   dforwardPoly[1]->pPoints[2].sLen = 0.240248;
    00377   dforwardPoly[1]->pPoints[3].point.x = 25.9895;
    00378   dforwardPoly[1]->pPoints[3].point.y = -2.41216;
    00379   dforwardPoly[1]->pPoints[3].point.z = 3.08891;
    00380   dforwardPoly[1]->pPoints[3].sLen = 0.525348;
    00381   dforwardPoly[2]->numPoints = 4;
    00382   dforwardPoly[2]->rollAngle = 0;
    00383   dforwardPoly[2]->stepLength = 51;
    00384   dforwardPoly[2]->pPoints[0].point.x = -33.7782;
    00385   dforwardPoly[2]->pPoints[0].point.y = 0.304222;
    00386   dforwardPoly[2]->pPoints[0].point.z = -7.59602;
    00387   dforwardPoly[2]->pPoints[0].sLen = 0.0432646;
    00388   dforwardPoly[2]->pPoints[1].point.x = -37.4151;
    00389   dforwardPoly[2]->pPoints[1].point.y = 0.0659411;
    00390   dforwardPoly[2]->pPoints[1].point.z = 13.7733;
    00391   dforwardPoly[2]->pPoints[1].sLen = 0.40272;
    00392   dforwardPoly[2]->pPoints[2].point.x = 13.946;
    00393   dforwardPoly[2]->pPoints[2].point.y = -11.4001;
    00394   dforwardPoly[2]->pPoints[2].point.z = 25.7176;
    00395   dforwardPoly[2]->pPoints[2].sLen = 0.0602455;
    00396   dforwardPoly[2]->pPoints[3].point.x = 31.9201;
    00397   dforwardPoly[2]->pPoints[3].point.y = -9.57183;
    00398   dforwardPoly[2]->pPoints[3].point.z = -1.83454;
    00399   dforwardPoly[2]->pPoints[3].sLen = 0.49377;
    00400   dforwardPoly[3]->numPoints = 4;
    00401   dforwardPoly[3]->rollAngle = 0;
    00402   dforwardPoly[3]->stepLength = 51;
    00403   dforwardPoly[3]->pPoints[0].point.x = -33.7782;
    00404   dforwardPoly[3]->pPoints[0].point.y = -0.304222;
    00405   dforwardPoly[3]->pPoints[0].point.z = -7.59602;
    00406   dforwardPoly[3]->pPoints[0].sLen = 0.0432646;
    00407   dforwardPoly[3]->pPoints[1].point.x = -37.4151;
    00408   dforwardPoly[3]->pPoints[1].point.y = -0.0659411;
    00409   dforwardPoly[3]->pPoints[1].point.z = 13.7733;
    00410   dforwardPoly[3]->pPoints[1].sLen = 0.40272;
    00411   dforwardPoly[3]->pPoints[2].point.x = 13.946;
    00412   dforwardPoly[3]->pPoints[2].point.y = 11.4001;
    00413   dforwardPoly[3]->pPoints[2].point.z = 25.7176;
    00414   dforwardPoly[3]->pPoints[2].sLen = 0.0602455;
    00415   dforwardPoly[3]->pPoints[3].point.x = 31.9201;
    00416   dforwardPoly[3]->pPoints[3].point.y = 9.57183;
    00417   dforwardPoly[3]->pPoints[3].point.z = -1.83454;
    00418   dforwardPoly[3]->pPoints[3].sLen = 0.49377;
    00419   dforwardPoly[0]->finalize();
    00420   dforwardPoly[1]->finalize();
    00421   dforwardPoly[2]->finalize();
    00422   dforwardPoly[3]->finalize();
    00423
    00424   //backwardPoly parameters
    00425   backwardPoly[0]->numPoints = 4;
    00426   backwardPoly[0]->rollAngle = 0;
    00427   backwardPoly[0]->stepLength = 53;
    00428   backwardPoly[0]->pPoints[0].point.x = -37.4699;
    00429   backwardPoly[0]->pPoints[0].point.y = -6.69303;
    00430   backwardPoly[0]->pPoints[0].point.z = 11.2389;
    00431   backwardPoly[0]->pPoints[0].sLen = 0.209155;
    00432   backwardPoly[0]->pPoints[1].point.x = -13.7743;
    00433   backwardPoly[0]->pPoints[1].point.y = 33.8323;
    00434   backwardPoly[0]->pPoints[1].point.z = 3.96052;
    00435   backwardPoly[0]->pPoints[1].sLen = 0.326859;
    00436   backwardPoly[0]->pPoints[2].point.x = 33.4119;
    00437   backwardPoly[0]->pPoints[2].point.y = -6.3983;
    00438   backwardPoly[0]->pPoints[2].point.z = 13.7076;
    00439   backwardPoly[0]->pPoints[2].sLen = 0.0860842;
    00440   backwardPoly[0]->pPoints[3].point.x = 19.7998;
    00441   backwardPoly[0]->pPoints[3].point.y = -6.85233;
    00442   backwardPoly[0]->pPoints[3].point.z = 2.52472;
    00443   backwardPoly[0]->pPoints[3].sLen = 0.377902;
    00444   backwardPoly[1]->numPoints = 4;
    00445   backwardPoly[1]->rollAngle = 0;
    00446   backwardPoly[1]->stepLength = 53;
    00447   backwardPoly[1]->pPoints[0].point.x = -37.4699;
    00448   backwardPoly[1]->pPoints[0].point.y = 6.69303;
    00449   backwardPoly[1]->pPoints[0].point.z = 11.2389;
    00450   backwardPoly[1]->pPoints[0].sLen = 0.209155;
    00451   backwardPoly[1]->pPoints[1].point.x = -13.7743;
    00452   backwardPoly[1]->pPoints[1].point.y = -33.8323;
    00453   backwardPoly[1]->pPoints[1].point.z = 3.96052;
    00454   backwardPoly[1]->pPoints[1].sLen = 0.326859;
    00455   backwardPoly[1]->pPoints[2].point.x = 33.4119;
    00456   backwardPoly[1]->pPoints[2].point.y = 6.3983;
    00457   backwardPoly[1]->pPoints[2].point.z = 13.7076;
    00458   backwardPoly[1]->pPoints[2].sLen = 0.0860842;
    00459   backwardPoly[1]->pPoints[3].point.x = 19.7998;
    00460   backwardPoly[1]->pPoints[3].point.y = 6.85233;
    00461   backwardPoly[1]->pPoints[3].point.z = 2.52472;
    00462   backwardPoly[1]->pPoints[3].sLen = 0.377902;
    00463   backwardPoly[2]->numPoints = 4;
    00464   backwardPoly[2]->rollAngle = 0;
    00465   backwardPoly[2]->stepLength = 53;
    00466   backwardPoly[2]->pPoints[0].point.x = -35.1536;
    00467   backwardPoly[2]->pPoints[0].point.y = 33.4067;
    00468   backwardPoly[2]->pPoints[0].point.z = -12.6396;
    00469   backwardPoly[2]->pPoints[0].sLen = 0.172122;
    00470   backwardPoly[2]->pPoints[1].point.x = -11.4648;
    00471   backwardPoly[2]->pPoints[1].point.y = -5.42247;
    00472   backwardPoly[2]->pPoints[1].point.z = 21.2323;
    00473   backwardPoly[2]->pPoints[1].sLen = 0.317296;
    00474   backwardPoly[2]->pPoints[2].point.x = 32.6411;
    00475   backwardPoly[2]->pPoints[2].point.y = 3.09892;
    00476   backwardPoly[2]->pPoints[2].point.z = 38.4976;
    00477   backwardPoly[2]->pPoints[2].sLen = -0.132091;
    00478   backwardPoly[2]->pPoints[3].point.x = 30.5193;
    00479   backwardPoly[2]->pPoints[3].point.y = 4.43412;
    00480   backwardPoly[2]->pPoints[3].point.z = 9.44578;
    00481   backwardPoly[2]->pPoints[3].sLen = 0.642673;
    00482   backwardPoly[3]->numPoints = 4;
    00483   backwardPoly[3]->rollAngle = 0;
    00484   backwardPoly[3]->stepLength = 53;
    00485   backwardPoly[3]->pPoints[0].point.x = -35.1536;
    00486   backwardPoly[3]->pPoints[0].point.y = -33.4067;
    00487   backwardPoly[3]->pPoints[0].point.z = -12.6396;
    00488   backwardPoly[3]->pPoints[0].sLen = 0.172122;
    00489   backwardPoly[3]->pPoints[1].point.x = -11.4648;
    00490   backwardPoly[3]->pPoints[1].point.y = 5.42247;
    00491   backwardPoly[3]->pPoints[1].point.z = 21.2323;
    00492   backwardPoly[3]->pPoints[1].sLen = 0.317296;
    00493   backwardPoly[3]->pPoints[2].point.x = 32.6411;
    00494   backwardPoly[3]->pPoints[2].point.y = -3.09892;
    00495   backwardPoly[3]->pPoints[2].point.z = 38.4976;
    00496   backwardPoly[3]->pPoints[2].sLen = -0.132091;
    00497   backwardPoly[3]->pPoints[3].point.x = 30.5193;
    00498   backwardPoly[3]->pPoints[3].point.y = -4.43412;
    00499   backwardPoly[3]->pPoints[3].point.z = 9.44578;
    00500   backwardPoly[3]->pPoints[3].sLen = 0.642673;
    00501   backwardPoly[0]->finalize();
    00502   backwardPoly[1]->finalize();
    00503   backwardPoly[2]->finalize();
    00504   backwardPoly[3]->finalize();
    00505
    00506   //sbackwardPoly parameters
    00507   sbackwardPoly[0]->numPoints = 4;
    00508   sbackwardPoly[0]->rollAngle = 0;
    00509   sbackwardPoly[0]->stepLength = 61;
    00510   sbackwardPoly[0]->pPoints[0].point.x = -33.7375;
    00511   sbackwardPoly[0]->pPoints[0].point.y = -6.70116;
    00512   sbackwardPoly[0]->pPoints[0].point.z = 10.663;
    00513   sbackwardPoly[0]->pPoints[0].sLen = 0.160136;
    00514   sbackwardPoly[0]->pPoints[1].point.x = -21.0508;
    00515   sbackwardPoly[0]->pPoints[1].point.y = 17.7523;
    00516   sbackwardPoly[0]->pPoints[1].point.z = 8.24884;
    00517   sbackwardPoly[0]->pPoints[1].sLen = 0.297181;
    00518   sbackwardPoly[0]->pPoints[2].point.x = 35.8702;
    00519   sbackwardPoly[0]->pPoints[2].point.y = -7.50427;
    00520   sbackwardPoly[0]->pPoints[2].point.z = 25.9035;
    00521   sbackwardPoly[0]->pPoints[2].sLen = 0.0713092;
    00522   sbackwardPoly[0]->pPoints[3].point.x = 21.3798;
    00523   sbackwardPoly[0]->pPoints[3].point.y = -4.7102;
    00524   sbackwardPoly[0]->pPoints[3].point.z = 0.83056;
    00525   sbackwardPoly[0]->pPoints[3].sLen = 0.471374;
    00526   sbackwardPoly[1]->numPoints = 4;
    00527   sbackwardPoly[1]->rollAngle = 0;
    00528   sbackwardPoly[1]->stepLength = 61;
    00529   sbackwardPoly[1]->pPoints[0].point.x = -33.7375;
    00530   sbackwardPoly[1]->pPoints[0].point.y = 6.70116;
    00531   sbackwardPoly[1]->pPoints[0].point.z = 10.663;
    00532   sbackwardPoly[1]->pPoints[0].sLen = 0.160136;
    00533   sbackwardPoly[1]->pPoints[1].point.x = -21.0508;
    00534   sbackwardPoly[1]->pPoints[1].point.y = -17.7523;
    00535   sbackwardPoly[1]->pPoints[1].point.z = 8.24884;
    00536   sbackwardPoly[1]->pPoints[1].sLen = 0.297181;
    00537   sbackwardPoly[1]->pPoints[2].point.x = 35.8702;
    00538   sbackwardPoly[1]->pPoints[2].point.y = 7.50427;
    00539   sbackwardPoly[1]->pPoints[2].point.z = 25.9035;
    00540   sbackwardPoly[1]->pPoints[2].sLen = 0.0713092;
    00541   sbackwardPoly[1]->pPoints[3].point.x = 21.3798;
    00542   sbackwardPoly[1]->pPoints[3].point.y = 4.7102;
    00543   sbackwardPoly[1]->pPoints[3].point.z = 0.83056;
    00544   sbackwardPoly[1]->pPoints[3].sLen = 0.471374;
    00545   sbackwardPoly[2]->numPoints = 4;
    00546   sbackwardPoly[2]->rollAngle = 0;
    00547   sbackwardPoly[2]->stepLength = 61;
    00548   sbackwardPoly[2]->pPoints[0].point.x = -19.6924;
    00549   sbackwardPoly[2]->pPoints[0].point.y = 5.31819;
    00550   sbackwardPoly[2]->pPoints[0].point.z = -10.9156;
    00551   sbackwardPoly[2]->pPoints[0].sLen = 0.102415;
    00552   sbackwardPoly[2]->pPoints[1].point.x = -4.86939;
    00553   sbackwardPoly[2]->pPoints[1].point.y = -4.86016;
    00554   sbackwardPoly[2]->pPoints[1].point.z = 10.6604;
    00555   sbackwardPoly[2]->pPoints[1].sLen = 0.324365;
    00556   sbackwardPoly[2]->pPoints[2].point.x = 37.444;
    00557   sbackwardPoly[2]->pPoints[2].point.y = 7.2039;
    00558   sbackwardPoly[2]->pPoints[2].point.z = 45.862;
    00559   sbackwardPoly[2]->pPoints[2].sLen = -0.104819;
    00560   sbackwardPoly[2]->pPoints[3].point.x = 29.7827;
    00561   sbackwardPoly[2]->pPoints[3].point.y = 0.697765;
    00562   sbackwardPoly[2]->pPoints[3].point.z = 19.9519;
    00563   sbackwardPoly[2]->pPoints[3].sLen = 0.678039;
    00564   sbackwardPoly[3]->numPoints = 4;
    00565   sbackwardPoly[3]->rollAngle = 0;
    00566   sbackwardPoly[3]->stepLength = 61;
    00567   sbackwardPoly[3]->pPoints[0].point.x = -19.6924;
    00568   sbackwardPoly[3]->pPoints[0].point.y = -5.31819;
    00569   sbackwardPoly[3]->pPoints[0].point.z = -10.9156;
    00570   sbackwardPoly[3]->pPoints[0].sLen = 0.102415;
    00571   sbackwardPoly[3]->pPoints[1].point.x = -4.86939;
    00572   sbackwardPoly[3]->pPoints[1].point.y = 4.86016;
    00573   sbackwardPoly[3]->pPoints[1].point.z = 10.6604;
    00574   sbackwardPoly[3]->pPoints[1].sLen = 0.324365;
    00575   sbackwardPoly[3]->pPoints[2].point.x = 37.444;
    00576   sbackwardPoly[3]->pPoints[2].point.y = -7.2039;
    00577   sbackwardPoly[3]->pPoints[2].point.z = 45.862;
    00578   sbackwardPoly[3]->pPoints[2].sLen = -0.104819;
    00579   sbackwardPoly[3]->pPoints[3].point.x = 29.7827;
    00580   sbackwardPoly[3]->pPoints[3].point.y = -0.697765;
    00581   sbackwardPoly[3]->pPoints[3].point.z = 19.9519;
    00582   sbackwardPoly[3]->pPoints[3].sLen = 0.678039;
    00583   sbackwardPoly[0]->finalize();
    00584   sbackwardPoly[1]->finalize();
    00585   sbackwardPoly[2]->finalize();
    00586   sbackwardPoly[3]->finalize();
    00587
    00588   //dbackwardPoly parameters
    00589   dbackwardPoly[0]->numPoints = 4;
    00590   dbackwardPoly[0]->rollAngle = 0;
    00591   dbackwardPoly[0]->stepLength = 52;
    00592   dbackwardPoly[0]->pPoints[0].point.x = -33.5813;
    00593   dbackwardPoly[0]->pPoints[0].point.y = -0.1943;
    00594   dbackwardPoly[0]->pPoints[0].point.z = 7.56177;
    00595   dbackwardPoly[0]->pPoints[0].sLen = -0.00454608;
    00596   dbackwardPoly[0]->pPoints[1].point.x = -15.7947;
    00597   dbackwardPoly[0]->pPoints[1].point.y = 21.9034;
    00598   dbackwardPoly[0]->pPoints[1].point.z = 0.786908;
    00599   dbackwardPoly[0]->pPoints[1].sLen = 0.364702;
    00600   dbackwardPoly[0]->pPoints[2].point.x = 38.1074;
    00601   dbackwardPoly[0]->pPoints[2].point.y = -4.01859;
    00602   dbackwardPoly[0]->pPoints[2].point.z = 21.7376;
    00603   dbackwardPoly[0]->pPoints[2].sLen = 0.187669;
    00604   dbackwardPoly[0]->pPoints[3].point.x = 24.8112;
    00605   dbackwardPoly[0]->pPoints[3].point.y = 2.65664;
    00606   dbackwardPoly[0]->pPoints[3].point.z = 10.205;
    00607   dbackwardPoly[0]->pPoints[3].sLen = 0.452174;
    00608   dbackwardPoly[1]->numPoints = 4;
    00609   dbackwardPoly[1]->rollAngle = 0;
    00610   dbackwardPoly[1]->stepLength = 52;
    00611   dbackwardPoly[1]->pPoints[0].point.x = -33.5813;
    00612   dbackwardPoly[1]->pPoints[0].point.y = 0.1943;
    00613   dbackwardPoly[1]->pPoints[0].point.z = 7.56177;
    00614   dbackwardPoly[1]->pPoints[0].sLen = -0.00454608;
    00615   dbackwardPoly[1]->pPoints[1].point.x = -15.7947;
    00616   dbackwardPoly[1]->pPoints[1].point.y = -21.9034;
    00617   dbackwardPoly[1]->pPoints[1].point.z = 0.786908;
    00618   dbackwardPoly[1]->pPoints[1].sLen = 0.364702;
    00619   dbackwardPoly[1]->pPoints[2].point.x = 38.1074;
    00620   dbackwardPoly[1]->pPoints[2].point.y = 4.01859;
    00621   dbackwardPoly[1]->pPoints[2].point.z = 21.7376;
    00622   dbackwardPoly[1]->pPoints[2].sLen = 0.187669;
    00623   dbackwardPoly[1]->pPoints[3].point.x = 24.8112;
    00624   dbackwardPoly[1]->pPoints[3].point.y = -2.65664;
    00625   dbackwardPoly[1]->pPoints[3].point.z = 10.205;
    00626   dbackwardPoly[1]->pPoints[3].sLen = 0.452174;
    00627   dbackwardPoly[2]->numPoints = 4;
    00628   dbackwardPoly[2]->rollAngle = 0;
    00629   dbackwardPoly[2]->stepLength = 52;
    00630   dbackwardPoly[2]->pPoints[0].point.x = -27.5297;
    00631   dbackwardPoly[2]->pPoints[0].point.y = 11.0379;
    00632   dbackwardPoly[2]->pPoints[0].point.z = -6.243;
    00633   dbackwardPoly[2]->pPoints[0].sLen = 0.152211;
    00634   dbackwardPoly[2]->pPoints[1].point.x = -2.84302;
    00635   dbackwardPoly[2]->pPoints[1].point.y = -4.34425;
    00636   dbackwardPoly[2]->pPoints[1].point.z = 11.8847;
    00637   dbackwardPoly[2]->pPoints[1].sLen = 0.265556;
    00638   dbackwardPoly[2]->pPoints[2].point.x = 41.8341;
    00639   dbackwardPoly[2]->pPoints[2].point.y = 10.6103;
    00640   dbackwardPoly[2]->pPoints[2].point.z = 46.5946;
    00641   dbackwardPoly[2]->pPoints[2].sLen = 0.0330728;
    00642   dbackwardPoly[2]->pPoints[3].point.x = 34.0414;
    00643   dbackwardPoly[2]->pPoints[3].point.y = -0.346163;
    00644   dbackwardPoly[2]->pPoints[3].point.z = 16.6259;
    00645   dbackwardPoly[2]->pPoints[3].sLen = 0.549161;
    00646   dbackwardPoly[3]->numPoints = 4;
    00647   dbackwardPoly[3]->rollAngle = 0;
    00648   dbackwardPoly[3]->stepLength = 52;
    00649   dbackwardPoly[3]->pPoints[0].point.x = -27.5297;
    00650   dbackwardPoly[3]->pPoints[0].point.y = -11.0379;
    00651   dbackwardPoly[3]->pPoints[0].point.z = -6.243;
    00652   dbackwardPoly[3]->pPoints[0].sLen = 0.152211;
    00653   dbackwardPoly[3]->pPoints[1].point.x = -2.84302;
    00654   dbackwardPoly[3]->pPoints[1].point.y = 4.34425;
    00655   dbackwardPoly[3]->pPoints[1].point.z = 11.8847;
    00656   dbackwardPoly[3]->pPoints[1].sLen = 0.265556;
    00657   dbackwardPoly[3]->pPoints[2].point.x = 41.8341;
    00658   dbackwardPoly[3]->pPoints[2].point.y = -10.6103;
    00659   dbackwardPoly[3]->pPoints[2].point.z = 46.5946;
    00660   dbackwardPoly[3]->pPoints[2].sLen = 0.0330728;
    00661   dbackwardPoly[3]->pPoints[3].point.x = 34.0414;
    00662   dbackwardPoly[3]->pPoints[3].point.y = 0.346163;
    00663   dbackwardPoly[3]->pPoints[3].point.z = 16.6259;
    00664   dbackwardPoly[3]->pPoints[3].sLen = 0.549161;
    00665   dbackwardPoly[0]->finalize();
    00666   dbackwardPoly[1]->finalize();
    00667   dbackwardPoly[2]->finalize();
    00668   dbackwardPoly[3]->finalize();
    00669
    00670   //sidewardsPoly parameters
    00671   sidewardsPoly[0]->numPoints = 4;
    00672   sidewardsPoly[0]->rollAngle = 0;
    00673   sidewardsPoly[0]->stepLength = 56;
    00674   sidewardsPoly[0]->pPoints[0].point.x = -15.3573;
    00675   sidewardsPoly[0]->pPoints[0].point.y = -7.10737;
    00676   sidewardsPoly[0]->pPoints[0].point.z = -11.1891;
    00677   sidewardsPoly[0]->pPoints[0].sLen = 0.0420935;
    00678   sidewardsPoly[0]->pPoints[1].point.x = -9.09527;
    00679   sidewardsPoly[0]->pPoints[1].point.y = 3.34993;
    00680   sidewardsPoly[0]->pPoints[1].point.z = -4.0212;
    00681   sidewardsPoly[0]->pPoints[1].sLen = 0.462489;
    00682   sidewardsPoly[0]->pPoints[2].point.x = 22.3719;
    00683   sidewardsPoly[0]->pPoints[2].point.y = -4.84692;
    00684   sidewardsPoly[0]->pPoints[2].point.z = 8.82873;
    00685   sidewardsPoly[0]->pPoints[2].sLen = -0.0516716;
    00686   sidewardsPoly[0]->pPoints[3].point.x = 20.7936;
    00687   sidewardsPoly[0]->pPoints[3].point.y = 22.176;
    00688   sidewardsPoly[0]->pPoints[3].point.z = 3.78541;
    00689   sidewardsPoly[0]->pPoints[3].sLen = 0.547089;
    00690   sidewardsPoly[1]->numPoints = 4;
    00691   sidewardsPoly[1]->rollAngle = 0;
    00692   sidewardsPoly[1]->stepLength = 56;
    00693   sidewardsPoly[1]->pPoints[0].point.x = -15.3573;
    00694   sidewardsPoly[1]->pPoints[0].point.y = 7.10737;
    00695   sidewardsPoly[1]->pPoints[0].point.z = -11.1891;
    00696   sidewardsPoly[1]->pPoints[0].sLen = 0.0420935;
    00697   sidewardsPoly[1]->pPoints[1].point.x = -9.09527;
    00698   sidewardsPoly[1]->pPoints[1].point.y = -3.34993;
    00699   sidewardsPoly[1]->pPoints[1].point.z = -4.0212;
    00700   sidewardsPoly[1]->pPoints[1].sLen = 0.462489;
    00701   sidewardsPoly[1]->pPoints[2].point.x = 22.3719;
    00702   sidewardsPoly[1]->pPoints[2].point.y = 4.84692;
    00703   sidewardsPoly[1]->pPoints[2].point.z = 8.82873;
    00704   sidewardsPoly[1]->pPoints[2].sLen = -0.0516716;
    00705   sidewardsPoly[1]->pPoints[3].point.x = 20.7936;
    00706   sidewardsPoly[1]->pPoints[3].point.y = -22.176;
    00707   sidewardsPoly[1]->pPoints[3].point.z = 3.78541;
    00708   sidewardsPoly[1]->pPoints[3].sLen = 0.547089;
    00709   sidewardsPoly[2]->numPoints = 4;
    00710   sidewardsPoly[2]->rollAngle = 0;
    00711   sidewardsPoly[2]->stepLength = 56;
    00712   sidewardsPoly[2]->pPoints[0].point.x = -25.3751;
    00713   sidewardsPoly[2]->pPoints[0].point.y = -14.5053;
    00714   sidewardsPoly[2]->pPoints[0].point.z = -0.76603;
    00715   sidewardsPoly[2]->pPoints[0].sLen = -0.0382207;
    00716   sidewardsPoly[2]->pPoints[1].point.x = -24.1757;
    00717   sidewardsPoly[2]->pPoints[1].point.y = 1.42974;
    00718   sidewardsPoly[2]->pPoints[1].point.z = 28.0637;
    00719   sidewardsPoly[2]->pPoints[1].sLen = 0.406898;
    00720   sidewardsPoly[2]->pPoints[2].point.x = 7.94299;
    00721   sidewardsPoly[2]->pPoints[2].point.y = -5.45393;
    00722   sidewardsPoly[2]->pPoints[2].point.z = 26.9968;
    00723   sidewardsPoly[2]->pPoints[2].sLen = 0.115272;
    00724   sidewardsPoly[2]->pPoints[3].point.x = 3.20244;
    00725   sidewardsPoly[2]->pPoints[3].point.y = -19.7213;
    00726   sidewardsPoly[2]->pPoints[3].point.z = 1.12852;
    00727   sidewardsPoly[2]->pPoints[3].sLen = 0.516051;
    00728   sidewardsPoly[3]->numPoints = 4;
    00729   sidewardsPoly[3]->rollAngle = 0;
    00730   sidewardsPoly[3]->stepLength = 56;
    00731   sidewardsPoly[3]->pPoints[0].point.x = -25.3751;
    00732   sidewardsPoly[3]->pPoints[0].point.y = 14.5053;
    00733   sidewardsPoly[3]->pPoints[0].point.z = -0.76603;
    00734   sidewardsPoly[3]->pPoints[0].sLen = -0.0382207;
    00735   sidewardsPoly[3]->pPoints[1].point.x = -24.1757;
    00736   sidewardsPoly[3]->pPoints[1].point.y = -1.42974;
    00737   sidewardsPoly[3]->pPoints[1].point.z = 28.0637;
    00738   sidewardsPoly[3]->pPoints[1].sLen = 0.406898;
    00739   sidewardsPoly[3]->pPoints[2].point.x = 7.94299;
    00740   sidewardsPoly[3]->pPoints[2].point.y = 5.45393;
    00741   sidewardsPoly[3]->pPoints[2].point.z = 26.9968;
    00742   sidewardsPoly[3]->pPoints[2].sLen = 0.115272;
    00743   sidewardsPoly[3]->pPoints[3].point.x = 3.20244;
    00744   sidewardsPoly[3]->pPoints[3].point.y = 19.7213;
    00745   sidewardsPoly[3]->pPoints[3].point.z = 1.12852;
    00746   sidewardsPoly[3]->pPoints[3].sLen = 0.516051;
    00747   sidewardsPoly[0]->finalize();
    00748   sidewardsPoly[1]->finalize();
    00749   sidewardsPoly[2]->finalize();
    00750   sidewardsPoly[3]->finalize();
    00751
    00752   //boostPoly parameters
    00753   boostPoly[0]->numPoints = 4;
    00754   boostPoly[0]->rollAngle = 0;
    00755   boostPoly[0]->stepLength = 58;
    00756   boostPoly[0]->pPoints[0].point.x = -54.1131;
    00757   boostPoly[0]->pPoints[0].point.y = 3.85926;
    00758   boostPoly[0]->pPoints[0].point.z = -49.6204;
    00759   boostPoly[0]->pPoints[0].sLen = 0.0817619;
    00760   boostPoly[0]->pPoints[1].point.x = -40.627;
    00761   boostPoly[0]->pPoints[1].point.y = 8.24308;
    00762   boostPoly[0]->pPoints[1].point.z = -23.3423;
    00763   boostPoly[0]->pPoints[1].sLen = 0.288787;
    00764   boostPoly[0]->pPoints[2].point.x = 5.69701;
    00765   boostPoly[0]->pPoints[2].point.y = 26.2382;
    00766   boostPoly[0]->pPoints[2].point.z = -11.3584;
    00767   boostPoly[0]->pPoints[2].sLen = 0.0623604;
    00768   boostPoly[0]->pPoints[3].point.x = -0.820145;
    00769   boostPoly[0]->pPoints[3].point.y = 5.34335;
    00770   boostPoly[0]->pPoints[3].point.z = -57.6659;
    00771   boostPoly[0]->pPoints[3].sLen = 0.567091;
    00772   boostPoly[1]->numPoints = 4;
    00773   boostPoly[1]->rollAngle = 0;
    00774   boostPoly[1]->stepLength = 58;
    00775   boostPoly[1]->pPoints[0].point.x = -54.1131;
    00776   boostPoly[1]->pPoints[0].point.y = -3.85926;
    00777   boostPoly[1]->pPoints[0].point.z = -49.6204;
    00778   boostPoly[1]->pPoints[0].sLen = 0.0817619;
    00779   boostPoly[1]->pPoints[1].point.x = -40.627;
    00780   boostPoly[1]->pPoints[1].point.y = -8.24308;
    00781   boostPoly[1]->pPoints[1].point.z = -23.3423;
    00782   boostPoly[1]->pPoints[1].sLen = 0.288787;
    00783   boostPoly[1]->pPoints[2].point.x = 5.69701;
    00784   boostPoly[1]->pPoints[2].point.y = -26.2382;
    00785   boostPoly[1]->pPoints[2].point.z = -11.3584;
    00786   boostPoly[1]->pPoints[2].sLen = 0.0623604;
    00787   boostPoly[1]->pPoints[3].point.x = -0.820145;
    00788   boostPoly[1]->pPoints[3].point.y = -5.34335;
    00789   boostPoly[1]->pPoints[3].point.z = -57.6659;
    00790   boostPoly[1]->pPoints[3].sLen = 0.567091;
    00791   boostPoly[2]->numPoints = 4;
    00792   boostPoly[2]->rollAngle = 0;
    00793   boostPoly[2]->stepLength = 58;
    00794   boostPoly[2]->pPoints[0].point.x = -26.0478;
    00795   boostPoly[2]->pPoints[0].point.y = -7.82422;
    00796   boostPoly[2]->pPoints[0].point.z = -16.88;
    00797   boostPoly[2]->pPoints[0].sLen = 0.0654221;
    00798   boostPoly[2]->pPoints[1].point.x = -27.2295;
    00799   boostPoly[2]->pPoints[1].point.y = 7.91244;
    00800   boostPoly[2]->pPoints[1].point.z = -22.6058;
    00801   boostPoly[2]->pPoints[1].sLen = 0.39671;
    00802   boostPoly[2]->pPoints[2].point.x = 31.8959;
    00803   boostPoly[2]->pPoints[2].point.y = -1.54275;
    00804   boostPoly[2]->pPoints[2].point.z = -11.4453;
    00805   boostPoly[2]->pPoints[2].sLen = -0.0241281;
    00806   boostPoly[2]->pPoints[3].point.x = 37.7492;
    00807   boostPoly[2]->pPoints[3].point.y = 38.3235;
    00808   boostPoly[2]->pPoints[3].point.z = -25.741;
    00809   boostPoly[2]->pPoints[3].sLen = 0.561996;
    00810   boostPoly[3]->numPoints = 4;
    00811   boostPoly[3]->rollAngle = 0;
    00812   boostPoly[3]->stepLength = 58;
    00813   boostPoly[3]->pPoints[0].point.x = -26.0478;
    00814   boostPoly[3]->pPoints[0].point.y = 7.82422;
    00815   boostPoly[3]->pPoints[0].point.z = -16.88;
    00816   boostPoly[3]->pPoints[0].sLen = 0.0654221;
    00817   boostPoly[3]->pPoints[1].point.x = -27.2295;
    00818   boostPoly[3]->pPoints[1].point.y = -7.91244;
    00819   boostPoly[3]->pPoints[1].point.z = -22.6058;
    00820   boostPoly[3]->pPoints[1].sLen = 0.39671;
    00821   boostPoly[3]->pPoints[2].point.x = 31.8959;
    00822   boostPoly[3]->pPoints[2].point.y = 1.54275;
    00823   boostPoly[3]->pPoints[2].point.z = -11.4453;
    00824   boostPoly[3]->pPoints[2].sLen = -0.0241281;
    00825   boostPoly[3]->pPoints[3].point.x = 37.7492;
    00826   boostPoly[3]->pPoints[3].point.y = -38.3235;
    00827   boostPoly[3]->pPoints[3].point.z = -25.741;
    00828   boostPoly[3]->pPoints[3].sLen = 0.561996;
    00829   boostPoly[0]->finalize();
    00830   boostPoly[1]->finalize();
    00831   boostPoly[2]->finalize();
    00832   boostPoly[3]->finalize();
    00833
    00834   //grabPoly parameters
    00835   grabPoly[0]->numPoints = 4;
    00836   grabPoly[0]->rollAngle = 0;
    00837   grabPoly[0]->stepLength = 42;
    00838   grabPoly[0]->pPoints[0].point.x = 2.5;
    00839   grabPoly[0]->pPoints[0].point.y = 0;
    00840   grabPoly[0]->pPoints[0].point.z = -25;
    00841   grabPoly[0]->pPoints[0].sLen = 0.06;
    00842   grabPoly[0]->pPoints[1].point.x = 2.5;
    00843   grabPoly[0]->pPoints[1].point.y = 0;
    00844   grabPoly[0]->pPoints[1].point.z = -10;
    00845   grabPoly[0]->pPoints[1].sLen = 0.39;
    00846   grabPoly[0]->pPoints[2].point.x = 12.5;
    00847   grabPoly[0]->pPoints[2].point.y = 0;
    00848   grabPoly[0]->pPoints[2].point.z = -10;
    00849   grabPoly[0]->pPoints[2].sLen = 0.06;
    00850   grabPoly[0]->pPoints[3].point.x = 12.5;
    00851   grabPoly[0]->pPoints[3].point.y = 0;
    00852   grabPoly[0]->pPoints[3].point.z = -25;
    00853   grabPoly[0]->pPoints[3].sLen = 0.49;
    00854   grabPoly[1]->numPoints = 4;
    00855   grabPoly[1]->rollAngle = 0;
    00856   grabPoly[1]->stepLength = 42;
    00857   grabPoly[1]->pPoints[0].point.x = 2.5;
    00858   grabPoly[1]->pPoints[0].point.y = 0;
    00859   grabPoly[1]->pPoints[0].point.z = -25;
    00860   grabPoly[1]->pPoints[0].sLen = 0.06;
    00861   grabPoly[1]->pPoints[1].point.x = 2.5;
    00862   grabPoly[1]->pPoints[1].point.y = 0;
    00863   grabPoly[1]->pPoints[1].point.z = -10;
    00864   grabPoly[1]->pPoints[1].sLen = 0.39;
    00865   grabPoly[1]->pPoints[2].point.x = 12.5;
    00866   grabPoly[1]->pPoints[2].point.y = 0;
    00867   grabPoly[1]->pPoints[2].point.z = -10;
    00868   grabPoly[1]->pPoints[2].sLen = 0.06;
    00869   grabPoly[1]->pPoints[3].point.x = 12.5;
    00870   grabPoly[1]->pPoints[3].point.y = 0;
    00871   grabPoly[1]->pPoints[3].point.z = -25;
    00872   grabPoly[1]->pPoints[3].sLen = 0.49;
    00873   grabPoly[2]->numPoints = 4;
    00874   grabPoly[2]->rollAngle = 0;
    00875   grabPoly[2]->stepLength = 42;
    00876   grabPoly[2]->pPoints[0].point.x = -25;
    00877   grabPoly[2]->pPoints[0].point.y = 0;
    00878   grabPoly[2]->pPoints[0].point.z = 0;
    00879   grabPoly[2]->pPoints[0].sLen = 0.06;
    00880   grabPoly[2]->pPoints[1].point.x = -25;
    00881   grabPoly[2]->pPoints[1].point.y = 0;
    00882   grabPoly[2]->pPoints[1].point.z = 25;
    00883   grabPoly[2]->pPoints[1].sLen = 0.39;
    00884   grabPoly[2]->pPoints[2].point.x = 25;
    00885   grabPoly[2]->pPoints[2].point.y = 0;
    00886   grabPoly[2]->pPoints[2].point.z = 25;
    00887   grabPoly[2]->pPoints[2].sLen = 0.06;
    00888   grabPoly[2]->pPoints[3].point.x = 25;
    00889   grabPoly[2]->pPoints[3].point.y = 0;
    00890   grabPoly[2]->pPoints[3].point.z = 0;
    00891   grabPoly[2]->pPoints[3].sLen = 0.49;
    00892   grabPoly[3]->numPoints = 4;
    00893   grabPoly[3]->rollAngle = 0;
    00894   grabPoly[3]->stepLength = 42;
    00895   grabPoly[3]->pPoints[0].point.x = -25;
    00896   grabPoly[3]->pPoints[0].point.y = 0;
    00897   grabPoly[3]->pPoints[0].point.z = 0;
    00898   grabPoly[3]->pPoints[0].sLen = 0.06;
    00899   grabPoly[3]->pPoints[1].point.x = -25;
    00900   grabPoly[3]->pPoints[1].point.y = 0;
    00901   grabPoly[3]->pPoints[1].point.z = 25;
    00902   grabPoly[3]->pPoints[1].sLen = 0.39;
    00903   grabPoly[3]->pPoints[2].point.x = 25;
    00904   grabPoly[3]->pPoints[2].point.y = 0;
    00905   grabPoly[3]->pPoints[2].point.z = 25;
    00906   grabPoly[3]->pPoints[2].sLen = 0.06;
    00907   grabPoly[3]->pPoints[3].point.x = 25;
    00908   grabPoly[3]->pPoints[3].point.y = 0;
    00909   grabPoly[3]->pPoints[3].point.z = 0;
    00910   grabPoly[3]->pPoints[3].sLen = 0.49;
    00911   grabPoly[0]->finalize();
    00912   grabPoly[1]->finalize();
    00913   grabPoly[2]->finalize();
    00914   grabPoly[3]->finalize();
    00915
    00916   //rgrabPoly parameters
    00917   rgrabPoly[0]->numPoints = 4;
    00918   rgrabPoly[0]->rollAngle = 0;
    00919   rgrabPoly[0]->stepLength = 42;
    00920   rgrabPoly[0]->pPoints[0].point.x = -4;
    00921   rgrabPoly[0]->pPoints[0].point.y = 0;
    00922   rgrabPoly[0]->pPoints[0].point.z = -6;
    00923   rgrabPoly[0]->pPoints[0].sLen = 0.06;
    00924   rgrabPoly[0]->pPoints[1].point.x = -5;
    00925   rgrabPoly[0]->pPoints[1].point.y = 0;
    00926   rgrabPoly[0]->pPoints[1].point.z = -1;
    00927   rgrabPoly[0]->pPoints[1].sLen = 0.39;
    00928   rgrabPoly[0]->pPoints[2].point.x = 43;
    00929   rgrabPoly[0]->pPoints[2].point.y = 0;
    00930   rgrabPoly[0]->pPoints[2].point.z = 11;
    00931   rgrabPoly[0]->pPoints[2].sLen = 0.06;
    00932   rgrabPoly[0]->pPoints[3].point.x = 44;
    00933   rgrabPoly[0]->pPoints[3].point.y = 0;
    00934   rgrabPoly[0]->pPoints[3].point.z = 6;
    00935   rgrabPoly[0]->pPoints[3].sLen = 0.49;
    00936   rgrabPoly[1]->numPoints = 4;
    00937   rgrabPoly[1]->rollAngle = 0;
    00938   rgrabPoly[1]->stepLength = 42;
    00939   rgrabPoly[1]->pPoints[0].point.x = -4;
    00940   rgrabPoly[1]->pPoints[0].point.y = 0;
    00941   rgrabPoly[1]->pPoints[0].point.z = -6;
    00942   rgrabPoly[1]->pPoints[0].sLen = 0.06;
    00943   rgrabPoly[1]->pPoints[1].point.x = -5;
    00944   rgrabPoly[1]->pPoints[1].point.y = 0;
    00945   rgrabPoly[1]->pPoints[1].point.z = -1;
    00946   rgrabPoly[1]->pPoints[1].sLen = 0.39;
    00947   rgrabPoly[1]->pPoints[2].point.x = 43;
    00948   rgrabPoly[1]->pPoints[2].point.y = 0;
    00949   rgrabPoly[1]->pPoints[2].point.z = 11;
    00950   rgrabPoly[1]->pPoints[2].sLen = 0.06;
    00951   rgrabPoly[1]->pPoints[3].point.x = 44;
    00952   rgrabPoly[1]->pPoints[3].point.y = 0;
    00953   rgrabPoly[1]->pPoints[3].point.z = 6;
    00954   rgrabPoly[1]->pPoints[3].sLen = 0.49;
    00955   rgrabPoly[2]->numPoints = 4;
    00956   rgrabPoly[2]->rollAngle = 0;
    00957   rgrabPoly[2]->stepLength = 42;
    00958   rgrabPoly[2]->pPoints[0].point.x = -25;
    00959   rgrabPoly[2]->pPoints[0].point.y = 0;
    00960   rgrabPoly[2]->pPoints[0].point.z = 0;
    00961   rgrabPoly[2]->pPoints[0].sLen = 0.06;
    00962   rgrabPoly[2]->pPoints[1].point.x = -25;
    00963   rgrabPoly[2]->pPoints[1].point.y = 0;
    00964   rgrabPoly[2]->pPoints[1].point.z = 24;
    00965   rgrabPoly[2]->pPoints[1].sLen = 0.39;
    00966   rgrabPoly[2]->pPoints[2].point.x = 25;
    00967   rgrabPoly[2]->pPoints[2].point.y = 0;
    00968   rgrabPoly[2]->pPoints[2].point.z = 24;
    00969   rgrabPoly[2]->pPoints[2].sLen = 0.06;
    00970   rgrabPoly[2]->pPoints[3].point.x = 25;
    00971   rgrabPoly[2]->pPoints[3].point.y = 0;
    00972   rgrabPoly[2]->pPoints[3].point.z = 0;
    00973   rgrabPoly[2]->pPoints[3].sLen = 0.49;
    00974   rgrabPoly[3]->numPoints = 4;
    00975   rgrabPoly[3]->rollAngle = 0;
    00976   rgrabPoly[3]->stepLength = 42;
    00977   rgrabPoly[3]->pPoints[0].point.x = -25;
    00978   rgrabPoly[3]->pPoints[0].point.y = 0;
    00979   rgrabPoly[3]->pPoints[0].point.z = 0;
    00980   rgrabPoly[3]->pPoints[0].sLen = 0.06;
    00981   rgrabPoly[3]->pPoints[1].point.x = -25;
    00982   rgrabPoly[3]->pPoints[1].point.y = 0;
    00983   rgrabPoly[3]->pPoints[1].point.z = 24;
    00984   rgrabPoly[3]->pPoints[1].sLen = 0.39;
    00985   rgrabPoly[3]->pPoints[2].point.x = 25;
    00986   rgrabPoly[3]->pPoints[2].point.y = 0;
    00987   rgrabPoly[3]->pPoints[2].point.z = 24;
    00988   rgrabPoly[3]->pPoints[2].sLen = 0.06;
    00989   rgrabPoly[3]->pPoints[3].point.x = 25;
    00990   rgrabPoly[3]->pPoints[3].point.y = 0;
    00991   rgrabPoly[3]->pPoints[3].point.z = 0;
    00992   rgrabPoly[3]->pPoints[3].sLen = 0.49;
    00993   rgrabPoly[0]->finalize();
    00994   rgrabPoly[1]->finalize();
    00995   rgrabPoly[2]->finalize();
    00996   rgrabPoly[3]->finalize();
    00997 }
    00998
    00999 GT2005Polygon* GT2005Parameters::calculateCurrentPoly(int leg, Pose2D mRequest, WalkRequest::WalkType walkType)
    01000 {
    01001   /*/ remember last time in boost mode
    01002   if (walkType == WalkRequest::boost)
    01003   {
    01004     framesSinceBoost = SystemCall::getCurrentSystemTime();
    01005   }
    01006   unsigned long bTime = SystemCall::getCurrentSystemTime() - framesSinceBoost;
    01007   // boost mode shutdown sequence
    01008   if ((bTime > 10) && (bTime < 2000))
    01009   {
    01010     mRequest.translation.x = min(mRequest.translation.x, 200.0 + (100.0 * (1.0 - (bTime / 1990.0))));
    01011   }//*/
    01012
    01013   if (activePoints > 0)
    01014   {
    01015     if (specialMode)
    01016     {
    01017       GT2005Polygon* temp = rotPoly[leg]->copy();
    01018       return temp;
    01019     }
    01020     else
    01021     {
    01022       GT2005Polygon* temp;
    01023       //temp = evoPoly[leg]->copy();
    01024       // reactivate for normal mode
    01025       switch (walkType)
    01026       {
    01027         case WalkRequest::normal:
    01028         case WalkRequest::boost:
    01029         {
    01030           if (mRequest.translation.x < 0)
    01031           {
    01032             if (mRequest.translation.x < -200) // mix fast backward
    01033             {
    01034               double a = (mRequest.translation.x + 200) / -100.0;
    01035               temp = sbackwardPoly[leg]->interpolateShape(a, backwardPoly[leg]);
    01036             }
    01037             else // mix normal/smooth backward
    01038             {
    01039               double a = mRequest.translation.x / -200.0;
    01040               temp = omniPoly[leg]->interpolateShape(a, sbackwardPoly[leg]);
    01041             }
    01042           }
    01043           else
    01044           {
    01045             if (mRequest.translation.x > 200) // mix fast forward
    01046             {
    01047               if (walkType == WalkRequest::boost)
    01048               {
    01049                 double a = (mRequest.translation.x - 200) / 100.0;
    01050                 temp = sforwardPoly[leg]->interpolateShape(a, boostPoly[leg]);
    01051               }
    01052               else
    01053               {
    01054                 double a = (mRequest.translation.x - 200) / 100.0;
    01055                 temp = sforwardPoly[leg]->interpolateShape(a, forwardPoly[leg]);
    01056               }
    01057             }
    01058             else // mix normal/smooth forward
    01059             {
    01060               double a = mRequest.translation.x / 200.0;
    01061               temp = omniPoly[leg]->interpolateShape(a, sforwardPoly[leg]);
    01062             }
    01063           }
    01064           // mix sidestepping
    01065           if (fabs(mRequest.translation.y) > 20)
    01066           {
    01067             GT2005Polygon* temp2;
    01068             GT2005Polygon* temp3;
    01069             double a;
    01070             // calculate best sidestepping polygon
    01071             if (mRequest.translation.x < 0)
    01072             {
    01073               a = mRequest.translation.x / -300.0;
    01074               temp2 = sidewardsPoly[leg]->interpolateShape(a, dbackwardPoly[leg]);
    01075             }
    01076             else
    01077             {
    01078               a = mRequest.translation.x / 300.0;
    01079               temp2 = sidewardsPoly[leg]->interpolateShape(a, dforwardPoly[leg]);
    01080             }
    01081             // mix it up
    01082             temp3 = temp->copy();
    01083             delete temp;
    01084             a = (fabs(mRequest.translation.y) - 20) / 280.0;
    01085             temp = temp3->interpolateShape(a, temp2);
    01086             delete temp2;
    01087             delete temp3;
    01088           }
    01089
    01090           // mix rotation
    01091           if (fabs(mRequest.rotation) > 0.2)
    01092           {
    01093             GT2005Polygon* temp2;
    01094             temp2 = temp->copy();
    01095             delete temp;
    01096             double a = min((fabs(mRequest.rotation) / 5.236), 1.0); //7.8
    01097             temp = temp2->interpolateShape(a, rotPoly[leg]);
    01098             delete temp2;
    01099           }
    01100           temp->finalize(); // this is required for accurate ground phase approximation
    01101           break;
    01102         }
    01103         case WalkRequest::walkWithBall:
    01104         {
    01105           temp = grabPoly[leg]->copy();
    01106           // mix rotation
    01107           if (fabs(mRequest.rotation) > 0.2)
    01108           {
    01109             GT2005Polygon* temp2;
    01110             temp2 = temp->copy();
    01111             delete temp;
    01112             double a = min((fabs(mRequest.rotation) / 5.236), 1.0);
    01113             temp = temp2->interpolateShape(a, rgrabPoly[leg]);
    01114             delete temp2;
    01115           }
    01116           temp->finalize(); // this is required for accurate ground phase approximation
    01117           break;
    01118         }
    01119         default:
    01120         {
    01121           temp = evoPoly[leg]->copy();
    01122           break;
    01123         }
    01124       }
    01125       //*/
    01126
    01127       return temp;
    01128     }
    01129   }
    01130   return NULL;
    01131 }
    01132
    01133 bool GT2005Parameters::load(char* filename)
    01134 {
    01135   char name[128];
    01136   if (filename != 0)
    01137   {
    01138     strcpy(name, filename);
    01139   }
    01140   InBinaryFile file(name);
    01141   if (file.exists())
    01142   {
    01143     file >> *this;
    01144     return true;
    01145   }
    01146   return false;
    01147 }
    01148
    01149 void GT2005Parameters::save(char* filename)
    01150 {
    01151   char name[128];
    01152   if (filename != 0)
    01153   {
    01154     strcpy(name, filename);
    01155   }
    01156   OutBinaryFile file(name);
    01157   file << *this;
    01158 }
    01159
    01160 In& operator>>(In& stream, GT2005Parameters& Parameters)
    01161 {
    01162
    01163   stream >> Parameters.activePoints;
    01164   stream >> Parameters.stepLength;
    01165   stream >> Parameters.counterRotation;
    01166
    01167   for (int i = 0; i < 4; i++)
    01168   {
    01169     stream >> Parameters.footPhase[i];
    01170     stream >> Parameters.footZeroPosition[i];
    01171     stream >> *Parameters.omniPoly[i];
    01172     stream >> *Parameters.rotPoly[i];
    01173     stream >> *Parameters.sforwardPoly[i];
    01174     stream >> *Parameters.forwardPoly[i];
    01175     stream >> *Parameters.dforwardPoly[i];
    01176     stream >> *Parameters.sbackwardPoly[i];
    01177     stream >> *Parameters.backwardPoly[i];
    01178     stream >> *Parameters.dbackwardPoly[i];
    01179     stream >> *Parameters.sidewardsPoly[i];
    01180     stream >> *Parameters.boostPoly[i];
    01181     stream >> *Parameters.grabPoly[i];
    01182     stream >> *Parameters.rgrabPoly[i];
    01183     stream >> *Parameters.evoPoly[i];
    01184   }
    01185   return stream;
    01186 }
    01187
    01188 Out& operator<<(Out& stream, const GT2005Parameters& Parameters)
    01189 {
    01190   stream << Parameters.activePoints;
    01191   stream << Parameters.stepLength;
    01192   stream << Parameters.counterRotation;
    01193   for (int i = 0; i < 4; i++)
    01194   {
    01195     stream << Parameters.footPhase[i];
    01196     stream << Parameters.footZeroPosition[i];
    01197     stream << *Parameters.omniPoly[i];
    01198     stream << *Parameters.rotPoly[i];
    01199     stream << *Parameters.sforwardPoly[i];
    01200     stream << *Parameters.forwardPoly[i];
    01201     stream << *Parameters.dforwardPoly[i];
    01202     stream << *Parameters.sbackwardPoly[i];
    01203     stream << *Parameters.backwardPoly[i];
    01204     stream << *Parameters.dbackwardPoly[i];
    01205     stream << *Parameters.sidewardsPoly[i];
    01206     stream << *Parameters.boostPoly[i];
    01207     stream << *Parameters.grabPoly[i];
    01208     stream << *Parameters.rgrabPoly[i];
    01209     stream << *Parameters.evoPoly[i];
    01210   }
    01211   return stream;
    01212 }
    01213
    01214 GTWalkParametersIndividual::GTWalkParametersIndividual()
    01215 {
    01216   fitness = 0;
    01217   calculateTauTau0();
    01218   //initalize strategyParameters
    01219   for (int i = 0; i < 2; i++)
    01220   {
    01221     for (int j = 0; j < 32; j += 4)
    01222     {
    01223       strategyParameters[i][j] = 4.5; // x axis 2.5
    01224       strategyParameters[i][j + 1] = 4; // y axis 2
    01225       strategyParameters[i][j + 2] = 4.5; // z axis 2.5
    01226       strategyParameters[i][j + 3] = 0.08;  // segment length 0.04
    01227     }
    01228   }
    01229   strategyParameterStepLength = 5;
    01230   bMessage = 0;
    01231 }
    01232
    01233 GTWalkParametersIndividual& GTWalkParametersIndividual::operator=(const GTWalkParametersIndividual& other)
    01234 {
    01235   int i, j;
    01236
    01237   bMessage = other.bMessage;
    01238   fitness = other.fitness;
    01239   for (i = 0; i < 2; i++)
    01240   {
    01241     for (j = 0; j < 32; j++)
    01242     {
    01243       strategyParameters[i][j] = other.strategyParameters[i][j];
    01244     }
    01245   }
    01246   strategyParameterStepLength = other.strategyParameterStepLength;
    01247
    01248   for (i = 0; i < 4; i++)
    01249   {
    01250     delete walkingParameters.evoPoly[i];
    01251     walkingParameters.evoPoly[i] = other.walkingParameters.evoPoly[i]->copy();
    01252   }
    01253
    01254   return *this;
    01255 }
    01256
    01257 void GTWalkParametersIndividual::mutate()
    01258 {
    01259   double randGTau0 = exp(randG(tau0));
    01260   int i;
    01261
    01262   //mutate strategy parameters
    01263   strategyParameterStepLength = strategyParameterStepLength * randGTau0 * exp(randG(tau));
    01264   for (i = 0; i < 2; i++)
    01265   {
    01266     for (int j = 0; j < 32; j++)
    01267     {
    01268       strategyParameters[i][j] = strategyParameters[i][j] * randGTau0 * exp(randG(tau));
    01269     }
    01270   }
    01271
    01272   //mutate objectParameters
    01273   int tmpStepLength = (int)(randG(strategyParameterStepLength));
    01274   walkingParameters.evoPoly[GT2005Parameters::Leg_FR]->stepLength += tmpStepLength;
    01275   walkingParameters.evoPoly[GT2005Parameters::Leg_FL]->stepLength += tmpStepLength;
    01276   walkingParameters.evoPoly[GT2005Parameters::Leg_HR]->stepLength += tmpStepLength;
    01277   walkingParameters.evoPoly[GT2005Parameters::Leg_HL]->stepLength += tmpStepLength;
    01278   Vector3<double> polyPoint;
    01279   double segment;
    01280   for (i = 0; i < walkingParameters.evoPoly[GT2005Parameters::Leg_FR]->numPoints; i++)
    01281   {
    01282     polyPoint = walkingParameters.evoPoly[GT2005Parameters::Leg_FR]->getPointCoord(i);
    01283     polyPoint.x += randG(strategyParameters[0][i * 4]);
    01284     polyPoint.y += randG(strategyParameters[0][i * 4 + 1]);
    01285     polyPoint.z += randG(strategyParameters[0][i * 4 + 2]);
    01286     segment = walkingParameters.evoPoly[GT2005Parameters::Leg_FR]->getSegmentLength(i);
    01287     segment += randG(strategyParameters[0][i * 4 + 3]);
    01288     walkingParameters.evoPoly[GT2005Parameters::Leg_FR]->setSegment(i, segment, polyPoint);
    01289     //mirror point for other side
    01290     polyPoint.y = -polyPoint.y;
    01291     walkingParameters.evoPoly[GT2005Parameters::Leg_FL]->setSegment(i, segment, polyPoint);
    01292   }
    01293   for (i = 0; i < walkingParameters.evoPoly[GT2005Parameters::Leg_HR]->numPoints; i++)
    01294   {
    01295     polyPoint = walkingParameters.evoPoly[GT2005Parameters::Leg_HR]->getPointCoord(i);
    01296     polyPoint.x += randG(strategyParameters[1][i * 4]);
    01297     polyPoint.y += randG(strategyParameters[1][i * 4 + 1]);
    01298     polyPoint.z += randG(strategyParameters[1][i * 4 + 2]);
    01299     segment = walkingParameters.evoPoly[GT2005Parameters::Leg_HR]->getSegmentLength(i);
    01300     segment += randG(strategyParameters[1][i * 4 + 3]);
    01301     walkingParameters.evoPoly[GT2005Parameters::Leg_HR]->setSegment(i, segment, polyPoint);
    01302     //mirror point for other side
    01303     polyPoint.y = -polyPoint.y;
    01304     walkingParameters.evoPoly[GT2005Parameters::Leg_HL]->setSegment(i, segment, polyPoint);
    01305   }
    01306
    01307   //hier noch sicherstellen, dass die segmente zusammen 1 ergeben
    01308 }
    01309
    01310 void GTWalkParametersIndividual::crossOver(GTWalkParametersIndividual* parent2)
    01311 {
    01312   //cross over strategy parameters
    01313   strategyParameterStepLength = (strategyParameterStepLength + parent2->strategyParameterStepLength) / 2;
    01314   for (int i = 0; i < 2; i++)
    01315   {
    01316     for (int j = 0; j < 32; j++)
    01317     {
    01318       strategyParameters[i][j] = (strategyParameters[i][j] + parent2->strategyParameters[i][j]) / 2;
    01319     }
    01320   }
    01321
    01322   //using intermediate recombination
    01323   //walkingParameters.stepLength = (int)((walkingParameters.stepLength + parent2->walkingParameters.stepLength) / 2);
    01324
    01325   //blend the two polys by 50% and store the result in the own evoPoly set
    01326   walkingParameters.evoPoly[GT2005Parameters::Leg_FR]->interpolateShapeP(0.5, parent2->walkingParameters.evoPoly[GT2005Parameters::Leg_FR]);
    01327   walkingParameters.evoPoly[GT2005Parameters::Leg_FL]->interpolateShapeP(0.5, parent2->walkingParameters.evoPoly[GT2005Parameters::Leg_FL]);
    01328   walkingParameters.evoPoly[GT2005Parameters::Leg_HR]->interpolateShapeP(0.5, parent2->walkingParameters.evoPoly[GT2005Parameters::Leg_HR]);
    01329   walkingParameters.evoPoly[GT2005Parameters::Leg_HL]->interpolateShapeP(0.5, parent2->walkingParameters.evoPoly[GT2005Parameters::Leg_HL]);
    01330 }
    01331
    01332 void GTWalkParametersIndividual::crossingOverOf(GTWalkParametersIndividual* parent1, GTWalkParametersIndividual* parent2)
    01333 {
    01334   //cross over strategy parameters
    01335   strategyParameterStepLength = (parent1->strategyParameterStepLength + parent2->strategyParameterStepLength) / 2;
    01336   for (int i = 0; i < 2; i++)
    01337   {
    01338     for (int j = 0; j < 32; j++)
    01339     {
    01340       strategyParameters[i][j] = (parent1->strategyParameters[i][j] + parent2->strategyParameters[i][j]) / 2;
    01341     }
    01342   }
    01343
    01344   //using intermediate recombination
    01345   //walkingParameters.stepLength = (int)((parent1->walkingParameters.stepLength+parent2->walkingParameters.stepLength) / 2);
    01346
    01347   //copy Konstruktor benutzen
    01348   delete walkingParameters.evoPoly[GT2005Parameters::Leg_FR];
    01349   delete walkingParameters.evoPoly[GT2005Parameters::Leg_FL];
    01350   delete walkingParameters.evoPoly[GT2005Parameters::Leg_HR];
    01351   delete walkingParameters.evoPoly[GT2005Parameters::Leg_HL];
    01352
    01353   walkingParameters.evoPoly[GT2005Parameters::Leg_FR] = parent1->walkingParameters.evoPoly[GT2005Parameters::Leg_FR]->interpolateShape(0.5, parent2->walkingParameters.evoPoly[GT2005Parameters::Leg_FR]);
    01354   walkingParameters.evoPoly[GT2005Parameters::Leg_FL] = parent1->walkingParameters.evoPoly[GT2005Parameters::Leg_FL]->interpolateShape(0.5, parent2->walkingParameters.evoPoly[GT2005Parameters::Leg_FL]);
    01355   walkingParameters.evoPoly[GT2005Parameters::Leg_HR] = parent1->walkingParameters.evoPoly[GT2005Parameters::Leg_HR]->interpolateShape(0.5, parent2->walkingParameters.evoPoly[GT2005Parameters::Leg_HR]);
    01356   walkingParameters.evoPoly[GT2005Parameters::Leg_HL] = parent1->walkingParameters.evoPoly[GT2005Parameters::Leg_HL]->interpolateShape(0.5, parent2->walkingParameters.evoPoly[GT2005Parameters::Leg_HL]);
    01357 }
    01358
    01359 void GTWalkParametersIndividual::calculateTauTau0()
    01360 {
    01361   int n;
    01362   n = walkingParameters.evoPoly[GT2005Parameters::Leg_FR]->numPoints * 4 * 2 + 1;
    01363   tau = 1 / (sqrt((double)(2 * sqrt((double)n))));
    01364   tau0 = 1 / (sqrt((double)(2 * n)));
    01365 }
    01366
    01367 double GTWalkParametersIndividual::randG(const double mutationStrength)
    01368 {
    01369   return mutationStrength * sqrt(-2.0 * log(random())) * sin(pi2 * random());
    01370 }
    01371
    01372 In& operator>>(In& stream, GTWalkParametersIndividual& individualParameters)
    01373 {
    01374   int i, j;
    01375
    01376   stream >> individualParameters.bMessage;
    01377   stream >> individualParameters.fitness;
    01378   for (i = 0; i < 2; i++)
    01379   {
    01380     for (j = 0; j < 32; j++)
    01381     {
    01382       stream >> individualParameters.strategyParameters[i][j];
    01383     }
    01384   }
    01385   stream >> individualParameters.strategyParameterStepLength;
    01386
    01387   for (i = 0; i < 4; i++)
    01388   {
    01389     stream >> *individualParameters.walkingParameters.evoPoly[i];
    01390   }
    01391   return stream;
    01392 }
    01393
    01394 Out& operator<<(Out& stream, const GTWalkParametersIndividual& individualParameters)
    01395 {
    01396   int i, j;
    01397
    01398   stream << individualParameters.bMessage;
    01399   stream << individualParameters.fitness;
    01400   for (i = 0; i < 2; i++)
    01401   {
    01402     for (j = 0; j < 32; j++)
    01403     {
    01404       stream << individualParameters.strategyParameters[i][j];
    01405     }
    01406   }
    01407   stream << individualParameters.strategyParameterStepLength;
    01408
    01409   for (i = 0; i < 4; i++)
    01410   {
    01411     stream << *individualParameters.walkingParameters.evoPoly[i];
    01412   }
    01413   return stream;
    01414 }