diff --git a/kraken/KRDSP_slow.cpp b/kraken/KRDSP_slow.cpp index 906952a..55056ad 100644 --- a/kraken/KRDSP_slow.cpp +++ b/kraken/KRDSP_slow.cpp @@ -54,8 +54,9 @@ void FFTWorkspace::create(size_t length) cos_table = new float[size]; sin_table = new float[size]; for (int i = 0; i < size / 2; i++) { - cos_table[i] = cos(2 * M_PI * i / length); - sin_table[i] = sin(2 * M_PI * i / length); + float a = 2 * M_PI * i / length; + cos_table[i] = cos(a); + sin_table[i] = sin(a); } } @@ -73,27 +74,88 @@ void FFTWorkspace::destroy() void FFTForward(const FFTWorkspace &workspace, SplitComplex *src, size_t count) { -#error TODO - Implement + // Radix-2 Decimation in Time FFT Algorithm + // http://en.dsplib.org/content/fft_dec_in_time.html + + // Only power-of-two sizes supported + assert((count & (count - 1)) == 0); + + int levels = 0; + while (1 << levels <= count) { + levels++; + } + + for (size_t i = 0; i < count; i++) { + size_t j = 0; + for (int k = 0; k < levels; k++) { + j <<= 1; + j |= ((i >> k) & 1); + } + if (j > i) { + float temp = src->realp[i]; + src->realp[i] = src->realp[j]; + src->realp[j] = temp; + temp = src->imagp[i]; + src->imagp[i] = src->imagp[j]; + src->imagp[j] = temp; + } + } + + for (size_t size = 2; size <= count; size *= 2) { + size_t halfsize = size / 2; + size_t step = count / size; + for (size_t i = 0; i < count; i += size) { + for (size_t j = i, k = 0; j < i + halfsize; j++, k += step) { + float temp_real = src->realp[j + halfsize] * workspace.cos_table[k]; + temp_real += src->imagp[j + halfsize] * workspace.sin_table[k]; + float temp_imag = -src->realp[j + halfsize] * workspace.sin_table[k]; + temp_imag += src->imagp[j + halfsize] * workspace.cos_table[k]; + src->realp[j + halfsize] = src->realp[j] - temp_real; + src->imagp[j + halfsize] = src->imagp[j] - temp_imag; + src->realp[j] += temp_real; + src->imagp[j] += temp_imag; + } + } + } } void FFTInverse(const FFTWorkspace &workspace, SplitComplex *src, size_t count) { -#error TODO - Implement + SplitComplex swapped; + swapped.imagp = src->realp; + swapped.realp = src->imagp; + FFTForward(workspace, &swapped, count); } void Int16ToFloat(const short *src, size_t srcStride, float *dest, size_t destStride, size_t count) { -#error TODO - Implement + const short *r = src; + float *w = dest; + while (w < dest + destStride * count) { + *w = (float)*r; + r += srcStride; + w += destStride; + } } void Scale(float *buffer, float scale, size_t count) { -#error TODO - Implement + float *w = buffer; + while (w < buffer + count) { + *w *= scale; + w++; + } } void ScaleCopy(const float *src, float scale, float *dest, size_t count) { -#error TODO - Implement + const float *r = src; + float *w = dest; + while (w < dest + count) { + *w = *r * scale; + w++; + r++; + } } void ScaleCopy(const SplitComplex *src, float scale, SplitComplex *dest, size_t count) @@ -104,23 +166,40 @@ void ScaleCopy(const SplitComplex *src, float scale, SplitComplex *dest, size_t void ScaleRamp(float *buffer, float scaleStart, float scaleStep, size_t count) { -#error TODO - Implement + float *w = buffer; + float s = scaleStart; + while (w < buffer + count) { + *w *= s; + w++; + s += scaleStep; + } } void Accumulate(float *buffer, size_t bufferStride, const float *buffer2, size_t buffer2Stride, size_t count) { -#error TODO - Implement + float *w = buffer; + const float *r = buffer2; + while (w < buffer + bufferStride * count) { + *w *= *r; + w += bufferStride; + r += buffer2Stride; + } } void Accumulate(SplitComplex *buffer, const SplitComplex *buffer2, size_t count) { -#error TODO - Implement + for (size_t i = 0; i < count; i++) { + buffer->imagp[i] += buffer2->imagp[i]; + buffer->realp[i] += buffer2->realp[i]; + } } - void Multiply(const SplitComplex *a, const SplitComplex *b, SplitComplex *c, size_t count) { -#error TODO - Implement + for (size_t i = 0; i < count; i++) { + c->realp[i] = a->realp[i] * b->realp[i] - a->imagp[i] * b->imagp[i]; + c->imagp[i] = a->realp[i] * b->imagp[i] + a->imagp[i] * b->realp[i]; + } } } // namespace KRDSP