update code.

VocieProcess/modules/audio_processing/utility/cascaded_biquad_filter.cc

@@ -0,0 +1,126 @@
+/*
+ *  Copyright (c) 2016 The WebRTC project authors. All Rights Reserved.
+ *
+ *  Use of this source code is governed by a BSD-style license
+ *  that can be found in the LICENSE file in the root of the source
+ *  tree. An additional intellectual property rights grant can be found
+ *  in the file PATENTS.  All contributing project authors may
+ *  be found in the AUTHORS file in the root of the source tree.
+ */
+#include "modules/audio_processing/utility/cascaded_biquad_filter.h"
+
+#include <algorithm>
+
+#include "rtc_base/checks.h"
+
+namespace webrtc {
+
+CascadedBiQuadFilter::BiQuadParam::BiQuadParam(std::complex<float> zero,
+                                               std::complex<float> pole,
+                                               float gain,
+                                               bool mirror_zero_along_i_axis)
+    : zero(zero),
+      pole(pole),
+      gain(gain),
+      mirror_zero_along_i_axis(mirror_zero_along_i_axis) {}
+
+CascadedBiQuadFilter::BiQuadParam::BiQuadParam(const BiQuadParam&) = default;
+
+CascadedBiQuadFilter::BiQuad::BiQuad(
+    const CascadedBiQuadFilter::BiQuadParam& param)
+    : x(), y() {
+  float z_r = std::real(param.zero);
+  float z_i = std::imag(param.zero);
+  float p_r = std::real(param.pole);
+  float p_i = std::imag(param.pole);
+  float gain = param.gain;
+
+  if (param.mirror_zero_along_i_axis) {
+    // Assuming zeroes at z_r and -z_r.
+    RTC_DCHECK(z_i == 0.f);
+    coefficients.b[0] = gain * 1.f;
+    coefficients.b[1] = 0.f;
+    coefficients.b[2] = gain * -(z_r * z_r);
+  } else {
+    // Assuming zeros at (z_r + z_i*i) and (z_r - z_i*i).
+    coefficients.b[0] = gain * 1.f;
+    coefficients.b[1] = gain * -2.f * z_r;
+    coefficients.b[2] = gain * (z_r * z_r + z_i * z_i);
+  }
+
+  // Assuming poles at (p_r + p_i*i) and (p_r - p_i*i).
+  coefficients.a[0] = -2.f * p_r;
+  coefficients.a[1] = p_r * p_r + p_i * p_i;
+}
+
+void CascadedBiQuadFilter::BiQuad::BiQuad::Reset() {
+  x[0] = x[1] = y[0] = y[1] = 0.f;
+}
+
+CascadedBiQuadFilter::CascadedBiQuadFilter(
+    const CascadedBiQuadFilter::BiQuadCoefficients& coefficients,
+    size_t num_biquads)
+    : biquads_(num_biquads, BiQuad(coefficients)) {}
+
+CascadedBiQuadFilter::CascadedBiQuadFilter(
+    const std::vector<CascadedBiQuadFilter::BiQuadParam>& biquad_params) {
+  for (const auto& param : biquad_params) {
+    biquads_.push_back(BiQuad(param));
+  }
+}
+
+CascadedBiQuadFilter::~CascadedBiQuadFilter() = default;
+
+void CascadedBiQuadFilter::Process(rtc::ArrayView<const float> x,
+                                   rtc::ArrayView<float> y) {
+  if (biquads_.size() > 0) {
+    ApplyBiQuad(x, y, &biquads_[0]);
+    for (size_t k = 1; k < biquads_.size(); ++k) {
+      ApplyBiQuad(y, y, &biquads_[k]);
+    }
+  } else {
+    std::copy(x.begin(), x.end(), y.begin());
+  }
+}
+
+void CascadedBiQuadFilter::Process(rtc::ArrayView<float> y) {
+  for (auto& biquad : biquads_) {
+    ApplyBiQuad(y, y, &biquad);
+  }
+}
+
+void CascadedBiQuadFilter::Reset() {
+  for (auto& biquad : biquads_) {
+    biquad.Reset();
+  }
+}
+
+void CascadedBiQuadFilter::ApplyBiQuad(rtc::ArrayView<const float> x,
+                                       rtc::ArrayView<float> y,
+                                       CascadedBiQuadFilter::BiQuad* biquad) {
+  RTC_DCHECK_EQ(x.size(), y.size());
+  const float c_a_0 = biquad->coefficients.a[0];
+  const float c_a_1 = biquad->coefficients.a[1];
+  const float c_b_0 = biquad->coefficients.b[0];
+  const float c_b_1 = biquad->coefficients.b[1];
+  const float c_b_2 = biquad->coefficients.b[2];
+  float m_x_0 = biquad->x[0];
+  float m_x_1 = biquad->x[1];
+  float m_y_0 = biquad->y[0];
+  float m_y_1 = biquad->y[1];
+  for (size_t k = 0; k < x.size(); ++k) {
+    const float tmp = x[k];
+    y[k] = c_b_0 * tmp + c_b_1 * m_x_0 + c_b_2 * m_x_1 - c_a_0 * m_y_0 -
+           c_a_1 * m_y_1;
+    m_x_1 = m_x_0;
+    m_x_0 = tmp;
+    m_y_1 = m_y_0;
+    m_y_0 = y[k];
+  }
+  biquad->x[0] = m_x_0;
+  biquad->x[1] = m_x_1;
+  biquad->y[0] = m_y_0;
+  biquad->y[1] = m_y_1;
+}
+
+}  // namespace webrtc

VocieProcess/modules/audio_processing/utility/cascaded_biquad_filter.h

@@ -0,0 +1,80 @@
+/*
+ *  Copyright (c) 2016 The WebRTC project authors. All Rights Reserved.
+ *
+ *  Use of this source code is governed by a BSD-style license
+ *  that can be found in the LICENSE file in the root of the source
+ *  tree. An additional intellectual property rights grant can be found
+ *  in the file PATENTS.  All contributing project authors may
+ *  be found in the AUTHORS file in the root of the source tree.
+ */
+
+#ifndef MODULES_AUDIO_PROCESSING_UTILITY_CASCADED_BIQUAD_FILTER_H_
+#define MODULES_AUDIO_PROCESSING_UTILITY_CASCADED_BIQUAD_FILTER_H_
+
+#include <stddef.h>
+
+#include <complex>
+#include <vector>
+
+#include "api/array_view.h"
+
+namespace webrtc {
+
+// Applies a number of biquads in a cascaded manner. The filter implementation
+// is direct form 1.
+class CascadedBiQuadFilter {
+ public:
+  struct BiQuadParam {
+    BiQuadParam(std::complex<float> zero,
+                std::complex<float> pole,
+                float gain,
+                bool mirror_zero_along_i_axis = false);
+    explicit BiQuadParam(const BiQuadParam&);
+    std::complex<float> zero;
+    std::complex<float> pole;
+    float gain;
+    bool mirror_zero_along_i_axis;
+  };
+
+  struct BiQuadCoefficients {
+    float b[3];
+    float a[2];
+  };
+
+  struct BiQuad {
+    explicit BiQuad(const BiQuadCoefficients& coefficients)
+        : coefficients(coefficients), x(), y() {}
+    explicit BiQuad(const CascadedBiQuadFilter::BiQuadParam& param);
+    void Reset();
+    BiQuadCoefficients coefficients;
+    float x[2];
+    float y[2];
+  };
+
+  CascadedBiQuadFilter(
+      const CascadedBiQuadFilter::BiQuadCoefficients& coefficients,
+      size_t num_biquads);
+  explicit CascadedBiQuadFilter(
+      const std::vector<CascadedBiQuadFilter::BiQuadParam>& biquad_params);
+  ~CascadedBiQuadFilter();
+  CascadedBiQuadFilter(const CascadedBiQuadFilter&) = delete;
+  CascadedBiQuadFilter& operator=(const CascadedBiQuadFilter&) = delete;
+
+  // Applies the biquads on the values in x in order to form the output in y.
+  void Process(rtc::ArrayView<const float> x, rtc::ArrayView<float> y);
+  // Applies the biquads on the values in y in an in-place manner.
+  void Process(rtc::ArrayView<float> y);
+  // Resets the filter to its initial state.
+  void Reset();
+
+ private:
+  void ApplyBiQuad(rtc::ArrayView<const float> x,
+                   rtc::ArrayView<float> y,
+                   CascadedBiQuadFilter::BiQuad* biquad);
+
+  std::vector<BiQuad> biquads_;
+};
+
+}  // namespace webrtc
+
+#endif  // MODULES_AUDIO_PROCESSING_UTILITY_CASCADED_BIQUAD_FILTER_H_