soft_clip.rs

fn pcm_soft_clip(pcm: &mut [f32], nb_samples: i32, nb_channels: i32, declip_mem: &mut [f32]) {

    if nb_samples < 1 || nb_channels < 1 {
        return;
    }

    /* First thing: saturate everything to +/- 2 which is the highest level our
       non-linearity can handle. At the point where the signal reaches +/-2,
       the derivative will be zero anyway, so this doesn't introduce any
       discontinuity in the derivative. */
    for samp in pcm.iter_mut() {
        *samp = (-2.0_f32).max(2.0_f32.min(*samp));
    }

    for c in 0..nb_channels as usize {
        let mut x = &mut pcm[c..];
        let mut a = declip_mem[c];
        /* Continue applying the non-linearity from the previous frame to avoid
           any discontinuity. */
        for i in 0..nb_samples {
            let idx = (i*nb_channels) as usize;
            if x[idx]*a >= 0.0 {
                break;
            }
            x[idx] = x[idx] + a*x[idx]*x[idx];
        }

        let mut curr = 0;
        let x0 = x[0];
        loop {
            let mut i = curr;
            for n in curr..nb_samples {
                let idx = (n*nb_channels) as usize;
                if x[idx] > 1.0 || x[idx] < -1.0 {
                    i = n;
                    break;
                }
            }

            if i == nb_samples {
                a = 0.0;
                break;
            }

            let mut peak_pos = i;
            let mut start = i;
            let mut end = i;

            let mut maxval = x[(i*nb_channels) as usize].abs();

            /* Look for first zero crossing before clipping */
            while start > 0 && x[(i*nb_channels) as usize]*x[((start - 1)*nb_channels) as usize] >= 0.0 {
                start -= 1;
            }

            /* Look for first zero crossing after clipping */
            while end < nb_samples && x[(i*nb_channels) as usize]*x[(end*nb_channels) as usize] >= 0.0  {
                /* Look for other peaks until the next zero-crossing. */
                if x[(end*nb_channels) as usize].abs() > maxval {
                    maxval = x[(end*nb_channels) as usize].abs();
                    peak_pos = end;
                }
                end += 1;
            }

            /* Detect the special case where we clip before the first zero crossing */
            let special = start == 0 && x[(i*nb_channels) as usize]*x[0] >= 0.0;

            /* Compute a such that maxval + a*maxval^2 = 1 */
            a = (maxval - 1.0)/(maxval*maxval);

            /* Slightly boost "a" by 2^-22. This is just enough to ensure -ffast-math
               does not cause output values larger than +/-1, but small enough not
               to matter even for 24-bit output.  */
            a += a*2.4e-7_f32;

            if x[(i*nb_channels) as usize] > 0.0 {
                a = -a;
            }

            /* Apply soft clipping */
            for i in start..end  {
                let idx = (i*nb_channels) as usize;
                x[idx] = x[idx] + a*x[idx]*x[idx];
            }

            if special && peak_pos >= 2 {
                /* Add a linear ramp from the first sample to the signal peak.
                   This avoids a discontinuity at the beginning of the frame. */
                let mut offset = x0 - x[0];
                let delta = offset / peak_pos as f32;
                for i in curr..peak_pos {
                    let idx = (i*nb_channels) as usize;
                    offset -= delta;
                    x[idx] += offset;
                    x[idx] = (-1_f32).max(1_f32.min(x[idx]));
                }
            }
            curr = end;
            if curr == nb_samples {
                break;
            }
        }
        declip_mem[c] = a;
    }
}

fn main() {

    use std::io;
    /* Read in floats one line at a time */
    let mut vec = Vec::new();
    loop {
        let mut input = String::new();
        match io::stdin().read_line(&mut input) { Ok(n) => {
            if n == 0 {
                break;
            }
            for val in input.split_whitespace() {
                match val.parse::<f32>() {
                    Ok(val) => vec.push(val),
                    Err(_) => (),
                }
            }
        }
        Err(error) => println!("error: {}", error),
        }
    }

    const NB_CHANNELS: usize = 2;
    let mut declip_mem: [f32; NB_CHANNELS] = [0.0, 0.0];
    let nb_samples = vec.len() / NB_CHANNELS;
    pcm_soft_clip(&mut vec, nb_samples as i32, NB_CHANNELS as i32, &mut declip_mem);
    for v in vec {
        println!("{}", v);
    }
}

Test data is avaliable at https://people.videolan.org/~tmatth/softclip/