123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421 |
- /* Spectrum_extensions.cpp
- *
- * Copyright (C) 1993-2017 David Weenink
- *
- * This code is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or (at
- * your option) any later version.
- *
- * This code is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this work. If not, see <http://www.gnu.org/licenses/>.
- */
- /*
- djmw 20010718
- djmw 20020813 GPL header
- djmw 20030929 Added a warning in Spectrum_drawPhases.
- djmw 20031023 New: Spectra_multiply, Spectrum_conjugate
- djmw 20040506 Changed warning message in Spectrum_drawPhases.
- djmw 20041124 Changed call to Sound_to_Spectrum.
- djmw 20061218 Introduction of Melder_information<12...9>
- djmw 20071022 phase_unwrap initialize phase = 0.
- djmw 20080122 float -> double
- djmw 20080202 Warning in Spectrum_drawPhases to wchar
- djmw 20080411 Removed define NUM2pi
- */
- #include "Ltas.h"
- #include "Spectrum_extensions.h"
- #include "Sound_and_Spectrum.h"
- #include "NUM2.h"
- #define SIGN(x,s) ((s) < 0 ? -fabs (x) : fabs(x))
- #define THLCON 0.5
- #define THLINC 1.5
- #define EXP2 12
- #define PPVPHA(x,y,test) ((test) ? atan2 (-(y),-(x)) : atan2 ((y),(x)))
- #define PHADVT(xr,xi,yr,yi,xa) ((xa) > 0 ? ((xr)*(yr)+(xi)*(yi))/ (xa) : 0)
- struct tribolet_struct {
- double thlinc, thlcon;
- double ddf, dvtmn2;
- double *x;
- integer nx, l, count;
- int reverse_sign;
- };
- /*
- Perform modified Goertzel algorithm to calculate, at frequency 'freq_rad',
- the real and imaginary part of the spectrum and the d/df of the
- spectrum of x.
- Reference: Bonzanigo (1978), IEEE Trans. ASSP, Vol. 26.
- */
- static void getSpectralValues (struct tribolet_struct *tbs, double freq_rad, double *xr, double *xi, double *nxr, double *nxi) {
- double cosf = cos (freq_rad), sinf = sin (freq_rad);
- double a = 2 * cosf, b, u1 = 0, u2 = u1, w1 = u1, w2 = u1;
- double *x = tbs -> x;
- integer nx = tbs -> nx;
- for (integer j = 1; j <= nx; j ++) {
- double xj = x [j];
- double u0 = xj + a * u1 - u2;
- double w0 = (j - 1) * xj + a * w1 - w2;
- u2 = u1;
- u1 = u0;
- w2 = w1;
- w1 = w0;
- }
- // Bonzanigo's phase correction
- a = freq_rad * (nx - 1);
- u1 = cos (a);
- u2 = - sin (a);
- a = u1 - u2 * cosf;
- b = u2 * sinf;
- *xr = u1 * a - u2 * b;
- *xi = u2 * a + u1 * b;
- a = w1 - w2 * cosf;
- b = w2 * sinf;
- *nxr = u1 * a - u2 * b;
- *nxi = u2 * a + u1 * b;
- tbs -> count ++;
- }
- // Find the closest unwrapped phase estimate from the two admissible phase values (a1 & a2).
- static int phase_check (double pv, double *phase, double thlcon) {
- double a0 = (*phase - pv) / NUM2pi;
- integer k = Melder_ifloor (a0); // ppgb: instead of truncation toward zero
- double a1 = pv + k * NUM2pi;
- double a2 = a1 + SIGN (NUM2pi, a0);
- double a3 = fabs (a1 - *phase);
- double a4 = fabs (a2 - *phase);
- if (a3 > thlcon && a4 > thlcon) {
- return 0;
- }
- *phase = a3 > a4 ? a2 : a1;
- return 1;
- }
- // Phase unwrapping based on Tribolet's adaptive integration method.
- // the unwrapped phase estimate is returned.
- static double phase_unwrap (struct tribolet_struct *tbs, double pfreq, double ppv, double pdvt, double *pphase, double *ppdvt) {
- double sdvt [25], sppv [25];
- double freq, phase = 0, phase_inc;
- double delta, xr, xi, xmsq, nxr, nxi;
- integer k, sindex [25], pindex = 1, sp = 1;
- sppv [sp] = ppv;
- sdvt [sp] = pdvt;
- sindex [sp] = tbs -> l + 1;
- goto p40;
- p20:
- /*
- When the routine runs out of stack space, there probably is
- a zero very near the unit circle that results in a jump of
- pi in the phase.
- */
- if ((sindex [sp] - pindex) <= 1) {
- return phase;
- }
- /* p30:
- Get the intermediate frequency value and compute its phase
- derivative and principal value.
- */
- k = (sindex [sp] + pindex) / 2;
- freq = pfreq + (k - 1) * tbs -> ddf;
- getSpectralValues (tbs, freq, &xr, &xi, &nxr, &nxi);
- sindex [ ++sp] = k;
- sppv [sp] = PPVPHA (xr, xi, tbs -> reverse_sign);
- xmsq = xr * xr + xi * xi;
- sdvt [sp] = PHADVT (xr, xi, nxr, nxi, xmsq);
- p40:
- /*
- Evaluate the phase increment.
- If the phase increment, reduced by the expected linear phase
- increment, is greater than the specified threshold, adapt step size.
- */
- delta = 0.5 * tbs -> ddf * (sindex [sp] - pindex);
- phase_inc = delta * (*ppdvt + sdvt [sp]);
- if (fabs (phase_inc - delta * tbs -> dvtmn2) > tbs -> thlinc) {
- goto p20;
- }
- phase = *pphase + phase_inc;
- if (! phase_check (sppv [sp], &phase, tbs -> thlcon)) {
- goto p20;
- }
- if (fabs (phase - *pphase) > NUMpi) {
- goto p20;
- }
- if (sp == 1) {
- return phase;
- }
- // p10: Update previous estimate.
- pindex = sindex [sp];
- *pphase = phase;
- *ppdvt = sdvt [sp--];
- goto p40;
- }
- autoMatrix Spectrum_unwrap (Spectrum me) {
- try {
- struct tribolet_struct tbs;
- int remove_linear_part = 1;
- integer nfft = 2;
- while (nfft < my nx - 1) {
- nfft *= 2;
- }
- nfft *= 2;
- Melder_require (nfft / 2 == my nx - 1, U"Dimension of Spectrum should be a power of 2 - 1.");
- autoSound x = Spectrum_to_Sound (me);
- autoSound nx = Data_copy (x.get());
- for (integer i = 1; i <= x -> nx; i ++) {
- nx -> z [1] [i] *= (i - 1);
- }
- autoSpectrum snx = Sound_to_Spectrum (nx.get(), true);
- autoMatrix thee = Matrix_create (my xmin, my xmax, my nx, my dx, my x1, 1, 2, 2, 1, 1);
- // Common variables.
- tbs.thlinc = THLINC;
- tbs.thlcon = THLCON;
- tbs.x = x -> z [1];
- tbs.nx = x -> nx;
- tbs.l = Melder_ifloor (pow (2, EXP2) + 0.1);
- tbs.ddf = NUM2pi / ( (tbs.l) * nfft);
- tbs.reverse_sign = my z [1] [1] < 0;
- tbs.count = 0;
- // Reuse snx : put phase derivative (d/df) in imaginary part.
- tbs.dvtmn2 = 0;
- for (integer i = 1; i <= my nx; i ++) {
- double xr = my z [1] [i], xi = my z [2] [i];
- double nxr = snx -> z [1] [i], nxi = snx -> z [2] [i];
- double xmsq = xr * xr + xi * xi;
- double pdvt = PHADVT (xr, xi, nxr, nxi, xmsq);
- thy z [1] [i] = xmsq;
- snx -> z [2] [i] = pdvt;
- tbs.dvtmn2 += pdvt;
- }
- tbs.dvtmn2 = (2 * tbs.dvtmn2 - snx -> z [2] [1] - snx -> z [2] [my nx]) / (my nx - 1);
- autoMelderProgress progress (U"Phase unwrapping");
- double pphase = 0, phase = 0;
- double ppdvt = snx -> z [2] [1];
- thy z [2] [1] = PPVPHA (my z [1] [1], my z [2] [1], tbs.reverse_sign);
- for (integer i = 2; i <= my nx; i ++) {
- double pfreq = NUM2pi * (i - 1) / nfft;
- double pdvt = snx -> z [2] [i];
- double ppv = PPVPHA (my z [1] [i], my z [2] [i], tbs.reverse_sign);
- phase = phase_unwrap (&tbs, pfreq, ppv, pdvt, &pphase, &ppdvt);
- ppdvt = pdvt;
- thy z [2] [i] = pphase = phase;
- Melder_progress ( (double) i / my nx, i,
- U" unwrapped phases from ", my nx, U".");
- }
- integer iphase = Melder_ifloor (phase / NUMpi + 0.1); // ppgb: better than truncation toward zero
- if (remove_linear_part) {
- phase /= my nx - 1;
- for (integer i = 2; i <= my nx; i ++) {
- thy z [2] [i] -= phase * (i - 1);
- }
- }
- Melder_information (U"Number of spectral values: ", tbs.count);
- Melder_information (U" iphase = ", iphase);
- return thee;
- } catch (MelderError) {
- Melder_throw (me, U": not unwrapped.");
- }
- }
- void Spectrum_drawPhases (Spectrum me, Graphics g, double fmin, double fmax, double phase_min, double phase_max, int unwrap, int garnish) {
- autoMatrix thee;
- int reverse_sign = my z [1] [1] < 0;
- if (unwrap) {
- thee = Spectrum_unwrap (me);
- } else {
- thee = Matrix_create (my xmin, my xmax, my nx, my dx, my x1, 1.0, 2.0, 2, 1.0, 1.0);
- for (integer i = 1; i <= my nx; i ++) {
- thy z [2] [i] = PPVPHA (my z [1] [i], my z [2] [i], reverse_sign);
- }
- }
- Matrix_drawRows (thee.get(), g, fmin, fmax, 1.9, 2.1, phase_min, phase_max);
- if (garnish) {
- }
- }
- autoSpectrum Spectra_multiply (Spectrum me, Spectrum thee) {
- try {
- Melder_require (my nx == thy nx && my x1 == thy x1 && my xmax == thy xmax && my dx == thy dx,
- U"Dimensions of both spectra should be the same.");
- autoSpectrum him = Data_copy (me);
- for (integer i = 1; i <= his nx; i ++) {
- his z [1] [i] = my z [1] [i] * thy z [1] [i] - my z [2] [i] * thy z [2] [i];
- his z [2] [i] = my z [1] [i] * thy z [2] [i] + my z [2] [i] * thy z [1] [i];
- }
- return him;
- } catch (MelderError) {
- Melder_throw (me, U": not multiplied.");
- }
- }
- void Spectrum_conjugate (Spectrum me) {
- for (integer i = 1; i <= my nx; i ++) {
- my z [2] [i] = - my z [2] [i];
- }
- }
- autoSpectrum Spectrum_resample (Spectrum me, integer numberOfFrequencies) {
- try {
- double newSamplingFrequency = (1 / my dx) * numberOfFrequencies / my nx;
- // resample real and imaginary part !
- autoSound thee = Sound_resample ((Sound) me, newSamplingFrequency, 50);
- autoSpectrum him = Spectrum_create (my xmax, numberOfFrequencies);
- matrixcopy_preallocated (his z.get(), thy z.get());
- return him;
- } catch (MelderError) {
- Melder_throw (me, U": not resampled.");
- }
- }
- #if 0
- static autoSpectrum Spectrum_shiftFrequencies2 (Spectrum me, double shiftBy, bool changeMaximumFrequency) {
- try {
- double xmax = my xmax;
- integer numberOfFrequencies = my nx, interpolationDepth = 50;
- if (changeMaximumFrequency) {
- xmax += shiftBy;
- numberOfFrequencies += (xmax - my xmax) / my dx;
- }
- autoSpectrum thee = Spectrum_create (xmax, numberOfFrequencies);
- // shiftBy >= 0
- for (integer i = 1; i <= thy nx; i ++) {
- double thyf = thy x1 + (i - 1) * thy dx;
- double myf = thyf - shiftBy;
- if (myf >= my xmin && myf <= my xmax) {
- double index = Sampled_xToIndex (me, myf);
- thy z [1] [i] = NUM_interpolate_sinc (my z [1], my nx, index, interpolationDepth);
- thy z [2] [i] = NUM_interpolate_sinc (my z [2], my nx, index, interpolationDepth);
- }
- }
- return thee;
- } catch (MelderError) {
- Melder_throw (me, U": not shifted.");
- }
- }
- #endif
- autoSpectrum Spectrum_shiftFrequencies (Spectrum me, double shiftBy, double newMaximumFrequency, integer interpolationDepth) {
- try {
- double xmax = my xmax;
- integer numberOfFrequencies = my nx;
- if (newMaximumFrequency != 0.0) {
- numberOfFrequencies = Melder_ifloor (newMaximumFrequency / my dx) + 1;
- xmax = newMaximumFrequency;
- }
- autoSpectrum thee = Spectrum_create (xmax, numberOfFrequencies);
- // shiftBy >= 0
- for (integer i = 1; i <= thy nx; i ++) {
- double thyf = thy x1 + (i - 1) * thy dx;
- double myf = thyf - shiftBy;
- if (myf >= my xmin && myf <= my xmax) {
- double index = Sampled_xToIndex (me, myf);
- thy z [1] [i] = NUM_interpolate_sinc (my z [1], my nx, index, interpolationDepth);
- thy z [2] [i] = NUM_interpolate_sinc (my z [2], my nx, index, interpolationDepth);
- }
- }
- // Make imaginary part of first and last sample zero
- // so Spectrum_to_Sound uses FFT if numberOfSamples was power of 2!
- double amp = sqrt (thy z [1] [1] * thy z [1] [1] + thy z [2] [1] * thy z [2] [1]);
- thy z [1] [1] = amp; thy z [2] [1] = 0;
- amp = sqrt (thy z [1] [thy nx] * thy z [1] [thy nx] + thy z [2] [thy nx] * thy z [2] [thy nx]);
- thy z [1] [thy nx] = amp; thy z [2] [thy nx] = 0;
- return thee;
- } catch (MelderError) {
- Melder_throw (me, U": not shifted.");
- }
- }
- autoSpectrum Spectrum_compressFrequencyDomain (Spectrum me, double fmax, integer interpolationDepth, int freqscale, int method) {
- try {
- double fdomain = my xmax - my xmin, factor = fdomain / fmax ;
- //integer numberOfFrequencies = 1.0 + fmax / my dx; // keep dx the same, otherwise the "duration" changes
- double xmax = my xmax / factor;
- integer numberOfFrequencies = Melder_ifloor (my nx / factor); // keep dx the same, otherwise the "duration" changes
- autoSpectrum thee = Spectrum_create (xmax, numberOfFrequencies);
- thy z [1] [1] = my z [1] [1]; thy z [2] [1] = my z [2] [1];
- double df = freqscale == 1 ? factor * my dx : log10 (fdomain) / (numberOfFrequencies - 1);
- for (integer i = 2; i <= numberOfFrequencies; i ++) {
- double f = my xmin + (freqscale == 1 ? (i - 1) * df : pow (10.0, (i - 1) * df));
- double x, y, index = (f - my x1) / my dx + 1;
- if (index > my nx) {
- break;
- }
- if (method == 1) {
- x = NUM_interpolate_sinc (my z [1], my nx, index, interpolationDepth);
- y = NUM_interpolate_sinc (my z [2], my nx, index, interpolationDepth);
- } else {
- x = undefined; // ppgb: better than data from random memory
- y = undefined;
- }
- thy z [1] [i] = x; thy z [2] [i] = y;
- }
- return thee;
- } catch (MelderError) {
- Melder_throw (me, U": not compressed.");
- }
- }
- #if 0
- static void Spectrum_fitTiltLine (Spectrum me, double fmin, double fmax, bool logf, double bandwidth, double *a, double *intercept, int method) {
- (void) me; (void) fmin; (void) fmax; (void) logf; (void) bandwidth; (void) a; (void) intercept; (void) method;
- try {
- } catch (MelderError) {
- Melder_throw (U"Tilt line not fitted.");
- }
- }
- #endif
- /* End of file Spectrum_extensions.cpp */
|