| 1 | use crate::formats::{NAChannelMap, NAChannelType}; |
| 2 | use crate::frame::{NAAudioInfo, NABufferType}; |
| 3 | use super::*; |
| 4 | |
| 5 | #[derive(Clone,Copy,Default)] |
| 6 | struct FracPos { |
| 7 | ipos: usize, |
| 8 | frac: usize, |
| 9 | } |
| 10 | |
| 11 | impl FracPos { |
| 12 | fn new() -> Self { Self::default() } |
| 13 | fn add(&mut self, frac: Fraction) { |
| 14 | self.frac += frac.num; |
| 15 | while self.frac >= frac.den { |
| 16 | self.frac -= frac.den; |
| 17 | self.ipos += 1; |
| 18 | } |
| 19 | } |
| 20 | } |
| 21 | |
| 22 | #[derive(Clone,Copy,PartialEq)] |
| 23 | struct Fraction { |
| 24 | num: usize, |
| 25 | den: usize, |
| 26 | } |
| 27 | |
| 28 | impl Fraction { |
| 29 | fn new(a: usize, b: usize) -> Self { |
| 30 | let cur_gcd = Self::gcd(a, b); |
| 31 | Self { |
| 32 | num: a / cur_gcd, |
| 33 | den: b / cur_gcd, |
| 34 | } |
| 35 | } |
| 36 | fn gcd(mut a: usize, mut b: usize) -> usize { |
| 37 | while a != 0 && b != 0 { |
| 38 | if a >= b { |
| 39 | a -= b; |
| 40 | } else { |
| 41 | b -= a; |
| 42 | } |
| 43 | } |
| 44 | a.max(b) |
| 45 | } |
| 46 | } |
| 47 | |
| 48 | fn bessel_i0(mut x: f64) -> f64 { |
| 49 | let mut i0 = 1.0; |
| 50 | let mut ival = 1.0; |
| 51 | let mut n = 1.0; |
| 52 | x = x * x * 0.5; |
| 53 | while ival > 1e-10 { |
| 54 | ival *= x; |
| 55 | ival /= n * n; |
| 56 | n += 1.0; |
| 57 | i0 += ival; |
| 58 | } |
| 59 | i0 |
| 60 | } |
| 61 | |
| 62 | trait Sinc { |
| 63 | fn sinc(self) -> Self; |
| 64 | } |
| 65 | |
| 66 | impl Sinc for f32 { |
| 67 | fn sinc(self) -> Self { self.sin() / self } |
| 68 | } |
| 69 | |
| 70 | const BESSEL_BETA: f64 = 8.0; |
| 71 | fn gen_sinc_coeffs(order: usize, num: usize, den: usize, norm: f32) -> Vec<f32> { |
| 72 | let mut coeffs = vec![0.0; order * 2]; |
| 73 | |
| 74 | let sinc_scale = std::f32::consts::PI * norm / (den as f32); |
| 75 | let win_scale = 1.0 / ((order * den) as f32); |
| 76 | for (i, coef) in coeffs.iter_mut().enumerate() { |
| 77 | let pos = (((i as i32) - (order as i32)) * (den as i32) + (num as i32)) as f32; |
| 78 | if pos == 0.0 { |
| 79 | *coef = norm; |
| 80 | continue; |
| 81 | } |
| 82 | let wval = f64::from((pos * win_scale).max(-1.0).min(1.0)); |
| 83 | let win = bessel_i0(BESSEL_BETA - (1.0 - wval * wval).sqrt()) as f32; |
| 84 | *coef = norm * (pos * sinc_scale).sinc() * win; |
| 85 | } |
| 86 | |
| 87 | // norm |
| 88 | let sum = coeffs.iter().fold(0.0f32, |acc, &x| acc + x) * norm; |
| 89 | for el in coeffs.iter_mut() { |
| 90 | *el /= sum; |
| 91 | } |
| 92 | |
| 93 | coeffs |
| 94 | } |
| 95 | |
| 96 | /// Context for continuous audio resampling and format conversion. |
| 97 | pub struct NAResample { |
| 98 | coeffs: Vec<Vec<f32>>, |
| 99 | ratio: Fraction, |
| 100 | order: usize, |
| 101 | |
| 102 | pos: FracPos, |
| 103 | hist_i: Vec<Vec<i32>>, |
| 104 | hist_f: Vec<Vec<f32>>, |
| 105 | deficit: usize, |
| 106 | |
| 107 | dst_info: NAAudioInfo, |
| 108 | dst_chmap: NAChannelMap, |
| 109 | src_rate: u32, |
| 110 | } |
| 111 | |
| 112 | impl NAResample { |
| 113 | pub fn new(src_rate: u32, dst_info: &NAAudioInfo, dst_chmap: &NAChannelMap, order: usize) -> Self { |
| 114 | let ratio = Fraction::new(src_rate as usize, dst_info.sample_rate as usize); |
| 115 | let nchannels = usize::from(dst_info.channels); |
| 116 | let mut coeffs = Vec::with_capacity(ratio.den); |
| 117 | let norm = if ratio.num < ratio.den { 1.0 } else { (ratio.den as f32) / (ratio.num as f32) }; |
| 118 | for i in 0..ratio.den { |
| 119 | coeffs.push(gen_sinc_coeffs(order, i, ratio.den, norm)); |
| 120 | } |
| 121 | |
| 122 | let mut hist_i = Vec::with_capacity(nchannels); |
| 123 | let mut hist_f = Vec::with_capacity(nchannels); |
| 124 | if !dst_info.format.is_float() { |
| 125 | for _ in 0..(order * 2) { |
| 126 | hist_i.push(vec![0; nchannels]); |
| 127 | } |
| 128 | } else { |
| 129 | for _ in 0..(order * 2) { |
| 130 | hist_f.push(vec![0.0; nchannels]); |
| 131 | } |
| 132 | } |
| 133 | |
| 134 | Self { |
| 135 | order, coeffs, ratio, |
| 136 | pos: FracPos::new(), |
| 137 | hist_i, hist_f, |
| 138 | deficit: 0, |
| 139 | |
| 140 | dst_info: *dst_info, |
| 141 | dst_chmap: dst_chmap.clone(), |
| 142 | src_rate, |
| 143 | } |
| 144 | } |
| 145 | fn reinit(&mut self, src_rate: u32) { |
| 146 | self.src_rate = src_rate; |
| 147 | self.ratio = Fraction::new(src_rate as usize, self.dst_info.sample_rate as usize); |
| 148 | self.coeffs.clear(); |
| 149 | let norm = if self.ratio.num < self.ratio.den { 1.0 } else { (self.ratio.den as f32) / (self.ratio.num as f32) }; |
| 150 | for i in 0..self.ratio.den { |
| 151 | self.coeffs.push(gen_sinc_coeffs(self.order, i, self.ratio.den, norm)); |
| 152 | } |
| 153 | self.pos = FracPos::new(); |
| 154 | } |
| 155 | fn estimate_output(&self, nsamples: usize) -> usize { |
| 156 | ((nsamples + 1) * self.ratio.den - self.pos.frac) / self.ratio.num + 16 |
| 157 | } |
| 158 | fn add_samples_i32(&mut self, samples: &[i32]) -> bool { |
| 159 | if self.deficit == 0 { |
| 160 | return false; |
| 161 | } |
| 162 | self.deficit -= 1; |
| 163 | self.hist_i[self.deficit].copy_from_slice(samples); |
| 164 | true |
| 165 | } |
| 166 | fn get_samples_i32(&mut self, samples: &mut [i32]) -> bool { |
| 167 | if self.deficit != 0 { |
| 168 | return false; |
| 169 | } |
| 170 | let ret = self.pos.ipos == 0; |
| 171 | if self.pos.ipos == 0 { |
| 172 | for (i, dst) in samples.iter_mut().enumerate() { |
| 173 | let mut sum = 0.0; |
| 174 | for (hist, &coef) in self.hist_i.iter().zip(self.coeffs[self.pos.frac].iter()) { |
| 175 | sum += (hist[i] as f32) * coef; |
| 176 | } |
| 177 | *dst = sum as i32; |
| 178 | } |
| 179 | self.pos.add(self.ratio); |
| 180 | } |
| 181 | if self.pos.ipos > 0 { |
| 182 | self.deficit = self.pos.ipos.min(self.hist_i.len()); |
| 183 | for i in (0..(self.hist_i.len() - self.deficit)).rev() { |
| 184 | self.hist_i.swap(i, i + self.deficit) |
| 185 | } |
| 186 | self.pos.ipos -= self.deficit; |
| 187 | } |
| 188 | ret |
| 189 | } |
| 190 | fn add_samples_f32(&mut self, samples: &[f32]) -> bool { |
| 191 | if self.deficit == 0 { |
| 192 | return false; |
| 193 | } |
| 194 | self.deficit -= 1; |
| 195 | self.hist_f[self.deficit].copy_from_slice(samples); |
| 196 | true |
| 197 | } |
| 198 | fn get_samples_f32(&mut self, samples: &mut [f32]) -> bool { |
| 199 | if self.deficit != 0 { |
| 200 | return false; |
| 201 | } |
| 202 | let ret = self.pos.ipos == 0; |
| 203 | if self.pos.ipos == 0 { |
| 204 | for (i, dst) in samples.iter_mut().enumerate() { |
| 205 | let mut sum = 0.0; |
| 206 | for (hist, &coef) in self.hist_f.iter().zip(self.coeffs[self.pos.frac].iter()) { |
| 207 | sum += hist[i]* coef; |
| 208 | } |
| 209 | *dst = sum; |
| 210 | } |
| 211 | self.pos.add(self.ratio); |
| 212 | } |
| 213 | if self.pos.ipos > 0 { |
| 214 | self.deficit = self.pos.ipos.min(self.hist_f.len()); |
| 215 | for i in (0..(self.hist_f.len() - self.deficit)).rev() { |
| 216 | self.hist_f.swap(i, i + self.deficit) |
| 217 | } |
| 218 | self.pos.ipos -= self.deficit; |
| 219 | } |
| 220 | ret |
| 221 | } |
| 222 | pub fn convert_audio_frame(&mut self, src: &NABufferType) -> Result<NABufferType, SoundConvertError> { |
| 223 | let nsamples = src.get_audio_length(); |
| 224 | if nsamples == 0 { |
| 225 | return Err(SoundConvertError::InvalidInput); |
| 226 | } |
| 227 | let src_chmap = src.get_chmap().unwrap(); |
| 228 | let src_info = src.get_audio_info().unwrap(); |
| 229 | if (src_chmap.num_channels() == 0) || (self.dst_chmap.num_channels() == 0) { |
| 230 | return Err(SoundConvertError::InvalidInput); |
| 231 | } |
| 232 | |
| 233 | let needs_remix = src_chmap.num_channels() != self.dst_chmap.num_channels(); |
| 234 | let no_channel_needs = !needs_remix && channel_maps_equal(src_chmap, &self.dst_chmap); |
| 235 | let needs_reorder = !needs_remix && !no_channel_needs && channel_maps_reordered(src_chmap, &self.dst_chmap); |
| 236 | |
| 237 | if src_info.sample_rate != self.src_rate { |
| 238 | self.reinit(src_info.sample_rate); |
| 239 | } |
| 240 | let needs_resample = src_info.sample_rate != self.dst_info.sample_rate; |
| 241 | |
| 242 | let channel_op = if no_channel_needs { |
| 243 | ChannelOp::Passthrough |
| 244 | } else if needs_reorder { |
| 245 | let reorder_mat = calculate_reorder_matrix(src_chmap, &self.dst_chmap); |
| 246 | ChannelOp::Reorder(reorder_mat) |
| 247 | } else if src_chmap.num_channels() > 1 { |
| 248 | let remix_mat = calculate_remix_matrix(src_chmap, &self.dst_chmap); |
| 249 | ChannelOp::Remix(remix_mat) |
| 250 | } else { |
| 251 | let mut dup_mat: Vec<bool> = Vec::with_capacity(self.dst_chmap.num_channels()); |
| 252 | for i in 0..self.dst_chmap.num_channels() { |
| 253 | let ch = self.dst_chmap.get_channel(i); |
| 254 | if ch.is_left() || ch.is_right() || ch == NAChannelType::C { |
| 255 | dup_mat.push(true); |
| 256 | } else { |
| 257 | dup_mat.push(false); |
| 258 | } |
| 259 | } |
| 260 | ChannelOp::DupMono(dup_mat) |
| 261 | }; |
| 262 | |
| 263 | let src_fmt = src_info.get_format(); |
| 264 | let dst_fmt = self.dst_info.get_format(); |
| 265 | let mut no_conversion = src_fmt == dst_fmt; |
| 266 | |
| 267 | // packed PCM needs to be processed |
| 268 | if no_conversion && matches!(src, NABufferType::AudioPacked(_)) && !src_fmt.is_packed() && ((src_fmt.bits % 8) == 0) { |
| 269 | no_conversion = false; |
| 270 | } |
| 271 | |
| 272 | if no_conversion && no_channel_needs && !needs_resample { |
| 273 | return Ok(src.clone()); |
| 274 | } |
| 275 | |
| 276 | let dst_nsamples = if !needs_resample { |
| 277 | nsamples |
| 278 | } else { |
| 279 | self.estimate_output(nsamples) |
| 280 | }; |
| 281 | let ret = alloc_audio_buffer(self.dst_info, dst_nsamples, self.dst_chmap.clone()); |
| 282 | if ret.is_err() { |
| 283 | return Err(SoundConvertError::AllocError); |
| 284 | } |
| 285 | let mut dst_buf = ret.unwrap(); |
| 286 | |
| 287 | let sstep = src.get_audio_step().max(1); |
| 288 | let dstep = dst_buf.get_audio_step().max(1); |
| 289 | let sr: Box<dyn SampleReader> = match src { |
| 290 | NABufferType::AudioU8(ref ab) => { |
| 291 | let stride = ab.get_stride(); |
| 292 | let data = ab.get_data(); |
| 293 | if !src_fmt.signed { |
| 294 | Box::new(GenericSampleReader { data, stride }) |
| 295 | } else { |
| 296 | Box::new(S8SampleReader { data, stride }) |
| 297 | } |
| 298 | }, |
| 299 | NABufferType::AudioI16(ref ab) => { |
| 300 | let data = ab.get_data(); |
| 301 | let stride = ab.get_stride(); |
| 302 | Box::new(GenericSampleReader { data, stride }) |
| 303 | }, |
| 304 | NABufferType::AudioI32(ref ab) => { |
| 305 | let data = ab.get_data(); |
| 306 | let stride = ab.get_stride(); |
| 307 | Box::new(GenericSampleReader { data, stride }) |
| 308 | }, |
| 309 | NABufferType::AudioF32(ref ab) => { |
| 310 | let data = ab.get_data(); |
| 311 | let stride = ab.get_stride(); |
| 312 | Box::new(GenericSampleReader { data, stride }) |
| 313 | }, |
| 314 | NABufferType::AudioPacked(ref ab) => { |
| 315 | let data = ab.get_data(); |
| 316 | Box::new(PackedSampleReader::new(data, src_fmt)) |
| 317 | }, |
| 318 | _ => unimplemented!(), |
| 319 | }; |
| 320 | let mut sw: Box<dyn SampleWriter> = match dst_buf { |
| 321 | NABufferType::AudioU8(ref mut ab) => { |
| 322 | let stride = ab.get_stride(); |
| 323 | let data = ab.get_data_mut().unwrap(); |
| 324 | Box::new(GenericSampleWriter { data, stride }) |
| 325 | }, |
| 326 | NABufferType::AudioI16(ref mut ab) => { |
| 327 | let stride = ab.get_stride(); |
| 328 | let data = ab.get_data_mut().unwrap(); |
| 329 | Box::new(GenericSampleWriter { data, stride }) |
| 330 | }, |
| 331 | NABufferType::AudioI32(ref mut ab) => { |
| 332 | let stride = ab.get_stride(); |
| 333 | let data = ab.get_data_mut().unwrap(); |
| 334 | Box::new(GenericSampleWriter { data, stride }) |
| 335 | }, |
| 336 | NABufferType::AudioF32(ref mut ab) => { |
| 337 | let stride = ab.get_stride(); |
| 338 | let data = ab.get_data_mut().unwrap(); |
| 339 | Box::new(GenericSampleWriter { data, stride }) |
| 340 | }, |
| 341 | NABufferType::AudioPacked(ref mut ab) => { |
| 342 | let data = ab.get_data_mut().unwrap(); |
| 343 | Box::new(PackedSampleWriter::new(data, dst_fmt)) |
| 344 | }, |
| 345 | _ => unimplemented!(), |
| 346 | }; |
| 347 | |
| 348 | let into_float = dst_fmt.float; |
| 349 | let mut new_len = 0; |
| 350 | if !into_float { |
| 351 | let mut svec = vec![0; src_chmap.num_channels()]; |
| 352 | let mut dvec = vec![0; self.dst_chmap.num_channels()]; |
| 353 | let mut tvec = vec![0; self.dst_chmap.num_channels()]; |
| 354 | let mut spos = 0; |
| 355 | let mut dpos = 0; |
| 356 | for _ in 0..nsamples { |
| 357 | sr.get_samples_i32(spos, &mut svec); |
| 358 | if !channel_op.is_remix() { |
| 359 | apply_channel_op(&channel_op, &svec, &mut dvec); |
| 360 | } else { |
| 361 | remix_i32(&channel_op, &svec, &mut dvec); |
| 362 | } |
| 363 | if !needs_resample { |
| 364 | sw.store_samples_i32(dpos, &dvec); |
| 365 | dpos += dstep; |
| 366 | } else { |
| 367 | while self.get_samples_i32(&mut tvec) { |
| 368 | sw.store_samples_i32(dpos, &tvec); |
| 369 | dpos += dstep; |
| 370 | } |
| 371 | self.add_samples_i32(&dvec); |
| 372 | } |
| 373 | spos += sstep; |
| 374 | } |
| 375 | if needs_resample { |
| 376 | while self.get_samples_i32(&mut tvec) { |
| 377 | sw.store_samples_i32(dpos, &tvec); |
| 378 | dpos += dstep; |
| 379 | } |
| 380 | new_len = dpos / dstep; |
| 381 | } |
| 382 | } else { |
| 383 | let mut svec = vec![0.0; src_chmap.num_channels()]; |
| 384 | let mut dvec = vec![0.0; self.dst_chmap.num_channels()]; |
| 385 | let mut tvec = vec![0.0; self.dst_chmap.num_channels()]; |
| 386 | let mut spos = 0; |
| 387 | let mut dpos = 0; |
| 388 | for _ in 0..nsamples { |
| 389 | sr.get_samples_f32(spos, &mut svec); |
| 390 | if !channel_op.is_remix() { |
| 391 | apply_channel_op(&channel_op, &svec, &mut dvec); |
| 392 | } else { |
| 393 | remix_f32(&channel_op, &svec, &mut dvec); |
| 394 | } |
| 395 | if !needs_resample { |
| 396 | sw.store_samples_f32(dpos, &dvec); |
| 397 | dpos += dstep; |
| 398 | } else { |
| 399 | while self.get_samples_f32(&mut tvec) { |
| 400 | sw.store_samples_f32(dpos, &tvec); |
| 401 | dpos += dstep; |
| 402 | } |
| 403 | self.add_samples_f32(&dvec); |
| 404 | } |
| 405 | spos += sstep; |
| 406 | } |
| 407 | if needs_resample { |
| 408 | while self.get_samples_f32(&mut tvec) { |
| 409 | sw.store_samples_f32(dpos, &tvec); |
| 410 | dpos += dstep; |
| 411 | } |
| 412 | new_len = dpos / dstep; |
| 413 | } |
| 414 | } |
| 415 | drop(sw); |
| 416 | |
| 417 | if new_len != 0 { |
| 418 | dst_buf.truncate_audio(new_len); |
| 419 | } |
| 420 | |
| 421 | Ok(dst_buf) |
| 422 | } |
| 423 | } |