Tools/Math/FourierCoefficient.cpp

Go to the documentation of this file.

/**
* @file FourierCoefficient.cpp
*
* Implementation of class FourierCoefficient.
*/

#include "FourierCoefficient.h"
#include "Tools/Debugging/Debugging.h"
#include "Tools/Streams/InStreams.h"
#include "Tools/Math/Common.h"

FourierCoefficient::FourierCoefficient():lengthOfPeriod(110)
{
  /** init all coefficients to zero */
  for(int joint = 0; joint < JointData::numOfJoint; joint++)
  {
    useLookUp[joint] = false;
    for(int c = 0; c < FourierCoefficient::numOfCoeffs; c++)
    { 
      real[joint][c]        = 0.0; 
      imaginary[joint][c]   = 0.0; 
      functionLUT[joint][c] = 0.0; 
      r[joint][c]           = 0.0; 
      phi[joint][c]         = 0.0; 
    }
  }
}


FourierCoefficient::~FourierCoefficient()
{ }


/* calculate the approximated function value from the fc's
*/
long FourierCoefficient::fourierSynth(JointData::JointID joint, unsigned long time, int cMax, double scalingFactor)
{
  double functionValue = 0;
  long onePeriod = numOfCoeffs * 8;

  time %= onePeriod;

  /* first coefficient is equal to the 
  mean value over one period, the first complex
  coefficient is always = 0.0, therefore theere is
  nothing to be calculated and the loops below 
  start with n = 1 rather then n = 0 */
  functionValue = real[joint][0]/sqrt((double)numOfCoeffs);

  /*  if the scaling factor is smaller than 0.0, inverse
  the direction of the function (play the function backwards) */
  long mytime=(long)time;
  if (scalingFactor < 0) mytime *= -1;

  //if (useLookUp[joint])
  //  return (long )(functionValue + functionLUT[joint][(int)time] * scalingFactor);

  /*  loop through the coefficents 
  first do all sines, then all cosines starting 
  with the second complex coefficient pair in 
  the array */
  
  double t, b = 2/sqrt((double)numOfCoeffs) * scalingFactor;

  for (int c = 1; c <= cMax; c++)
  {
    t = mytime*pi2*c/onePeriod;
    functionValue += b * (imaginary[joint][c] * sin(t) + real[joint][c] * cos(t));
  }
  
  return (long )functionValue;
}

long FourierCoefficient::fourierSynth(JointData* jointData, unsigned long time, int cMax)
{
  double functionValue = 0;
  long onePeriod = numOfCoeffs * 8;

  time %= onePeriod;
  double sinTab[200];
  double cosTab[200];

  /*  if the scaling factor is smaller than 0.0, inverse
  the direction of the function (play the function backwards) */
  
  double t, b = 2/sqrt((double)numOfCoeffs);

  for (int c = 1; c <= cMax; c++)
  {
    t = time*pi2*c/onePeriod;
    sinTab[c]=b*sin(t);
    cosTab[c]=b*cos(t);
  }

  for(int j = JointData::legFR1; j <= JointData::legHL3; j++)
  {
    functionValue = real[j][0]/sqrt((double)numOfCoeffs);
    for (int c = 1; c <= cMax; c++)
    {
      functionValue += imaginary[j][c]*sinTab[c] + real[j][c]*cosTab[c];
    }
    jointData->data[j] = (long)functionValue;
  }
  
  return (long )functionValue;
}








bool FourierCoefficient::calcLUT(JointData::JointID joint, int cMax)
{
  useLookUp[joint] = false;

  for (int t = 0; t < lengthOfPeriod; t++)
    functionLUT[joint][t] = fourierSynth((JointData::JointID) joint, t, cMax, 1.0) - real[joint][0]/sqrt((double)numOfCoeffs);

  useLookUp[joint] = true;
  return true;
}

bool FourierCoefficient::calcAllLUTs(int cMax)
{ 
  for (int joint = 0; joint < JointData::numOfJoint; joint++)
    calcLUT((JointData::JointID)joint, cMax);
  return true;
}



/* function to calculate the coefficients
*/
bool FourierCoefficient::calculate(long *functionValues, JointData::JointID joint)
{
  double sinPart, cosPart;

  /* Loop through all sines/cosines with freqency c 
  * and do correllation
  */
  for (int c = 0; c < numOfCoeffs; c++)
  { 
    sinPart = 0.0;
    cosPart = 0.0;
    for (int i = 0; i < numOfCoeffs; i++)
    {
      sinPart += functionValues[i] * sin(pi2*i*c/numOfCoeffs);
      cosPart += functionValues[i] * cos(pi2*i*c/numOfCoeffs);
    }
    real[joint][c]      = cosPart/sqrt((double )numOfCoeffs);
    imaginary[joint][c] = sinPart/sqrt((double )numOfCoeffs);
  }

  return true;
}


/* method to shift all phases so that the first sine of the joint "j"
starts with 0 (accordingly, the first cosine amplitude of this joint
is 0).
*/
double FourierCoefficient::phaseAlign(JointData::JointID j)
{
  calcPhiAndR();
  double currentPhaseShift = phi[j][1]; // note that the 1st and not the 0th is used!!

  for (int joint = JointData::legFR1; joint <= JointData::legHL3; joint++)  
  {
    for(int c = 1; c < FourierCoefficient::numOfCoeffs; c++)
    {
      phi[joint][c] -= currentPhaseShift * c/numOfCoeffs;

      /*
      if (phi[joint][c] > pi2) 
        phi[joint][c] -= pi2;
      else if (phi[joint][c] < -pi2) 
        phi[joint][c] += pi2;
*/
      //while (phi[joint][c] > pi2) phi[joint][c] -= pi2;
    }
  }
  calcReAndIm();
  return currentPhaseShift;
}


bool FourierCoefficient::merge(FourierCoefficient *fc0, FourierCoefficient *fc1, double w1, FourierCoefficient *fc2, double w2, FourierCoefficient *fc3, double w3, int numOfC)
{
  double phi1, phi2, phi3;

  for (int joint = JointData::legFR1; joint <= JointData::legHL3; joint++)
  {
    for(int c = 0; c < numOfC; c++)
    {
      r[joint][c] = fc0->r[joint][c] +
                    (fc1->r[joint][c] - fc0->r[joint][c])*w1 +
                    (fc2->r[joint][c] - fc0->r[joint][c])*w2 +
                    (fc3->r[joint][c] - fc0->r[joint][c])*w3;

      /** always use the (direction of) difference with the smaller abs() */
      phi1 = fc1->phi[joint][c] - fc0->phi[joint][c];
      if (phi1>pi) phi1-=pi2; else if (phi1<-pi) phi1+=pi2;
      phi2 = fc2->phi[joint][c] - fc0->phi[joint][c];
      if (phi2>pi) phi2-=pi2; else if (phi2<-pi) phi2+=pi2;
      phi3 = fc3->phi[joint][c] - fc0->phi[joint][c];
      if (phi3>pi) phi3-=pi2; else if (phi3<-pi) phi3+=pi2;

      phi[joint][c] = fc0->phi[joint][c] + phi1*w1 + phi2*w2 + phi3*w3;
    }
  }
  calcReAndIm(numOfC);
  return true;
}



bool FourierCoefficient::merge(FourierCoefficient *fc1, double w1, FourierCoefficient *fc2, double w2, int numOfC)
{
  double phi1, phi2;

  for (int joint = JointData::legFR1; joint <= JointData::legHL3; joint++)
  {
    for(int c = 0; c < numOfC; c++)
    {
      r[joint][c] = fc1->r[joint][c]*w1 + fc2->r[joint][c]*w2;

      phi1 = fc1->phi[joint][c]; 
      phi2 = fc2->phi[joint][c];

      if (phi2 - phi1 > pi) phi2 -= pi2; 
      if (phi2 - phi1 < -pi) phi2 += pi2; 

      phi[joint][c] = phi1*w1 + phi2*w2;
    }
  }
  calcReAndIm(numOfC);
  return true;
}



bool FourierCoefficient::calcPhiAndR(int numOfC)
{
  for (int joint = JointData::legFR1; joint <= JointData::legHL3; joint++)
  {
    for(int c = 0; c < numOfC; c++)
    {
      r[joint][c] = sqrt(real[joint][c]*real[joint][c] + imaginary[joint][c]*imaginary[joint][c]);
      phi[joint][c] = atan2(imaginary[joint][c],real[joint][c]);
    }
  }

  return true;
}

bool FourierCoefficient::calcReAndIm(int numOfC)
{
  for (int joint = JointData::legFR1; joint <= JointData::legHL3; joint++)
  {
    for(int c = 0; c < numOfC; c++)
    {
      real[joint][c]      = r[joint][c]*cos(phi[joint][c]);
      imaginary[joint][c] = r[joint][c]*sin(phi[joint][c]);
    }
  }

  return true;
}


In& operator>>(In& stream,FourierCoefficient& fc)
{
  /** @todo read lengthOfPeriod */
  for (int joint = JointData::legFR1; joint <= JointData::legHL3; joint++)
  {
    for(int c = 0; c < FourierCoefficient::numOfCoeffs; c++)
    {
      stream >> fc.real[joint][c] >> fc.imaginary[joint][c];
    }
  }
  return stream;
}

Out& operator<<(Out& stream,FourierCoefficient& fc)
{
  /** @todo write lengthOfPeriod */
  for (int joint = JointData::legFR1; joint <= JointData::legHL3; joint++)
  {
    for(int c = 0; c < FourierCoefficient::numOfCoeffs; c++)
    {
      stream << fc.real[joint][c] << fc.imaginary[joint][c];
    }
    stream << endl;
  }
  return stream;
}

---