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;
}


/*
 * Change log :
 * 
 * $Log: FourierCoefficient.cpp,v $
 * Revision 1.1.1.1  2004/05/22 17:37:02  cvsadm
 * created new repository GT2004_WM
 *
 * Revision 1.1  2003/10/07 10:13:24  cvsadm
 * Created GT2004 (M.J.)
 *
 * Revision 1.1  2003/09/26 11:40:40  juengel
 * - sorted tools
 * - clean-up in DataTypes
 *
 * Revision 1.1.1.1  2003/07/02 09:40:22  cvsadm
 * created new repository for the competitions in Padova from the 
 * tamara CVS (Tuesday 2:00 pm)
 *
 * removed unused solutions
 *
 * Revision 1.18  2003/05/26 12:56:34  dueffert
 * bug fixed
 *
 * Revision 1.17  2003/02/02 23:30:30  jhoffman
 * added merge for only two types of motion and more debugging
 * and control stuff (mostly needed for performance testing)
 *
 * Revision 1.16  2003/01/22 22:16:48  jhoffman
 * phase align updated and debugged
 *
 * Revision 1.15  2003/01/21 14:58:39  jhoffman
 * increased performance by precalculating sin/cos
 *
 * Revision 1.14  2003/01/20 18:40:08  dueffert
 * optimized: synthesis should do the same but faster
 *
 * Revision 1.13  2003/01/18 08:26:35  jhoffman
 * changes in order to improve performance
 *
 * Revision 1.12  2003/01/16 09:55:10  jhoffman
 * Added "Generic Debug Data" data type. This can be used
 * for quick and dirty optimization and debugging, e.g. to send
 * parameters to a module through WLAN to adjust it's settings.
 * The DebugMessageGenerator is used to for parsing and
 * sendig the data
 *
 * Revision 1.11  2002/12/13 09:54:42  jhoffman
 * walking engine now performes walk-like motions, but still not fully functional
 *
 * Revision 1.10  2002/12/10 16:07:05  dueffert
 * Fourier working now
 *
 * Revision 1.9  2002/12/10 10:47:04  jhoffman
 * debugged and pretty much working
 *
 * Revision 1.8  2002/12/09 15:24:06  dueffert
 * some optimisation
 *
 * Revision 1.7  2002/12/09 14:15:40  jhoffman
 * no message
 *
 * Revision 1.6  2002/12/04 12:21:18  jhoffman
 * no message
 *
 * Revision 1.5  2002/11/22 13:42:08  dueffert
 * cleanup
 *
 * Revision 1.4  2002/11/22 13:41:21  loetzsch
 * - removed the FourierCoefficient::loadLegs and ::saveLegs functions
 *   (streaming operators are now used)
 * - .fcb files have text format now
 * . moved the .fcb files from /Config to /Config/Fourier
 *
 * Revision 1.3  2002/11/19 17:14:14  risler
 * coding conventions: renamed JointData::joint to JointID, GetName to getName
 *
 * Revision 1.2  2002/09/22 18:40:54  risler
 * added new math functions, removed GTMath library
 *
 * Revision 1.1  2002/09/10 15:26:40  cvsadm
 * Created new project GT2003 (M.L.)
 * - Cleaned up the /Src/DataTypes directory
 * - Removed Challenge Code
 * - Removed processing of incoming audio data
 * - Renamed AcousticMessage to SoundRequest
 *
 * Revision 1.2  2002/07/23 13:32:57  loetzsch
 * new streaming classes
 *
 * removed many #include statements
 *
 * Revision 1.1.1.1  2002/05/10 12:40:13  cvsadm
 * Moved GT2002 Project from ute to tamara.
 *
 * Revision 1.10  2002/05/02 12:12:33  kallnik
 * GTMath
 *
 * Revision 1.9  2002/04/25 14:50:37  kallnik
 * changed double/float to double
 * added several #include GTMath
 *
 * PLEASE use double
 *
 * Revision 1.8  2002/04/23 15:00:06  jhoffman
 * changes and additions
 *
 * Revision 1.7  2002/04/03 16:44:31  jhoffman
 * added "stabilizeRobot" to motionControl (which is turned off as a default)
 *
 * Revision 1.4  2002/03/19 13:23:50  jhoffman
 * no message
 *
 * Revision 1.3  2002/03/19 12:11:14  jhoffman
 * extended functionality (synth, load, save, ...)
 *
 * Revision 1.2  2002/03/12 14:10:48  jhoffman
 * added fourier synthesis functionality which calculates the FT from the coefficients
 *
 * Revision 1.1  2002/01/26 20:22:52  juengel
 * no message
 *
 *
 */

---