1 /* Spectrum_extensions.cpp 2 * 3 * Copyright (C) 1993-2021 David Weenink 4 * 5 * This code is free software; you can redistribute it and/or modify 6 * it under the terms of the GNU General Public License as published by 7 * the Free Software Foundation; either version 2 of the License, or (at 8 * your option) any later version. 9 * 10 * This code is distributed in the hope that it will be useful, but 11 * WITHOUT ANY WARRANTY; without even the implied warranty of 12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 * General Public License for more details. 14 * 15 * You should have received a copy of the GNU General Public License 16 * along with this work. If not, see <http://www.gnu.org/licenses/>. 17 */ 18 19 /* 20 djmw 20010718 21 djmw 20020813 GPL header 22 djmw 20030929 Added a warning in Spectrum_drawPhases. 23 djmw 20031023 New: Spectra_multiply, Spectrum_conjugate 24 djmw 20040506 Changed warning message in Spectrum_drawPhases. 25 djmw 20041124 Changed call to Sound_to_Spectrum. 26 djmw 20061218 Introduction of Melder_information<12...9> 27 djmw 20071022 phase_unwrap initialize phase = 0. 28 djmw 20080122 float -> double 29 djmw 20080202 Warning in Spectrum_drawPhases to wchar 30 djmw 20080411 Removed define NUM2pi 31 */ 32 33 #include "Ltas.h" 34 #include "Spectrum_extensions.h" 35 #include "Sound_and_Spectrum.h" 36 #include "Sound_and_Spectrum_dft.h" 37 #include "NUM2.h" 38 39 #define SIGN(x,s) ((s) < 0 ? -fabs (x) : fabs(x)) 40 41 #define THLCON 0.5 42 #define THLINC 1.5 43 #define EXP2 12 44 45 #define PPVPHA(x,y,test) ((test) ? atan2 (-(y),-(x)) : atan2 ((y),(x))) 46 #define PHADVT(xr,xi,yr,yi,xa) ((xa) > 0 ? ((xr)*(yr)+(xi)*(yi))/ (xa) : 0) 47 48 struct tribolet_struct { 49 double thlinc, thlcon; 50 double ddf, dvtmn2; 51 double *x; 52 integer nx, l, count; 53 bool reverse_sign; 54 }; 55 56 /* 57 Perform modified Goertzel algorithm to calculate, at frequency 'freq_rad', 58 the real and imaginary part of the spectrum and the d/df of the 59 spectrum of x. 60 Reference: Bonzanigo (1978), IEEE Trans. ASSP, Vol. 26. 61 */ 62 static void getSpectralValues (struct tribolet_struct *tbs, double freq_rad, double *xr, double *xi, double *nxr, double *nxi) { 63 const double cosf = cos (freq_rad), sinf = sin (freq_rad); 64 double a = 2 * cosf; 65 double b, u1 = 0, u2 = u1, w1 = u1, w2 = u1; 66 const double *x = tbs -> x; 67 const integer nx = tbs -> nx; 68 69 for (integer j = 1; j <= nx; j ++) { 70 double xj = x [j]; 71 double u0 = xj + a * u1 - u2; 72 double w0 = (j - 1) * xj + a * w1 - w2; 73 u2 = u1; 74 u1 = u0; 75 w2 = w1; 76 w1 = w0; 77 } 78 79 // Bonzanigo's phase correction 80 81 a = freq_rad * (nx - 1); 82 u1 = cos (a); 83 u2 = - sin (a); 84 85 a = u1 - u2 * cosf; 86 b = u2 * sinf; 87 *xr = u1 * a - u2 * b; 88 *xi = u2 * a + u1 * b; 89 90 a = w1 - w2 * cosf; 91 b = w2 * sinf; 92 *nxr = u1 * a - u2 * b; 93 *nxi = u2 * a + u1 * b; 94 tbs -> count ++; 95 } 96 97 // Find the closest unwrapped phase estimate from the two admissible phase values (a1 & a2). 98 99 static int phase_check (double pv, double *inout_phase, double thlcon) { 100 const double a0 = (*inout_phase - pv) / NUM2pi; 101 const integer k = Melder_ifloor (a0); // ppgb: instead of truncation toward zero 102 const double a1 = pv + k * NUM2pi; 103 const double a2 = a1 + SIGN (NUM2pi, a0); 104 const double a3 = fabs (a1 - *inout_phase); 105 const double a4 = fabs (a2 - *inout_phase); 106 107 if (a3 > thlcon && a4 > thlcon) 108 return 0; 109 *inout_phase = a3 > a4 ? a2 : a1; 110 return 1; 111 } 112 113 /* 114 Phase unwrapping based on Tribolet's adaptive integration method. 115 the unwrapped phase estimate is returned. 116 */ 117 static double phase_unwrap (struct tribolet_struct *tbs, double pfreq, double ppv, double pdvt, double *pphase, double *ppdvt) { 118 double sdvt [25], sppv [25]; 119 double freq, phase = 0.0; 120 double xr, xi, xmsq, nxr, nxi; 121 integer k, sindex [25], pindex = 1, sp = 1; 122 123 sppv [sp] = ppv; 124 sdvt [sp] = pdvt; 125 sindex [sp] = tbs -> l + 1; 126 127 goto p40; 128 p20: 129 /* 130 When the routine runs out of stack space, there probably is 131 a zero very near the unit circle that results in a jump of 132 pi in the phase. 133 */ 134 if ((sindex [sp] - pindex) <= 1) 135 return phase; 136 /* 137 p30: 138 Get the intermediate frequency value and compute its phase 139 derivative and principal value. 140 */ 141 k = (sindex [sp] + pindex) / 2; 142 freq = pfreq + (k - 1) * tbs -> ddf; 143 getSpectralValues (tbs, freq, & xr, & xi, & nxr, & nxi); 144 sindex [ ++sp] = k; 145 sppv [sp] = PPVPHA (xr, xi, tbs -> reverse_sign); 146 xmsq = xr * xr + xi * xi; 147 sdvt [sp] = PHADVT (xr, xi, nxr, nxi, xmsq); 148 149 p40: 150 /* 151 Evaluate the phase increment. 152 If the phase increment, reduced by the expected linear phase 153 increment, is greater than the specified threshold, adapt step size. 154 */ 155 double delta = 0.5 * tbs -> ddf * (sindex [sp] - pindex); 156 double phase_inc = delta * (*ppdvt + sdvt [sp]); 157 158 if (fabs (phase_inc - delta * tbs -> dvtmn2) > tbs -> thlinc) 159 goto p20; 160 161 phase = *pphase + phase_inc; 162 163 if (! phase_check (sppv [sp], &phase, tbs -> thlcon)) 164 goto p20; 165 if (fabs (phase - *pphase) > NUMpi) 166 goto p20; 167 if (sp == 1) 168 return phase; 169 /* 170 p10: Update previous estimate. 171 */ 172 pindex = sindex [sp]; 173 *pphase = phase; 174 *ppdvt = sdvt [sp--]; 175 176 goto p40; 177 } 178 179 autoMatrix Spectrum_unwrap (Spectrum me) { 180 try { 181 struct tribolet_struct tbs; 182 int remove_linear_part = 1; 183 184 const integer nfft = 2 * Melder_clippedLeft (2_integer, Melder_iroundUpToPowerOfTwo (my nx - 1)); // TODO: explain edge case 185 Melder_require (nfft / 2 == my nx - 1, 186 U"Dimension of Spectrum should be a power of 2 - 1."); 187 188 autoSound x = Spectrum_to_Sound (me); 189 autoSound nx = Data_copy (x.get()); 190 191 for (integer i = 1; i <= x -> nx; i ++) 192 nx -> z [1] [i] *= (i - 1); 193 194 autoSpectrum snx = Sound_to_Spectrum (nx.get(), true); 195 autoMatrix thee = Matrix_create (my xmin, my xmax, my nx, my dx, my x1, 1, 2, 2, 1, 1); 196 197 tbs.thlinc = THLINC; 198 tbs.thlcon = THLCON; 199 tbs.x = & x -> z [1] [0]; 200 tbs.nx = x -> nx; 201 tbs.l = Melder_ifloor (pow (2, EXP2) + 0.1); 202 tbs.ddf = NUM2pi / ( (tbs.l) * nfft); 203 tbs.reverse_sign = my z [1] [1] < 0; 204 tbs.count = 0; 205 /* 206 Reuse snx : put phase derivative (d/df) in imaginary part. 207 */ 208 tbs.dvtmn2 = 0.0; 209 for (integer i = 1; i <= my nx; i ++) { 210 const double xr = my z [1] [i], xi = my z [2] [i]; 211 const double nxr = snx -> z [1] [i], nxi = snx -> z [2] [i]; 212 const double xmsq = xr * xr + xi * xi; 213 const double pdvt = PHADVT (xr, xi, nxr, nxi, xmsq); 214 thy z [1] [i] = xmsq; 215 snx -> z [2] [i] = pdvt; 216 tbs.dvtmn2 += pdvt; 217 } 218 219 tbs.dvtmn2 = (2.0 * tbs.dvtmn2 - snx -> z [2] [1] - snx -> z [2] [my nx]) / (my nx - 1); 220 221 autoMelderProgress progress (U"Phase unwrapping"); 222 223 double pphase = 0.0, phase = 0.0; 224 double ppdvt = snx -> z [2] [1]; 225 thy z [2] [1] = PPVPHA (my z [1] [1], my z [2] [1], tbs.reverse_sign); 226 for (integer i = 2; i <= my nx; i ++) { 227 const double pfreq = NUM2pi * (i - 1) / nfft; 228 const double pdvt = snx -> z [2] [i]; 229 const double ppv = PPVPHA (my z [1] [i], my z [2] [i], tbs.reverse_sign); 230 phase = phase_unwrap (&tbs, pfreq, ppv, pdvt, &pphase, &ppdvt); 231 ppdvt = pdvt; 232 thy z [2] [i] = pphase = phase; 233 Melder_progress ( (double) i / my nx, i, 234 U" unwrapped phases from ", my nx, U"."); 235 } 236 237 const integer iphase = Melder_ifloor (phase / NUMpi + 0.1); 238 239 if (remove_linear_part) { 240 phase /= my nx - 1; 241 for (integer i = 2; i <= my nx; i ++) 242 thy z [2] [i] -= phase * (i - 1); 243 } 244 Melder_information (U"Number of spectral values: ", tbs.count); 245 Melder_information (U" iphase = ", iphase); 246 return thee; 247 } catch (MelderError) { 248 Melder_throw (me, U": not unwrapped."); 249 } 250 } 251 252 void Spectrum_drawPhases (Spectrum me, Graphics g, double fmin, double fmax, double phase_min, double phase_max, int unwrap, bool /* garnish */) { 253 autoMatrix thee; 254 bool reverse_sign = my z [1] [1] < 0; 255 256 if (unwrap) 257 thee = Spectrum_unwrap (me); 258 else { 259 thee = Matrix_create (my xmin, my xmax, my nx, my dx, my x1, 1.0, 2.0, 2, 1.0, 1.0); 260 for (integer i = 1; i <= my nx; i ++) { 261 thy z [2] [i] = PPVPHA (my z [1] [i], my z [2] [i], reverse_sign); 262 } 263 } 264 Matrix_drawRows (thee.get(), g, fmin, fmax, 1.9, 2.1, phase_min, phase_max); 265 } 266 267 autoSpectrum Spectra_multiply (Spectrum me, Spectrum thee) { 268 try { 269 Melder_require (my nx == thy nx && my x1 == thy x1 && my xmax == thy xmax && my dx == thy dx, 270 U"Dimensions of both spectra should be the same."); 271 272 autoSpectrum him = Data_copy (me); 273 274 for (integer i = 1; i <= his nx; i ++) { 275 his z [1] [i] = my z [1] [i] * thy z [1] [i] - my z [2] [i] * thy z [2] [i]; 276 his z [2] [i] = my z [1] [i] * thy z [2] [i] + my z [2] [i] * thy z [1] [i]; 277 } 278 return him; 279 } catch (MelderError) { 280 Melder_throw (me, U": not multiplied."); 281 } 282 } 283 284 void Spectrum_shiftPhaseBy90Degrees (Spectrum me) { 285 // shifting +pi/2 by a multiplication with i 286 for (integer i = 2; i <= my nx - 1; i ++) { 287 std::swap (my z [1] [i], my z [2] [i]); 288 my z [1] [i] = - my z [1] [i]; 289 } 290 } 291 292 void Spectrum_unshiftPhaseBy90Degrees (Spectrum me) { 293 // shifting -pi/2 by a multiplication with -i 294 for (integer i = 2; i <= my nx - 1; i ++) { 295 my z [1] [i] = - my z [1] [i]; 296 std::swap (my z [1] [i], my z [2] [i]); 297 } 298 } 299 300 void Spectrum_conjugate (Spectrum me) { 301 for (integer i = 1; i <= my nx; i ++) 302 my z [2] [i] = - my z [2] [i]; 303 } 304 305 autoSpectrum Spectrum_resample (Spectrum me, integer numberOfFrequencies) { 306 try { 307 const double newSamplingFrequency = (1 / my dx) * numberOfFrequencies / my nx; 308 // resample real and imaginary part ! 309 autoSound thee = Sound_resample ((Sound) me, newSamplingFrequency, 50); 310 autoSpectrum him = Spectrum_create (my xmax, numberOfFrequencies); 311 his z.all() <<= thy z.all(); 312 return him; 313 } catch (MelderError) { 314 Melder_throw (me, U": not resampled."); 315 } 316 } 317 /* 318 autoSpectrum Spectrum_resample2 (Spectrum me, integer numberOfFrequencies) { 319 try { 320 autoSound sound = Spectrum_to_Sound (me); 321 const double newSamplingFrequency = (1.0 / my dx) * numberOfFrequencies / my nx; 322 const double resampleFactor = (my nx - 1.0) / numberOfFrequencies; 323 autoSound resampled = Sound_resample (sound.get(), resampleFactor / sound -> dx, 50); 324 autoSpectrum extendedSpectrum = Sound_to_Spectrum_dft (resampled.get(), 50); 325 autoSpectrum him = Spectrum_create (my xmax, numberOfFrequencies); 326 his z.all() <<= extendedSpectrum -> z.all(); 327 return him; 328 } catch (MelderError) { 329 Melder_throw (me, U": not resampled."); 330 } 331 } 332 */ 333 #if 0 334 static autoSpectrum Spectrum_shiftFrequencies2 (Spectrum me, double shiftBy, bool changeMaximumFrequency) { 335 try { 336 double xmax = my xmax; 337 integer numberOfFrequencies = my nx, interpolationDepth = 50; 338 if (changeMaximumFrequency) { 339 xmax += shiftBy; 340 numberOfFrequencies += (xmax - my xmax) / my dx; 341 } 342 autoSpectrum thee = Spectrum_create (xmax, numberOfFrequencies); 343 // shiftBy >= 0 344 for (integer i = 1; i <= thy nx; i ++) { 345 const double thyf = thy x1 + (i - 1) * thy dx; 346 const double myf = thyf - shiftBy; 347 if (myf >= my xmin && myf <= my xmax) { 348 const double index = Sampled_xToIndex (me, myf); 349 thy z [1] [i] = NUM_interpolate_sinc (my z.row (1), index, interpolationDepth); 350 thy z [2] [i] = NUM_interpolate_sinc (my z.row (2), index, interpolationDepth); 351 } 352 } 353 return thee; 354 } catch (MelderError) { 355 Melder_throw (me, U": not shifted."); 356 } 357 } 358 #endif 359 360 autoSpectrum Spectrum_shiftFrequencies (Spectrum me, double shiftBy, double newMaximumFrequency, integer interpolationDepth) { 361 try { 362 double xmax = my xmax; 363 integer numberOfFrequencies = my nx; 364 if (newMaximumFrequency != 0.0) { 365 numberOfFrequencies = Melder_ifloor (newMaximumFrequency / my dx) + 1; 366 xmax = newMaximumFrequency; 367 } 368 autoSpectrum thee = Spectrum_create (xmax, numberOfFrequencies); 369 for (integer i = 1; i <= thy nx; i ++) { 370 const double thyf = thy x1 + (i - 1) * thy dx; 371 const double myf = thyf - shiftBy; 372 if (myf >= my xmin && myf <= my xmax) { 373 const double index = Sampled_xToIndex (me, myf); 374 thy z [1] [i] = NUM_interpolate_sinc (my z.row (1), index, interpolationDepth); 375 thy z [2] [i] = NUM_interpolate_sinc (my z.row (2), index, interpolationDepth); 376 } 377 } 378 /* 379 Make imaginary part of first and last sample zero 380 so Spectrum_to_Sound uses FFT if numberOfSamples was power of 2! 381 */ 382 thy z [1] [1] = hypot (thy z [1] [1], thy z [2] [1]); 383 thy z [2] [1] = 0.0; 384 thy z [1] [thy nx] = hypot (thy z [1] [thy nx], thy z [2] [thy nx]); 385 thy z [2] [thy nx] = 0.0; 386 return thee; 387 } catch (MelderError) { 388 Melder_throw (me, U": not shifted."); 389 } 390 } 391 392 autoSpectrum Spectrum_compressFrequencyDomain (Spectrum me, double fmax, integer interpolationDepth, int freqscale, int method) { 393 try { 394 const double fdomain = my xmax - my xmin, factor = fdomain / fmax ; 395 //integer numberOfFrequencies = 1.0 + fmax / my dx; // keep dx the same, otherwise the "duration" changes 396 const double xmax = my xmax / factor; 397 const integer numberOfFrequencies = Melder_ifloor (my nx / factor); // keep dx the same, otherwise the "duration" changes 398 autoSpectrum thee = Spectrum_create (xmax, numberOfFrequencies); 399 thy z [1] [1] = my z [1] [1]; 400 thy z [2] [1] = my z [2] [1]; 401 const double df = freqscale == 1 ? factor * my dx : log10 (fdomain) / (numberOfFrequencies - 1); 402 for (integer i = 2; i <= numberOfFrequencies; i ++) { 403 const double f = my xmin + (freqscale == 1 ? (i - 1) * df : pow (10.0, (i - 1) * df)); 404 const double index = (f - my x1) / my dx + 1; 405 double x, y; 406 if (index > my nx) 407 break; 408 if (method == 1) { 409 x = NUM_interpolate_sinc (my z.row (1), index, interpolationDepth); 410 y = NUM_interpolate_sinc (my z.row (2), index, interpolationDepth); 411 } else { 412 x = undefined; // ppgb: better than data from random memory 413 y = undefined; 414 } 415 thy z [1] [i] = x; 416 thy z [2] [i] = y; 417 } 418 return thee; 419 } catch (MelderError) { 420 Melder_throw (me, U": not compressed."); 421 } 422 } 423 424 /* End of file Spectrum_extensions.cpp */ 425