]>
Commit | Line | Data |
---|---|---|
dcbb8668 KS |
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 | } | |
d3c13e0e | 82 | let wval = f64::from((pos * win_scale).max(-1.0).min(1.0)); |
dcbb8668 KS |
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(); | |
84082d37 | 265 | let mut no_conversion = src_fmt == dst_fmt; |
dcbb8668 | 266 | |
84082d37 KS |
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 { | |
dcbb8668 KS |
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 | } |