freesurround_decoder.cpp (10661B)
1 // SPDX-FileCopyrightText: 2007-2010 Christian Kothe, 2024 Connor McLaughlin <stenzek@gmail.com> 2 // SPDX-License-Identifier: GPL-2.0+ 3 4 #include "freesurround_decoder.h" 5 6 #include <algorithm> 7 #include <cmath> 8 9 static constexpr float pi = 3.141592654f; 10 static constexpr float epsilon = 0.000001f; 11 12 template<typename T> 13 static inline T sqr(T x) 14 { 15 return x * x; 16 } 17 18 template<typename T> 19 static inline T clamp1(T x) 20 { 21 return std::clamp(x, static_cast<T>(-1), static_cast<T>(1)); 22 } 23 24 template<typename T> 25 static inline T sign(T x) 26 { 27 return x < static_cast<T>(0) ? static_cast<T>(-1) : (x > static_cast<T>(0) ? static_cast<T>(1) : static_cast<T>(0)); 28 } 29 30 static inline double amplitude(const std::complex<double>& x) 31 { 32 return sqrt(sqr(x.real()) + sqr(x.imag())); 33 } 34 static inline double phase(const std::complex<double>& x) 35 { 36 return atan2(x.imag(), x.real()); 37 } 38 static inline std::complex<double> polar(double a, double p) 39 { 40 return std::complex<double>(a * std::cos(p), a * std::sin(p)); 41 } 42 43 // get the distance of the soundfield edge, along a given angle 44 static inline double edgedistance(double a) 45 { 46 return std::min(std::sqrt(1 + sqr(std::tan(a))), std::sqrt(1 + sqr(1 / std::tan(a)))); 47 } 48 49 static void transform_decode(double a, double p, double& x, double& y); 50 51 // apply a circular_wrap transformation to some position 52 static void transform_circular_wrap(double& x, double& y, double refangle); 53 54 // apply a focus transformation to some position 55 static void transform_focus(double& x, double& y, double focus); 56 57 // get the index (and fractional offset!) in a piecewise-linear channel allocation grid 58 int FreeSurroundDecoder::MapToGrid(double& x) 59 { 60 const double gp = ((x + 1) * 0.5) * (grid_res - 1); 61 const double i = std::min(static_cast<double>(grid_res - 2), std::floor(gp)); 62 x = gp - i; 63 return static_cast<int>(i); 64 } 65 66 FreeSurroundDecoder::FreeSurroundDecoder(ChannelSetup setup, unsigned blocksize) 67 : cmap(s_channel_maps[static_cast<size_t>(setup)]) 68 { 69 N = blocksize; 70 C = static_cast<unsigned>(cmap.luts.size()); 71 wnd.resize(blocksize); 72 lt.resize(blocksize); 73 rt.resize(blocksize); 74 dst.resize(blocksize); 75 lf.resize(blocksize / 2 + 1); 76 rf.resize(blocksize / 2 + 1); 77 78 forward = kiss_fftr_alloc(blocksize, 0, 0, 0); 79 inverse = kiss_fftr_alloc(blocksize, 1, 0, 0); 80 81 // allocate per-channel buffers 82 inbuf.resize(3 * N); 83 outbuf.resize((N + N / 2) * C); 84 signal.resize(C, std::vector<cplx>(N)); 85 86 // init the window function 87 for (unsigned k = 0; k < N; k++) 88 wnd[k] = sqrt(0.5 * (1 - cos(2 * pi * k / N)) / N); 89 90 // set default parameters 91 SetCircularWrap(90); 92 SetShift(0); 93 SetDepth(1); 94 SetFocus(0); 95 SetCenterImage(1); 96 SetFrontSeparation(1); 97 SetRearSeparation(1); 98 SetLowCutoff(40.0f / 22050.0f); 99 SetHighCutoff(90.0f / 22050.0f); 100 SetBassRedirection(false); 101 } 102 103 FreeSurroundDecoder::~FreeSurroundDecoder() 104 { 105 kiss_fftr_free(forward); 106 kiss_fftr_free(inverse); 107 } 108 109 // decode a stereo chunk, produces a multichannel chunk of the same size (lagged) 110 float* FreeSurroundDecoder::Decode(float* input) 111 { 112 // append incoming data to the end of the input buffer 113 memcpy(&inbuf[N], &input[0], 8 * N); 114 // process first and second half, overlapped 115 BufferedDecode(&inbuf[0]); 116 BufferedDecode(&inbuf[N]); 117 // shift last half of the input to the beginning (for overlapping with a future block) 118 memcpy(&inbuf[0], &inbuf[2 * N], 4 * N); 119 buffer_empty = false; 120 return &outbuf[0]; 121 } 122 123 // flush the internal buffers 124 void FreeSurroundDecoder::Flush() 125 { 126 memset(&outbuf[0], 0, outbuf.size() * 4); 127 memset(&inbuf[0], 0, inbuf.size() * 4); 128 buffer_empty = true; 129 } 130 131 // number of samples currently held in the buffer 132 unsigned FreeSurroundDecoder::GetSamplesBuffered() 133 { 134 return buffer_empty ? 0 : N / 2; 135 } 136 137 // set soundfield & rendering parameters 138 void FreeSurroundDecoder::SetCircularWrap(float v) 139 { 140 circular_wrap = v; 141 } 142 void FreeSurroundDecoder::SetShift(float v) 143 { 144 shift = v; 145 } 146 void FreeSurroundDecoder::SetDepth(float v) 147 { 148 depth = v; 149 } 150 void FreeSurroundDecoder::SetFocus(float v) 151 { 152 focus = v; 153 } 154 void FreeSurroundDecoder::SetCenterImage(float v) 155 { 156 center_image = v; 157 } 158 void FreeSurroundDecoder::SetFrontSeparation(float v) 159 { 160 front_separation = v; 161 } 162 void FreeSurroundDecoder::SetRearSeparation(float v) 163 { 164 rear_separation = v; 165 } 166 void FreeSurroundDecoder::SetLowCutoff(float v) 167 { 168 lo_cut = v * (N / 2); 169 } 170 void FreeSurroundDecoder::SetHighCutoff(float v) 171 { 172 hi_cut = v * (N / 2); 173 } 174 void FreeSurroundDecoder::SetBassRedirection(bool v) 175 { 176 use_lfe = v; 177 } 178 179 // decode a block of data and overlap-add it into outbuf 180 void FreeSurroundDecoder::BufferedDecode(float* input) 181 { 182 // demultiplex and apply window function 183 for (unsigned k = 0; k < N; k++) 184 { 185 lt[k] = wnd[k] * input[k * 2 + 0]; 186 rt[k] = wnd[k] * input[k * 2 + 1]; 187 } 188 189 // map into spectral domain 190 kiss_fftr(forward, <[0], (kiss_fft_cpx*)&lf[0]); 191 kiss_fftr(forward, &rt[0], (kiss_fft_cpx*)&rf[0]); 192 193 // compute multichannel output signal in the spectral domain 194 for (unsigned f = 1; f < N / 2; f++) 195 { 196 // get Lt/Rt amplitudes & phases 197 double ampL = amplitude(lf[f]), ampR = amplitude(rf[f]); 198 double phaseL = phase(lf[f]), phaseR = phase(rf[f]); 199 // calculate the amplitude & phase differences 200 double ampDiff = clamp1((ampL + ampR < epsilon) ? 0 : (ampR - ampL) / (ampR + ampL)); 201 double phaseDiff = abs(phaseL - phaseR); 202 if (phaseDiff > pi) 203 phaseDiff = 2 * pi - phaseDiff; 204 205 // decode into x/y soundfield position 206 double x, y; 207 transform_decode(ampDiff, phaseDiff, x, y); 208 // add wrap control 209 transform_circular_wrap(x, y, circular_wrap); 210 // add shift control 211 y = clamp1(y - shift); 212 // add depth control 213 y = clamp1(1 - (1 - y) * depth); 214 // add focus control 215 transform_focus(x, y, focus); 216 // add crossfeed control 217 x = clamp1(x * (front_separation * (1 + y) / 2 + rear_separation * (1 - y) / 2)); 218 219 // get total signal amplitude 220 double amp_total = sqrt(ampL * ampL + ampR * ampR); 221 // and total L/C/R signal phases 222 double phase_of[] = {phaseL, atan2(lf[f].imag() + rf[f].imag(), lf[f].real() + rf[f].real()), phaseR}; 223 // compute 2d channel map indexes p/q and update x/y to fractional offsets in the map grid 224 int p = MapToGrid(x), q = MapToGrid(y); 225 // map position to channel volumes 226 for (unsigned c = 0; c < C - 1; c++) 227 { 228 // look up channel map at respective position (with bilinear interpolation) and build the signal 229 const auto& a = cmap.luts[c]; 230 signal[c][f] = polar(amp_total * ((1 - x) * (1 - y) * a[q][p] + x * (1 - y) * a[q][p + 1] + 231 (1 - x) * y * a[q + 1][p] + x * y * a[q + 1][p + 1]), 232 phase_of[1 + (int)sign(cmap.xsf[c])]); 233 } 234 235 // optionally redirect bass 236 if (use_lfe && f < hi_cut) 237 { 238 // level of LFE channel according to normalized frequency 239 double lfe_level = f < lo_cut ? 1 : 0.5 * (1 + cos(pi * (f - lo_cut) / (hi_cut - lo_cut))); 240 // assign LFE channel 241 signal[C - 1][f] = lfe_level * polar(amp_total, phase_of[1]); 242 // subtract the signal from the other channels 243 for (unsigned c = 0; c < C - 1; c++) 244 signal[c][f] *= (1 - lfe_level); 245 } 246 } 247 248 // shift the last 2/3 to the first 2/3 of the output buffer 249 memmove(&outbuf[0], &outbuf[C * N / 2], N * C * 4); 250 // and clear the rest 251 memset(&outbuf[C * N], 0, C * 4 * N / 2); 252 // backtransform each channel and overlap-add 253 for (unsigned c = 0; c < C; c++) 254 { 255 // back-transform into time domain 256 kiss_fftri(inverse, (kiss_fft_cpx*)&signal[c][0], &dst[0]); 257 // add the result to the last 2/3 of the output buffer, windowed (and remultiplex) 258 for (unsigned k = 0; k < N; k++) 259 outbuf[C * (k + N / 2) + c] = static_cast<float>(outbuf[C * (k + N / 2) + c] + (wnd[k] * dst[k])); 260 } 261 } 262 263 // transform amp/phase difference space into x/y soundfield space 264 void transform_decode(double a, double p, double& x, double& y) 265 { 266 x = clamp1(1.0047 * a + 0.46804 * a * p * p * p - 0.2042 * a * p * p * p * p + 267 0.0080586 * a * p * p * p * p * p * p * p - 0.0001526 * a * p * p * p * p * p * p * p * p * p * p - 268 0.073512 * a * a * a * p - 0.2499 * a * a * a * p * p * p * p + 269 0.016932 * a * a * a * p * p * p * p * p * p * p - 270 0.00027707 * a * a * a * p * p * p * p * p * p * p * p * p * p + 271 0.048105 * a * a * a * a * a * p * p * p * p * p * p * p - 272 0.0065947 * a * a * a * a * a * p * p * p * p * p * p * p * p * p * p + 273 0.0016006 * a * a * a * a * a * p * p * p * p * p * p * p * p * p * p * p - 274 0.0071132 * a * a * a * a * a * a * a * p * p * p * p * p * p * p * p * p + 275 0.0022336 * a * a * a * a * a * a * a * p * p * p * p * p * p * p * p * p * p * p - 276 0.0004804 * a * a * a * a * a * a * a * p * p * p * p * p * p * p * p * p * p * p * p); 277 y = clamp1(0.98592 - 0.62237 * p + 0.077875 * p * p - 0.0026929 * p * p * p * p * p + 0.4971 * a * a * p - 278 0.00032124 * a * a * p * p * p * p * p * p + 279 9.2491e-006 * a * a * a * a * p * p * p * p * p * p * p * p * p * p + 280 0.051549 * a * a * a * a * a * a * a * a + 1.0727e-014 * a * a * a * a * a * a * a * a * a * a); 281 } 282 283 // apply a circular_wrap transformation to some position 284 void transform_circular_wrap(double& x, double& y, double refangle) 285 { 286 if (refangle == 90) 287 return; 288 refangle = refangle * pi / 180; 289 double baseangle = 90 * pi / 180; 290 // translate into edge-normalized polar coordinates 291 double ang = atan2(x, y), len = sqrt(x * x + y * y); 292 len = len / edgedistance(ang); 293 // apply circular_wrap transform 294 if (abs(ang) < baseangle / 2) 295 // angle falls within the front region (to be enlarged) 296 ang *= refangle / baseangle; 297 else 298 // angle falls within the rear region (to be shrunken) 299 ang = pi - (-(((refangle - 2 * pi) * (pi - abs(ang)) * sign(ang)) / (2 * pi - baseangle))); 300 // translate back into soundfield position 301 len = len * edgedistance(ang); 302 x = clamp1(sin(ang) * len); 303 y = clamp1(cos(ang) * len); 304 } 305 306 // apply a focus transformation to some position 307 void transform_focus(double& x, double& y, double focus) 308 { 309 if (focus == 0) 310 return; 311 // translate into edge-normalized polar coordinates 312 double ang = atan2(x, y), len = clamp1(sqrt(x * x + y * y) / edgedistance(ang)); 313 // apply focus 314 len = focus > 0 ? 1 - pow(1 - len, 1 + focus * 20) : pow(len, 1 - focus * 20); 315 // back-transform into euclidian soundfield position 316 len = len * edgedistance(ang); 317 x = clamp1(sin(ang) * len); 318 y = clamp1(cos(ang) * len); 319 }