core/soundcvt: add resampling support
[nihav.git] / nihav-core / src / soundcvt / resample.rs
CommitLineData
dcbb8668
KS
1use crate::formats::{NAChannelMap, NAChannelType};
2use crate::frame::{NAAudioInfo, NABufferType};
3use super::*;
4
5#[derive(Clone,Copy,Default)]
6struct FracPos {
7 ipos: usize,
8 frac: usize,
9}
10
11impl 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)]
23struct Fraction {
24 num: usize,
25 den: usize,
26}
27
28impl 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
48fn 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
62trait Sinc {
63 fn sinc(self) -> Self;
64}
65
66impl Sinc for f32 {
67 fn sinc(self) -> Self { self.sin() / self }
68}
69
70const BESSEL_BETA: f64 = 8.0;
71fn 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).min(-1.0).max(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.
97pub 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
112impl 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 no_conversion = src_fmt == dst_fmt;
266
267 if no_conversion && no_channel_needs {
268 return Ok(src.clone());
269 }
270
271 let dst_nsamples = if !needs_resample {
272 nsamples
273 } else {
274 self.estimate_output(nsamples)
275 };
276 let ret = alloc_audio_buffer(self.dst_info, dst_nsamples, self.dst_chmap.clone());
277 if ret.is_err() {
278 return Err(SoundConvertError::AllocError);
279 }
280 let mut dst_buf = ret.unwrap();
281
282 let sstep = src.get_audio_step().max(1);
283 let dstep = dst_buf.get_audio_step().max(1);
284 let sr: Box<dyn SampleReader> = match src {
285 NABufferType::AudioU8(ref ab) => {
286 let stride = ab.get_stride();
287 let data = ab.get_data();
288 if !src_fmt.signed {
289 Box::new(GenericSampleReader { data, stride })
290 } else {
291 Box::new(S8SampleReader { data, stride })
292 }
293 },
294 NABufferType::AudioI16(ref ab) => {
295 let data = ab.get_data();
296 let stride = ab.get_stride();
297 Box::new(GenericSampleReader { data, stride })
298 },
299 NABufferType::AudioI32(ref ab) => {
300 let data = ab.get_data();
301 let stride = ab.get_stride();
302 Box::new(GenericSampleReader { data, stride })
303 },
304 NABufferType::AudioF32(ref ab) => {
305 let data = ab.get_data();
306 let stride = ab.get_stride();
307 Box::new(GenericSampleReader { data, stride })
308 },
309 NABufferType::AudioPacked(ref ab) => {
310 let data = ab.get_data();
311 Box::new(PackedSampleReader::new(data, src_fmt))
312 },
313 _ => unimplemented!(),
314 };
315 let mut sw: Box<dyn SampleWriter> = match dst_buf {
316 NABufferType::AudioU8(ref mut ab) => {
317 let stride = ab.get_stride();
318 let data = ab.get_data_mut().unwrap();
319 Box::new(GenericSampleWriter { data, stride })
320 },
321 NABufferType::AudioI16(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::AudioI32(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::AudioF32(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::AudioPacked(ref mut ab) => {
337 let data = ab.get_data_mut().unwrap();
338 Box::new(PackedSampleWriter::new(data, dst_fmt))
339 },
340 _ => unimplemented!(),
341 };
342
343 let into_float = dst_fmt.float;
344 let mut new_len = 0;
345 if !into_float {
346 let mut svec = vec![0; src_chmap.num_channels()];
347 let mut dvec = vec![0; self.dst_chmap.num_channels()];
348 let mut tvec = vec![0; self.dst_chmap.num_channels()];
349 let mut spos = 0;
350 let mut dpos = 0;
351 for _ in 0..nsamples {
352 sr.get_samples_i32(spos, &mut svec);
353 if !channel_op.is_remix() {
354 apply_channel_op(&channel_op, &svec, &mut dvec);
355 } else {
356 remix_i32(&channel_op, &svec, &mut dvec);
357 }
358 if !needs_resample {
359 sw.store_samples_i32(dpos, &dvec);
360 dpos += dstep;
361 } else {
362 while self.get_samples_i32(&mut tvec) {
363 sw.store_samples_i32(dpos, &tvec);
364 dpos += dstep;
365 }
366 self.add_samples_i32(&dvec);
367 }
368 spos += sstep;
369 }
370 if needs_resample {
371 while self.get_samples_i32(&mut tvec) {
372 sw.store_samples_i32(dpos, &tvec);
373 dpos += dstep;
374 }
375 new_len = dpos / dstep;
376 }
377 } else {
378 let mut svec = vec![0.0; src_chmap.num_channels()];
379 let mut dvec = vec![0.0; self.dst_chmap.num_channels()];
380 let mut tvec = vec![0.0; self.dst_chmap.num_channels()];
381 let mut spos = 0;
382 let mut dpos = 0;
383 for _ in 0..nsamples {
384 sr.get_samples_f32(spos, &mut svec);
385 if !channel_op.is_remix() {
386 apply_channel_op(&channel_op, &svec, &mut dvec);
387 } else {
388 remix_f32(&channel_op, &svec, &mut dvec);
389 }
390 if !needs_resample {
391 sw.store_samples_f32(dpos, &dvec);
392 dpos += dstep;
393 } else {
394 while self.get_samples_f32(&mut tvec) {
395 sw.store_samples_f32(dpos, &tvec);
396 dpos += dstep;
397 }
398 self.add_samples_f32(&dvec);
399 }
400 spos += sstep;
401 }
402 if needs_resample {
403 while self.get_samples_f32(&mut tvec) {
404 sw.store_samples_f32(dpos, &tvec);
405 dpos += dstep;
406 }
407 new_len = dpos / dstep;
408 }
409 }
410 drop(sw);
411
412 if new_len != 0 {
413 dst_buf.truncate_audio(new_len);
414 }
415
416 Ok(dst_buf)
417 }
418}