Debian Codegraph @

plugins/LadspaEffect/swh/util/pitchscale.c

raw
1 /****************************************************************************
2 *
3 * NAME: smsPitchScale.cp
4 * VERSION: 1.01
5 * HOME URL: http://www.dspdimension.com
6 * KNOWN BUGS: none
7 *
8 * SYNOPSIS: Routine for doing pitch scaling while maintaining
9 * duration using the Short Time Fourier Transform.
10 *
11 * DESCRIPTION: The routine takes a pitchScale factor value which is between 0.5
12 * (one octave down) and 2. (one octave up). A value of exactly 1 does not change
13 * the pitch. numSampsToProcess tells the routine how many samples in indata[0...
14 * numSampsToProcess-1] should be pitch scaled and moved to outdata[0 ...
15 * numSampsToProcess-1]. The two buffers can be identical (ie. it can process the
16 * data in-place). fftFrameLength defines the FFT frame size used for the
17 * processing. Typical values are 1024, 2048 and 4096. It may be any value <=
18 * MAX_FFT_FRAME_LENGTH but it MUST be a power of 2. osamp is the STFT
19 * oversampling factor which also determines the overlap between adjacent STFT
20 * frames. It should at least be 4 for moderate scaling ratios. A value of 32 is
21 * recommended for best quality. sampleRate takes the sample rate for the signal
22 * in unit Hz, ie. 44100 for 44.1 kHz audio. The data passed to the routine in
23 * indata[] should be in the range [-1.0, 1.0), which is also the output range
24 * for the data.
25 *
26 * COPYRIGHT 1999 Stephan M. Sprenger <[email protected]>
27 *
28 * The Wide Open License (WOL)
29 *
30 * Permission to use, copy, modify, distribute and sell this software and its
31 * documentation for any purpose is hereby granted without fee, provided that
32 * the above copyright notice and this license appear in all source copies.
33 * THIS SOFTWARE IS PROVIDED "AS IS" WITHOUT EXPRESS OR IMPLIED WARRANTY OF
34 * ANY KIND. See http://www.dspguru.com/wol.htm for more information.
35 *
36 *****************************************************************************/
37
38 #include <string.h>
39 #include "../config.h"
40 #include <math.h>
41
42 #include "pitchscale.h"
43
44 static float ps_in[MAX_FRAME_LENGTH*2], ps_out[MAX_FRAME_LENGTH*2];
45 static fft_plan aplan = NULL, splan = NULL;
46
47 void pitch_scale(sbuffers *buffers, const double pitchScale, const long
48 fftFrameLength, const long osamp, const long numSampsToProcess,
49 const double sampleRate, const float *indata, float *outdata,
50 const int adding, const float gain) {
51 /*
52 Routine smsPitchScale(). See top of file for explanation Purpose: doing
53 pitch scaling while maintaining duration using the Short Time Fourier
54 Transform. Author: (c)1999 Stephan M. Sprenger <[email protected]>
55 */
56 double magn, phase, tmp;
57 double freqPerBin, expct;
58 long i,k, qpd, index, inFifoLatency, stepSize,
59 fftFrameSize2;
60 double phaseArr[MAX_FRAME_LENGTH];
61 float ri[16];
62
63 float *gInFIFO = buffers->gInFIFO;
64 float *gOutFIFO = buffers->gOutFIFO;
65 float *gLastPhase = buffers->gLastPhase;
66 float *gSumPhase = buffers->gSumPhase;
67 float *gOutputAccum = buffers->gOutputAccum;
68 float *gAnaFreq = buffers->gAnaFreq;
69 float *gAnaMagn = buffers->gAnaMagn;
70 float *gSynFreq = buffers->gSynFreq;
71 float *gSynMagn = buffers->gSynMagn;
72 float *gWindow = buffers->gWindow;
73 long gRover = buffers->gRover;
74
75 if (aplan == NULL) {
76 int i;
77
78 for (i=0; i<fftFrameLength; i++) {
79 ps_in[i + fftFrameLength] = 0.0f;
80 }
81 #ifdef FFTW3
82 aplan = fftwf_plan_r2r_1d(fftFrameLength, ps_in, ps_out, FFTW_R2HC, FFTW_MEASURE);
83 splan = fftwf_plan_r2r_1d(fftFrameLength, ps_in, ps_out, FFTW_HC2R, FFTW_MEASURE);
84 #else
85 aplan = rfftw_create_plan(fftFrameLength, FFTW_REAL_TO_COMPLEX, FFTW_ESTIMATE);
86 splan = rfftw_create_plan(fftFrameLength, FFTW_COMPLEX_TO_REAL, FFTW_ESTIMATE);
87 #endif
88 }
89
90 /* set up some handy variables */
91 fftFrameSize2 = fftFrameLength/2;
92 stepSize = fftFrameLength/osamp;
93 freqPerBin = sampleRate*2.0/(double)fftFrameLength;
94 expct = 2.0*M_PI*(double)stepSize/(double)fftFrameLength;
95 inFifoLatency = fftFrameLength-stepSize;
96 if (gRover == false) gRover = inFifoLatency;
97
98 /* main processing loop */
99 for (i = 0; i < numSampsToProcess; i++){
100
101 /* As long as we have not yet collected enough data just read in */
102 gInFIFO[gRover] = indata[i];
103 if (adding) {
104 outdata[i] += (gOutFIFO[gRover-inFifoLatency] * gain);
105 } else {
106 outdata[i] = gOutFIFO[gRover-inFifoLatency];
107 }
108 gRover++;
109
110 /* As long as we have not yet collected enough data just read in */
111 /* now we have enough data for processing */
112 if (gRover >= fftFrameLength) {
113 gRover = inFifoLatency;
114
115 /* do windowing and store */
116 for (k = 0; k < fftFrameLength; k++) {
117 ps_in[k] = gInFIFO[k] * gWindow[k];
118 }
119
120 /* As long as we have not yet collected enough data just read in */
121
122 /* ***************** ANALYSIS ******************* */
123 /* do transform */
124 #ifdef FFTW3
125 fftwf_execute(aplan);
126 #else
127 rfftw_one(aplan, ps_in, ps_out);
128 #endif
129
130 /* this is the analysis step */
131
132 /* Hard math first, we can 3dnow this */
133 for (k = 1; k <= fftFrameSize2; k+=8) {
134 float *mb = &gAnaMagn[k];
135
136 ri[0] = ps_out[k];
137 ri[2] = ps_out[k+1];
138 ri[4] = ps_out[k+2];
139 ri[6] = ps_out[k+3];
140 ri[8] = ps_out[k+4];
141 ri[10] = ps_out[k+5];
142 ri[12] = ps_out[k+6];
143 ri[14] = ps_out[k+7];
144
145 ri[1] = ps_out[fftFrameLength - k];
146 ri[3] = ps_out[fftFrameLength - (k + 1)];
147 ri[5] = ps_out[fftFrameLength - (k + 2)];
148 ri[7] = ps_out[fftFrameLength - (k + 3)];
149 ri[9] = ps_out[fftFrameLength - (k + 4)];
150 ri[11] = ps_out[fftFrameLength - (k + 5)];
151 ri[13] = ps_out[fftFrameLength - (k + 6)];
152 ri[15] = ps_out[fftFrameLength - (k + 7)];
153
154 /* compute magnitude and phase. */
155 #ifdef ACCEL_3DNOW
156 #warning Using processor specific 3DNow! accelerations
157 __asm__ __volatile__ (
158 " \n\
159 femms \n\
160 movq (%%eax), %%mm0 \n\
161 movq 8(%%eax), %%mm1 \n\
162 movq 16(%%eax), %%mm2 \n\
163 movq 24(%%eax), %%mm3 \n\
164 movq 32(%%eax), %%mm4 \n\
165 movq 40(%%eax), %%mm5 \n\
166 movq 48(%%eax), %%mm6 \n\
167 movq 56(%%eax), %%mm7 \n\
168 # do the squares and add \n\
169 pfmul %%mm0, %%mm0 \n\
170 pfacc %%mm0, %%mm0 \n\
171 pfmul %%mm1, %%mm1 \n\
172 pfacc %%mm1, %%mm1 \n\
173 pfmul %%mm2, %%mm2 \n\
174 pfacc %%mm2, %%mm2 \n\
175 pfmul %%mm3, %%mm3 \n\
176 pfacc %%mm3, %%mm3 \n\
177 pfmul %%mm4, %%mm4 \n\
178 pfacc %%mm4, %%mm4 \n\
179 pfmul %%mm5, %%mm5 \n\
180 pfacc %%mm5, %%mm5 \n\
181 pfmul %%mm6, %%mm6 \n\
182 pfacc %%mm6, %%mm6 \n\
183 pfmul %%mm7, %%mm7 \n\
184 pfacc %%mm7, %%mm7 \n\
185 # Recip square roots. \n\
186 pfrsqrt %%mm0, %%mm0 \n\
187 pfrsqrt %%mm1, %%mm1 \n\
188 pfrsqrt %%mm2, %%mm2 \n\
189 pfrsqrt %%mm3, %%mm3 \n\
190 pfrsqrt %%mm4, %%mm4 \n\
191 pfrsqrt %%mm5, %%mm5 \n\
192 pfrsqrt %%mm6, %%mm6 \n\
193 pfrsqrt %%mm7, %%mm7 \n\
194 pfrcp %%mm0, %%mm0 \n\
195 pfrcp %%mm1, %%mm1 \n\
196 pfrcp %%mm2, %%mm2 \n\
197 pfrcp %%mm3, %%mm3 \n\
198 pfrcp %%mm4, %%mm4 \n\
199 pfrcp %%mm5, %%mm5 \n\
200 pfrcp %%mm6, %%mm6 \n\
201 pfrcp %%mm7, %%mm7 \n\
202 # ship em out \n\
203 movd %%mm0, (%%edx) \n\
204 movd %%mm1, 4(%%edx) \n\
205 movd %%mm2, 8(%%edx) \n\
206 movd %%mm3, 12(%%edx) \n\
207 movd %%mm4, 16(%%edx) \n\
208 movd %%mm5, 20(%%edx) \n\
209 movd %%mm6, 24(%%edx) \n\
210 movd %%mm7, 28(%%edx) \n\
211 femms \n\
212 "
213 :
214 : "a" (ri), "d" (mb)
215 : "memory");
216
217
218 #else
219 mb[0] = sqrt(ri[0]*ri[0]+ ri[1]*ri[1]);
220 mb[1] = sqrt(ri[2]*ri[2] + ri[3]*ri[3]);
221 mb[2] = sqrt(ri[4]*ri[4] + ri[5]*ri[5]);
222 mb[3] = sqrt(ri[6]*ri[6] + ri[7]*ri[7]);
223 #endif
224
225 phaseArr[k] = atan2(ri[1], ri[0]);
226 phaseArr[k+1] = atan2(ri[3], ri[2]);
227 phaseArr[k+2] = atan2(ri[5], ri[4]);
228 phaseArr[k+3] = atan2(ri[7], ri[6]);
229 phaseArr[k+4] = atan2(ri[9], ri[8]);
230 phaseArr[k+5] = atan2(ri[11], ri[10]);
231 phaseArr[k+6] = atan2(ri[13], ri[12]);
232 phaseArr[k+7] = atan2(ri[15], ri[14]);
233 }
234
235 for (k = 1; k <= fftFrameSize2; k++) {
236
237 /* compute phase difference */
238 tmp = phaseArr[k] - gLastPhase[k];
239 gLastPhase[k] = phaseArr[k];
240
241 /* subtract expected phase difference */
242 tmp -= (double)k*expct;
243
244 /* map delta phase into +/- Pi interval */
245 qpd = tmp/M_PI;
246 if (qpd >= 0) qpd += qpd&1;
247 else qpd -= qpd&1;
248 tmp -= M_PI*(double)qpd;
249
250 /* get deviation from bin frequency from the +/- Pi interval */
251 tmp = osamp*tmp/(2.0f*M_PI);
252
253 /* compute the k-th partials' true frequency */
254 tmp = (double)k*freqPerBin + tmp*freqPerBin;
255
256 /* store magnitude and true frequency in analysis arrays */
257 gAnaFreq[k] = tmp;
258
259 }
260
261 /* ***************** PROCESSING ******************* */
262 /* this does the actual pitch scaling */
263 memset(gSynMagn, 0, fftFrameLength*sizeof(float));
264 memset(gSynFreq, 0, fftFrameLength*sizeof(float));
265 for (k = 0; k <= fftFrameSize2; k++) {
266 index = k/pitchScale;
267 if (index <= fftFrameSize2) {
268 /* new bin overrides existing if magnitude is higher */
269 if (gAnaMagn[index] > gSynMagn[k]) {
270 gSynMagn[k] = gAnaMagn[index];
271 gSynFreq[k] = gAnaFreq[index] * pitchScale;
272 }
273
274 /* fill empty bins with nearest neighbour */
275
276 if ((gSynFreq[k] == 0.) && (k > 0)) {
277 gSynFreq[k] = gSynFreq[k-1];
278 gSynMagn[k] = gSynMagn[k-1];
279 }
280 }
281 }
282
283
284 /* ***************** SYNTHESIS ******************* */
285 /* this is the synthesis step */
286 for (k = 1; k <= fftFrameSize2; k++) {
287
288 /* get magnitude and true frequency from synthesis arrays */
289 magn = gSynMagn[k];
290 tmp = gSynFreq[k];
291
292 /* subtract bin mid frequency */
293 tmp -= (double)k*freqPerBin;
294
295 /* get bin deviation from freq deviation */
296 tmp /= freqPerBin;
297
298 /* take osamp into account */
299 tmp = 2.*M_PI*tmp/osamp;
300
301 /* add the overlap phase advance back in */
302 tmp += (double)k*expct;
303
304 /* accumulate delta phase to get bin phase */
305 gSumPhase[k] += tmp;
306 phase = gSumPhase[k];
307
308 ps_in[k] = magn*cosf(phase);
309 ps_in[fftFrameLength - k] = magn*sinf(phase);
310 }
311
312 /* do inverse transform */
313 #ifdef FFTW3
314 fftwf_execute(splan);
315 #else
316 rfftw_one(splan, ps_in, ps_out);
317 #endif
318
319 /* do windowing and add to output accumulator */
320 for(k=0; k < fftFrameLength; k++) {
321 gOutputAccum[k] += 2.0f*gWindow[k]*ps_out[k]/(fftFrameSize2*osamp);
322 }
323 for (k = 0; k < stepSize; k++) gOutFIFO[k] = gOutputAccum[k];
324
325 /* shift accumulator */
326 memmove(gOutputAccum, gOutputAccum+stepSize, fftFrameLength*sizeof(float));
327
328 /* move input FIFO */
329 for (k = 0; k < inFifoLatency; k++) gInFIFO[k] = gInFIFO[k+stepSize];
330 }
331 }
332
333 buffers->gRover = gRover;
334 }