bbx_dsp/blocks/effectors/
panner.rs1#[cfg(feature = "simd")]
6use std::simd::{StdFloat, f64x4, num::SimdFloat};
7
8#[cfg(feature = "simd")]
9use crate::sample::SIMD_LANES;
10use crate::{
11 block::Block,
12 channel::{ChannelConfig, ChannelLayout},
13 context::DspContext,
14 graph::MAX_BLOCK_OUTPUTS,
15 parameter::{ModulationOutput, Parameter},
16 sample::Sample,
17 smoothing::LinearSmoothedValue,
18};
19
20const MAX_BUFFER_SIZE: usize = 4096;
22
23#[derive(Debug, Clone, Copy, PartialEq, Eq, Default)]
25pub enum PannerMode {
26 #[default]
28 Stereo,
29 Surround,
31 Ambisonic,
33}
34
35pub struct PannerBlock<S: Sample> {
49 pub position: Parameter<S>,
51
52 pub azimuth: Parameter<S>,
55
56 pub elevation: Parameter<S>,
59
60 mode: PannerMode,
61 output_layout: ChannelLayout,
62
63 position_smoother: LinearSmoothedValue<S>,
64 azimuth_smoother: LinearSmoothedValue<S>,
65 elevation_smoother: LinearSmoothedValue<S>,
66
67 speaker_azimuths: [f64; MAX_BLOCK_OUTPUTS],
68}
69
70impl<S: Sample> PannerBlock<S> {
71 pub fn new(position: f64) -> Self {
73 Self::new_stereo(position)
74 }
75
76 pub fn new_stereo(position: f64) -> Self {
78 let pos = S::from_f64(position);
79 Self {
80 position: Parameter::Constant(pos),
81 azimuth: Parameter::Constant(S::ZERO),
82 elevation: Parameter::Constant(S::ZERO),
83 mode: PannerMode::Stereo,
84 output_layout: ChannelLayout::Stereo,
85 position_smoother: LinearSmoothedValue::new(pos),
86 azimuth_smoother: LinearSmoothedValue::new(S::ZERO),
87 elevation_smoother: LinearSmoothedValue::new(S::ZERO),
88 speaker_azimuths: [0.0; MAX_BLOCK_OUTPUTS],
89 }
90 }
91
92 pub fn centered() -> Self {
94 Self::new_stereo(0.0)
95 }
96
97 pub fn new_surround(layout: ChannelLayout) -> Self {
102 let mut panner = Self {
103 position: Parameter::Constant(S::ZERO),
104 azimuth: Parameter::Constant(S::ZERO),
105 elevation: Parameter::Constant(S::ZERO),
106 mode: PannerMode::Surround,
107 output_layout: layout,
108 position_smoother: LinearSmoothedValue::new(S::ZERO),
109 azimuth_smoother: LinearSmoothedValue::new(S::ZERO),
110 elevation_smoother: LinearSmoothedValue::new(S::ZERO),
111 speaker_azimuths: [0.0; MAX_BLOCK_OUTPUTS],
112 };
113 panner.initialize_speaker_positions();
114 panner
115 }
116
117 pub fn new_ambisonic(order: usize) -> Self {
125 let layout = ChannelLayout::from_ambisonic_order(order).unwrap_or(ChannelLayout::AmbisonicFoa);
126
127 Self {
128 position: Parameter::Constant(S::ZERO),
129 azimuth: Parameter::Constant(S::ZERO),
130 elevation: Parameter::Constant(S::ZERO),
131 mode: PannerMode::Ambisonic,
132 output_layout: layout,
133 position_smoother: LinearSmoothedValue::new(S::ZERO),
134 azimuth_smoother: LinearSmoothedValue::new(S::ZERO),
135 elevation_smoother: LinearSmoothedValue::new(S::ZERO),
136 speaker_azimuths: [0.0; MAX_BLOCK_OUTPUTS],
137 }
138 }
139
140 fn initialize_speaker_positions(&mut self) {
142 match self.output_layout {
143 ChannelLayout::Surround51 => {
144 self.speaker_azimuths[0] = 30.0; self.speaker_azimuths[1] = -30.0; self.speaker_azimuths[2] = 0.0; self.speaker_azimuths[3] = 0.0; self.speaker_azimuths[4] = 110.0; self.speaker_azimuths[5] = -110.0; }
152 ChannelLayout::Surround71 => {
153 self.speaker_azimuths[0] = 30.0; self.speaker_azimuths[1] = -30.0; self.speaker_azimuths[2] = 0.0; self.speaker_azimuths[3] = 0.0; self.speaker_azimuths[4] = 90.0; self.speaker_azimuths[5] = -90.0; self.speaker_azimuths[6] = 150.0; self.speaker_azimuths[7] = -150.0; }
163 _ => {}
164 }
165 }
166
167 #[inline]
169 fn calculate_stereo_gains(&self, position: f64) -> (f64, f64) {
170 let normalized = (position + 100.0) / 200.0;
171 let normalized = normalized.clamp(0.0, 1.0);
172
173 let angle = normalized * S::FRAC_PI_2.to_f64();
174 let left_gain = angle.cos();
175 let right_gain = angle.sin();
176
177 (left_gain, right_gain)
178 }
179
180 fn calculate_vbap_gains(&self, azimuth_deg: f64, _elevation_deg: f64, gains: &mut [f64; MAX_BLOCK_OUTPUTS]) {
182 let num_speakers = self.output_layout.channel_count();
183 let azimuth_rad = azimuth_deg.to_radians();
184
185 for (i, gain) in gains.iter_mut().enumerate().take(num_speakers) {
186 if i == 3
187 && matches!(
188 self.output_layout,
189 ChannelLayout::Surround51 | ChannelLayout::Surround71
190 )
191 {
192 *gain = 0.0;
193 continue;
194 }
195
196 let speaker_rad = self.speaker_azimuths[i].to_radians();
197 let angle_diff = (azimuth_rad - speaker_rad).abs();
198 let angle_diff = if angle_diff > std::f64::consts::PI {
199 2.0 * std::f64::consts::PI - angle_diff
200 } else {
201 angle_diff
202 };
203
204 let spread = std::f64::consts::FRAC_PI_2;
205 if angle_diff < spread {
206 *gain = ((spread - angle_diff) / spread * std::f64::consts::FRAC_PI_2).cos();
207 *gain = gain.max(0.0);
208 } else {
209 *gain = 0.0;
210 }
211 }
212
213 let sum_sq: f64 = gains.iter().take(num_speakers).map(|g| g * g).sum();
214 if sum_sq > 1e-10 {
215 let norm = 1.0 / sum_sq.sqrt();
216 for gain in gains.iter_mut().take(num_speakers) {
217 *gain *= norm;
218 }
219 }
220 }
221
222 fn calculate_ambisonic_gains(&self, azimuth_deg: f64, elevation_deg: f64, gains: &mut [f64; MAX_BLOCK_OUTPUTS]) {
224 let az = azimuth_deg.to_radians();
225 let el = elevation_deg.to_radians();
226
227 let cos_el = el.cos();
228 let sin_el = el.sin();
229 let cos_az = az.cos();
230 let sin_az = az.sin();
231
232 let order = self.output_layout.ambisonic_order().unwrap_or(1);
233
234 gains[0] = 1.0; if order >= 1 {
238 gains[1] = cos_el * sin_az; gains[2] = sin_el; gains[3] = cos_el * cos_az; }
243
244 if order >= 2 {
245 let cos_2az = (2.0 * az).cos();
247 let sin_2az = (2.0 * az).sin();
248 let sin_2el = (2.0 * el).sin();
249 let cos_el_sq = cos_el * cos_el;
250
251 gains[4] = 0.8660254037844386 * cos_el_sq * sin_2az; gains[5] = 0.8660254037844386 * sin_2el * sin_az; gains[6] = 0.5 * (3.0 * sin_el * sin_el - 1.0); gains[7] = 0.8660254037844386 * sin_2el * cos_az; gains[8] = 0.8660254037844386 * cos_el_sq * cos_2az; }
257
258 if order >= 3 {
259 let cos_3az = (3.0 * az).cos();
261 let sin_3az = (3.0 * az).sin();
262 let cos_el_sq = cos_el * cos_el;
263 let cos_el_cu = cos_el_sq * cos_el;
264 let sin_el_sq = sin_el * sin_el;
265
266 gains[9] = 0.7905694150420949 * cos_el_cu * sin_3az; gains[10] = 1.9364916731037085 * cos_el_sq * sin_el * (2.0 * az).sin(); gains[11] = 0.6123724356957945 * cos_el * (5.0 * sin_el_sq - 1.0) * sin_az; gains[12] = 0.5 * sin_el * (5.0 * sin_el_sq - 3.0); gains[13] = 0.6123724356957945 * cos_el * (5.0 * sin_el_sq - 1.0) * cos_az; gains[14] = 1.9364916731037085 * cos_el_sq * sin_el * (2.0 * az).cos(); gains[15] = 0.7905694150420949 * cos_el_cu * cos_3az; }
274 }
275
276 fn process_stereo(&mut self, inputs: &[&[S]], outputs: &mut [&mut [S]], modulation_values: &[S]) {
277 if inputs.is_empty() || outputs.is_empty() {
278 return;
279 }
280
281 let target_position = self.position.get_value(modulation_values);
282 if (target_position - self.position_smoother.target()).abs() > S::EPSILON {
283 self.position_smoother.set_target_value(target_position);
284 }
285
286 let mono_in = inputs[0];
287 let has_stereo_output = outputs.len() > 1;
288
289 let num_samples = mono_in
290 .len()
291 .min(outputs.first().map(|o| o.len()).unwrap_or(0))
292 .min(MAX_BUFFER_SIZE);
293
294 let mut positions: [f64; MAX_BUFFER_SIZE] = [0.0; MAX_BUFFER_SIZE];
295 for position in positions.iter_mut().take(num_samples) {
296 *position = self.position_smoother.get_next_value().to_f64();
297 }
298
299 let mut left_gains: [S; MAX_BUFFER_SIZE] = [S::ZERO; MAX_BUFFER_SIZE];
300 let mut right_gains: [S; MAX_BUFFER_SIZE] = [S::ZERO; MAX_BUFFER_SIZE];
301
302 #[cfg(feature = "simd")]
303 {
304 let chunks = num_samples / SIMD_LANES;
305 let remainder_start = chunks * SIMD_LANES;
306
307 for chunk_idx in 0..chunks {
308 let offset = chunk_idx * SIMD_LANES;
309 let pos_vec = f64x4::from_slice(&positions[offset..]);
310
311 let normalized = ((pos_vec + f64x4::splat(100.0)) / f64x4::splat(200.0))
312 .simd_clamp(f64x4::splat(0.0), f64x4::splat(1.0));
313
314 let angle = normalized * f64x4::splat(S::FRAC_PI_2.to_f64());
315
316 let l_arr = angle.cos().to_array();
317 let r_arr = angle.sin().to_array();
318 for i in 0..SIMD_LANES {
319 left_gains[offset + i] = S::from_f64(l_arr[i]);
320 right_gains[offset + i] = S::from_f64(r_arr[i]);
321 }
322 }
323
324 for i in remainder_start..num_samples {
325 let (l, r) = self.calculate_stereo_gains(positions[i]);
326 left_gains[i] = S::from_f64(l);
327 right_gains[i] = S::from_f64(r);
328 }
329 }
330
331 #[cfg(not(feature = "simd"))]
332 {
333 for i in 0..num_samples {
334 let (l, r) = self.calculate_stereo_gains(positions[i]);
335 left_gains[i] = S::from_f64(l);
336 right_gains[i] = S::from_f64(r);
337 }
338 }
339
340 #[cfg(feature = "simd")]
341 {
342 let chunks = num_samples / SIMD_LANES;
343 let remainder_start = chunks * SIMD_LANES;
344
345 for chunk_idx in 0..chunks {
346 let offset = chunk_idx * SIMD_LANES;
347
348 let input = S::simd_from_slice(&mono_in[offset..]);
349 let l_gain = S::simd_from_slice(&left_gains[offset..]);
350 let l_out = input * l_gain;
351 outputs[0][offset..offset + SIMD_LANES].copy_from_slice(&S::simd_to_array(l_out));
352
353 if has_stereo_output {
354 let r_gain = S::simd_from_slice(&right_gains[offset..]);
355 let r_out = input * r_gain;
356 outputs[1][offset..offset + SIMD_LANES].copy_from_slice(&S::simd_to_array(r_out));
357 }
358 }
359
360 for i in remainder_start..num_samples {
361 outputs[0][i] = mono_in[i] * left_gains[i];
362 if has_stereo_output {
363 outputs[1][i] = mono_in[i] * right_gains[i];
364 }
365 }
366 }
367
368 #[cfg(not(feature = "simd"))]
369 {
370 for i in 0..num_samples {
371 outputs[0][i] = mono_in[i] * left_gains[i];
372 if has_stereo_output {
373 outputs[1][i] = mono_in[i] * right_gains[i];
374 }
375 }
376 }
377 }
378
379 fn process_surround(&mut self, inputs: &[&[S]], outputs: &mut [&mut [S]], modulation_values: &[S]) {
380 if inputs.is_empty() || outputs.is_empty() {
381 return;
382 }
383
384 let target_azimuth = self.azimuth.get_value(modulation_values);
385 let target_elevation = self.elevation.get_value(modulation_values);
386
387 if (target_azimuth - self.azimuth_smoother.target()).abs() > S::EPSILON {
388 self.azimuth_smoother.set_target_value(target_azimuth);
389 }
390 if (target_elevation - self.elevation_smoother.target()).abs() > S::EPSILON {
391 self.elevation_smoother.set_target_value(target_elevation);
392 }
393
394 let mono_in = inputs[0];
395 let num_outputs = self.output_layout.channel_count().min(outputs.len());
396 let num_samples = mono_in.len().min(outputs[0].len()).min(MAX_BUFFER_SIZE);
397
398 let mut gains: [f64; MAX_BLOCK_OUTPUTS] = [0.0; MAX_BLOCK_OUTPUTS];
399
400 for (i, &input_sample) in mono_in.iter().enumerate().take(num_samples) {
401 let azimuth = self.azimuth_smoother.get_next_value().to_f64();
402 let elevation = self.elevation_smoother.get_next_value().to_f64();
403
404 self.calculate_vbap_gains(azimuth, elevation, &mut gains);
405
406 for ch in 0..num_outputs {
407 outputs[ch][i] = input_sample * S::from_f64(gains[ch]);
408 }
409 }
410 }
411
412 fn process_ambisonic(&mut self, inputs: &[&[S]], outputs: &mut [&mut [S]], modulation_values: &[S]) {
413 if inputs.is_empty() || outputs.is_empty() {
414 return;
415 }
416
417 let target_azimuth = self.azimuth.get_value(modulation_values);
418 let target_elevation = self.elevation.get_value(modulation_values);
419
420 if (target_azimuth - self.azimuth_smoother.target()).abs() > S::EPSILON {
421 self.azimuth_smoother.set_target_value(target_azimuth);
422 }
423 if (target_elevation - self.elevation_smoother.target()).abs() > S::EPSILON {
424 self.elevation_smoother.set_target_value(target_elevation);
425 }
426
427 let mono_in = inputs[0];
428 let num_outputs = self.output_layout.channel_count().min(outputs.len());
429 let num_samples = mono_in.len().min(outputs[0].len()).min(MAX_BUFFER_SIZE);
430
431 let mut gains: [f64; MAX_BLOCK_OUTPUTS] = [0.0; MAX_BLOCK_OUTPUTS];
432
433 for (i, &input_sample) in mono_in.iter().enumerate().take(num_samples) {
434 let azimuth = self.azimuth_smoother.get_next_value().to_f64();
435 let elevation = self.elevation_smoother.get_next_value().to_f64();
436
437 self.calculate_ambisonic_gains(azimuth, elevation, &mut gains);
438
439 for ch in 0..num_outputs {
440 outputs[ch][i] = input_sample * S::from_f64(gains[ch]);
441 }
442 }
443 }
444}
445
446impl<S: Sample> Block<S> for PannerBlock<S> {
447 fn process(&mut self, inputs: &[&[S]], outputs: &mut [&mut [S]], modulation_values: &[S], _context: &DspContext) {
448 match self.mode {
449 PannerMode::Stereo => self.process_stereo(inputs, outputs, modulation_values),
450 PannerMode::Surround => self.process_surround(inputs, outputs, modulation_values),
451 PannerMode::Ambisonic => self.process_ambisonic(inputs, outputs, modulation_values),
452 }
453 }
454
455 #[inline]
456 fn input_count(&self) -> usize {
457 1
458 }
459
460 #[inline]
461 fn output_count(&self) -> usize {
462 self.output_layout.channel_count()
463 }
464
465 #[inline]
466 fn modulation_outputs(&self) -> &[ModulationOutput] {
467 &[]
468 }
469
470 #[inline]
471 fn channel_config(&self) -> ChannelConfig {
472 ChannelConfig::Explicit
473 }
474
475 fn set_smoothing(&mut self, sample_rate: f64, ramp_time_ms: f64) {
476 self.position_smoother.reset(sample_rate, ramp_time_ms);
477 self.azimuth_smoother.reset(sample_rate, ramp_time_ms);
478 self.elevation_smoother.reset(sample_rate, ramp_time_ms);
479 }
480}
481
482#[cfg(test)]
483mod tests {
484 use super::*;
485 use crate::context::DspContext;
486
487 fn test_context(buffer_size: usize) -> DspContext {
488 DspContext {
489 sample_rate: 44100.0,
490 num_channels: 2,
491 buffer_size,
492 current_sample: 0,
493 channel_layout: ChannelLayout::Stereo,
494 }
495 }
496
497 #[test]
498 fn test_stereo_panner_centered() {
499 let mut panner = PannerBlock::<f32>::centered();
500 let context = test_context(4);
501
502 let input = [1.0f32; 4];
503 let mut left_out = [0.0f32; 4];
504 let mut right_out = [0.0f32; 4];
505
506 let inputs: [&[f32]; 1] = [&input];
507 let mut outputs: [&mut [f32]; 2] = [&mut left_out, &mut right_out];
508
509 panner.process(&inputs, &mut outputs, &[], &context);
510
511 let expected_gain = (std::f32::consts::FRAC_PI_4).cos();
512 for i in 0..4 {
513 assert!((left_out[i] - expected_gain).abs() < 0.01);
514 assert!((right_out[i] - expected_gain).abs() < 0.01);
515 }
516 }
517
518 #[test]
519 fn test_stereo_panner_full_left() {
520 let mut panner = PannerBlock::<f32>::new(-100.0);
521 let context = test_context(4);
522
523 let input = [1.0f32; 4];
524 let mut left_out = [0.0f32; 4];
525 let mut right_out = [0.0f32; 4];
526
527 let inputs: [&[f32]; 1] = [&input];
528 let mut outputs: [&mut [f32]; 2] = [&mut left_out, &mut right_out];
529
530 panner.process(&inputs, &mut outputs, &[], &context);
531
532 for i in 0..4 {
533 assert!((left_out[i] - 1.0).abs() < 0.01);
534 assert!(right_out[i].abs() < 0.01);
535 }
536 }
537
538 #[test]
539 fn test_surround_panner_output_count() {
540 let panner_51 = PannerBlock::<f32>::new_surround(ChannelLayout::Surround51);
541 assert_eq!(panner_51.output_count(), 6);
542
543 let panner_71 = PannerBlock::<f32>::new_surround(ChannelLayout::Surround71);
544 assert_eq!(panner_71.output_count(), 8);
545 }
546
547 #[test]
548 fn test_ambisonic_panner_output_count() {
549 let panner_foa = PannerBlock::<f32>::new_ambisonic(1);
550 assert_eq!(panner_foa.output_count(), 4);
551
552 let panner_soa = PannerBlock::<f32>::new_ambisonic(2);
553 assert_eq!(panner_soa.output_count(), 9);
554
555 let panner_toa = PannerBlock::<f32>::new_ambisonic(3);
556 assert_eq!(panner_toa.output_count(), 16);
557 }
558
559 #[test]
560 fn test_panner_input_count_is_mono() {
561 let stereo = PannerBlock::<f32>::centered();
562 let surround = PannerBlock::<f32>::new_surround(ChannelLayout::Surround51);
563 let ambisonic = PannerBlock::<f32>::new_ambisonic(1);
564
565 assert_eq!(stereo.input_count(), 1);
566 assert_eq!(surround.input_count(), 1);
567 assert_eq!(ambisonic.input_count(), 1);
568 }
569
570 #[test]
571 fn test_panner_channel_config_is_explicit() {
572 let stereo = PannerBlock::<f32>::centered();
573 let surround = PannerBlock::<f32>::new_surround(ChannelLayout::Surround51);
574 let ambisonic = PannerBlock::<f32>::new_ambisonic(1);
575
576 assert_eq!(stereo.channel_config(), ChannelConfig::Explicit);
577 assert_eq!(surround.channel_config(), ChannelConfig::Explicit);
578 assert_eq!(ambisonic.channel_config(), ChannelConfig::Explicit);
579 }
580
581 #[test]
582 fn test_ambisonic_front_encoding() {
583 let mut panner = PannerBlock::<f32>::new_ambisonic(1);
584 let context = test_context(1);
585
586 let input = [1.0f32];
587 let mut w = [0.0f32];
588 let mut y = [0.0f32];
589 let mut z = [0.0f32];
590 let mut x = [0.0f32];
591
592 let inputs: [&[f32]; 1] = [&input];
593 let mut outputs: [&mut [f32]; 4] = [&mut w, &mut y, &mut z, &mut x];
594
595 panner.process(&inputs, &mut outputs, &[], &context);
596
597 assert!((w[0] - 1.0).abs() < 0.01, "W should be 1.0 for front");
598 assert!(y[0].abs() < 0.01, "Y should be 0 for front");
599 assert!(z[0].abs() < 0.01, "Z should be 0 for horizon");
600 assert!((x[0] - 1.0).abs() < 0.01, "X should be 1.0 for front");
601 }
602}