freesurround_decoder.cpp (12096B)
1 /* 2 Copyright (C) 2007-2010 Christian Kothe 3 4 This program is free software; you can redistribute it and/or 5 modify it under the terms of the GNU General Public License 6 as published by the Free Software Foundation; either version 2 7 of the License, or (at your option) any later version. 8 9 This program is distributed in the hope that it will be useful, 10 but WITHOUT ANY WARRANTY; without even the implied warranty of 11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 GNU General Public License for more details. 13 14 You should have received a copy of the GNU General Public License 15 along with this program; if not, write to the Free Software 16 Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 17 */ 18 19 #include <cmath> 20 #include <vector> 21 #include <complex> 22 #include "kiss_fftr.h" 23 #include "freesurround_decoder.h" 24 #include "channelmaps.h" 25 26 typedef std::complex<double> cplx; 27 const double pi = M_PI; 28 const double epsilon = 0.000001; 29 using namespace std; 30 31 #undef min 32 #undef max 33 34 // FreeSurround implementation 35 class decoder_impl { 36 public: 37 // instantiate the decoder with a given channel setup and processing block size (in samples) 38 decoder_impl(channel_setup setup, unsigned N) 39 : N(N), C((unsigned)chn_alloc[setup].size()), setup(setup), 40 lt(N), rt(N), dst(N), lf(N/2+1), rf(N/2+1), 41 forward(kiss_fftr_alloc(N,0,0,0)), inverse(kiss_fftr_alloc(N,1,0,0)), 42 buffer_empty(true), inbuf(3*N), wnd(N) 43 { 44 // allocate per-channel buffers 45 outbuf.resize((N+N/2)*C); 46 signal.resize(C,vector<cplx>(N)); 47 48 // init the window function 49 for (unsigned k=0;k<N;k++) 50 wnd[k] = sqrt(0.5*(1-cos(2*pi*k/N))/N); 51 52 // set default parameters 53 set_circular_wrap(90); 54 set_shift(0); 55 set_depth(1); 56 set_focus(0); 57 set_front_separation(1); 58 set_rear_separation(1); 59 set_low_cutoff(40.0/22050); 60 set_high_cutoff(90.0/22050); 61 set_bass_redirection(false); 62 } 63 64 ~decoder_impl() 65 { 66 // kiss_fftr_alloc does new char[] inside... 67 delete[] reinterpret_cast<char*>(forward); 68 delete[] reinterpret_cast<char*>(inverse); 69 } 70 71 // decode a stereo chunk, produces a multichannel chunk of the same size (lagged) 72 const float* decode(const float *input) { 73 // append incoming data to the end of the input buffer 74 memcpy(&inbuf[N], &input[0], 8*N); 75 // process first and second half, overlapped 76 buffered_decode(&inbuf[0]); 77 buffered_decode(&inbuf[N]); 78 // shift last half of the input to the beginning (for overlapping with a future block) 79 memcpy(&inbuf[0], &inbuf[2*N], 4*N); 80 buffer_empty = false; 81 return &outbuf[0]; 82 } 83 84 // flush the internal buffers 85 void flush() { 86 memset(&outbuf[0],0,outbuf.size()*4); 87 memset(&inbuf[0],0,inbuf.size()*4); 88 buffer_empty = true; 89 } 90 91 // number of samples currently held in the buffer 92 unsigned buffered() { return buffer_empty ? 0 : N/2; } 93 94 // set soundfield & rendering parameters 95 void set_circular_wrap(float v) { circular_wrap = v; } 96 void set_shift(float v) { shift = v; } 97 void set_depth(float v) { depth = v; } 98 void set_focus(float v) { focus = v; } 99 void set_front_separation(float v) { front_separation = v; } 100 void set_rear_separation(float v) { rear_separation = v; } 101 void set_low_cutoff(float v) { lo_cut = v*(N/2); } 102 void set_high_cutoff(float v) { hi_cut = v*(N/2); } 103 void set_bass_redirection(bool v) { use_lfe = v; } 104 105 private: 106 // helper functions 107 static inline double sqr(double x) { return x*x; } 108 static inline double amplitude(const cplx &x) { return sqrt(sqr(x.real()) + sqr(x.imag())); } 109 static inline double phase(const cplx &x) { return atan2(x.imag(),x.real()); } 110 static inline cplx polar(double a, double p) { return cplx(a*cos(p),a*sin(p)); } 111 static inline double min(double a, double b) { return a<b?a:b; } 112 static inline double max(double a, double b) { return a>b?a:b; } 113 static inline double clamp(double x) { return max(-1,min(1,x)); } 114 static inline double sign(double x) { return x<0?-1:(x>0?1:0); } 115 // get the distance of the soundfield edge, along a given angle 116 static inline double edgedistance(double a) { return min(sqrt(1+sqr(tan(a))),sqrt(1+sqr(1/tan(a)))); } 117 // get the index (and fractional offset!) in a piecewise-linear channel allocation grid 118 int 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 i; } 119 120 // decode a block of data and overlap-add it into outbuf 121 void buffered_decode(float *input) { 122 // demultiplex and apply window function 123 for (unsigned k=0;k<N;k++) { 124 lt[k] = wnd[k]*input[k*2+0]; 125 rt[k] = wnd[k]*input[k*2+1]; 126 } 127 128 // map into spectral domain 129 kiss_fftr(forward,<[0],(kiss_fft_cpx*)&lf[0]); 130 kiss_fftr(forward,&rt[0],(kiss_fft_cpx*)&rf[0]); 131 132 // compute multichannel output signal in the spectral domain 133 for (unsigned f=1;f<N/2;f++) { 134 // get Lt/Rt amplitudes & phases 135 double ampL = amplitude(lf[f]), ampR = amplitude(rf[f]); 136 double phaseL = phase(lf[f]), phaseR = phase(rf[f]); 137 // calculate the amplitude & phase differences 138 double ampDiff = clamp((ampL+ampR < epsilon) ? 0 : (ampR-ampL) / (ampR+ampL)); 139 double phaseDiff = abs(phaseL - phaseR); 140 if (phaseDiff > pi) phaseDiff = 2*pi - phaseDiff; 141 142 // decode into x/y soundfield position 143 double x,y; transform_decode(ampDiff,phaseDiff,x,y); 144 // add wrap control 145 transform_circular_wrap(x,y,circular_wrap); 146 // add shift control 147 y = clamp(y - shift); 148 // add depth control 149 y = clamp(1 - (1-y)*depth); 150 // add focus control 151 transform_focus(x,y,focus); 152 // add crossfeed control 153 x = clamp(x * (front_separation*(1+y)/2 + rear_separation*(1-y)/2)); 154 155 // get total signal amplitude 156 double amp_total = sqrt(ampL*ampL + ampR*ampR); 157 // and total L/C/R signal phases 158 double phase_of[] = {phaseL,atan2(lf[f].imag()+rf[f].imag(),lf[f].real()+rf[f].real()),phaseR}; 159 // compute 2d channel map indexes p/q and update x/y to fractional offsets in the map grid 160 int p=map_to_grid(x), q=map_to_grid(y); 161 // map position to channel volumes 162 for (unsigned c=0;c<C-1;c++) { 163 // look up channel map at respective position (with bilinear interpolation) and build the signal 164 vector<float*> &a = chn_alloc[setup][c]; 165 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]), 166 phase_of[1+(int)sign(chn_xsf[setup][c])]); 167 } 168 169 // optionally redirect bass 170 if (use_lfe && f < hi_cut) { 171 // level of LFE channel according to normalized frequency 172 double lfe_level = f < lo_cut ? 1 : 0.5*(1+cos(pi*(f-lo_cut)/(hi_cut-lo_cut))); 173 // assign LFE channel 174 signal[C-1][f] = lfe_level * polar(amp_total,phase_of[1]); 175 // subtract the signal from the other channels 176 for (unsigned c=0;c<C-1;c++) 177 signal[c][f] *= (1-lfe_level); 178 } 179 } 180 181 // shift the last 2/3 to the first 2/3 of the output buffer 182 memmove(&outbuf[0], &outbuf[C*N/2], N*C*4); 183 // and clear the rest 184 memset(&outbuf[C*N], 0, C*4*N/2); 185 // backtransform each channel and overlap-add 186 for (unsigned c=0;c<C;c++) { 187 // back-transform into time domain 188 kiss_fftri(inverse,(kiss_fft_cpx*)&signal[c][0],&dst[0]); 189 // add the result to the last 2/3 of the output buffer, windowed (and remultiplex) 190 for (unsigned k=0;k<N;k++) 191 outbuf[C*(k+N/2)+c] += wnd[k]*dst[k]; 192 } 193 } 194 195 // transform amp/phase difference space into x/y soundfield space 196 void transform_decode(double a, double p, double &x, double &y) { 197 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 198 - 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 199 + 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 200 - 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 201 - 0.0004804*a*a*a*a*a*a*a*p*p*p*p*p*p*p*p*p*p*p*p); 202 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 203 + 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); 204 } 205 206 // apply a circular_wrap transformation to some position 207 void transform_circular_wrap(double &x, double &y, double refangle) { 208 if (refangle == 90) 209 return; 210 refangle = refangle*pi/180; 211 double baseangle = 90*pi/180; 212 // translate into edge-normalized polar coordinates 213 double ang = atan2(x,y), len = sqrt(x*x+y*y); 214 len = len / edgedistance(ang); 215 // apply circular_wrap transform 216 if (abs(ang) < baseangle/2) 217 // angle falls within the front region (to be enlarged) 218 ang *= refangle / baseangle; 219 else 220 // angle falls within the rear region (to be shrunken) 221 ang = pi - (-(((refangle - 2*pi)*(pi - abs(ang))*sign(ang))/(2*pi - baseangle))); 222 // translate back into soundfield position 223 len = len * edgedistance(ang); 224 x = clamp(sin(ang)*len); 225 y = clamp(cos(ang)*len); 226 } 227 228 // apply a focus transformation to some position 229 void transform_focus(double &x, double &y, double focus) { 230 if (focus == 0) 231 return; 232 // translate into edge-normalized polar coordinates 233 double ang = atan2(x,y), len = clamp(sqrt(x*x+y*y)/edgedistance(ang)); 234 // apply focus 235 len = focus > 0 ? 1-pow(1-len,1+focus*20) : pow(len,1-focus*20); 236 // back-transform into euclidian soundfield position 237 len = len * edgedistance(ang); 238 x = clamp(sin(ang)*len); 239 y = clamp(cos(ang)*len); 240 } 241 242 // constants 243 unsigned N,C; // number of samples per input/output block, number of output channels 244 channel_setup setup; // the channel setup 245 246 // parameters 247 float circular_wrap; // angle of the front soundstage around the listener (90°=default) 248 float shift; // forward/backward offset of the soundstage 249 float depth; // backward extension of the soundstage 250 float focus; // localization of the sound events 251 float front_separation; // front stereo separation 252 float rear_separation; // rear stereo separation 253 float lo_cut, hi_cut; // LFE cutoff frequencies 254 bool use_lfe; // whether to use the LFE channel 255 256 // FFT data structures 257 vector<double> lt,rt,dst; // left total, right total (source arrays), time-domain destination buffer array 258 vector<cplx> lf,rf; // left total / right total in frequency domain 259 kiss_fftr_cfg forward,inverse; // FFT buffers 260 261 // buffers 262 bool buffer_empty; // whether the buffer is currently empty or dirty 263 vector<float> inbuf; // stereo input buffer (multiplexed) 264 vector<float> outbuf; // multichannel output buffer (multiplexed) 265 vector<double> wnd; // the window function, precomputed 266 vector<vector<cplx> > signal; // the signal to be constructed in every channel, in the frequency domain 267 }; 268 269 270 // implementation of the shell class 271 freesurround_decoder::freesurround_decoder(channel_setup setup, unsigned blocksize): impl(new decoder_impl(setup,blocksize)) { } 272 freesurround_decoder::~freesurround_decoder() { delete impl; } 273 const float* freesurround_decoder::decode(const float* input) { return impl->decode(input); } 274 void freesurround_decoder::flush() { impl->flush(); } 275 void freesurround_decoder::circular_wrap(float v) { impl->set_circular_wrap(v); } 276 void freesurround_decoder::shift(float v) { impl->set_shift(v); } 277 void freesurround_decoder::depth(float v) { impl->set_depth(v); } 278 void freesurround_decoder::focus(float v) { impl->set_focus(v); } 279 void freesurround_decoder::front_separation(float v) { impl->set_front_separation(v); } 280 void freesurround_decoder::rear_separation(float v) { impl->set_rear_separation(v); } 281 void freesurround_decoder::low_cutoff(float v) { impl->set_low_cutoff(v); } 282 void freesurround_decoder::high_cutoff(float v) { impl->set_high_cutoff(v); } 283 void freesurround_decoder::bass_redirection(bool v) { impl->set_bass_redirection(v); } 284 unsigned freesurround_decoder::buffered() { return impl->buffered(); } 285 unsigned freesurround_decoder::num_channels(channel_setup s) { return chn_id[s].size(); } 286 channel_id freesurround_decoder::channel_at(channel_setup s, unsigned i) { return i < chn_id[s].size() ? chn_id[s][i] : ci_none; }