| 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 |
} |