| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,131 @@ | ||
| #ifndef KISS_FFT_H | ||
| #define KISS_FFT_H | ||
|
|
||
| #include <math.h> | ||
| #include <stdio.h> | ||
| #include <stdlib.h> | ||
| #include <string.h> | ||
|
|
||
| #ifdef __cplusplus | ||
| extern "C" { | ||
| #endif | ||
|
|
||
| // we're using doubles here... | ||
| #define kiss_fft_scalar double | ||
|
|
||
| /* | ||
| ATTENTION! | ||
| If you would like a : | ||
| -- a utility that will handle the caching of fft objects | ||
| -- real-only (no imaginary time component ) FFT | ||
| -- a multi-dimensional FFT | ||
| -- a command-line utility to perform ffts | ||
| -- a command-line utility to perform fast-convolution filtering | ||
| Then see kfc.h kiss_fftr.h kiss_fftnd.h fftutil.c kiss_fastfir.c | ||
| in the tools/ directory. | ||
| */ | ||
|
|
||
| #ifdef USE_SIMD | ||
| #include <xmmintrin.h> | ||
| #define kiss_fft_scalar __m128 | ||
| #define KISS_FFT_MALLOC(nbytes) _mm_malloc(nbytes, 16) | ||
| #define KISS_FFT_FREE _mm_free | ||
| #else | ||
| #define KISS_FFT_MALLOC malloc | ||
| #define KISS_FFT_FREE free | ||
| #endif | ||
|
|
||
| #ifdef FIXED_POINT | ||
| #include <sys/types.h> | ||
| #if (FIXED_POINT == 32) | ||
| #define kiss_fft_scalar int32_t | ||
| #else | ||
| #define kiss_fft_scalar int16_t | ||
| #endif | ||
| #else | ||
| #ifndef kiss_fft_scalar | ||
| /* default is float */ | ||
| #define kiss_fft_scalar float | ||
| #endif | ||
| #endif | ||
|
|
||
| typedef struct { | ||
| kiss_fft_scalar r; | ||
| kiss_fft_scalar i; | ||
| } kiss_fft_cpx; | ||
|
|
||
| typedef struct kiss_fft_state *kiss_fft_cfg; | ||
|
|
||
| /* | ||
| * kiss_fft_alloc | ||
| * | ||
| * Initialize a FFT (or IFFT) algorithm's cfg/state buffer. | ||
| * | ||
| * typical usage: kiss_fft_cfg mycfg=kiss_fft_alloc(1024,0,NULL,NULL); | ||
| * | ||
| * The return value from fft_alloc is a cfg buffer used internally | ||
| * by the fft routine or NULL. | ||
| * | ||
| * If lenmem is NULL, then kiss_fft_alloc will allocate a cfg buffer using | ||
| * malloc. | ||
| * The returned value should be free()d when done to avoid memory leaks. | ||
| * | ||
| * The state can be placed in a user supplied buffer 'mem': | ||
| * If lenmem is not NULL and mem is not NULL and *lenmem is large enough, | ||
| * then the function places the cfg in mem and the size used in *lenmem | ||
| * and returns mem. | ||
| * | ||
| * If lenmem is not NULL and ( mem is NULL or *lenmem is not large enough), | ||
| * then the function returns NULL and places the minimum cfg | ||
| * buffer size in *lenmem. | ||
| * */ | ||
|
|
||
| kiss_fft_cfg kiss_fft_alloc(int nfft, int inverse_fft, void *mem, | ||
| size_t *lenmem); | ||
|
|
||
| /* | ||
| * kiss_fft(cfg,in_out_buf) | ||
| * | ||
| * Perform an FFT on a complex input buffer. | ||
| * for a forward FFT, | ||
| * fin should be f[0] , f[1] , ... ,f[nfft-1] | ||
| * fout will be F[0] , F[1] , ... ,F[nfft-1] | ||
| * Note that each element is complex and can be accessed like | ||
| f[k].r and f[k].i | ||
| * */ | ||
| void kiss_fft(kiss_fft_cfg cfg, const kiss_fft_cpx *fin, kiss_fft_cpx *fout); | ||
|
|
||
| /* | ||
| A more generic version of the above function. It reads its input from every Nth | ||
| sample. | ||
| * */ | ||
| void kiss_fft_stride(kiss_fft_cfg cfg, const kiss_fft_cpx *fin, | ||
| kiss_fft_cpx *fout, int fin_stride); | ||
|
|
||
| /* If kiss_fft_alloc allocated a buffer, it is one contiguous | ||
| buffer and can be simply free()d when no longer needed*/ | ||
| #define kiss_fft_free free | ||
|
|
||
| /* | ||
| Cleans up some memory that gets managed internally. Not necessary to call, but | ||
| it might clean up | ||
| your compiler output to call this before you exit. | ||
| */ | ||
| void kiss_fft_cleanup(void); | ||
|
|
||
| /* | ||
| * Returns the smallest integer k, such that k>=n and k has only "fast" factors | ||
| * (2,3,5) | ||
| */ | ||
| int kiss_fft_next_fast_size(int n); | ||
|
|
||
| /* for real ffts, we need an even size */ | ||
| #define kiss_fftr_next_fast_size_real(n) \ | ||
| (kiss_fft_next_fast_size(((n) + 1) >> 1) << 1) | ||
|
|
||
| #ifdef __cplusplus | ||
| } | ||
| #endif | ||
|
|
||
| #endif |
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,47 @@ | ||
| #ifndef KISS_FTR_H | ||
| #define KISS_FTR_H | ||
|
|
||
| #include "KissFFT.h" | ||
| #ifdef __cplusplus | ||
| extern "C" { | ||
| #endif | ||
|
|
||
| /* | ||
| Real optimized version can save about 45% cpu time vs. complex fft of a real | ||
| seq. | ||
| */ | ||
|
|
||
| typedef struct kiss_fftr_state *kiss_fftr_cfg; | ||
|
|
||
| kiss_fftr_cfg kiss_fftr_alloc(int nfft, int inverse_fft, void *mem, | ||
| size_t *lenmem); | ||
| /* | ||
| nfft must be even | ||
| If you don't care to allocate space, use mem = lenmem = NULL | ||
| */ | ||
|
|
||
| void kiss_fftr(kiss_fftr_cfg cfg, const kiss_fft_scalar *timedata, | ||
| kiss_fft_cpx *freqdata); | ||
| /* | ||
| input timedata has nfft scalar points | ||
| output freqdata has nfft/2+1 complex points | ||
| */ | ||
|
|
||
| void kiss_fftri(kiss_fftr_cfg cfg, const kiss_fft_cpx *freqdata, | ||
| kiss_fft_scalar *timedata); | ||
| /* | ||
| input freqdata has nfft/2+1 complex points | ||
| output timedata has nfft scalar points | ||
| */ | ||
|
|
||
| #define kiss_fftr_free free | ||
|
|
||
| #ifdef __cplusplus | ||
| } | ||
| #endif | ||
| #endif |
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,202 @@ | ||
| /* | ||
| Copyright (c) 2003-2010, Mark Borgerding | ||
| All rights reserved. | ||
| Redistribution and use in source and binary forms, with or without modification, | ||
| are permitted | ||
| provided that the following conditions are met: | ||
| * Redistributions of source code must retain the above copyright notice, | ||
| this list of conditions | ||
| and the following disclaimer. | ||
| * Redistributions in binary form must reproduce the above copyright notice, | ||
| this list of | ||
| conditions and the following disclaimer in the documentation and/or other | ||
| materials provided with | ||
| the distribution. | ||
| * Neither the author nor the names of any contributors may be used to | ||
| endorse or promote | ||
| products derived from this software without specific prior written permission. | ||
| THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND | ||
| ANY EXPRESS OR | ||
| IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF | ||
| MERCHANTABILITY AND | ||
| FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT | ||
| OWNER OR | ||
| CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, | ||
| OR CONSEQUENTIAL | ||
| DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR | ||
| SERVICES; LOSS OF USE, | ||
| DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF | ||
| LIABILITY, WHETHER | ||
| IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) | ||
| ARISING IN ANY WAY OUT OF | ||
| THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. | ||
| */ | ||
|
|
||
| /* kiss_fft.h | ||
| defines kiss_fft_scalar as either short or a float type | ||
| and defines | ||
| typedef struct { kiss_fft_scalar r; kiss_fft_scalar i; }kiss_fft_cpx; */ | ||
| #include "KissFFT.h" | ||
| #include <limits.h> | ||
|
|
||
| #define MAXFACTORS 32 | ||
| /* e.g. an fft of length 128 has 4 factors | ||
| as far as kissfft is concerned | ||
| 4*4*4*2 | ||
| */ | ||
|
|
||
| struct kiss_fft_state { | ||
| int nfft; | ||
| int inverse; | ||
| int factors[2 * MAXFACTORS]; | ||
| kiss_fft_cpx twiddles[1]; | ||
| }; | ||
|
|
||
| /* | ||
| Explanation of macros dealing with complex math: | ||
| C_MUL(m,a,b) : m = a*b | ||
| C_FIXDIV( c , div ) : if a fixed point impl., c /= div. noop otherwise | ||
| C_SUB( res, a,b) : res = a - b | ||
| C_SUBFROM( res , a) : res -= a | ||
| C_ADDTO( res , a) : res += a | ||
| * */ | ||
| #ifdef FIXED_POINT | ||
| #if (FIXED_POINT == 32) | ||
| #define FRACBITS 31 | ||
| #define SAMPPROD int64_t | ||
| #define SAMP_MAX 2147483647 | ||
| #else | ||
| #define FRACBITS 15 | ||
| #define SAMPPROD int32_t | ||
| #define SAMP_MAX 32767 | ||
| #endif | ||
|
|
||
| #define SAMP_MIN -SAMP_MAX | ||
|
|
||
| #if defined(CHECK_OVERFLOW) | ||
| #define CHECK_OVERFLOW_OP(a, op, b) \ | ||
| if ((SAMPPROD)(a)op(SAMPPROD)(b) > SAMP_MAX || \ | ||
| (SAMPPROD)(a)op(SAMPPROD)(b) < SAMP_MIN) { \ | ||
| fprintf(stderr, \ | ||
| "WARNING:overflow @ " __FILE__ "(%d): (%d " #op " %d) = %ld\n", \ | ||
| __LINE__, (a), (b), (SAMPPROD)(a)op(SAMPPROD)(b)); \ | ||
| } | ||
| #endif | ||
|
|
||
| #define smul(a, b) ((SAMPPROD)(a) * (b)) | ||
| #define sround(x) (kiss_fft_scalar)(((x) + (1 << (FRACBITS - 1))) >> FRACBITS) | ||
|
|
||
| #define S_MUL(a, b) sround(smul(a, b)) | ||
|
|
||
| #define C_MUL(m, a, b) \ | ||
| do { \ | ||
| (m).r = sround(smul((a).r, (b).r) - smul((a).i, (b).i)); \ | ||
| (m).i = sround(smul((a).r, (b).i) + smul((a).i, (b).r)); \ | ||
| } while (0) | ||
|
|
||
| #define DIVSCALAR(x, k) (x) = sround(smul(x, SAMP_MAX / k)) | ||
|
|
||
| #define C_FIXDIV(c, div) \ | ||
| do { \ | ||
| DIVSCALAR((c).r, div); \ | ||
| DIVSCALAR((c).i, div); \ | ||
| } while (0) | ||
|
|
||
| #define C_MULBYSCALAR(c, s) \ | ||
| do { \ | ||
| (c).r = sround(smul((c).r, s)); \ | ||
| (c).i = sround(smul((c).i, s)); \ | ||
| } while (0) | ||
|
|
||
| #else /* not FIXED_POINT*/ | ||
|
|
||
| #define S_MUL(a, b) ((a) * (b)) | ||
| #define C_MUL(m, a, b) \ | ||
| do { \ | ||
| (m).r = (a).r * (b).r - (a).i * (b).i; \ | ||
| (m).i = (a).r * (b).i + (a).i * (b).r; \ | ||
| } while (0) | ||
| #define C_FIXDIV(c, div) /* NOOP */ | ||
| #define C_MULBYSCALAR(c, s) \ | ||
| do { \ | ||
| (c).r *= (s); \ | ||
| (c).i *= (s); \ | ||
| } while (0) | ||
| #endif | ||
|
|
||
| #ifndef CHECK_OVERFLOW_OP | ||
| #define CHECK_OVERFLOW_OP(a, op, b) /* noop */ | ||
| #endif | ||
|
|
||
| #define C_ADD(res, a, b) \ | ||
| do { \ | ||
| CHECK_OVERFLOW_OP((a).r, +, (b).r) \ | ||
| CHECK_OVERFLOW_OP((a).i, +, (b).i) \ | ||
| (res).r = (a).r + (b).r; \ | ||
| (res).i = (a).i + (b).i; \ | ||
| } while (0) | ||
| #define C_SUB(res, a, b) \ | ||
| do { \ | ||
| CHECK_OVERFLOW_OP((a).r, -, (b).r) \ | ||
| CHECK_OVERFLOW_OP((a).i, -, (b).i) \ | ||
| (res).r = (a).r - (b).r; \ | ||
| (res).i = (a).i - (b).i; \ | ||
| } while (0) | ||
| #define C_ADDTO(res, a) \ | ||
| do { \ | ||
| CHECK_OVERFLOW_OP((res).r, +, (a).r) \ | ||
| CHECK_OVERFLOW_OP((res).i, +, (a).i) \ | ||
| (res).r += (a).r; \ | ||
| (res).i += (a).i; \ | ||
| } while (0) | ||
|
|
||
| #define C_SUBFROM(res, a) \ | ||
| do { \ | ||
| CHECK_OVERFLOW_OP((res).r, -, (a).r) \ | ||
| CHECK_OVERFLOW_OP((res).i, -, (a).i) \ | ||
| (res).r -= (a).r; \ | ||
| (res).i -= (a).i; \ | ||
| } while (0) | ||
|
|
||
| #ifdef FIXED_POINT | ||
| #define KISS_FFT_COS(phase) floor(.5 + SAMP_MAX * cos(phase)) | ||
| #define KISS_FFT_SIN(phase) floor(.5 + SAMP_MAX * sin(phase)) | ||
| #define HALF_OF(x) ((x) >> 1) | ||
| #elif defined(USE_SIMD) | ||
| #define KISS_FFT_COS(phase) _mm_set1_ps(cos(phase)) | ||
| #define KISS_FFT_SIN(phase) _mm_set1_ps(sin(phase)) | ||
| #define HALF_OF(x) ((x)*_mm_set1_ps(.5)) | ||
| #else | ||
| #define KISS_FFT_COS(phase) (kiss_fft_scalar) cos(phase) | ||
| #define KISS_FFT_SIN(phase) (kiss_fft_scalar) sin(phase) | ||
| #define HALF_OF(x) ((x)*.5) | ||
| #endif | ||
|
|
||
| #define kf_cexp(x, phase) \ | ||
| do { \ | ||
| (x)->r = KISS_FFT_COS(phase); \ | ||
| (x)->i = KISS_FFT_SIN(phase); \ | ||
| } while (0) | ||
|
|
||
| /* a debugging function */ | ||
| #define pcpx(c) \ | ||
| fprintf(stderr, "%g + %gi\n", (double)((c)->r), (double)((c)->i)) | ||
|
|
||
| #ifdef KISS_FFT_USE_ALLOCA | ||
| // define this to allow use of alloca instead of malloc for temporary buffers | ||
| // Temporary buffers are used in two case: | ||
| // 1. FFT sizes that have "bad" factors. i.e. not 2,3 and 5 | ||
| // 2. "in-place" FFTs. Notice the quotes, since kissfft does not really do an | ||
| // in-place transform. | ||
| #include <alloca.h> | ||
| #define KISS_FFT_TMP_ALLOC(nbytes) alloca(nbytes) | ||
| #define KISS_FFT_TMP_FREE(ptr) | ||
| #else | ||
| #define KISS_FFT_TMP_ALLOC(nbytes) KISS_FFT_MALLOC(nbytes) | ||
| #define KISS_FFT_TMP_FREE(ptr) KISS_FFT_FREE(ptr) | ||
| #endif |
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,310 @@ | ||
| /* | ||
| Copyright (C) 2007-2010 Christian Kothe | ||
| This program 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 program 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 program; if not, write to the Free Software | ||
| Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. | ||
| */ | ||
|
|
||
| #include "FreeSurround/FreeSurroundDecoder.h" | ||
| #include "FreeSurround/ChannelMaps.h" | ||
| #include <cmath> | ||
|
|
||
| #undef min | ||
| #undef max | ||
|
|
||
| // FreeSurround implementation | ||
| // DPL2FSDecoder::Init() must be called before using the decoder. | ||
| DPL2FSDecoder::DPL2FSDecoder() { | ||
| initialized = false; | ||
| buffer_empty = true; | ||
| } | ||
|
|
||
| DPL2FSDecoder::~DPL2FSDecoder() { | ||
| #pragma warning(suppress : 4150) | ||
| delete forward; | ||
| #pragma warning(suppress : 4150) | ||
| delete inverse; | ||
| } | ||
|
|
||
| void DPL2FSDecoder::Init(channel_setup chsetup, unsigned int blsize, | ||
| unsigned int sample_rate) { | ||
| if (!initialized) { | ||
| setup = chsetup; | ||
| N = blsize; | ||
| samplerate = sample_rate; | ||
|
|
||
| // Initialize the parameters | ||
| wnd = std::vector<double>(N); | ||
| inbuf = std::vector<float>(3 * N); | ||
| lt = std::vector<double>(N); | ||
| rt = std::vector<double>(N); | ||
| dst = std::vector<double>(N); | ||
| lf = std::vector<cplx>(N / 2 + 1); | ||
| rf = std::vector<cplx>(N / 2 + 1); | ||
| forward = kiss_fftr_alloc(N, 0, 0, 0); | ||
| inverse = kiss_fftr_alloc(N, 1, 0, 0); | ||
| C = static_cast<unsigned int>(chn_alloc[setup].size()); | ||
|
|
||
| // Allocate per-channel buffers | ||
| outbuf.resize((N + N / 2) * C); | ||
| signal.resize(C, std::vector<cplx>(N)); | ||
|
|
||
| // Init the window function | ||
| for (unsigned int k = 0; k < N; k++) | ||
| wnd[k] = sqrt(0.5 * (1 - cos(2 * pi * k / N)) / N); | ||
|
|
||
| // set default parameters | ||
| set_circular_wrap(90); | ||
| set_shift(0); | ||
| set_depth(1); | ||
| set_focus(0); | ||
| set_center_image(1); | ||
| set_front_separation(1); | ||
| set_rear_separation(1); | ||
| set_low_cutoff(40.0f / samplerate * 2); | ||
| set_high_cutoff(90.0f / samplerate * 2); | ||
| set_bass_redirection(false); | ||
|
|
||
| initialized = true; | ||
| } | ||
| } | ||
|
|
||
| // decode a stereo chunk, produces a multichannel chunk of the same size | ||
| // (lagged) | ||
| float *DPL2FSDecoder::decode(float *input) { | ||
| if (initialized) { | ||
| // append incoming data to the end of the input buffer | ||
| memcpy(&inbuf[N], &input[0], 8 * N); | ||
| // process first and second half, overlapped | ||
| buffered_decode(&inbuf[0]); | ||
| buffered_decode(&inbuf[N]); | ||
| // shift last half of the input to the beginning (for overlapping with a | ||
| // future block) | ||
| memcpy(&inbuf[0], &inbuf[2 * N], 4 * N); | ||
| buffer_empty = false; | ||
| return &outbuf[0]; | ||
| } | ||
| return 0; | ||
| } | ||
|
|
||
| // flush the internal buffers | ||
| void DPL2FSDecoder::flush() { | ||
| memset(&outbuf[0], 0, outbuf.size() * 4); | ||
| memset(&inbuf[0], 0, inbuf.size() * 4); | ||
| buffer_empty = true; | ||
| } | ||
|
|
||
| // number of samples currently held in the buffer | ||
| unsigned int DPL2FSDecoder::buffered() { return buffer_empty ? 0 : N / 2; } | ||
|
|
||
| // set soundfield & rendering parameters | ||
| void DPL2FSDecoder::set_circular_wrap(float v) { circular_wrap = v; } | ||
| void DPL2FSDecoder::set_shift(float v) { shift = v; } | ||
| void DPL2FSDecoder::set_depth(float v) { depth = v; } | ||
| void DPL2FSDecoder::set_focus(float v) { focus = v; } | ||
| void DPL2FSDecoder::set_center_image(float v) { center_image = v; } | ||
| void DPL2FSDecoder::set_front_separation(float v) { front_separation = v; } | ||
| void DPL2FSDecoder::set_rear_separation(float v) { rear_separation = v; } | ||
| void DPL2FSDecoder::set_low_cutoff(float v) { lo_cut = v * (N / 2); } | ||
| void DPL2FSDecoder::set_high_cutoff(float v) { hi_cut = v * (N / 2); } | ||
| void DPL2FSDecoder::set_bass_redirection(bool v) { use_lfe = v; } | ||
|
|
||
| // helper functions | ||
| inline float DPL2FSDecoder::sqr(double x) { return static_cast<float>(x * x); } | ||
| inline double DPL2FSDecoder::amplitude(const cplx &x) { | ||
| return sqrt(sqr(x.real()) + sqr(x.imag())); | ||
| } | ||
| inline double DPL2FSDecoder::phase(const cplx &x) { | ||
| return atan2(x.imag(), x.real()); | ||
| } | ||
| inline cplx DPL2FSDecoder::polar(double a, double p) { | ||
| return cplx(a * cos(p), a * sin(p)); | ||
| } | ||
| inline float DPL2FSDecoder::min(double a, double b) { | ||
| return static_cast<float>(a < b ? a : b); | ||
| } | ||
| inline float DPL2FSDecoder::max(double a, double b) { | ||
| return static_cast<float>(a > b ? a : b); | ||
| } | ||
| inline float DPL2FSDecoder::clamp(double x) { return max(-1, min(1, x)); } | ||
| inline float DPL2FSDecoder::sign(double x) { | ||
| return static_cast<float>(x < 0 ? -1 : (x > 0 ? 1 : 0)); | ||
| } | ||
| // get the distance of the soundfield edge, along a given angle | ||
| inline double DPL2FSDecoder::edgedistance(double a) { | ||
| return min(sqrt(1 + sqr(tan(a))), sqrt(1 + sqr(1 / tan(a)))); | ||
| } | ||
| // get the index (and fractional offset!) in a piecewise-linear channel | ||
| // allocation grid | ||
| int DPL2FSDecoder::map_to_grid(double &x) { | ||
| double gp = ((x + 1) * 0.5) * (grid_res - 1), | ||
| i = min(grid_res - 2, floor(gp)); | ||
| x = gp - i; | ||
| return static_cast<int>(i); | ||
| } | ||
|
|
||
| // decode a block of data and overlap-add it into outbuf | ||
| void DPL2FSDecoder::buffered_decode(float *input) { | ||
| // demultiplex and apply window function | ||
| for (unsigned int k = 0; k < N; k++) { | ||
| lt[k] = wnd[k] * input[k * 2 + 0]; | ||
| rt[k] = wnd[k] * input[k * 2 + 1]; | ||
| } | ||
|
|
||
| // map into spectral domain | ||
| kiss_fftr(forward, <[0], (kiss_fft_cpx *)&lf[0]); | ||
| kiss_fftr(forward, &rt[0], (kiss_fft_cpx *)&rf[0]); | ||
|
|
||
| // compute multichannel output signal in the spectral domain | ||
| for (unsigned int f = 1; f < N / 2; f++) { | ||
| // get Lt/Rt amplitudes & phases | ||
| double ampL = amplitude(lf[f]), ampR = amplitude(rf[f]); | ||
| double phaseL = phase(lf[f]), phaseR = phase(rf[f]); | ||
| // calculate the amplitude & phase differences | ||
| double ampDiff = | ||
| clamp((ampL + ampR < epsilon) ? 0 : (ampR - ampL) / (ampR + ampL)); | ||
| double phaseDiff = abs(phaseL - phaseR); | ||
| if (phaseDiff > pi) | ||
| phaseDiff = 2 * pi - phaseDiff; | ||
|
|
||
| // decode into x/y soundfield position | ||
| double x, y; | ||
| transform_decode(ampDiff, phaseDiff, x, y); | ||
| // add wrap control | ||
| transform_circular_wrap(x, y, circular_wrap); | ||
| // add shift control | ||
| y = clamp(y - shift); | ||
| // add depth control | ||
| y = clamp(1 - (1 - y) * depth); | ||
| // add focus control | ||
| transform_focus(x, y, focus); | ||
| // add crossfeed control | ||
| x = clamp(x * | ||
| (front_separation * (1 + y) / 2 + rear_separation * (1 - y) / 2)); | ||
|
|
||
| // get total signal amplitude | ||
| double amp_total = sqrt(ampL * ampL + ampR * ampR); | ||
| // and total L/C/R signal phases | ||
| double phase_of[] = { | ||
| phaseL, atan2(lf[f].imag() + rf[f].imag(), lf[f].real() + rf[f].real()), | ||
| phaseR}; | ||
| // compute 2d channel map indexes p/q and update x/y to fractional offsets | ||
| // in the map grid | ||
| int p = map_to_grid(x), q = map_to_grid(y); | ||
| // map position to channel volumes | ||
| for (unsigned int c = 0; c < C - 1; c++) { | ||
| // look up channel map at respective position (with bilinear | ||
| // interpolation) and build the | ||
| // signal | ||
| std::vector<float *> &a = chn_alloc[setup][c]; | ||
| signal[c][f] = polar( | ||
| amp_total * ((1 - x) * (1 - y) * a[q][p] + x * (1 - y) * a[q][p + 1] + | ||
| (1 - x) * y * a[q + 1][p] + x * y * a[q + 1][p + 1]), | ||
| phase_of[1 + static_cast<int>(sign(chn_xsf[setup][c]))]); | ||
| } | ||
|
|
||
| // optionally redirect bass | ||
| if (use_lfe && f < hi_cut) { | ||
| // level of LFE channel according to normalized frequency | ||
| double lfe_level = | ||
| f < lo_cut ? 1 | ||
| : 0.5 * (1 + cos(pi * (f - lo_cut) / (hi_cut - lo_cut))); | ||
| // assign LFE channel | ||
| signal[C - 1][f] = lfe_level * polar(amp_total, phase_of[1]); | ||
| // subtract the signal from the other channels | ||
| for (unsigned int c = 0; c < C - 1; c++) | ||
| signal[c][f] *= (1 - lfe_level); | ||
| } | ||
| } | ||
|
|
||
| // shift the last 2/3 to the first 2/3 of the output buffer | ||
| memcpy(&outbuf[0], &outbuf[C * N / 2], N * C * 4); | ||
| // and clear the rest | ||
| memset(&outbuf[C * N], 0, C * 4 * N / 2); | ||
| // backtransform each channel and overlap-add | ||
| for (unsigned int c = 0; c < C; c++) { | ||
| // back-transform into time domain | ||
| kiss_fftri(inverse, (kiss_fft_cpx *)&signal[c][0], &dst[0]); | ||
| // add the result to the last 2/3 of the output buffer, windowed (and | ||
| // remultiplex) | ||
| for (unsigned int k = 0; k < N; k++) | ||
| outbuf[C * (k + N / 2) + c] += static_cast<float>(wnd[k] * dst[k]); | ||
| } | ||
| } | ||
|
|
||
| // transform amp/phase difference space into x/y soundfield space | ||
| void DPL2FSDecoder::transform_decode(double a, double p, double &x, double &y) { | ||
| x = clamp(1.0047 * a + 0.46804 * a * p * p * p - 0.2042 * a * p * p * p * p + | ||
| 0.0080586 * a * p * p * p * p * p * p * p - | ||
| 0.0001526 * a * p * p * p * p * p * p * p * p * p * p - | ||
| 0.073512 * a * a * a * p - 0.2499 * a * a * a * p * p * p * p + | ||
| 0.016932 * a * a * a * p * p * p * p * p * p * p - | ||
| 0.00027707 * a * a * a * p * p * p * p * p * p * p * p * p * p + | ||
| 0.048105 * a * a * a * a * a * p * p * p * p * p * p * p - | ||
| 0.0065947 * a * a * a * a * a * p * p * p * p * p * p * p * p * p * | ||
| p + | ||
| 0.0016006 * a * a * a * a * a * p * p * p * p * p * p * p * p * p * | ||
| p * p - | ||
| 0.0071132 * a * a * a * a * a * a * a * p * p * p * p * p * p * p * | ||
| p * p + | ||
| 0.0022336 * a * a * a * a * a * a * a * p * p * p * p * p * p * p * | ||
| p * p * p * p - | ||
| 0.0004804 * a * a * a * a * a * a * a * p * p * p * p * p * p * p * | ||
| p * p * p * p * p); | ||
| y = clamp( | ||
| 0.98592 - 0.62237 * p + 0.077875 * p * p - 0.0026929 * p * p * p * p * p + | ||
| 0.4971 * a * a * p - 0.00032124 * a * a * p * p * p * p * p * p + | ||
| 9.2491e-006 * a * a * a * a * p * p * p * p * p * p * p * p * p * p + | ||
| 0.051549 * a * a * a * a * a * a * a * a + | ||
| 1.0727e-014 * a * a * a * a * a * a * a * a * a * a); | ||
| } | ||
|
|
||
| // apply a circular_wrap transformation to some position | ||
| void DPL2FSDecoder::transform_circular_wrap(double &x, double &y, | ||
| double refangle) { | ||
| if (refangle == 90) | ||
| return; | ||
| refangle = refangle * pi / 180; | ||
| double baseangle = 90 * pi / 180; | ||
| // translate into edge-normalized polar coordinates | ||
| double ang = atan2(x, y), len = sqrt(x * x + y * y); | ||
| len = len / edgedistance(ang); | ||
| // apply circular_wrap transform | ||
| if (abs(ang) < baseangle / 2) | ||
| // angle falls within the front region (to be enlarged) | ||
| ang *= refangle / baseangle; | ||
| else | ||
| // angle falls within the rear region (to be shrunken) | ||
| ang = pi - (-(((refangle - 2 * pi) * (pi - abs(ang)) * sign(ang)) / | ||
| (2 * pi - baseangle))); | ||
| // translate back into soundfield position | ||
| len = len * edgedistance(ang); | ||
| x = clamp(sin(ang) * len); | ||
| y = clamp(cos(ang) * len); | ||
| } | ||
|
|
||
| // apply a focus transformation to some position | ||
| void DPL2FSDecoder::transform_focus(double &x, double &y, double focus) { | ||
| if (focus == 0) | ||
| return; | ||
| // translate into edge-normalized polar coordinates | ||
| double ang = atan2(x, y), | ||
| len = clamp(sqrt(x * x + y * y) / edgedistance(ang)); | ||
| // apply focus | ||
| len = focus > 0 ? 1 - pow(1 - len, 1 + focus * 20) : pow(len, 1 - focus * 20); | ||
| // back-transform into euclidian soundfield position | ||
| len = len * edgedistance(ang); | ||
| x = clamp(sin(ang) * len); | ||
| y = clamp(cos(ang) * len); | ||
| } |
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,185 @@ | ||
| /* | ||
| Copyright (c) 2003-2004, Mark Borgerding | ||
| All rights reserved. | ||
| Redistribution and use in source and binary forms, with or without modification, | ||
| are permitted | ||
| provided that the following conditions are met: | ||
| * Redistributions of source code must retain the above copyright notice, | ||
| this list of conditions | ||
| and the following disclaimer. | ||
| * Redistributions in binary form must reproduce the above copyright notice, | ||
| this list of | ||
| conditions and the following disclaimer in the documentation and/or other | ||
| materials provided with | ||
| the distribution. | ||
| * Neither the author nor the names of any contributors may be used to | ||
| endorse or promote | ||
| products derived from this software without specific prior written permission. | ||
| THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND | ||
| ANY EXPRESS OR | ||
| IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF | ||
| MERCHANTABILITY AND | ||
| FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT | ||
| OWNER OR | ||
| CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, | ||
| OR CONSEQUENTIAL | ||
| DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR | ||
| SERVICES; LOSS OF USE, | ||
| DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF | ||
| LIABILITY, WHETHER | ||
| IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) | ||
| ARISING IN ANY WAY OUT OF | ||
| THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. | ||
| */ | ||
|
|
||
| #include "FreeSurround/KissFFTR.h" | ||
| #include "FreeSurround/_KissFFTGuts.h" | ||
|
|
||
| struct kiss_fftr_state { | ||
| kiss_fft_cfg substate; | ||
| kiss_fft_cpx *tmpbuf; | ||
| kiss_fft_cpx *super_twiddles; | ||
| #ifdef USE_SIMD | ||
| void *pad; | ||
| #endif | ||
| }; | ||
|
|
||
| kiss_fftr_cfg kiss_fftr_alloc(int nfft, int inverse_fft, void *mem, | ||
| size_t *lenmem) { | ||
| int i; | ||
| kiss_fftr_cfg st = NULL; | ||
| size_t subsize = 65536 * 4, memneeded = 0; | ||
|
|
||
| if (nfft & 1) { | ||
| fprintf(stderr, "Real FFT optimization must be even.\n"); | ||
| return NULL; | ||
| } | ||
| nfft >>= 1; | ||
|
|
||
| kiss_fft_alloc(nfft, inverse_fft, NULL, &subsize); | ||
| memneeded = sizeof(struct kiss_fftr_state) + subsize + | ||
| sizeof(kiss_fft_cpx) * (nfft * 3 / 2); | ||
|
|
||
| if (lenmem == NULL) { | ||
| st = (kiss_fftr_cfg) new char[memneeded]; | ||
| } else { | ||
| if (*lenmem >= memneeded) | ||
| st = (kiss_fftr_cfg)mem; | ||
| *lenmem = memneeded; | ||
| } | ||
| if (!st) | ||
| return NULL; | ||
|
|
||
| st->substate = (kiss_fft_cfg)(st + 1); /*just beyond kiss_fftr_state struct */ | ||
| st->tmpbuf = (kiss_fft_cpx *)(((char *)st->substate) + subsize); | ||
| st->super_twiddles = st->tmpbuf + nfft; | ||
| kiss_fft_alloc(nfft, inverse_fft, st->substate, &subsize); | ||
|
|
||
| for (i = 0; i < nfft / 2; ++i) { | ||
| double phase = | ||
| -3.14159265358979323846264338327 * ((double)(i + 1) / nfft + .5); | ||
| if (inverse_fft) | ||
| phase *= -1; | ||
| kf_cexp(st->super_twiddles + i, phase); | ||
| } | ||
| return st; | ||
| } | ||
|
|
||
| void kiss_fftr(kiss_fftr_cfg st, const kiss_fft_scalar *timedata, | ||
| kiss_fft_cpx *freqdata) { | ||
| /* input buffer timedata is stored row-wise */ | ||
| int k, ncfft; | ||
| kiss_fft_cpx fpnk, fpk, f1k, f2k, tw, tdc; | ||
|
|
||
| if (st->substate->inverse) { | ||
| fprintf(stderr, "kiss fft usage error: improper alloc\n"); | ||
| exit(1); | ||
| } | ||
|
|
||
| ncfft = st->substate->nfft; | ||
|
|
||
| /*perform the parallel fft of two real signals packed in real,imag*/ | ||
| kiss_fft(st->substate, (const kiss_fft_cpx *)timedata, st->tmpbuf); | ||
| /* The real part of the DC element of the frequency spectrum in st->tmpbuf | ||
| * contains the sum of the even-numbered elements of the input time sequence | ||
| * The imag part is the sum of the odd-numbered elements | ||
| * | ||
| * The sum of tdc.r and tdc.i is the sum of the input time sequence. | ||
| * yielding DC of input time sequence | ||
| * The difference of tdc.r - tdc.i is the sum of the input (dot product) | ||
| * [1,-1,1,-1... | ||
| * yielding Nyquist bin of input time sequence | ||
| */ | ||
|
|
||
| tdc.r = st->tmpbuf[0].r; | ||
| tdc.i = st->tmpbuf[0].i; | ||
| C_FIXDIV(tdc, 2); | ||
| CHECK_OVERFLOW_OP(tdc.r, +, tdc.i); | ||
| CHECK_OVERFLOW_OP(tdc.r, -, tdc.i); | ||
| freqdata[0].r = tdc.r + tdc.i; | ||
| freqdata[ncfft].r = tdc.r - tdc.i; | ||
| #ifdef USE_SIMD | ||
| freqdata[ncfft].i = freqdata[0].i = _mm_set1_ps(0); | ||
| #else | ||
| freqdata[ncfft].i = freqdata[0].i = 0; | ||
| #endif | ||
|
|
||
| for (k = 1; k <= ncfft / 2; ++k) { | ||
| fpk = st->tmpbuf[k]; | ||
| fpnk.r = st->tmpbuf[ncfft - k].r; | ||
| fpnk.i = -st->tmpbuf[ncfft - k].i; | ||
| C_FIXDIV(fpk, 2); | ||
| C_FIXDIV(fpnk, 2); | ||
|
|
||
| C_ADD(f1k, fpk, fpnk); | ||
| C_SUB(f2k, fpk, fpnk); | ||
| C_MUL(tw, f2k, st->super_twiddles[k - 1]); | ||
|
|
||
| freqdata[k].r = HALF_OF(f1k.r + tw.r); | ||
| freqdata[k].i = HALF_OF(f1k.i + tw.i); | ||
| freqdata[ncfft - k].r = HALF_OF(f1k.r - tw.r); | ||
| freqdata[ncfft - k].i = HALF_OF(tw.i - f1k.i); | ||
| } | ||
| } | ||
|
|
||
| void kiss_fftri(kiss_fftr_cfg st, const kiss_fft_cpx *freqdata, | ||
| kiss_fft_scalar *timedata) { | ||
| /* input buffer timedata is stored row-wise */ | ||
| int k, ncfft; | ||
|
|
||
| if (st->substate->inverse == 0) { | ||
| fprintf(stderr, "kiss fft usage error: improper alloc\n"); | ||
| exit(1); | ||
| } | ||
|
|
||
| ncfft = st->substate->nfft; | ||
|
|
||
| st->tmpbuf[0].r = freqdata[0].r + freqdata[ncfft].r; | ||
| st->tmpbuf[0].i = freqdata[0].r - freqdata[ncfft].r; | ||
| C_FIXDIV(st->tmpbuf[0], 2); | ||
|
|
||
| for (k = 1; k <= ncfft / 2; ++k) { | ||
| kiss_fft_cpx fk, fnkc, fek, fok, tmp; | ||
| fk = freqdata[k]; | ||
| fnkc.r = freqdata[ncfft - k].r; | ||
| fnkc.i = -freqdata[ncfft - k].i; | ||
| C_FIXDIV(fk, 2); | ||
| C_FIXDIV(fnkc, 2); | ||
|
|
||
| C_ADD(fek, fk, fnkc); | ||
| C_SUB(tmp, fk, fnkc); | ||
| C_MUL(fok, tmp, st->super_twiddles[k - 1]); | ||
| C_ADD(st->tmpbuf[k], fek, fok); | ||
| C_SUB(st->tmpbuf[ncfft - k], fek, fok); | ||
| #ifdef USE_SIMD | ||
| st->tmpbuf[ncfft - k].i *= _mm_set1_ps(-1.0); | ||
| #else | ||
| st->tmpbuf[ncfft - k].i *= -1; | ||
| #endif | ||
| } | ||
| kiss_fft(st->substate, st->tmpbuf, (kiss_fft_cpx *)timedata); | ||
| } |
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,93 @@ | ||
| // Copyright 2017 Dolphin Emulator Project | ||
| // Licensed under GPLv2+ | ||
| // Refer to the license.txt file included. | ||
|
|
||
| #include <FreeSurround/FreeSurroundDecoder.h> | ||
| #include <limits> | ||
|
|
||
| #include "AudioCommon/SurroundDecoder.h" | ||
|
|
||
| namespace AudioCommon | ||
| { | ||
| constexpr size_t STEREO_CHANNELS = 2; | ||
| constexpr size_t SURROUND_CHANNELS = 6; | ||
|
|
||
| SurroundDecoder::SurroundDecoder(u32 sample_rate, u32 frame_block_size) | ||
| : m_sample_rate(sample_rate), m_frame_block_size(frame_block_size) | ||
| { | ||
| m_fsdecoder = std::make_unique<DPL2FSDecoder>(); | ||
| m_fsdecoder->Init(cs_5point1, m_frame_block_size, m_sample_rate); | ||
| } | ||
|
|
||
| SurroundDecoder::~SurroundDecoder() = default; | ||
|
|
||
| void SurroundDecoder::Clear() | ||
| { | ||
| m_fsdecoder->flush(); | ||
| m_decoded_fifo.clear(); | ||
| } | ||
|
|
||
| // Currently only 6 channels are supported. | ||
| size_t SurroundDecoder::QueryFramesNeededForSurroundOutput(const size_t output_frames) const | ||
| { | ||
| if (m_decoded_fifo.size() < output_frames * SURROUND_CHANNELS) | ||
| { | ||
| // Output stereo frames needed to have at least the desired number of surround frames | ||
| size_t frames_needed = output_frames - m_decoded_fifo.size() / SURROUND_CHANNELS; | ||
| return frames_needed + m_frame_block_size - frames_needed % m_frame_block_size; | ||
| } | ||
|
|
||
| return 0; | ||
| } | ||
|
|
||
| // Receive and decode samples | ||
| void SurroundDecoder::PutFrames(const short* in, const size_t num_frames_in) | ||
| { | ||
| // Maybe check if it is really power-of-2? | ||
| s64 remaining_frames = static_cast<s64>(num_frames_in); | ||
| size_t frame_index = 0; | ||
|
|
||
| while (remaining_frames > 0) | ||
| { | ||
| // Convert to float | ||
| for (size_t i = 0, end = m_frame_block_size * STEREO_CHANNELS; i < end; ++i) | ||
| { | ||
| m_float_conversion_buffer[i] = in[i + frame_index * STEREO_CHANNELS] / | ||
| static_cast<float>(std::numeric_limits<short>::max()); | ||
| } | ||
|
|
||
| // Decode | ||
| const float* dpl2_fs = m_fsdecoder->decode(m_float_conversion_buffer.data()); | ||
|
|
||
| // Add to ring buffer and fix channel mapping | ||
| // Maybe modify FreeSurround to output the correct mapping? | ||
| // FreeSurround: | ||
| // FL | FC | FR | BL | BR | LFE | ||
| // Most backends: | ||
| // FL | FR | FC | LFE | BL | BR | ||
| for (size_t i = 0; i < m_frame_block_size; ++i) | ||
| { | ||
| m_decoded_fifo.push(dpl2_fs[i * SURROUND_CHANNELS + 0]); // LEFTFRONT | ||
| m_decoded_fifo.push(dpl2_fs[i * SURROUND_CHANNELS + 2]); // RIGHTFRONT | ||
| m_decoded_fifo.push(dpl2_fs[i * SURROUND_CHANNELS + 1]); // CENTREFRONT | ||
| m_decoded_fifo.push(dpl2_fs[i * SURROUND_CHANNELS + 5]); // sub/lfe | ||
| m_decoded_fifo.push(dpl2_fs[i * SURROUND_CHANNELS + 3]); // LEFTREAR | ||
| m_decoded_fifo.push(dpl2_fs[i * SURROUND_CHANNELS + 4]); // RIGHTREAR | ||
| } | ||
|
|
||
| remaining_frames = remaining_frames - static_cast<int>(m_frame_block_size); | ||
| frame_index = frame_index + m_frame_block_size; | ||
| } | ||
| } | ||
|
|
||
| void SurroundDecoder::ReceiveFrames(float* out, const size_t num_frames_out) | ||
| { | ||
| // Copy to output array with desired num_frames_out | ||
| for (size_t i = 0, num_samples_output = num_frames_out * SURROUND_CHANNELS; | ||
| i < num_samples_output; ++i) | ||
| { | ||
| out[i] = m_decoded_fifo.pop_front(); | ||
| } | ||
| } | ||
|
|
||
| } // namespace AudioCommon |
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,36 @@ | ||
| // Copyright 2017 Dolphin Emulator Project | ||
| // Licensed under GPLv2+ | ||
| // Refer to the license.txt file included. | ||
|
|
||
| #pragma once | ||
|
|
||
| #include <array> | ||
| #include <memory> | ||
|
|
||
| #include "Common/CommonTypes.h" | ||
| #include "Common/FixedSizeQueue.h" | ||
|
|
||
| class DPL2FSDecoder; | ||
|
|
||
| namespace AudioCommon | ||
| { | ||
| class SurroundDecoder | ||
| { | ||
| public: | ||
| explicit SurroundDecoder(u32 sample_rate, u32 frame_block_size); | ||
| ~SurroundDecoder(); | ||
| size_t QueryFramesNeededForSurroundOutput(const size_t output_frames) const; | ||
| void PutFrames(const short* in, const size_t num_frames_in); | ||
| void ReceiveFrames(float* out, const size_t num_frames_out); | ||
| void Clear(); | ||
|
|
||
| private: | ||
| u32 m_sample_rate; | ||
| u32 m_frame_block_size; | ||
|
|
||
| std::unique_ptr<DPL2FSDecoder> m_fsdecoder; | ||
| std::array<float, 32768> m_float_conversion_buffer; | ||
| FixedSizeQueue<float, 32768> m_decoded_fifo; | ||
| }; | ||
|
|
||
| } // AudioCommon |