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

# Modules/WalkingEngine/GT2005Polygon.cpp

Go to the documentation of this file.

    00001 /***************************************************************************
    00002 * @file GT2005Polygon.cpp
    00003 * class GT2005Polygon
    00004 * @author Denis Fisseler
    00005 ****************************************************************************/
    00006
    00007 #include "GT2005Polygon.h"
    00008 #include "Tools/Streams/InStreams.h"
    00009 #include "Tools/Streams/OutStreams.h"
    00010
    00011 GT2005Polygon::GT2005Polygon(int nPoints, int sLength)
    00012 {
    00013   if (nPoints > 0)
    00014   {
    00015     numPoints = nPoints;
    00016     stepLength = sLength;
    00017     rollAngle = 0;
    00018   }
    00019 }
    00020
    00021 GT2005Polygon::~GT2005Polygon()
    00022 {
    00023
    00024 }
    00025
    00026 Vector3<double> GT2005Polygon::getPointCoord(int pNum)
    00027 {
    00028   Vector3<double> dummy;
    00029
    00030   if ((numPoints > 0) && (pNum >= 0) && (pNum < numPoints))
    00031   {
    00032     return pPoints[pNum].point;
    00033   }
    00034   return dummy;
    00035 }
    00036
    00037 void GT2005Polygon::setPointCoord(int pNum, const Vector3<double> p)
    00038 {
    00039   if ((numPoints > 0) && (pNum >= 0) && (pNum < numPoints))
    00040   {
    00041     pPoints[pNum].point = p;
    00042   }
    00043 }
    00044
    00045 void GT2005Polygon::setSegment(int sNum, double sLen, Vector3<double> p)
    00046 {
    00047   if ((numPoints > 0) && (sNum >= 0) && (sNum < numPoints))
    00048   {
    00049     pPoints[sNum].sLen = sLen;
    00050     setPointCoord(sNum, p);
    00051   }
    00052 }
    00053
    00054 void GT2005Polygon::getSegment(int sNum, double &sLen, Vector3<double> &p)
    00055 {
    00056   if ((numPoints > 0) && (sNum >= 0) && (sNum < numPoints))
    00057   {
    00058     sLen = pPoints[sNum].sLen;
    00059     p = getPointCoord(sNum);
    00060   }
    00061 }
    00062
    00063 double GT2005Polygon::getSegmentLength(int sNum)
    00064 {
    00065   if ((numPoints > 0) && (sNum >= 0) && (sNum < numPoints))
    00066   {
    00067     return pPoints[sNum].sLen;
    00068   }
    00069   return 0;
    00070 }
    00071
    00072 Vector3<double> GT2005Polygon::getPositionOnShape(const double pos)
    00073 {
    00074   Vector3<double> temp;
    00075
    00076   double ssum = 0;
    00077   int currentSegment = 0;
    00078
    00079   // calcualte current segment
    00080   for (int i = 0; i < numPoints; i++)
    00081   {
    00082     ssum += pPoints[i].sLen;
    00083     if (ssum > pos)
    00084     {
    00085       ssum -= pPoints[i].sLen;
    00086       currentSegment = i;
    00087       break;
    00088     }
    00089   }
    00090
    00091   // calculate position on current segment
    00092   double sPos = (pos - ssum) / pPoints[currentSegment].sLen;
    00093   temp = pPoints[currentSegment].point + (pPoints[(currentSegment + 1) % numPoints].point - pPoints[currentSegment].point) * sPos;
    00094
    00095   return temp;
    00096 }
    00097
    00098 int GT2005Polygon::getMaxX()
    00099 {
    00100   int mPoint = 0;
    00101
    00102   if (numPoints > 0)
    00103   {
    00104     double max = pPoints[0].point.x;
    00105
    00106     for (int i = 0; i < numPoints; i++)
    00107     {
    00108       if (pPoints[i].point.x > max)
    00109       {
    00110         mPoint = i;
    00111       }
    00112       // take the lowest point if equal
    00113       if ((pPoints[i].point.x == max) && (pPoints[i].point.z < pPoints[mPoint].point.z))
    00114       {
    00115         mPoint = i;
    00116       }
    00117     }
    00118   }
    00119   return mPoint;
    00120 }
    00121
    00122 int GT2005Polygon::getMinX()
    00123 {
    00124   int mPoint = 0;
    00125
    00126   if (numPoints > 0)
    00127   {
    00128     double min = pPoints[0].point.x;
    00129
    00130     for (int i = 0; i < numPoints; i++)
    00131     {
    00132       if (pPoints[i].point.x < min) mPoint = i;
    00133       // take the lowest point if equal
    00134       if ((pPoints[i].point.x == min) && (pPoints[i].point.z < pPoints[mPoint].point.z)) mPoint = i;
    00135     }
    00136   }
    00137   return mPoint;
    00138 }
    00139
    00140
    00141 double GT2005Polygon::approxGroundTime() // get run through time for the most probable ground segment
    00142 {
    00143   int minPoint = getMinX();
    00144   int maxPoint = getMaxX();
    00145
    00146   double segLen1 = 0;
    00147   double segLen2 = 0;
    00148   double numSegs1 = 1;
    00149   double numSegs2 = 1;
    00150   double zPos1 = 0;
    00151   double zPos2 = 0;
    00152
    00153   // calculate average z height of points on segments
    00154   // calculate first segment data
    00155   int i = maxPoint;
    00156   do
    00157   {
    00158     segLen1 += pPoints[i].sLen;
    00159     numSegs1++;
    00160     zPos1 += pPoints[i].point.z;
    00161     i = (i + 1) % numPoints;
    00162   } while(i != minPoint);
    00163
    00164   // calculate second segment data
    00165   i = minPoint;
    00166   do
    00167   {
    00168     segLen2 += pPoints[i].sLen;
    00169     numSegs2++;
    00170     zPos2 += pPoints[i].point.z;
    00171     i = (i + 1) % numPoints;
    00172   } while(i != maxPoint);
    00173
    00174   zPos1 /= numSegs1;
    00175   zPos2 /= numSegs2;
    00176   if (zPos1 < zPos2)
    00177   {
    00178     return segLen1;
    00179   }
    00180   else
    00181   {
    00182     return segLen2;
    00183   }
    00184 }
    00185
    00186 Vector3<double> GT2005Polygon::calculateCenterOfMass()
    00187 {
    00188   int i;
    00189   double avg_x, avg_y, avg_z;
    00190   avg_x = avg_y = avg_z = 0.0;
    00191
    00192   for (i = 0; i < numPoints; i++)
    00193   {
    00194     avg_x += pPoints[i].point.x;
    00195     avg_y += pPoints[i].point.y;
    00196     avg_z += pPoints[i].point.z;
    00197   }
    00198   avg_x /= numPoints;
    00199   avg_y /= numPoints;
    00200   avg_z /= numPoints;
    00201
    00202   Vector3<double> temp;
    00203   temp.x = avg_x;
    00204   temp.y = avg_y;
    00205   temp.z = avg_z;
    00206   return temp;
    00207 }
    00208
    00209 void GT2005Polygon::finalize()
    00210 {
    00211   // normalize polygon lenght to 1
    00212   if (numPoints > 0)
    00213   {
    00214     double gLen = 0;
    00215     int i;
    00216     // determine overall length
    00217     for (i = 0; i < numPoints; i++)
    00218     {
    00219       gLen += pPoints[i].sLen;
    00220     }
    00221     // normalize
    00222     for (i = 0; i < numPoints; i++)
    00223     {
    00224       pPoints[i].sLen *= 1 / gLen;
    00225     }
    00226   }
    00227
    00228   //centerOfMassCorrection = calculateCenterOfMass(); // needed only for rotateZC
    00229   groundTime = approxGroundTime();
    00230   polyLengthX = pPoints[getMaxX()].point.x - pPoints[getMinX()].point.x;
    00231 }
    00232
    00233 GT2005Polygon GT2005Polygon::scaleX(double s)
    00234 {
    00235   GT2005Polygon temp;
    00236
    00237   if (numPoints > 0)
    00238   {
    00239     temp.numPoints = numPoints;
    00240     temp.polyLengthX = polyLengthX;
    00241     temp.groundTime = groundTime;
    00242     temp.stepLength = stepLength;
    00243
    00244     for (int i = 0; i < numPoints; i++)
    00245     {
    00246       Vector3<double> v;
    00247       v.x = pPoints[i].point.x * s;
    00248       v.y = pPoints[i].point.y;
    00249       v.z = pPoints[i].point.z;
    00250       temp.setSegment(i, pPoints[i].sLen, v);
    00251     }
    00252   }
    00253   return temp;
    00254 }
    00255
    00256 GT2005Polygon GT2005Polygon::scaleY(double s)
    00257 {
    00258   GT2005Polygon temp;
    00259
    00260   if (numPoints > 0)
    00261   {
    00262     temp.numPoints = numPoints;
    00263     temp.polyLengthX = polyLengthX;
    00264     temp.groundTime = groundTime;
    00265     temp.stepLength = stepLength;
    00266
    00267     for (int i = 0; i < numPoints; i++)
    00268     {
    00269       Vector3<double> v;
    00270       v.x = pPoints[i].point.x;
    00271       v.y = pPoints[i].point.y * s;
    00272       v.z = pPoints[i].point.z;
    00273       temp.setSegment(i, pPoints[i].sLen, v);
    00274     }
    00275   }
    00276   return temp;
    00277 }
    00278
    00279 GT2005Polygon GT2005Polygon::rotateX(double a)
    00280 {
    00281   GT2005Polygon temp;
    00282
    00283   if (numPoints > 0)
    00284   {
    00285     temp.numPoints = numPoints;
    00286     temp.polyLengthX = polyLengthX;
    00287     temp.groundTime = groundTime;
    00288     temp.stepLength = stepLength;
    00289
    00290     for (int i = 0; i < numPoints; i++)
    00291     {
    00292       Vector3<double> v;
    00293       v.x = pPoints[i].point.x;
    00294       v.y = pPoints[i].point.y * cos(a) - pPoints[i].point.z * sin(a);
    00295       v.z = pPoints[i].point.y * sin(a) + pPoints[i].point.z * cos(a);
    00296       temp.setSegment(i, pPoints[i].sLen, v);
    00297     }
    00298   }
    00299
    00300   return temp;
    00301 }
    00302
    00303 GT2005Polygon GT2005Polygon::rotateY(double a)
    00304 {
    00305   GT2005Polygon temp;
    00306
    00307   if (numPoints > 0)
    00308   {
    00309     temp.numPoints = numPoints;
    00310     temp.polyLengthX = polyLengthX;
    00311     temp.groundTime = groundTime;
    00312     temp.stepLength = stepLength;
    00313
    00314     for (int i = 0; i < numPoints; i++)
    00315     {
    00316       Vector3<double> v;
    00317       v.x = pPoints[i].point.z * sin(a) + pPoints[i].point.x * cos(a);
    00318       v.y = pPoints[i].point.y;
    00319       v.z = pPoints[i].point.z * cos(a) - pPoints[i].point.x * sin(a);
    00320       temp.setSegment(i, pPoints[i].sLen, v);
    00321     }
    00322   }
    00323
    00324   return temp;
    00325 }
    00326
    00327 GT2005Polygon GT2005Polygon::rotateZ(double a)
    00328 {
    00329   GT2005Polygon temp;
    00330
    00331   if (numPoints > 0)
    00332   {
    00333     temp.numPoints = numPoints;
    00334     temp.polyLengthX = polyLengthX;
    00335     temp.groundTime = groundTime;
    00336     temp.stepLength = stepLength;
    00337
    00338     for (int i = 0; i < numPoints; i++)
    00339     {
    00340       Vector3<double> v;
    00341       v.x = pPoints[i].point.x * cos(a) - pPoints[i].point.y * sin(a);
    00342       v.y = pPoints[i].point.x * sin(a) + pPoints[i].point.y * cos(a);
    00343       v.z = pPoints[i].point.z;
    00344       temp.setSegment(i, pPoints[i].sLen, v);
    00345     }
    00346   }
    00347
    00348   return temp;
    00349 }
    00350
    00351 void GT2005Polygon::rotateXP(double a)
    00352 {
    00353   if (numPoints > 0)
    00354   {
    00355     for (int i = 0; i < numPoints; i++)
    00356     {
    00357       Vector3<double> v;
    00358       v.x = pPoints[i].point.x;
    00359       v.y = pPoints[i].point.y * cos(a) - pPoints[i].point.z * sin(a);
    00360       v.z = pPoints[i].point.y * sin(a) + pPoints[i].point.z * cos(a);
    00361       setSegment(i, pPoints[i].sLen, v);
    00362     }
    00363   }
    00364 }
    00365
    00366 void GT2005Polygon::rotateYP(double a)
    00367 {
    00368   if (numPoints > 0)
    00369   {
    00370     for (int i = 0; i < numPoints; i++)
    00371     {
    00372       Vector3<double> v;
    00373       v.x = pPoints[i].point.z * sin(a) + pPoints[i].point.x * cos(a);
    00374       v.y = pPoints[i].point.y;
    00375       v.z = pPoints[i].point.z * cos(a) - pPoints[i].point.x * sin(a);
    00376       setSegment(i, pPoints[i].sLen, v);
    00377     }
    00378   }
    00379 }
    00380
    00381 void GT2005Polygon::rotateZP(double a)
    00382 {
    00383   if (numPoints > 0)
    00384   {
    00385     for (int i = 0; i < numPoints; i++)
    00386     {
    00387       Vector3<double> v;
    00388       v.x = pPoints[i].point.x * cos(a) - pPoints[i].point.y * sin(a);
    00389       v.y = pPoints[i].point.x * sin(a) + pPoints[i].point.y * cos(a);
    00390       v.z = pPoints[i].point.z;
    00391       setSegment(i, pPoints[i].sLen, v);
    00392     }
    00393   }
    00394 }
    00395
    00396 GT2005Polygon GT2005Polygon::rotateZC(double a)
    00397 {
    00398   GT2005Polygon temp;
    00399
    00400   if (numPoints > 0)
    00401   {
    00402     centerOfMassCorrection = calculateCenterOfMass();
    00403
    00404     temp.numPoints = numPoints;
    00405     temp.polyLengthX = polyLengthX;
    00406     temp.groundTime = groundTime;
    00407     temp.stepLength = stepLength;
    00408     temp.centerOfMassCorrection = centerOfMassCorrection;
    00409
    00410     for (int i = 0; i < numPoints; i++)
    00411     {
    00412       Vector3<double> v1;
    00413       Vector3<double> v2;
    00414
    00415       v1 = pPoints[i].point - centerOfMassCorrection; // transform to center of mass
    00416       v2.x = v1.x * cos(a) - v1.y * sin(a);
    00417       v2.y = v1.x * sin(a) + v1.y * cos(a);
    00418       v2.z = v1.z;
    00419       v2 += centerOfMassCorrection; // transform back
    00420       temp.setSegment(i, pPoints[i].sLen, v2);
    00421     }
    00422   }
    00423
    00424   return temp;
    00425 }
    00426
    00427 void GT2005Polygon::insertPoint(int pNum) // insert point in the middle between pNum and pNum+1
    00428 {
    00429   if ((numPoints > 0) && (pNum >= 0) && (pNum < numPoints) && (pNum < maxPoints))
    00430   {
    00431     numPoints++;
    00432     // move segments in array
    00433     for (int i = numPoints - 1; i > pNum + 1; i--)
    00434     {
    00435       pPoints[i].point = pPoints[i - 1].point;
    00436       pPoints[i].sLen = pPoints[i - 1].sLen;
    00437     }
    00438     // adjust previous segement length
    00439     pPoints[pNum].sLen = pPoints[pNum].sLen / 2;
    00440     // set data for inseretd point
    00441     pPoints[pNum + 1].point = pPoints[pNum].point + ((pPoints[(pNum + 2) % numPoints].point - pPoints[pNum].point) / 2);
    00442     pPoints[pNum + 1].sLen = pPoints[pNum].sLen;
    00443   }
    00444 }
    00445
    00446 GT2005Polygon* GT2005Polygon::interpolateShape(double amount, GT2005Polygon* s2)
    00447 {
    00448   GT2005Polygon* temp = new GT2005Polygon(numPoints, stepLength);
    00449
    00450   if (numPoints == s2->numPoints)
    00451   {
    00452     for (int i = 0; i < numPoints; i++)
    00453     {
    00454       Vector3<double> v;
    00455       v = pPoints[i].point + ((s2->getPointCoord(i) - pPoints[i].point) * amount);
    00456       double l = pPoints[i].sLen + ((s2->getSegmentLength(i) - pPoints[i].sLen) * amount);
    00457       temp->setSegment(i, l, v);
    00458     }
    00459     // calculate new stepLength
    00460     temp->stepLength = (int)(stepLength + ((s2->stepLength - stepLength) * amount));
    00461   }
    00462
    00463   return temp;
    00464 }
    00465
    00466 void GT2005Polygon::interpolateShapeP(double amount, GT2005Polygon* s2)
    00467 {
    00468   if (numPoints == s2->numPoints)
    00469   {
    00470     for (int i = 0; i < numPoints; i++)
    00471     {
    00472       Vector3<double> v;
    00473       v = pPoints[i].point + ((s2->getPointCoord(i) - pPoints[i].point) * amount);
    00474       double l = pPoints[i].sLen + ((s2->getSegmentLength(i) - pPoints[i].sLen) * amount);
    00475       setSegment(i, l, v);
    00476     }
    00477     // calculate new stepLength
    00478     stepLength = (int)(stepLength + ((s2->stepLength - stepLength) * amount));
    00479   }
    00480 }
    00481
    00482 GT2005Polygon* GT2005Polygon::copy()
    00483 {
    00484   if (numPoints > 0)
    00485   {
    00486     GT2005Polygon* temp = new GT2005Polygon(numPoints, stepLength);
    00487     temp->polyLengthX = polyLengthX;
    00488     temp->groundTime = groundTime;
    00489
    00490     for (int i = 0; i < numPoints; i++)
    00491     {
    00492       Vector3<double> v;
    00493       v.x = pPoints[i].point.x;
    00494       v.y = pPoints[i].point.y;
    00495       v.z = pPoints[i].point.z;
    00496       temp->setSegment(i, pPoints[i].sLen, v);
    00497     }
    00498     temp->finalize();
    00499     return temp;
    00500   }
    00501   return false;
    00502 }
    00503
    00504 GT2005Polygon* GT2005Polygon::mirrorY()
    00505 {
    00506   if (numPoints > 0)
    00507   {
    00508     GT2005Polygon* temp = new GT2005Polygon(numPoints, stepLength);
    00509     temp->polyLengthX = polyLengthX;
    00510     temp->groundTime = groundTime;
    00511
    00512     for (int i = 0; i < numPoints; i++)
    00513     {
    00514       Vector3<double> v;
    00515       v.x = pPoints[i].point.x;
    00516       v.y = -pPoints[i].point.y;
    00517       v.z = pPoints[i].point.z;
    00518       temp->setSegment(i, pPoints[i].sLen, v);
    00519     }
    00520     temp->finalize();
    00521     return temp;
    00522   }
    00523   return false;
    00524 }
    00525
    00526 bool GT2005Polygon::load(char* filename)
    00527 {
    00528   char name[128];
    00529   if (filename != 0)
    00530   {
    00531     strcpy(name, filename);
    00532   }
    00533   InBinaryFile file(name);
    00534   if (file.exists())
    00535   {
    00536     file >> *this;
    00537     return true;
    00538   }
    00539   return false;
    00540 }
    00541
    00542 void GT2005Polygon::save(char* filename)
    00543 {
    00544   char name[128];
    00545   if (filename != 0)
    00546   {
    00547     strcpy(name, filename);
    00548   }
    00549   OutBinaryFile file(name);
    00550   file << *this;
    00551 }
    00552
    00553 In& operator >> (In& stream, GT2005Polygon& polygon)
    00554 {
    00555   stream >> polygon.numPoints;
    00556   stream >> polygon.rollAngle;
    00557   stream >> polygon.stepLength;
    00558   for (int i = 0; i < polygon.numPoints; i++)
    00559   {
    00560     stream >> polygon.pPoints[i].point >> polygon.pPoints[i].sLen;
    00561   }
    00562   polygon.finalize();
    00563   return stream;
    00564 }
    00565
    00566 Out& operator << (Out& stream, const GT2005Polygon& polygon)
    00567 {
    00568   stream << polygon.numPoints;
    00569   stream << polygon.rollAngle;
    00570   stream << polygon.stepLength;
    00571   for (int i = 0; i < polygon.numPoints; i++)
    00572   {
    00573     stream << polygon.pPoints[i].point << polygon.pPoints[i].sLen;
    00574   }
    00575   return stream;
    00576 }