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

# Tools/Math/FourierCoefficient.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file FourierCoefficient.cpp
    00003 *
    00004 * Implementation of class FourierCoefficient.
    00005 */
    00006
    00007 #include "FourierCoefficient.h"
    00008 #include "Tools/Debugging/Debugging.h"
    00009 #include "Tools/Streams/InStreams.h"
    00010 #include "Tools/Math/Common.h"
    00011
    00012 FourierCoefficient::FourierCoefficient():lengthOfPeriod(110)
    00013 {
    00014   /** init all coefficients to zero */
    00015   for(int joint = 0; joint < JointData::numOfJoint; joint++)
    00016   {
    00017     useLookUp[joint] = false;
    00018     for(int c = 0; c < FourierCoefficient::numOfCoeffs; c++)
    00019     {
    00020       real[joint][c]        = 0.0;
    00021       imaginary[joint][c]   = 0.0;
    00022       functionLUT[joint][c] = 0.0;
    00023       r[joint][c]           = 0.0;
    00024       phi[joint][c]         = 0.0;
    00025     }
    00026   }
    00027 }
    00028
    00029
    00030 FourierCoefficient::~FourierCoefficient()
    00031 { }
    00032
    00033
    00034 /* calculate the approximated function value from the fc's
    00035 */
    00036 long FourierCoefficient::fourierSynth(JointData::JointID joint, unsigned long time, int cMax, double scalingFactor)
    00037 {
    00038   double functionValue = 0;
    00039   long onePeriod = numOfCoeffs * 8;
    00040
    00041   time %= onePeriod;
    00042
    00043   /* first coefficient is equal to the
    00044   mean value over one period, the first complex
    00045   coefficient is always = 0.0, therefore theere is
    00046   nothing to be calculated and the loops below
    00047   start with n = 1 rather then n = 0 */
    00048   functionValue = real[joint][0]/sqrt((double)numOfCoeffs);
    00049
    00050   /*  if the scaling factor is smaller than 0.0, inverse
    00051   the direction of the function (play the function backwards) */
    00052   long mytime=(long)time;
    00053   if (scalingFactor < 0) mytime *= -1;
    00054
    00055   //if (useLookUp[joint])
    00056   //  return (long )(functionValue + functionLUT[joint][(int)time] * scalingFactor);
    00057
    00058   /*  loop through the coefficents
    00059   first do all sines, then all cosines starting
    00060   with the second complex coefficient pair in
    00061   the array */
    00062
    00063   double t, b = 2/sqrt((double)numOfCoeffs) * scalingFactor;
    00064
    00065   for (int c = 1; c <= cMax; c++)
    00066   {
    00067     t = mytime*pi2*c/onePeriod;
    00068     functionValue += b * (imaginary[joint][c] * sin(t) + real[joint][c] * cos(t));
    00069   }
    00070
    00071   return (long )functionValue;
    00072 }
    00073
    00074 long FourierCoefficient::fourierSynth(JointData* jointData, unsigned long time, int cMax)
    00075 {
    00076   double functionValue = 0;
    00077   long onePeriod = numOfCoeffs * 8;
    00078
    00079   time %= onePeriod;
    00080   double sinTab[200];
    00081   double cosTab[200];
    00082
    00083   /*  if the scaling factor is smaller than 0.0, inverse
    00084   the direction of the function (play the function backwards) */
    00085
    00086   double t, b = 2/sqrt((double)numOfCoeffs);
    00087
    00088   for (int c = 1; c <= cMax; c++)
    00089   {
    00090     t = time*pi2*c/onePeriod;
    00091     sinTab[c]=b*sin(t);
    00092     cosTab[c]=b*cos(t);
    00093   }
    00094
    00095   for(int j = JointData::legFR1; j <= JointData::legHL3; j++)
    00096   {
    00097     functionValue = real[j][0]/sqrt((double)numOfCoeffs);
    00098     for (int c = 1; c <= cMax; c++)
    00099     {
    00100       functionValue += imaginary[j][c]*sinTab[c] + real[j][c]*cosTab[c];
    00101     }
    00102     jointData->data[j] = (long)functionValue;
    00103   }
    00104
    00105   return (long )functionValue;
    00106 }
    00107
    00108
    00109
    00110
    00111
    00112
    00113
    00114
    00115 bool FourierCoefficient::calcLUT(JointData::JointID joint, int cMax)
    00116 {
    00117   useLookUp[joint] = false;
    00118
    00119   for (int t = 0; t < lengthOfPeriod; t++)
    00120     functionLUT[joint][t] = fourierSynth((JointData::JointID) joint, t, cMax, 1.0) - real[joint][0]/sqrt((double)numOfCoeffs);
    00121
    00122   useLookUp[joint] = true;
    00123   return true;
    00124 }
    00125
    00126 bool FourierCoefficient::calcAllLUTs(int cMax)
    00127 {
    00128   for (int joint = 0; joint < JointData::numOfJoint; joint++)
    00129     calcLUT((JointData::JointID)joint, cMax);
    00130   return true;
    00131 }
    00132
    00133
    00134
    00135 /* function to calculate the coefficients
    00136 */
    00137 bool FourierCoefficient::calculate(long *functionValues, JointData::JointID joint)
    00138 {
    00139   double sinPart, cosPart;
    00140
    00141   /* Loop through all sines/cosines with freqency c
    00142   * and do correllation
    00143   */
    00144   for (int c = 0; c < numOfCoeffs; c++)
    00145   {
    00146     sinPart = 0.0;
    00147     cosPart = 0.0;
    00148     for (int i = 0; i < numOfCoeffs; i++)
    00149     {
    00150       sinPart += functionValues[i] * sin(pi2*i*c/numOfCoeffs);
    00151       cosPart += functionValues[i] * cos(pi2*i*c/numOfCoeffs);
    00152     }
    00153     real[joint][c]      = cosPart/sqrt((double )numOfCoeffs);
    00154     imaginary[joint][c] = sinPart/sqrt((double )numOfCoeffs);
    00155   }
    00156
    00157   return true;
    00158 }
    00159
    00160
    00161 /* method to shift all phases so that the first sine of the joint "j"
    00162 starts with 0 (accordingly, the first cosine amplitude of this joint
    00163 is 0).
    00164 */
    00165 double FourierCoefficient::phaseAlign(JointData::JointID j)
    00166 {
    00167   calcPhiAndR();
    00168   double currentPhaseShift = phi[j][1]; // note that the 1st and not the 0th is used!!
    00169
    00170   for (int joint = JointData::legFR1; joint <= JointData::legHL3; joint++)
    00171   {
    00172     for(int c = 1; c < FourierCoefficient::numOfCoeffs; c++)
    00173     {
    00174       phi[joint][c] -= currentPhaseShift * c/numOfCoeffs;
    00175
    00176       /*
    00177       if (phi[joint][c] > pi2)
    00178         phi[joint][c] -= pi2;
    00179       else if (phi[joint][c] < -pi2)
    00180         phi[joint][c] += pi2;
    00181 */
    00182       //while (phi[joint][c] > pi2) phi[joint][c] -= pi2;
    00183     }
    00184   }
    00185   calcReAndIm();
    00186   return currentPhaseShift;
    00187 }
    00188
    00189
    00190 bool FourierCoefficient::merge(FourierCoefficient *fc0, FourierCoefficient *fc1, double w1, FourierCoefficient *fc2, double w2, FourierCoefficient *fc3, double w3, int numOfC)
    00191 {
    00192   double phi1, phi2, phi3;
    00193
    00194   for (int joint = JointData::legFR1; joint <= JointData::legHL3; joint++)
    00195   {
    00196     for(int c = 0; c < numOfC; c++)
    00197     {
    00198       r[joint][c] = fc0->r[joint][c] +
    00199                     (fc1->r[joint][c] - fc0->r[joint][c])*w1 +
    00200                     (fc2->r[joint][c] - fc0->r[joint][c])*w2 +
    00201                     (fc3->r[joint][c] - fc0->r[joint][c])*w3;
    00202
    00203       /** always use the (direction of) difference with the smaller abs() */
    00204       phi1 = fc1->phi[joint][c] - fc0->phi[joint][c];
    00205       if (phi1>pi) phi1-=pi2; else if (phi1<-pi) phi1+=pi2;
    00206       phi2 = fc2->phi[joint][c] - fc0->phi[joint][c];
    00207       if (phi2>pi) phi2-=pi2; else if (phi2<-pi) phi2+=pi2;
    00208       phi3 = fc3->phi[joint][c] - fc0->phi[joint][c];
    00209       if (phi3>pi) phi3-=pi2; else if (phi3<-pi) phi3+=pi2;
    00210
    00211       phi[joint][c] = fc0->phi[joint][c] + phi1*w1 + phi2*w2 + phi3*w3;
    00212     }
    00213   }
    00214   calcReAndIm(numOfC);
    00215   return true;
    00216 }
    00217
    00218
    00219
    00220 bool FourierCoefficient::merge(FourierCoefficient *fc1, double w1, FourierCoefficient *fc2, double w2, int numOfC)
    00221 {
    00222   double phi1, phi2;
    00223
    00224   for (int joint = JointData::legFR1; joint <= JointData::legHL3; joint++)
    00225   {
    00226     for(int c = 0; c < numOfC; c++)
    00227     {
    00228       r[joint][c] = fc1->r[joint][c]*w1 + fc2->r[joint][c]*w2;
    00229
    00230       phi1 = fc1->phi[joint][c];
    00231       phi2 = fc2->phi[joint][c];
    00232
    00233       if (phi2 - phi1 > pi) phi2 -= pi2;
    00234       if (phi2 - phi1 < -pi) phi2 += pi2;
    00235
    00236       phi[joint][c] = phi1*w1 + phi2*w2;
    00237     }
    00238   }
    00239   calcReAndIm(numOfC);
    00240   return true;
    00241 }
    00242
    00243
    00244
    00245 bool FourierCoefficient::calcPhiAndR(int numOfC)
    00246 {
    00247   for (int joint = JointData::legFR1; joint <= JointData::legHL3; joint++)
    00248   {
    00249     for(int c = 0; c < numOfC; c++)
    00250     {
    00251       r[joint][c] = sqrt(real[joint][c]*real[joint][c] + imaginary[joint][c]*imaginary[joint][c]);
    00252       phi[joint][c] = atan2(imaginary[joint][c],real[joint][c]);
    00253     }
    00254   }
    00255
    00256   return true;
    00257 }
    00258
    00259 bool FourierCoefficient::calcReAndIm(int numOfC)
    00260 {
    00261   for (int joint = JointData::legFR1; joint <= JointData::legHL3; joint++)
    00262   {
    00263     for(int c = 0; c < numOfC; c++)
    00264     {
    00265       real[joint][c]      = r[joint][c]*cos(phi[joint][c]);
    00266       imaginary[joint][c] = r[joint][c]*sin(phi[joint][c]);
    00267     }
    00268   }
    00269
    00270   return true;
    00271 }
    00272
    00273
    00274 In& operator>>(In& stream,FourierCoefficient& fc)
    00275 {
    00276   /** @todo read lengthOfPeriod */
    00277   for (int joint = JointData::legFR1; joint <= JointData::legHL3; joint++)
    00278   {
    00279     for(int c = 0; c < FourierCoefficient::numOfCoeffs; c++)
    00280     {
    00281       stream >> fc.real[joint][c] >> fc.imaginary[joint][c];
    00282     }
    00283   }
    00284   return stream;
    00285 }
    00286
    00287 Out& operator<<(Out& stream,FourierCoefficient& fc)
    00288 {
    00289   /** @todo write lengthOfPeriod */
    00290   for (int joint = JointData::legFR1; joint <= JointData::legHL3; joint++)
    00291   {
    00292     for(int c = 0; c < FourierCoefficient::numOfCoeffs; c++)
    00293     {
    00294       stream << fc.real[joint][c] << fc.imaginary[joint][c];
    00295     }
    00296     stream << endl;
    00297   }
    00298   return stream;
    00299 }
    00300