1use crate::{Error, Position3D, Result};
7use scirs2_core::ndarray::{Array1, Array2, Axis};
8use scirs2_core::Complex32;
9use serde::{Deserialize, Serialize};
10use std::f32::consts::PI;
11
12pub type AmbisonicsOrder = u32;
14
15pub fn channel_count(order: AmbisonicsOrder) -> usize {
17 ((order + 1) * (order + 1)) as usize
18}
19
20#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)]
22pub enum NormalizationScheme {
23 N3D,
25 SN3D,
27 FuMa,
29 MaxN,
31}
32
33#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)]
35pub enum ChannelOrdering {
36 ACN,
38 FuMa,
40 SID,
42}
43
44#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)]
46pub struct SphericalCoordinate {
47 pub azimuth: f32,
49 pub elevation: f32,
51 pub distance: f32,
53}
54
55impl SphericalCoordinate {
56 pub fn new(azimuth: f32, elevation: f32, distance: f32) -> Self {
58 Self {
59 azimuth,
60 elevation,
61 distance,
62 }
63 }
64
65 pub fn from_cartesian(pos: &Position3D) -> Self {
67 let distance = (pos.x * pos.x + pos.y * pos.y + pos.z * pos.z).sqrt();
68 let azimuth = pos.y.atan2(pos.x);
69 let elevation = (pos.z / distance).asin();
70
71 Self {
72 azimuth,
73 elevation,
74 distance,
75 }
76 }
77
78 pub fn to_cartesian(&self) -> Position3D {
80 let x = self.distance * self.elevation.cos() * self.azimuth.cos();
81 let y = self.distance * self.elevation.cos() * self.azimuth.sin();
82 let z = self.distance * self.elevation.sin();
83
84 Position3D::new(x, y, z)
85 }
86}
87
88pub struct SphericalHarmonics {
90 order: AmbisonicsOrder,
91 normalization: NormalizationScheme,
92}
93
94impl SphericalHarmonics {
95 pub fn new(order: AmbisonicsOrder, normalization: NormalizationScheme) -> Self {
97 Self {
98 order,
99 normalization,
100 }
101 }
102
103 pub fn calculate(&self, coord: &SphericalCoordinate) -> Array1<f32> {
105 let channel_count = channel_count(self.order);
106 let mut coefficients = Array1::zeros(channel_count);
107
108 let cos_el = coord.elevation.cos();
109 let sin_el = coord.elevation.sin();
110
111 let mut idx = 0;
112 for l in 0..=self.order {
113 for m in -(l as i32)..=(l as i32) {
114 let coeff = self.spherical_harmonic_yn(l, m, coord.azimuth, coord.elevation);
115 coefficients[idx] = self.apply_normalization(coeff, l, m);
116 idx += 1;
117 }
118 }
119
120 coefficients
121 }
122
123 fn spherical_harmonic_yn(&self, l: u32, m: i32, azimuth: f32, elevation: f32) -> f32 {
125 let cos_el = elevation.cos();
126 let sin_el = elevation.sin();
127
128 let legendre = self.associated_legendre_polynomial(l, m.unsigned_abs(), sin_el);
130
131 let azimuthal = if m >= 0 {
133 (m as f32 * azimuth).cos()
134 } else {
135 ((-m) as f32 * azimuth).sin()
136 };
137
138 legendre * azimuthal
139 }
140
141 fn associated_legendre_polynomial(&self, l: u32, m: u32, x: f32) -> f32 {
143 if m > l {
144 return 0.0;
145 }
146
147 let mut result = 1.0;
148 let one_minus_x2 = 1.0 - x * x;
149
150 if m > 0 {
152 result *= one_minus_x2.powf(m as f32 / 2.0);
153 for i in 1..=m {
154 result *= (2 * i - 1) as f32;
155 }
156 }
157
158 if l > m {
160 let mut p_prev = result;
161 let mut p_curr = x * (2 * m + 1) as f32 * p_prev;
162
163 for n in (m + 2)..=l {
164 let p_next = ((2 * n - 1) as f32 * x * p_curr - (n + m - 1) as f32 * p_prev)
165 / (n - m) as f32;
166 p_prev = p_curr;
167 p_curr = p_next;
168 }
169 result = p_curr;
170 }
171
172 result
173 }
174
175 fn apply_normalization(&self, coeff: f32, l: u32, m: i32) -> f32 {
177 match self.normalization {
178 NormalizationScheme::N3D => coeff * (2 * l + 1) as f32 / (4.0 * PI),
179 NormalizationScheme::SN3D => {
180 let norm_factor = if m == 0 { 1.0 } else { 2.0_f32.sqrt() };
181 coeff * norm_factor
182 }
183 NormalizationScheme::FuMa => {
184 if l == 0 {
186 coeff / 2.0_f32.sqrt()
187 } else {
188 coeff
189 }
190 }
191 NormalizationScheme::MaxN => coeff / (4.0 * PI).sqrt(),
192 }
193 }
194}
195
196pub struct AmbisonicsEncoder {
198 order: AmbisonicsOrder,
199 normalization: NormalizationScheme,
200 channel_ordering: ChannelOrdering,
201 spherical_harmonics: SphericalHarmonics,
202}
203
204impl AmbisonicsEncoder {
205 pub fn new(
207 order: AmbisonicsOrder,
208 normalization: NormalizationScheme,
209 channel_ordering: ChannelOrdering,
210 ) -> Self {
211 let spherical_harmonics = SphericalHarmonics::new(order, normalization);
212
213 Self {
214 order,
215 normalization,
216 channel_ordering,
217 spherical_harmonics,
218 }
219 }
220
221 pub fn encode_mono(
223 &self,
224 audio_samples: &Array1<f32>,
225 position: &Position3D,
226 ) -> Result<Array2<f32>> {
227 let coord = SphericalCoordinate::from_cartesian(position);
228 let coefficients = self.spherical_harmonics.calculate(&coord);
229 let channel_count = channel_count(self.order);
230 let sample_count = audio_samples.len();
231
232 let mut encoded = Array2::zeros((channel_count, sample_count));
233
234 let distance_gain = if coord.distance > 0.0 {
236 1.0 / coord.distance.max(0.1)
237 } else {
238 1.0
239 };
240
241 for (ch_idx, &coeff) in coefficients.iter().enumerate() {
243 let channel_idx = self.apply_channel_ordering(ch_idx)?;
244 for (sample_idx, &sample) in audio_samples.iter().enumerate() {
245 encoded[[channel_idx, sample_idx]] = sample * coeff * distance_gain;
246 }
247 }
248
249 Ok(encoded)
250 }
251
252 pub fn encode_multichannel(
254 &self,
255 audio_samples: &Array2<f32>, positions: &[Position3D],
257 ) -> Result<Array2<f32>> {
258 if audio_samples.shape()[0] != positions.len() {
259 return Err(Error::LegacyProcessing(
260 "Number of audio channels must match number of positions".to_string(),
261 ));
262 }
263
264 let channel_count = channel_count(self.order);
265 let sample_count = audio_samples.shape()[1];
266 let mut encoded = Array2::zeros((channel_count, sample_count));
267
268 for (input_ch, position) in positions.iter().enumerate() {
270 let input_samples = audio_samples.row(input_ch);
271 let input_array = Array1::from_iter(input_samples.iter().copied());
272 let channel_encoded = self.encode_mono(&input_array, position)?;
273
274 encoded = encoded + channel_encoded;
276 }
277
278 Ok(encoded)
279 }
280
281 fn apply_channel_ordering(&self, acn_index: usize) -> Result<usize> {
283 match self.channel_ordering {
284 ChannelOrdering::ACN => Ok(acn_index),
285 ChannelOrdering::FuMa => {
286 match acn_index {
288 0 => Ok(0), 1 => Ok(2), 2 => Ok(3), 3 => Ok(1), _ => Ok(acn_index), }
294 }
295 ChannelOrdering::SID => Ok(acn_index), }
297 }
298
299 pub fn get_config(&self) -> (AmbisonicsOrder, NormalizationScheme, ChannelOrdering) {
301 (self.order, self.normalization, self.channel_ordering)
302 }
303
304 pub fn get_channel_count(&self) -> usize {
306 channel_count(self.order)
307 }
308}
309
310pub struct AmbisonicsDecoder {
312 order: AmbisonicsOrder,
313 normalization: NormalizationScheme,
314 channel_ordering: ChannelOrdering,
315 speaker_positions: Vec<SphericalCoordinate>,
316 decoding_matrix: Array2<f32>,
317}
318
319impl AmbisonicsDecoder {
320 pub fn new(
322 order: AmbisonicsOrder,
323 normalization: NormalizationScheme,
324 channel_ordering: ChannelOrdering,
325 speaker_positions: Vec<SphericalCoordinate>,
326 ) -> Result<Self> {
327 let mut decoder = Self {
328 order,
329 normalization,
330 channel_ordering,
331 speaker_positions,
332 decoding_matrix: Array2::zeros((1, 1)), };
334
335 decoder.calculate_decoding_matrix()?;
336 Ok(decoder)
337 }
338
339 pub fn for_speaker_config(
341 order: AmbisonicsOrder,
342 config: SpeakerConfiguration,
343 ) -> Result<Self> {
344 let positions = Self::create_speaker_positions(config);
345 Self::new(
346 order,
347 NormalizationScheme::SN3D,
348 ChannelOrdering::ACN,
349 positions,
350 )
351 }
352
353 fn create_speaker_positions(config: SpeakerConfiguration) -> Vec<SphericalCoordinate> {
355 match config {
356 SpeakerConfiguration::Stereo => vec![
357 SphericalCoordinate::new(-PI / 6.0, 0.0, 1.0), SphericalCoordinate::new(PI / 6.0, 0.0, 1.0), ],
360 SpeakerConfiguration::Quadraphonic => vec![
361 SphericalCoordinate::new(-PI / 4.0, 0.0, 1.0), SphericalCoordinate::new(PI / 4.0, 0.0, 1.0), SphericalCoordinate::new(-3.0 * PI / 4.0, 0.0, 1.0), SphericalCoordinate::new(3.0 * PI / 4.0, 0.0, 1.0), ],
366 SpeakerConfiguration::FiveDotOne => vec![
367 SphericalCoordinate::new(-PI / 6.0, 0.0, 1.0), SphericalCoordinate::new(PI / 6.0, 0.0, 1.0), SphericalCoordinate::new(0.0, 0.0, 1.0), SphericalCoordinate::new(0.0, -PI / 4.0, 1.0), SphericalCoordinate::new(-2.0 * PI / 3.0, 0.0, 1.0), SphericalCoordinate::new(2.0 * PI / 3.0, 0.0, 1.0), ],
374 SpeakerConfiguration::SevenDotOne => vec![
375 SphericalCoordinate::new(-PI / 6.0, 0.0, 1.0), SphericalCoordinate::new(PI / 6.0, 0.0, 1.0), SphericalCoordinate::new(0.0, 0.0, 1.0), SphericalCoordinate::new(0.0, -PI / 4.0, 1.0), SphericalCoordinate::new(-PI / 2.0, 0.0, 1.0), SphericalCoordinate::new(PI / 2.0, 0.0, 1.0), SphericalCoordinate::new(-3.0 * PI / 4.0, 0.0, 1.0), SphericalCoordinate::new(3.0 * PI / 4.0, 0.0, 1.0), ],
384 SpeakerConfiguration::Cube => {
385 let mut positions = Vec::new();
387 for &elevation in &[-PI / 4.0, PI / 4.0] {
388 for i in 0..4 {
389 let azimuth = i as f32 * PI / 2.0;
390 positions.push(SphericalCoordinate::new(azimuth, elevation, 1.0));
391 }
392 }
393 positions
394 }
395 }
396 }
397
398 fn calculate_decoding_matrix(&mut self) -> Result<()> {
400 let channel_count = channel_count(self.order);
401 let speaker_count = self.speaker_positions.len();
402
403 let spherical_harmonics = SphericalHarmonics::new(self.order, self.normalization);
404 let mut matrix = Array2::zeros((speaker_count, channel_count));
405
406 for (speaker_idx, position) in self.speaker_positions.iter().enumerate() {
408 let coefficients = spherical_harmonics.calculate(position);
409 for (ch_idx, &coeff) in coefficients.iter().enumerate() {
410 matrix[[speaker_idx, ch_idx]] = coeff;
411 }
412 }
413
414 self.decoding_matrix = self.pseudo_inverse(&matrix)?;
416 Ok(())
417 }
418
419 fn pseudo_inverse(&self, matrix: &Array2<f32>) -> Result<Array2<f32>> {
421 Ok(matrix.clone())
426 }
427
428 pub fn decode(&self, ambisonics_audio: &Array2<f32>) -> Result<Array2<f32>> {
430 let ambi_channels = ambisonics_audio.shape()[0];
431 let sample_count = ambisonics_audio.shape()[1];
432 let speaker_count = self.speaker_positions.len();
433
434 if ambi_channels != channel_count(self.order) {
435 return Err(Error::LegacyProcessing(format!(
436 "Expected {} ambisonics channels, got {}",
437 channel_count(self.order),
438 ambi_channels
439 )));
440 }
441
442 let mut decoded = Array2::zeros((speaker_count, sample_count));
443
444 for sample_idx in 0..sample_count {
446 let ambi_sample = ambisonics_audio.column(sample_idx);
447
448 for speaker_idx in 0..speaker_count {
450 let mut sum = 0.0;
451 for ch_idx in 0..ambi_channels {
452 sum += self.decoding_matrix[[speaker_idx, ch_idx]] * ambi_sample[ch_idx];
453 }
454 decoded[[speaker_idx, sample_idx]] = sum;
455 }
456 }
457
458 Ok(decoded)
459 }
460
461 pub fn get_config(&self) -> (AmbisonicsOrder, NormalizationScheme, ChannelOrdering) {
463 (self.order, self.normalization, self.channel_ordering)
464 }
465
466 pub fn get_speaker_count(&self) -> usize {
468 self.speaker_positions.len()
469 }
470
471 pub fn get_speaker_positions(&self) -> &[SphericalCoordinate] {
473 &self.speaker_positions
474 }
475}
476
477#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)]
479pub enum SpeakerConfiguration {
480 Stereo,
482 Quadraphonic,
484 FiveDotOne,
486 SevenDotOne,
488 Cube,
490}
491
492pub struct AmbisonicsProcessor {
494 encoder: AmbisonicsEncoder,
495 decoder: AmbisonicsDecoder,
496}
497
498impl AmbisonicsProcessor {
499 pub fn new(order: AmbisonicsOrder, speaker_config: SpeakerConfiguration) -> Result<Self> {
501 let encoder =
502 AmbisonicsEncoder::new(order, NormalizationScheme::SN3D, ChannelOrdering::ACN);
503 let decoder = AmbisonicsDecoder::for_speaker_config(order, speaker_config)?;
504
505 Ok(Self { encoder, decoder })
506 }
507
508 pub fn process_multichannel(
510 &self,
511 audio_samples: &Array2<f32>,
512 positions: &[Position3D],
513 ) -> Result<Array2<f32>> {
514 let ambisonics = self.encoder.encode_multichannel(audio_samples, positions)?;
516
517 self.decoder.decode(&ambisonics)
519 }
520
521 pub fn process_mono(
523 &self,
524 audio_samples: &Array1<f32>,
525 position: &Position3D,
526 ) -> Result<Array2<f32>> {
527 let ambisonics = self.encoder.encode_mono(audio_samples, position)?;
529
530 self.decoder.decode(&ambisonics)
532 }
533
534 pub fn get_info(&self) -> (AmbisonicsOrder, usize, usize) {
536 (
537 self.encoder.order,
538 self.encoder.get_channel_count(),
539 self.decoder.get_speaker_count(),
540 )
541 }
542}
543
544#[cfg(test)]
545mod tests {
546 use super::*;
547
548 #[test]
549 fn test_channel_count_calculation() {
550 assert_eq!(channel_count(0), 1); assert_eq!(channel_count(1), 4); assert_eq!(channel_count(2), 9); assert_eq!(channel_count(3), 16); }
555
556 #[test]
557 fn test_spherical_coordinate_conversion() {
558 let cartesian = Position3D::new(1.0, 0.0, 0.0);
559 let spherical = SphericalCoordinate::from_cartesian(&cartesian);
560
561 assert!((spherical.azimuth - 0.0).abs() < 1e-6);
562 assert!((spherical.elevation - 0.0).abs() < 1e-6);
563 assert!((spherical.distance - 1.0).abs() < 1e-6);
564
565 let back_to_cartesian = spherical.to_cartesian();
566 assert!((back_to_cartesian.x - 1.0).abs() < 1e-6);
567 assert!((back_to_cartesian.y - 0.0).abs() < 1e-6);
568 assert!((back_to_cartesian.z - 0.0).abs() < 1e-6);
569 }
570
571 #[test]
572 fn test_spherical_harmonics_calculation() {
573 let harmonics = SphericalHarmonics::new(1, NormalizationScheme::SN3D);
574 let coord = SphericalCoordinate::new(0.0, 0.0, 1.0); let coefficients = harmonics.calculate(&coord);
577 assert_eq!(coefficients.len(), 4); assert!(coefficients[0].abs() > 1e-6);
581 }
582
583 #[test]
584 fn test_encoder_creation() {
585 let encoder = AmbisonicsEncoder::new(1, NormalizationScheme::SN3D, ChannelOrdering::ACN);
586 assert_eq!(encoder.get_channel_count(), 4);
587
588 let (order, norm, ordering) = encoder.get_config();
589 assert_eq!(order, 1);
590 assert_eq!(norm, NormalizationScheme::SN3D);
591 assert_eq!(ordering, ChannelOrdering::ACN);
592 }
593
594 #[test]
595 fn test_mono_encoding() {
596 let encoder = AmbisonicsEncoder::new(1, NormalizationScheme::SN3D, ChannelOrdering::ACN);
597 let position = Position3D::new(1.0, 0.0, 0.0); let audio = Array1::from_vec(vec![1.0, 0.5, -0.5, 0.0]);
599
600 let encoded = encoder.encode_mono(&audio, &position).unwrap();
601 assert_eq!(encoded.shape(), [4, 4]); let sum: f32 = encoded.iter().map(|x| x.abs()).sum();
605 assert!(sum > 1e-6);
606 }
607
608 #[test]
609 fn test_decoder_creation() {
610 let decoder = AmbisonicsDecoder::for_speaker_config(1, SpeakerConfiguration::Stereo);
611 assert!(decoder.is_ok());
612
613 let decoder = decoder.unwrap();
614 assert_eq!(decoder.get_speaker_count(), 2);
615
616 let positions = decoder.get_speaker_positions();
617 assert_eq!(positions.len(), 2);
618 }
619
620 #[test]
621 fn test_speaker_configurations() {
622 let stereo_pos = AmbisonicsDecoder::create_speaker_positions(SpeakerConfiguration::Stereo);
623 assert_eq!(stereo_pos.len(), 2);
624
625 let quad_pos =
626 AmbisonicsDecoder::create_speaker_positions(SpeakerConfiguration::Quadraphonic);
627 assert_eq!(quad_pos.len(), 4);
628
629 let surround_pos =
630 AmbisonicsDecoder::create_speaker_positions(SpeakerConfiguration::FiveDotOne);
631 assert_eq!(surround_pos.len(), 6);
632 }
633
634 #[test]
635 fn test_combined_processor() {
636 let processor = AmbisonicsProcessor::new(1, SpeakerConfiguration::Stereo).unwrap();
637 let (order, ambi_channels, speaker_count) = processor.get_info();
638
639 assert_eq!(order, 1);
640 assert_eq!(ambi_channels, 4);
641 assert_eq!(speaker_count, 2);
642 }
643
644 #[test]
645 fn test_full_pipeline() {
646 let processor = AmbisonicsProcessor::new(1, SpeakerConfiguration::Stereo).unwrap();
647 let position = Position3D::new(1.0, 0.0, 0.0);
648 let audio = Array1::from_vec(vec![1.0, 0.5, -0.5, 0.0]);
649
650 let output = processor.process_mono(&audio, &position).unwrap();
651 assert_eq!(output.shape(), [2, 4]); let sum: f32 = output.iter().map(|x| x.abs()).sum();
655 assert!(sum > 1e-6);
656 }
657
658 #[test]
659 fn test_multichannel_processing() {
660 let processor = AmbisonicsProcessor::new(1, SpeakerConfiguration::Quadraphonic).unwrap();
661 let positions = vec![
662 Position3D::new(1.0, 0.0, 0.0), Position3D::new(0.0, 1.0, 0.0), ];
665 let audio = Array2::from_shape_vec(
666 (2, 4),
667 vec![
668 1.0, 0.5, -0.5, 0.0, 0.8, 0.3, -0.2, 0.1, ],
671 )
672 .unwrap();
673
674 let output = processor.process_multichannel(&audio, &positions).unwrap();
675 assert_eq!(output.shape(), [4, 4]); let sum: f32 = output.iter().map(|x| x.abs()).sum();
679 assert!(sum > 1e-6);
680 }
681
682 #[test]
683 fn test_normalization_schemes() {
684 let harmonics_n3d = SphericalHarmonics::new(1, NormalizationScheme::N3D);
685 let harmonics_sn3d = SphericalHarmonics::new(1, NormalizationScheme::SN3D);
686 let harmonics_fuma = SphericalHarmonics::new(1, NormalizationScheme::FuMa);
687
688 let coord = SphericalCoordinate::new(0.0, 0.0, 1.0);
689
690 let coeff_n3d = harmonics_n3d.calculate(&coord);
691 let coeff_sn3d = harmonics_sn3d.calculate(&coord);
692 let coeff_fuma = harmonics_fuma.calculate(&coord);
693
694 assert!(coeff_n3d[0] != coeff_sn3d[0]);
696 assert!(coeff_sn3d[0] != coeff_fuma[0]);
697 }
698}