Marsyas  0.6.0-alpha
/usr/src/RPM/BUILD/marsyas-0.6.0/src/marsyas/marsystems/PvConvert.cpp
Go to the documentation of this file.
00001 /*
00002 ** Copyright (C) 1998-2010 George Tzanetakis <gtzan@cs.uvic.ca>
00003 **
00004 ** This program is free software; you can redistribute it and/or modify
00005 ** it under the terms of the GNU General Public License as published by
00006 ** the Free Software Foundation; either version 2 of the License, or
00007 ** (at your option) any later version.
00008 **
00009 ** This program is distributed in the hope that it will be useful,
00010 ** but WITHOUT ANY WARRANTY; without even the implied warranty of
00011 ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012 ** GNU General Public License for more details.
00013 **
00014 ** You should have received a copy of the GNU General Public License
00015 ** along with this program; if not, write to the Free Software
00016 ** Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
00017 */
00018 
00019 
00020 #include "PvConvert.h"
00021 #include <algorithm>
00022 #include <functional>
00023 
00024 
00025 
00026 using std::ostringstream;
00027 using std::greater;
00028 using std::sort;
00029 
00030 using namespace Marsyas;
00031 
00032 PvConvert::PvConvert(mrs_string name):MarSystem("PvConvert",name)
00033 {
00034   //type_ = "PvConvert";
00035   //name_ = name;
00036 
00037   psize_ = 0;
00038   size_ = 0;
00039 
00040   addControls();
00041 }
00042 
00043 PvConvert::PvConvert(const PvConvert& a):MarSystem(a)
00044 {
00045   ctrl_mode_ = getctrl("mrs_string/mode");
00046   ctrl_phases_ = getctrl("mrs_realvec/phases");
00047   ctrl_regions_ = getctrl("mrs_realvec/regions");
00048 
00049   psize_ = 0;
00050 }
00051 
00052 
00053 
00054 PvConvert::~PvConvert()
00055 {
00056 }
00057 
00058 MarSystem*
00059 PvConvert::clone() const
00060 {
00061   return new PvConvert(*this);
00062 }
00063 
00064 
00065 void
00066 PvConvert::addControls()
00067 {
00068   addctrl("mrs_natural/Decimation",MRS_DEFAULT_SLICE_NSAMPLES/4);
00069   addctrl("mrs_natural/Sinusoids", 1);
00070   setctrlState("mrs_natural/Sinusoids", true);
00071   addctrl("mrs_string/mode", "sorted", ctrl_mode_);
00072   addctrl("mrs_realvec/phases", realvec(), ctrl_phases_);
00073   addctrl("mrs_realvec/regions", realvec(), ctrl_regions_);
00074 
00075 
00076 }
00077 
00078 void
00079 PvConvert::myUpdate(MarControlPtr sender)
00080 {
00081 
00082   (void) sender;  //suppress warning of unused parameter(s)
00083   setctrl("mrs_natural/onSamples", getctrl("mrs_natural/inSamples"));
00084   setctrl("mrs_natural/onObservations", getctrl("mrs_natural/inObservations")->to<mrs_natural>() + 2);
00085   setctrl("mrs_real/osrate", getctrl("mrs_real/israte")->to<mrs_real>() * getctrl("mrs_natural/inObservations")->to<mrs_natural>());
00086 
00087   //defaultUpdate(); [!]
00088   onObservations_ = getctrl("mrs_natural/onObservations")->to<mrs_natural>();
00089 
00090   size_ = onObservations_ /2 +1;
00091 
00092   if (size_ != psize_)
00093   {
00094     lastphase_.stretch(size_);
00095     // phase_.stretch(size_);
00096     MarControlAccessor acc(ctrl_phases_);
00097     realvec& phase = acc.to<mrs_realvec>();
00098     MarControlAccessor acc1(ctrl_regions_);
00099     realvec& regions = acc1.to<mrs_realvec>();
00100 
00101     phase.stretch(size_);
00102     regions.stretch(size_);
00103     mag_.stretch(size_);
00104     sortedmags_.stretch(size_);
00105     sortedpos_.stretch(size_);
00106   }
00107 
00108   psize_ = size_;
00109 
00110   factor_ = ((getctrl("mrs_real/osrate")->to<mrs_real>()) /
00111              (mrs_real)( getctrl("mrs_natural/Decimation")->to<mrs_natural>()* TWOPI));
00112 
00113 
00114 
00115   fundamental_ = (mrs_real) (getctrl("mrs_real/osrate")->to<mrs_real>() / (mrs_real)getctrl("mrs_natural/inObservations")->to<mrs_natural>());
00116 
00117 
00118 
00119 
00120   kmax_ = getctrl("mrs_natural/Sinusoids")->to<mrs_natural>();
00121 }
00122 
00123 void
00124 PvConvert::myProcessFull(realvec& in, realvec& out)
00125 {
00126   mrs_natural t;
00127   mrs_natural N2 = inObservations_/2;
00128 
00129   mrs_real a;
00130   mrs_real b;
00131   mrs_real phasediff;
00132 
00133   MarControlAccessor acc(ctrl_phases_);
00134   mrs_realvec& phases = acc.to<mrs_realvec>();
00135 
00136   MarControlAccessor acc1(ctrl_regions_);
00137   mrs_realvec& regions = acc1.to<mrs_realvec>();
00138 
00139 
00140   mrs_real decimation = getctrl("mrs_natural/Decimation")->to<mrs_natural>() * 1.0;
00141   mrs_real one_over_decimation = 1.0 / decimation;
00142 
00143   mrs_real omega_k;
00144 
00145   const mrs_string& mode = ctrl_mode_->to<mrs_string>();
00146 
00147 
00148 
00149   // handle amplitudes
00150   for (t=0; t <= N2; t++)
00151   {
00152     if (t==0)
00153     {
00154       a = in(0,0);
00155       b = 0.0;
00156     }
00157     else if (t == N2)
00158     {
00159       a = in(1, 0);
00160       b = 0.0;
00161     }
00162     else
00163     {
00164       a = in(2*t, 0);
00165       b = in(2*t+1, 0);
00166     }
00167 
00168     omega_k = (TWOPI * t) / (N2*2) ;
00169 
00170     // computer magnitude value
00171     out(2*t,0) = sqrt(a*a + b*b);
00172 
00173     if (out(2*t,0) == 0.0)
00174       phasediff = 0.0;
00175     else
00176     {
00177       phases(t) = -atan2(b,a);
00178 
00179       if (mode == "analysis_scaled_phaselock")
00180       {
00181         // scaled phase-locking
00182         phasediff = phases(t) - lastphase_((mrs_natural)regions(t)) - decimation * omega_k;
00183       }
00184       else
00185       {
00186         // classic, identity, loose phase_propagation
00187         phasediff = phases(t) - lastphase_(t) - decimation * omega_k;
00188       }
00189 
00190       lastphase_(t) = phases(t);
00191 
00192       while (phasediff > PI)
00193         phasediff -= TWOPI;
00194       while (phasediff < -PI)
00195         phasediff += TWOPI;
00196     }
00197 
00198 
00199     out(2*t+1, 0) = omega_k + one_over_decimation * phasediff;
00200   }
00201 }
00202 
00203 
00204 
00205 void
00206 PvConvert::myProcess(realvec& in, realvec& out)
00207 {
00208 
00209 
00210   const mrs_string& mode = ctrl_mode_->to<mrs_string>();
00211   if ((mode == "full")||(mode == "analysis_scaled_phaselock"))
00212     myProcessFull(in,out);
00213   else if (mode == "sorted")
00214     myProcessSorted(in,out);
00215   else if (mode == "neighbors")
00216     myProcessNeighbors(in,out);
00217 
00218 }
00219 
00220 
00221 
00222 
00223 void
00224 PvConvert::myProcessSorted(realvec& in, realvec& out)
00225 {
00226   mrs_natural c,t;
00227 
00228   MarControlAccessor acc(ctrl_phases_);
00229   mrs_realvec& phases = acc.to<mrs_realvec>();
00230 
00231   mrs_real decimation = getctrl("mrs_natural/Decimation")->to<mrs_natural>() * 1.0;
00232 
00233   mrs_real one_over_decimation = 1.0 / decimation;
00234 
00235 
00236   mrs_natural N2 = inObservations_/2;
00237   mrs_real a;
00238   mrs_real b;
00239   mrs_real phasediff;
00240 
00241   mrs_real omega_k;
00242 
00243   // handle amplitudes
00244   for (t=0; t <= N2; t++)
00245   {
00246     if (t==0)
00247     {
00248       a = in(2*t,0);
00249       b = 0.0;
00250     }
00251     else if (t == N2)
00252     {
00253       a = in(1, 0);
00254       b = 0.0;
00255     }
00256     else
00257     {
00258       a = in(2*t, 0);
00259       b = in(2*t+1, 0);
00260     }
00261 
00262     // computer magnitude value
00263     mag_(t) = sqrt(a*a + b*b);
00264     sortedmags_(t) = mag_(t);
00265     // compute phase
00266     phases(t) = -atan2(b,a);
00267 
00268   }
00269 
00270   mrs_real* data = sortedmags_.getData();
00271   sort(data, data+(N2+1), greater<mrs_real>());
00272 
00273   bool found;
00274   mrs_real val;
00275 
00276 
00277   mrs_real sum = 0;
00278   mrs_real sorted_sum = 0;
00279 
00280   for (t=0; t <= N2; t++)
00281     sum += mag_(t);
00282 
00283 
00284   int k = 0;
00285 
00286   for (t=0; t <= N2; t++)
00287   {
00288     found = false;
00289     val = mag_(t);
00290 
00291 
00292     for (c=0; c < kmax_; ++c)
00293     {
00294 
00295       if (val == sortedmags_(c))
00296       {
00297         sorted_sum += val;
00298         found = true;
00299         k++;
00300         break;
00301       }
00302     }
00303 
00304 
00305 
00306     out(2*t,0) = 0.0;
00307     out(2*t+1,0) = t * fundamental_;
00308 
00309     omega_k = (TWOPI * t) / (N2*2) ;
00310 
00311     phasediff = phases(t) - lastphase_(t) - decimation * omega_k;
00312     // phasediff = phases(t) - lastphase_(t);
00313     lastphase_(t) = phases(t);
00314 
00315     // phase unwrapping
00316     while (phasediff > PI)
00317       phasediff -= TWOPI;
00318     while (phasediff < -PI)
00319       phasediff += TWOPI;
00320 
00321     if (found)
00322     {
00323       if (val == 0.0)
00324         phasediff = 0.0;
00325       else
00326       {
00327         out(2*t,0) = val;
00328       }
00329       out(2*t+1, 0) = omega_k + one_over_decimation * phasediff;
00330     }
00331     else
00332     {
00333       out(2*t+1, 0) = omega_k + one_over_decimation * phasediff;
00334     }
00335   }
00336 
00337 
00338 
00339 
00340 }
00341 
00342 void
00343 PvConvert::myProcessNeighbors(realvec& in, realvec& out)
00344 {
00345 
00346 
00347   MarControlAccessor acc(ctrl_phases_);
00348   mrs_realvec& phases = acc.to<mrs_realvec>();
00349 
00350   mrs_natural t;
00351   mrs_natural N2 = inObservations_/2;
00352   mrs_real a;
00353   mrs_real b;
00354   mrs_real phasediff;
00355 
00356   // handle amplitudes
00357   for (t=0; t <= N2; t++)
00358   {
00359     if (t==0)
00360     {
00361       a = in(2*t,0);
00362       b = 0.0;
00363     }
00364     else if (t == N2)
00365     {
00366       a = in(1, 0);
00367       b = 0.0;
00368     }
00369     else
00370     {
00371       a = in(2*t, 0);
00372       b = in(2*t+1, 0);
00373     }
00374 
00375     // computer magnitude value
00376     mag_(t) = sqrt(a*a + b*b);
00377     sortedmags_(t) = mag_(t);
00378     // compute phase
00379     phases(t) = -atan2(b,a);
00380 
00381 
00382   }
00383 
00384   mrs_real* data = sortedmags_.getData();
00385   sort(data, data+(N2+1), greater<mrs_real>());
00386 
00387   mrs_real val;
00388   val = 0.0;
00389 
00390   for (t=0; t <= N2; t++)
00391   {
00392 
00393     phasediff = phases(t) - lastphase_(t);
00394     lastphase_(t) = phases(t);
00395 
00396     // phase unwrapping
00397     while (phasediff > PI)
00398       phasediff -= TWOPI;
00399     while (phasediff < -PI)
00400       phasediff += TWOPI;
00401 
00402     if ((t > 3) && (t <= N2-2))
00403     {
00404       if (
00405         (mag_(t) > mag_(t-1)) &&
00406         // (mag_(t) > mag_(t-2)) &&
00407         // (mag_(t) > mag_(t+2)) &&
00408         (mag_(t) > mag_(t+1))
00409       )
00410 
00411       {
00412         val = mag_(t);
00413       }
00414       else
00415         val = 0.0;
00416     }
00417 
00418     if (val == 0.0)
00419       phasediff = 0.0;
00420 
00421     out(2*t,0) = val;
00422     out(2*t+1, 0) = phasediff * factor_ + t * fundamental_;
00423   }
00424 }
00425 
00426