;; biquad low-pass filter
(bind-func static lpfbq_c
(lambda ()
(let* ((y1 0.0)
(y2 0.0)
(x1 0.0)
(x2 0.0)
(b0 0.0)
(b1 0.0)
(b2 0.0)
(a0 0.0)
(a1 0.0)
(a2 0.0)
(oldres 0.0)
(oldfreq 0.0))
(lambda (x freq res:SAMPLE)
;; if frequency changes
;; recalculate coefficients
(if (or (<> freq oldfreq)
(<> res oldres))
(let* ((omega (* STWOPI (/ freq SAMPLERATE)))
(sino (sin omega))
(coso (cos omega))
(alpha (/ sino (* 2.0 res))))
(set! oldfreq freq)
(set! oldres res)
(set! b0 (/ (- 1.0 coso) 2.0))
(set! b1 (- 1.0 coso))
(set! b2 b0)
(set! a0 (+ 1.0 alpha))
(set! a1 (* -2.0 coso))
(set! a2 (- 1.0 alpha))))
(let ((y (- (+ (* (/ b0 a0) x)
(* (/ b1 a0) x1)
(* (/ b2 a0) x2))
(* (/ a1 a0) y1)
(* (/ a2 a0) y2))))
(set! y2 y1)
(set! y1 y)
(set! x2 x1)
(set! x1 x)
y)))))