add organya interpolation modes

src/sound/mod.rs

@@ -1,6 +1,8 @@
 use std::io;
+use std::io::{BufRead, BufReader, Lines};
+use std::str::FromStr;
 use std::sync::mpsc;
-use std::sync::mpsc::{Receiver, Sender, TryRecvError};
+use std::sync::mpsc::{Receiver, Sender};
 use std::time::Duration;
 
 use cpal::traits::{DeviceTrait, HostTrait, StreamTrait};
@@ -23,9 +25,8 @@ use crate::sound::organya::Song;
 use crate::sound::pixtone::{PixToneParameters, PixTonePlayback};
 use crate::sound::wave_bank::SoundBank;
 use crate::str;
-use std::io::{BufReader, BufRead, Lines};
-use std::str::FromStr;
 
+mod fir;
 #[cfg(feature = "ogg-playback")]
 mod ogg_playback;
 mod org_playback;
@@ -50,6 +51,15 @@ enum SongFormat {
     OggMultiPart,
 }
 
+#[derive(Copy, Clone, PartialEq, Eq, serde::Serialize, serde::Deserialize)]
+pub enum InterpolationMode {
+    Nearest,
+    Linear,
+    Cosine,
+    Cubic,
+    Polyphase,
+}
+
 impl SoundManager {
     pub fn new(ctx: &mut Context) -> GameResult<SoundManager> {
         let (tx, rx): (Sender<PlaybackMessage>, Receiver<PlaybackMessage>) = mpsc::channel();
@@ -263,7 +273,9 @@ impl SoundManager {
 
                     let _ = splits.next();
                     if let Some(str) = splits.next() {
-                        return str.trim().parse::<T>().map_err(|_| GameError::ParseError("failed to parse the value as specified type.".to_string()))
+                        return str.trim().parse::<T>().map_err(|_| {
+                            GameError::ParseError("failed to parse the value as specified type.".to_string())
+                        });
                     } else {
                         break;
                     }
@@ -272,8 +284,8 @@ impl SoundManager {
                 }
             }
 
-            return Err(GameError::ParseError("unexpected end.".to_string()))
-        };
+            return Err(GameError::ParseError("unexpected end.".to_string()));
+        }
 
         for channel in params.channels.iter_mut() {
             channel.enabled = next_string::<u8, R>(&mut reader)? != 0;

src/sound/org_playback.rs

@@ -1,9 +1,30 @@
 use std::mem::MaybeUninit;
 
+use crate::sound::fir::FIR;
+use crate::sound::fir::FIR_STEP;
 use crate::sound::organya::{Song as Organya, Version};
 use crate::sound::stuff::*;
 use crate::sound::wav::*;
 use crate::sound::wave_bank::SoundBank;
+use crate::sound::InterpolationMode;
+
+#[derive(Clone)]
+pub struct FIRData {
+    cache: Vec<f32>,
+    pos: usize,
+}
+
+impl FIRData {
+    pub fn new() -> Self {
+        FIRData { cache: Vec::new(), pos: 0 }
+    }
+
+    pub fn ensure_initialized(&mut self) {
+        if self.cache.is_empty() {
+            self.cache.resize(FIR.len() * 4, 0.0);
+        }
+    }
+}
 
 pub(crate) struct OrgPlaybackEngine {
     song: Organya,
@@ -24,6 +45,7 @@ pub(crate) struct OrgPlaybackEngine {
     frames_this_tick: usize,
     frames_per_tick: usize,
     pub loops: usize,
+    pub interpolation: InterpolationMode,
 }
 
 pub struct SavedOrganyaPlaybackState {
@@ -33,10 +55,7 @@ pub struct SavedOrganyaPlaybackState {
 
 impl Clone for SavedOrganyaPlaybackState {
     fn clone(&self) -> SavedOrganyaPlaybackState {
-        SavedOrganyaPlaybackState {
-            song: self.song.clone(),
-            play_pos: self.play_pos,
-        }
+        SavedOrganyaPlaybackState { song: self.song.clone(), play_pos: self.play_pos }
     }
 }
 
@@ -58,14 +77,11 @@ impl OrgPlaybackEngine {
             keys: [255; 8],
             track_buffers: unsafe { std::mem::transmute(buffers) },
             play_pos: 0,
-            output_format: WavFormat {
-                channels: 2,
-                sample_rate: 44100,
-                bit_depth: 16,
-            },
+            output_format: WavFormat { channels: 2, sample_rate: 44100, bit_depth: 16 },
             frames_this_tick: 0,
             frames_per_tick,
             loops: 1,
+            interpolation: InterpolationMode::Linear,
         }
     }
 
@@ -81,10 +97,7 @@ impl OrgPlaybackEngine {
     }
 
     pub fn get_state(&self) -> SavedOrganyaPlaybackState {
-        SavedOrganyaPlaybackState {
-            song: self.song.clone(),
-            play_pos: self.play_pos,
-        }
+        SavedOrganyaPlaybackState { song: self.song.clone(), play_pos: self.play_pos }
     }
 
     pub fn set_state(&mut self, state: SavedOrganyaPlaybackState, samples: &SoundBank) {
@@ -97,11 +110,7 @@ impl OrgPlaybackEngine {
             let sound_index = song.tracks[i].inst.inst as usize;
             let sound = samples.get_wave(sound_index).iter().map(|&x| x ^ 128).collect();
 
-            let format = WavFormat {
-                channels: 1,
-                sample_rate: 22050,
-                bit_depth: 8,
-            };
+            let format = WavFormat { channels: 1, sample_rate: 22050, bit_depth: 8 };
 
             let rbuf = RenderBuffer::new_organya(WavSample { format, data: sound });
 
@@ -112,11 +121,7 @@ impl OrgPlaybackEngine {
             }
         }
 
-        for (idx, (track, buf)) in song.tracks[8..]
-            .iter()
-            .zip(self.track_buffers[128..].iter_mut())
-            .enumerate()
-        {
+        for (idx, (track, buf)) in song.tracks[8..].iter().zip(self.track_buffers[128..].iter_mut()).enumerate() {
             if self.song.version == Version::Extended {
                 *buf = RenderBuffer::new(samples.samples[track.inst.inst as usize].clone());
             } else {
@@ -292,7 +297,7 @@ impl OrgPlaybackEngine {
                 self.update_play_state()
             }
 
-            mix(std::slice::from_mut(frame), self.output_format, &mut self.track_buffers);
+            mix(std::slice::from_mut(frame), self.interpolation, self.output_format, &mut self.track_buffers);
 
             self.frames_this_tick += 1;
 
@@ -318,7 +323,7 @@ impl OrgPlaybackEngine {
 }
 
 // TODO: Create a MixingBuffer or something...
-pub fn mix(dst: &mut [u16], dst_fmt: WavFormat, srcs: &mut [RenderBuffer]) {
+fn mix(dst: &mut [u16], interpolation: InterpolationMode, dst_fmt: WavFormat, srcs: &mut [RenderBuffer]) {
     let freq = dst_fmt.sample_rate as f64;
 
     for buf in srcs {
@@ -343,55 +348,178 @@ pub fn mix(dst: &mut [u16], dst_fmt: WavFormat, srcs: &mut [RenderBuffer]) {
                 }
             }
 
-            #[allow(unused_variables)]
-            for frame in dst.iter_mut() {
-                let pos = buf.position as usize + buf.base_pos;
-                // -1..1
-                let s1 = (buf.sample.data[pos] as f32 - 128.0) / 128.0;
-                let s2 = (buf.sample.data[clamp(pos + 1, buf.base_pos + buf.len - 1)] as f32 - 128.0) / 128.0;
-                let s3 = (buf.sample.data[clamp(pos + 2, buf.base_pos + buf.len - 1)] as f32 - 128.0) / 128.0;
-                let s4 = (buf.sample.data[pos.saturating_sub(1)] as f32 - 128.0) / 128.0;
+            // s1: sample 1
+            // s2: sample 2
+            // sp: previous sample (before s1)
+            // sn: next sample (after s2)
+            // mu: position to interpolate for
+            fn cubic_interp(s1: f32, s2: f32, sp: f32, sn: f32, mu: f32) -> f32 {
+                let mu2 = mu * mu;
+                let a0 = sn - s2 - sp + s1;
+                let a1 = sp - s1 - a0;
+                let a2 = s2 - sp;
+                let a3 = s1;
 
-                use std::f32::consts::PI;
+                a0 * mu * mu2 + a1 * mu2 + a2 * mu + a3
+            }
 
-                let r1 = buf.position.fract() as f32;
-                let r2 = (1.0 - f32::cos(r1 * PI)) / 2.0;
+            if interpolation == InterpolationMode::Polyphase {
+                buf.fir.ensure_initialized();
+                let fir_step = (FIR_STEP * advance as f32).floor();
+                let fir_step = if fir_step == 0.0 { FIR_STEP } else { fir_step };
+                let fir_gain = fir_step / FIR_STEP;
 
-                //let s = s1; // No interp
-                //let s = s1 + (s2 - s1) * r1; // Linear interp
-                //let s = s1 * (1.0 - r2) + s2 * r2; // Cosine interp
-                let s = cubic_interp(s1, s2, s4, s3, r1); // Cubic interp
-                                                          // Ideally we want sinc/lanczos interpolation, since that's what DirectSound appears to use.
+                let mut count = 0isize;
 
-                // -128..128
-                let sl = s * pan_l * vol * 128.0;
-                let sr = s * pan_r * vol * 128.0;
+                // optimized for debug mode
+                // bound / arithmetic checks give a HUGE performance hit in this code
+                let fl = FIR.len() as f32;
 
-                buf.position += advance;
+                // raw pointer access is much faster than get_unchecked
+                let fir_ptr = FIR.as_ptr();
+                let cache_ptr = buf.fir.cache.as_mut_ptr();
 
-                if buf.position as usize >= buf.len {
-                    if buf.looping && buf.nloops != 1 {
-                        buf.position %= buf.len as f64;
-                        if buf.nloops != -1 {
-                            buf.nloops -= 1;
+                for (n, _) in dst.iter().enumerate() {
+                    count += 1;
+                    let pos = buf.position as usize + buf.base_pos;
+                    let i = (buf.fir.pos + n) % buf.fir.cache.len();
+
+                    let s1 = (buf.sample.data[pos] as f32 - 128.0) / 128.0;
+                    let s2 = (buf.sample.data[clamp(pos + 1, buf.base_pos + buf.len - 1)] as f32 - 128.0) / 128.0;
+                    let r1 = buf.position.fract() as f32;
+
+                    buf.fir.cache[i] = s1 + (s2 - s1) * r1;
+
+                    buf.position += advance;
+
+                    if buf.position as usize >= buf.len {
+                        if buf.looping && buf.nloops != 1 {
+                            buf.position %= buf.len as f64;
+                            if buf.nloops != -1 {
+                                buf.nloops -= 1;
+                            }
+                        } else {
+                            buf.position = 0.0;
+                            buf.playing = false;
+
+                            let silent_frames = dst.len() - n;
+                            for m in 0..silent_frames {
+                                let i = (buf.fir.pos + n + m) % buf.fir.cache.len();
+                                buf.fir.cache[i] = 0.0;
+                            }
+                            count += silent_frames as isize;
+
+                            break;
                         }
-                    } else {
-                        buf.position = 0.0;
-                        buf.playing = false;
-                        break;
                     }
                 }
 
-                let [mut l, mut r] = frame.to_le_bytes();
-                // -128..127
-                let xl = (l ^ 128) as i8;
-                let xr = (r ^ 128) as i8;
+                let cl = buf.fir.cache.len() as isize;
 
-                // 0..255
-                l = xl.saturating_add(sl as i8) as u8 ^ 128;
-                r = xr.saturating_add(sr as i8) as u8 ^ 128;
+                for n in 0..count {
+                    let mut insamp_idx = (buf.fir.pos as isize).wrapping_add(n).wrapping_rem(cl);
+                    let mut acc = 0.0;
+                    let mut step = 0.0;
 
-                *frame = u16::from_le_bytes([l, r]);
+                    while step < fl {
+                        unsafe {
+                            acc += (*fir_ptr.add(step as usize)) * (*cache_ptr.add((insamp_idx) as usize));
+                            insamp_idx = if insamp_idx == 0 { cl.wrapping_sub(1) } else { insamp_idx.wrapping_sub(1) };
+                            step += fir_step;
+                        }
+                    }
+
+                    acc *= fir_gain;
+
+                    let sl = acc * pan_l * vol * 128.0;
+                    let sr = acc * pan_r * vol * 128.0;
+
+                    let frame = unsafe { dst.get_unchecked_mut(n as usize) };
+                    let [mut l, mut r] = frame.to_be_bytes();
+                    // -128..127
+                    let xl = (l ^ 128) as i8;
+                    let xr = (r ^ 128) as i8;
+
+                    // 0..255
+                    l = xl.saturating_add(sl as i8) as u8 ^ 128;
+                    r = xr.saturating_add(sr as i8) as u8 ^ 128;
+
+                    *frame = u16::from_be_bytes([l, r]);
+                }
+
+                buf.fir.pos += count as usize;
+            } else {
+                for frame in dst.iter_mut() {
+                    let pos = buf.position as usize + buf.base_pos;
+
+                    // -1..1
+                    let s = match interpolation {
+                        InterpolationMode::Nearest => (buf.sample.data[pos] as f32 - 128.0) / 128.0,
+                        InterpolationMode::Linear => {
+                            let s1 = (buf.sample.data[pos] as f32 - 128.0) / 128.0;
+                            let s2 =
+                                (buf.sample.data[clamp(pos + 1, buf.base_pos + buf.len - 1)] as f32 - 128.0) / 128.0;
+                            let r1 = buf.position.fract() as f32;
+
+                            s1 + (s2 - s1) * r1
+                        }
+                        InterpolationMode::Cosine => {
+                            use std::f32::consts::PI;
+
+                            let s1 = (buf.sample.data[pos] as f32 - 128.0) / 128.0;
+                            let s2 =
+                                (buf.sample.data[clamp(pos + 1, buf.base_pos + buf.len - 1)] as f32 - 128.0) / 128.0;
+
+                            let r1 = buf.position.fract() as f32;
+                            let r2 = (1.0 - f32::cos(r1 * PI)) / 2.0;
+
+                            s1 * (1.0 - r2) + s2 * r2
+                        }
+                        InterpolationMode::Cubic => {
+                            let s1 = (buf.sample.data[pos] as f32 - 128.0) / 128.0;
+                            let s2 =
+                                (buf.sample.data[clamp(pos + 1, buf.base_pos + buf.len - 1)] as f32 - 128.0) / 128.0;
+                            let s3 =
+                                (buf.sample.data[clamp(pos + 2, buf.base_pos + buf.len - 1)] as f32 - 128.0) / 128.0;
+                            let s4 = (buf.sample.data[pos.saturating_sub(1)] as f32 - 128.0) / 128.0;
+
+                            let r1 = buf.position.fract() as f32;
+
+                            cubic_interp(s1, s2, s4, s3, r1)
+                        }
+                        InterpolationMode::Polyphase => unreachable!(),
+                    };
+
+                    // -128..128
+                    let sl = s * pan_l * vol * 128.0;
+                    let sr = s * pan_r * vol * 128.0;
+
+                    buf.position += advance;
+
+                    if buf.position as usize >= buf.len {
+                        if buf.looping && buf.nloops != 1 {
+                            buf.position %= buf.len as f64;
+                            if buf.nloops != -1 {
+                                buf.nloops -= 1;
+                            }
+                        } else {
+                            buf.position = 0.0;
+                            buf.playing = false;
+                            break;
+                        }
+                    }
+
+                    let [mut l, mut r] = frame.to_be_bytes();
+                    // -128..127
+                    let xl = (l ^ 128) as i8;
+                    let xr = (r ^ 128) as i8;
+
+                    // 0..255
+                    l = xl.saturating_add(sl as i8) as u8 ^ 128;
+                    r = xr.saturating_add(sr as i8) as u8 ^ 128;
+
+                    *frame = u16::from_be_bytes([l, r]);
+                }
             }
         }
     }
@@ -414,6 +542,7 @@ pub struct RenderBuffer {
     pub len: usize,
     // -1 = infinite
     pub nloops: i32,
+    pub fir: FIRData,
 }
 
 impl RenderBuffer {
@@ -429,6 +558,7 @@ impl RenderBuffer {
             looping: false,
             base_pos: 0,
             nloops: -1,
+            fir: FIRData::new(),
         }
     }
 
@@ -439,18 +569,12 @@ impl RenderBuffer {
             volume: 0,
             pan: 0,
             len: 0,
-            sample: WavSample {
-                format: WavFormat {
-                    channels: 2,
-                    sample_rate: 22050,
-                    bit_depth: 16,
-                },
-                data: vec![],
-            },
+            sample: WavSample { format: WavFormat { channels: 2, sample_rate: 22050, bit_depth: 16 }, data: vec![] },
             playing: false,
             looping: false,
             base_pos: 0,
             nloops: -1,
+            fir: FIRData::new(),
         }
     }
 

src/sound/wav.rs

@@ -1,16 +1,22 @@
+use std::fmt;
+use std::io;
+use std::io::ErrorKind;
+
+use byteorder::{ReadBytesExt, LE};
+
 #[derive(Copy, Clone, Eq, PartialEq, Debug)]
 pub struct RiffChunk {
     id: [u8; 4],
-    length: u32
+    length: u32,
 }
 
-use std::fmt;
-
 impl fmt::Display for RiffChunk {
     fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
         use std::ascii::escape_default as esc;
-        
-        write!(f, "chunk \"{}{}{}{}\", length: {}",
+
+        write!(
+            f,
+            "chunk \"{}{}{}{}\", length: {}",
             esc(self.id[0]),
             esc(self.id[1]),
             esc(self.id[2]),
@@ -24,28 +30,26 @@ impl fmt::Display for RiffChunk {
 pub struct WavFormat {
     pub channels: u16,
     pub sample_rate: u32,
-    pub bit_depth: u16
+    pub bit_depth: u16,
 }
 
 impl fmt::Display for WavFormat {
     fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
-        write!(f, "{} channels, {} Hz, {}-bit",
-            self.channels,
-            self.sample_rate,
-            self.bit_depth
-        )
+        write!(f, "{} channels, {} Hz, {}-bit", self.channels, self.sample_rate, self.bit_depth)
     }
 }
 
 #[derive(Clone)]
 pub struct WavSample {
     pub format: WavFormat,
-    pub data: Vec<u8>
+    pub data: Vec<u8>,
 }
 
 impl fmt::Display for WavSample {
     fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
-        write!(f, "{}, {} samples",
+        write!(
+            f,
+            "{}, {} samples",
             self.format,
             // num_bytes / bytes_per_sample
             self.data.len() / ((self.format.bit_depth / 8) * self.format.channels) as usize
@@ -53,16 +57,13 @@ impl fmt::Display for WavSample {
     }
 }
 
-use byteorder::{LE, ReadBytesExt};
-use std::io;
-
 impl RiffChunk {
     pub fn read_from<R: io::Read>(mut f: R) -> io::Result<RiffChunk> {
         let mut id = [0; 4];
-        
+
         f.read_exact(&mut id)?;
         let length = f.read_u32::<LE>()?;
-        
+
         Ok(RiffChunk { id, length })
     }
 }
@@ -70,51 +71,51 @@ impl RiffChunk {
 impl WavSample {
     pub fn read_from<R: io::Read>(mut f: R) -> io::Result<WavSample> {
         let riff = RiffChunk::read_from(&mut f)?;
-        
+
         match &riff.id {
-            b"RIFF" => {},
-            b"RIFX" => panic!("Cannot handle RIFX data!"),
-            _       => panic!("Expected RIFF signature, found {}", riff)
+            b"RIFF" => {}
+            b"RIFX" => return Err(io::Error::new(ErrorKind::InvalidData, "Cannot handle RIFX data!".to_owned())),
+            _ => {
+                return Err(io::Error::new(ErrorKind::InvalidData, format!("Expected RIFF signature, found {}", riff)))
+            }
         }
-        
+
         let mut rfmt = [0; 4];
-        
+
         f.read_exact(&mut rfmt)?;
-        
-        assert_eq!(rfmt, *b"WAVE");
-        
+
+        if rfmt != *b"WAVE" {
+            return Err(io::Error::new(ErrorKind::InvalidData, "Expected 'WAVE' RIFF chunk.".to_owned()));
+        }
+
         let fmt = RiffChunk::read_from(&mut f)?;
-        
-        assert_eq!(fmt.id, *b"fmt ");
-        //assert_eq!(fmt.length, 16);
-        
+
+        if fmt.id != *b"fmt " {
+            return Err(io::Error::new(ErrorKind::InvalidData, "Expected 'fmt ' RIFF chunk.".to_owned()));
+        }
+
         let afmt = f.read_u16::<LE>()?;
-        
-        debug_assert!(afmt == 1);
-        
+
+        if afmt != 1 {
+            return Err(io::Error::new(ErrorKind::InvalidData, "Only PCM audio data is supported.".to_owned()));
+        }
+
         let channels = f.read_u16::<LE>()?;
-        let samples  = f.read_u32::<LE>()?;
+        let samples = f.read_u32::<LE>()?;
         let _brate = f.read_u32::<LE>()?;
         let _balgn = f.read_u16::<LE>()?;
-        let bits     = f.read_u16::<LE>()?;
-        
+        let bits = f.read_u16::<LE>()?;
+
         let data = RiffChunk::read_from(&mut f)?;
-        
-        assert_eq!(data.id, *b"data");
-        
+
+        if data.id != *b"data" {
+            return Err(io::Error::new(ErrorKind::InvalidData, "Expected 'data' RIFF chunk.".to_owned()));
+        }
+
         let mut buf = vec![0; data.length as usize];
-        
+
         f.read_exact(&mut buf)?;
-        
-        Ok(
-            WavSample {
-                format: WavFormat {
-                    channels,
-                    sample_rate: samples,
-                    bit_depth: bits
-                },
-                data: buf
-            }
-        )
+
+        Ok(WavSample { format: WavFormat { channels, sample_rate: samples, bit_depth: bits }, data: buf })
     }
 }

src/sound/wave_bank.rs

@@ -4,8 +4,7 @@ use std::io;
 use std::fmt;
 
 pub struct SoundBank {
-    // FIXME: would prefer Box<[u8; 25600]>
-    pub wave100: Box<[u8]>,
+    pub wave100: Box<[u8; 25600]>,
     
     pub samples: Vec<wav::WavSample>
 }
@@ -24,10 +23,9 @@ impl fmt::Display for SoundBank {
 
 impl SoundBank {
     pub fn load_from<R: io::Read>(mut f: R) -> io::Result<SoundBank> {
-        // no box [0; 25600] yet
-        let mut wave100 = vec![0; 25600].into_boxed_slice();
+        let mut wave100 = Box::new([0u8; 25600]);
         
-        f.read_exact(&mut *wave100)?;
+        f.read_exact(wave100.as_mut())?;
         
         let mut samples = Vec::with_capacity(16);