Modules/WalkingEngine/GT2005Polygon.cpp

Go to the documentation of this file.

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

#include "GT2005Polygon.h"
#include "Tools/Streams/InStreams.h"
#include "Tools/Streams/OutStreams.h"

GT2005Polygon::GT2005Polygon(int nPoints, int sLength)
{
  if (nPoints > 0)
  {
    numPoints = nPoints;
    stepLength = sLength;
    rollAngle = 0;
  }
}

GT2005Polygon::~GT2005Polygon()
{

}

Vector3<double> GT2005Polygon::getPointCoord(int pNum)
{
  Vector3<double> dummy;

  if ((numPoints > 0) && (pNum >= 0) && (pNum < numPoints))
  {
    return pPoints[pNum].point;
  }
  return dummy;
}

void GT2005Polygon::setPointCoord(int pNum, const Vector3<double> p)
{
  if ((numPoints > 0) && (pNum >= 0) && (pNum < numPoints))
  {
    pPoints[pNum].point = p;
  }
}

void GT2005Polygon::setSegment(int sNum, double sLen, Vector3<double> p)
{
  if ((numPoints > 0) && (sNum >= 0) && (sNum < numPoints))
  {
    pPoints[sNum].sLen = sLen;
    setPointCoord(sNum, p);
  }
}

void GT2005Polygon::getSegment(int sNum, double &sLen, Vector3<double> &p)
{
  if ((numPoints > 0) && (sNum >= 0) && (sNum < numPoints))
  {
    sLen = pPoints[sNum].sLen;
    p = getPointCoord(sNum);
  }
}

double GT2005Polygon::getSegmentLength(int sNum)
{
  if ((numPoints > 0) && (sNum >= 0) && (sNum < numPoints))
  {
    return pPoints[sNum].sLen;
  }
  return 0;
}

Vector3<double> GT2005Polygon::getPositionOnShape(const double pos)
{
  Vector3<double> temp;

  double ssum = 0;
  int currentSegment = 0;
  
  // calcualte current segment
  for (int i = 0; i < numPoints; i++)
  {
    ssum += pPoints[i].sLen;
    if (ssum > pos)
    {
      ssum -= pPoints[i].sLen;
      currentSegment = i;
      break;
    }
  }
  
  // calculate position on current segment
  double sPos = (pos - ssum) / pPoints[currentSegment].sLen;
  temp = pPoints[currentSegment].point + (pPoints[(currentSegment + 1) % numPoints].point - pPoints[currentSegment].point) * sPos;

  return temp;
}

int GT2005Polygon::getMaxX()
{
  int mPoint = 0;

  if (numPoints > 0)
  {
    double max = pPoints[0].point.x;

    for (int i = 0; i < numPoints; i++)
    {
      if (pPoints[i].point.x > max)
      {
        mPoint = i;
      }
      // take the lowest point if equal
      if ((pPoints[i].point.x == max) && (pPoints[i].point.z < pPoints[mPoint].point.z))
      {
        mPoint = i;
      }
    }
  }
  return mPoint;
}

int GT2005Polygon::getMinX()
{
  int mPoint = 0;

  if (numPoints > 0)
  {
    double min = pPoints[0].point.x;

    for (int i = 0; i < numPoints; i++)
    {
      if (pPoints[i].point.x < min) mPoint = i;
      // take the lowest point if equal
      if ((pPoints[i].point.x == min) && (pPoints[i].point.z < pPoints[mPoint].point.z)) mPoint = i;
    }
  }
  return mPoint;
}


double GT2005Polygon::approxGroundTime() // get run through time for the most probable ground segment
{
  int minPoint = getMinX();
  int maxPoint = getMaxX();

  double segLen1 = 0;
  double segLen2 = 0;
  double numSegs1 = 1;
  double numSegs2 = 1;
  double zPos1 = 0;
  double zPos2 = 0;

  // calculate average z height of points on segments
  // calculate first segment data
  int i = maxPoint;
  do 
  {
    segLen1 += pPoints[i].sLen;
    numSegs1++;
    zPos1 += pPoints[i].point.z;
    i = (i + 1) % numPoints;
  } while(i != minPoint);

  // calculate second segment data
  i = minPoint;
  do
  {
    segLen2 += pPoints[i].sLen;
    numSegs2++;
    zPos2 += pPoints[i].point.z;
    i = (i + 1) % numPoints;
  } while(i != maxPoint);

  zPos1 /= numSegs1;
  zPos2 /= numSegs2;
  if (zPos1 < zPos2)
  {
    return segLen1;
  }
  else
  {
    return segLen2;
  }
}

Vector3<double> GT2005Polygon::calculateCenterOfMass()
{
  int i;
  double avg_x, avg_y, avg_z;
  avg_x = avg_y = avg_z = 0.0;

  for (i = 0; i < numPoints; i++)
  {
    avg_x += pPoints[i].point.x;
    avg_y += pPoints[i].point.y;
    avg_z += pPoints[i].point.z;
  }
  avg_x /= numPoints;
  avg_y /= numPoints;
  avg_z /= numPoints;

  Vector3<double> temp;
  temp.x = avg_x;
  temp.y = avg_y;
  temp.z = avg_z;
  return temp;
}

void GT2005Polygon::finalize()
{
  // normalize polygon lenght to 1
  if (numPoints > 0)
  {
    double gLen = 0;
    int i;
    // determine overall length
    for (i = 0; i < numPoints; i++)
    {
      gLen += pPoints[i].sLen;
    }
    // normalize
    for (i = 0; i < numPoints; i++)
    {
      pPoints[i].sLen *= 1 / gLen;
    }
  }

  //centerOfMassCorrection = calculateCenterOfMass(); // needed only for rotateZC
  groundTime = approxGroundTime();
  polyLengthX = pPoints[getMaxX()].point.x - pPoints[getMinX()].point.x;
}

GT2005Polygon GT2005Polygon::scaleX(double s)
{
  GT2005Polygon temp;

  if (numPoints > 0)
  {
    temp.numPoints = numPoints;
    temp.polyLengthX = polyLengthX;
    temp.groundTime = groundTime;
    temp.stepLength = stepLength;

    for (int i = 0; i < numPoints; i++)
    {
      Vector3<double> v;
      v.x = pPoints[i].point.x * s;
      v.y = pPoints[i].point.y;
      v.z = pPoints[i].point.z;
      temp.setSegment(i, pPoints[i].sLen, v);
    }
  }
  return temp;
}

GT2005Polygon GT2005Polygon::scaleY(double s)
{
  GT2005Polygon temp;

  if (numPoints > 0)
  {
    temp.numPoints = numPoints;
    temp.polyLengthX = polyLengthX;
    temp.groundTime = groundTime;
    temp.stepLength = stepLength;

    for (int i = 0; i < numPoints; i++)
    {
      Vector3<double> v;
      v.x = pPoints[i].point.x;
      v.y = pPoints[i].point.y * s;
      v.z = pPoints[i].point.z;
      temp.setSegment(i, pPoints[i].sLen, v);
    }
  }
  return temp;
}

GT2005Polygon GT2005Polygon::rotateX(double a)
{
  GT2005Polygon temp;

  if (numPoints > 0)
  {
    temp.numPoints = numPoints;
    temp.polyLengthX = polyLengthX;
    temp.groundTime = groundTime;
    temp.stepLength = stepLength;

    for (int i = 0; i < numPoints; i++)
    {
      Vector3<double> v;
      v.x = pPoints[i].point.x;
      v.y = pPoints[i].point.y * cos(a) - pPoints[i].point.z * sin(a);
      v.z = pPoints[i].point.y * sin(a) + pPoints[i].point.z * cos(a);
      temp.setSegment(i, pPoints[i].sLen, v);
    }
  }

  return temp;
}

GT2005Polygon GT2005Polygon::rotateY(double a)
{
  GT2005Polygon temp;

  if (numPoints > 0)
  {
    temp.numPoints = numPoints;
    temp.polyLengthX = polyLengthX;
    temp.groundTime = groundTime;
    temp.stepLength = stepLength;

    for (int i = 0; i < numPoints; i++)
    {
      Vector3<double> v;
      v.x = pPoints[i].point.z * sin(a) + pPoints[i].point.x * cos(a);
      v.y = pPoints[i].point.y;
      v.z = pPoints[i].point.z * cos(a) - pPoints[i].point.x * sin(a);
      temp.setSegment(i, pPoints[i].sLen, v);
    }
  }

  return temp;
}

GT2005Polygon GT2005Polygon::rotateZ(double a)
{
  GT2005Polygon temp;

  if (numPoints > 0)
  {
    temp.numPoints = numPoints;
    temp.polyLengthX = polyLengthX;
    temp.groundTime = groundTime;
    temp.stepLength = stepLength;

    for (int i = 0; i < numPoints; i++)
    {
      Vector3<double> v;
      v.x = pPoints[i].point.x * cos(a) - pPoints[i].point.y * sin(a);
      v.y = pPoints[i].point.x * sin(a) + pPoints[i].point.y * cos(a);
      v.z = pPoints[i].point.z;
      temp.setSegment(i, pPoints[i].sLen, v);
    }
  }

  return temp;
}

void GT2005Polygon::rotateXP(double a)
{
  if (numPoints > 0)
  {
    for (int i = 0; i < numPoints; i++)
    {
      Vector3<double> v;
      v.x = pPoints[i].point.x;
      v.y = pPoints[i].point.y * cos(a) - pPoints[i].point.z * sin(a);
      v.z = pPoints[i].point.y * sin(a) + pPoints[i].point.z * cos(a);
      setSegment(i, pPoints[i].sLen, v);
    }
  }
}

void GT2005Polygon::rotateYP(double a)
{
  if (numPoints > 0)
  {
    for (int i = 0; i < numPoints; i++)
    {
      Vector3<double> v;
      v.x = pPoints[i].point.z * sin(a) + pPoints[i].point.x * cos(a);
      v.y = pPoints[i].point.y;
      v.z = pPoints[i].point.z * cos(a) - pPoints[i].point.x * sin(a);
      setSegment(i, pPoints[i].sLen, v);
    }
  }
}

void GT2005Polygon::rotateZP(double a)
{
  if (numPoints > 0)
  {
    for (int i = 0; i < numPoints; i++)
    {
      Vector3<double> v;
      v.x = pPoints[i].point.x * cos(a) - pPoints[i].point.y * sin(a);
      v.y = pPoints[i].point.x * sin(a) + pPoints[i].point.y * cos(a);
      v.z = pPoints[i].point.z;
      setSegment(i, pPoints[i].sLen, v);
    }
  }
}

GT2005Polygon GT2005Polygon::rotateZC(double a)
{
  GT2005Polygon temp;

  if (numPoints > 0)
  {
    centerOfMassCorrection = calculateCenterOfMass();

    temp.numPoints = numPoints;
    temp.polyLengthX = polyLengthX;
    temp.groundTime = groundTime;
    temp.stepLength = stepLength;
    temp.centerOfMassCorrection = centerOfMassCorrection;

    for (int i = 0; i < numPoints; i++)
    {
      Vector3<double> v1;
      Vector3<double> v2;

      v1 = pPoints[i].point - centerOfMassCorrection; // transform to center of mass
      v2.x = v1.x * cos(a) - v1.y * sin(a);
      v2.y = v1.x * sin(a) + v1.y * cos(a);
      v2.z = v1.z;
      v2 += centerOfMassCorrection; // transform back
      temp.setSegment(i, pPoints[i].sLen, v2);
    }
  }

  return temp;
}

void GT2005Polygon::insertPoint(int pNum) // insert point in the middle between pNum and pNum+1
{
  if ((numPoints > 0) && (pNum >= 0) && (pNum < numPoints) && (pNum < maxPoints))
  {
    numPoints++;
    // move segments in array
    for (int i = numPoints - 1; i > pNum + 1; i--)
    {
      pPoints[i].point = pPoints[i - 1].point;
      pPoints[i].sLen = pPoints[i - 1].sLen;
    }
    // adjust previous segement length
    pPoints[pNum].sLen = pPoints[pNum].sLen / 2;
    // set data for inseretd point
    pPoints[pNum + 1].point = pPoints[pNum].point + ((pPoints[(pNum + 2) % numPoints].point - pPoints[pNum].point) / 2);
    pPoints[pNum + 1].sLen = pPoints[pNum].sLen;
  }
}

GT2005Polygon* GT2005Polygon::interpolateShape(double amount, GT2005Polygon* s2)
{
  GT2005Polygon* temp = new GT2005Polygon(numPoints, stepLength);

  if (numPoints == s2->numPoints)
  {
    for (int i = 0; i < numPoints; i++)
    {
      Vector3<double> v;
      v = pPoints[i].point + ((s2->getPointCoord(i) - pPoints[i].point) * amount);
      double l = pPoints[i].sLen + ((s2->getSegmentLength(i) - pPoints[i].sLen) * amount);
      temp->setSegment(i, l, v);
    }
    // calculate new stepLength
    temp->stepLength = (int)(stepLength + ((s2->stepLength - stepLength) * amount));
  }

  return temp;
}

void GT2005Polygon::interpolateShapeP(double amount, GT2005Polygon* s2)
{
  if (numPoints == s2->numPoints)
  {
    for (int i = 0; i < numPoints; i++)
    {
      Vector3<double> v;
      v = pPoints[i].point + ((s2->getPointCoord(i) - pPoints[i].point) * amount);
      double l = pPoints[i].sLen + ((s2->getSegmentLength(i) - pPoints[i].sLen) * amount);
      setSegment(i, l, v);
    }
    // calculate new stepLength
    stepLength = (int)(stepLength + ((s2->stepLength - stepLength) * amount));
  }
}

GT2005Polygon* GT2005Polygon::copy()
{
  if (numPoints > 0)
  {
    GT2005Polygon* temp = new GT2005Polygon(numPoints, stepLength);
    temp->polyLengthX = polyLengthX;
    temp->groundTime = groundTime;

    for (int i = 0; i < numPoints; i++)
    {
      Vector3<double> v;
      v.x = pPoints[i].point.x;
      v.y = pPoints[i].point.y;
      v.z = pPoints[i].point.z;
      temp->setSegment(i, pPoints[i].sLen, v);
    }
    temp->finalize();
    return temp;
  }
  return false;
}

GT2005Polygon* GT2005Polygon::mirrorY()
{
  if (numPoints > 0)
  {
    GT2005Polygon* temp = new GT2005Polygon(numPoints, stepLength);
    temp->polyLengthX = polyLengthX;
    temp->groundTime = groundTime;

    for (int i = 0; i < numPoints; i++)
    {
      Vector3<double> v;
      v.x = pPoints[i].point.x;
      v.y = -pPoints[i].point.y;
      v.z = pPoints[i].point.z;
      temp->setSegment(i, pPoints[i].sLen, v);
    }
    temp->finalize();
    return temp;
  }
  return false;
}

bool GT2005Polygon::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 GT2005Polygon::save(char* filename)
{
  char name[128];
  if (filename != 0)
  {
    strcpy(name, filename);
  }
  OutBinaryFile file(name);
  file << *this;
}

In& operator >> (In& stream, GT2005Polygon& polygon)
{
  stream >> polygon.numPoints;
  stream >> polygon.rollAngle;
  stream >> polygon.stepLength;
  for (int i = 0; i < polygon.numPoints; i++)
  {
    stream >> polygon.pPoints[i].point >> polygon.pPoints[i].sLen;
  }
  polygon.finalize();
  return stream;
}
 
Out& operator << (Out& stream, const GT2005Polygon& polygon)
{
  stream << polygon.numPoints;
  stream << polygon.rollAngle;
  stream << polygon.stepLength;
  for (int i = 0; i < polygon.numPoints; i++)
  {
    stream << polygon.pPoints[i].point << polygon.pPoints[i].sLen;
  }
  return stream;
}

---