186eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung/* 286eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * Copyright (C) 2013 The Android Open Source Project 386eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * 486eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * Licensed under the Apache License, Version 2.0 (the "License"); 586eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * you may not use this file except in compliance with the License. 686eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * You may obtain a copy of the License at 786eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * 886eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * http://www.apache.org/licenses/LICENSE-2.0 986eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * 1086eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * Unless required by applicable law or agreed to in writing, software 1186eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * distributed under the License is distributed on an "AS IS" BASIS, 1286eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 1386eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * See the License for the specific language governing permissions and 1486eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * limitations under the License. 1586eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung */ 1686eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung 1786eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung#ifndef ANDROID_AUDIO_RESAMPLER_FIR_GEN_H 1886eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung#define ANDROID_AUDIO_RESAMPLER_FIR_GEN_H 1986eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung 2036802bd18b7b4e8c87fa019c7e3068bee330d174Dan Albert#include "utils/Compat.h" 2136802bd18b7b4e8c87fa019c7e3068bee330d174Dan Albert 2286eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hungnamespace android { 2386eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung 2486eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung/* 256582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * generates a sine wave at equal steps. 2686eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * 276582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * As most of our functions use sine or cosine at equal steps, 286582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * it is very efficient to compute them that way (single multiply and subtract), 296582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * rather than invoking the math library sin() or cos() each time. 306582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 316582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * SineGen uses Goertzel's Algorithm (as a generator not a filter) 326582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * to calculate sine(wstart + n * wstep) or cosine(wstart + n * wstep) 336582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * by stepping through 0, 1, ... n. 346582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 356582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * e^i(wstart+wstep) = 2cos(wstep) * e^i(wstart) - e^i(wstart-wstep) 366582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 376582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * or looking at just the imaginary sine term, as the cosine follows identically: 386582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 396582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * sin(wstart+wstep) = 2cos(wstep) * sin(wstart) - sin(wstart-wstep) 406582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 416582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * Goertzel's algorithm is more efficient than the angle addition formula, 426582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * e^i(wstart+wstep) = e^i(wstart) * e^i(wstep), which takes up to 436582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 4 multiplies and 2 adds (or 3* and 3+) and requires both sine and 446582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * cosine generation due to the complex * complex multiply (full rotation). 456582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 466582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * See: http://en.wikipedia.org/wiki/Goertzel_algorithm 4786eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * 4886eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung */ 4986eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung 506582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hungclass SineGen { 516582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hungpublic: 526582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung SineGen(double wstart, double wstep, bool cosine = false) { 536582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung if (cosine) { 546582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung mCurrent = cos(wstart); 556582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung mPrevious = cos(wstart - wstep); 566582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung } else { 576582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung mCurrent = sin(wstart); 586582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung mPrevious = sin(wstart - wstep); 596582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung } 606582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung mTwoCos = 2.*cos(wstep); 6186eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung } 626582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung SineGen(double expNow, double expPrev, double twoCosStep) { 636582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung mCurrent = expNow; 646582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung mPrevious = expPrev; 656582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung mTwoCos = twoCosStep; 666582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung } 676582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung inline double value() const { 686582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung return mCurrent; 696582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung } 706582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung inline void advance() { 716582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung double tmp = mCurrent; 726582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung mCurrent = mCurrent*mTwoCos - mPrevious; 736582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung mPrevious = tmp; 746582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung } 756582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung inline double valueAdvance() { 766582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung double tmp = mCurrent; 776582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung mCurrent = mCurrent*mTwoCos - mPrevious; 786582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung mPrevious = tmp; 796582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung return tmp; 806582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung } 816582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung 826582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hungprivate: 836582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung double mCurrent; // current value of sine/cosine 846582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung double mPrevious; // previous value of sine/cosine 856582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung double mTwoCos; // stepping factor 866582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung}; 876582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung 886582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung/* 896582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * generates a series of sine generators, phase offset by fixed steps. 906582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 916582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * This is used to generate polyphase sine generators, one per polyphase 926582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * in the filter code below. 936582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 946582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * The SineGen returned by value() starts at innerStart = outerStart + n*outerStep; 956582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * increments by innerStep. 966582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 976582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung */ 986582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung 996582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hungclass SineGenGen { 1006582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hungpublic: 1016582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung SineGenGen(double outerStart, double outerStep, double innerStep, bool cosine = false) 1026582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung : mSineInnerCur(outerStart, outerStep, cosine), 1036582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung mSineInnerPrev(outerStart-innerStep, outerStep, cosine) 1046582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung { 1056582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung mTwoCos = 2.*cos(innerStep); 1066582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung } 1076582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung inline SineGen value() { 1086582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung return SineGen(mSineInnerCur.value(), mSineInnerPrev.value(), mTwoCos); 1096582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung } 1106582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung inline void advance() { 1116582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung mSineInnerCur.advance(); 1126582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung mSineInnerPrev.advance(); 1136582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung } 1146582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung inline SineGen valueAdvance() { 1156582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung return SineGen(mSineInnerCur.valueAdvance(), mSineInnerPrev.valueAdvance(), mTwoCos); 1166582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung } 1176582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung 1186582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hungprivate: 1196582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung SineGen mSineInnerCur; // generate the inner sine values (stepped by outerStep). 1206582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung SineGen mSineInnerPrev; // generate the inner sine previous values 1216582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung // (behind by innerStep, stepped by outerStep). 1226582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung double mTwoCos; // the inner stepping factor for the returned SineGen. 1236582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung}; 12486eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung 12586eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hungstatic inline double sqr(double x) { 12686eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung return x * x; 12786eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung} 12886eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung 12986eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung/* 13086eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * rounds a double to the nearest integer for FIR coefficients. 13186eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * 13286eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * One variant uses noise shaping, which must keep error history 13386eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * to work (the err parameter, initialized to 0). 13486eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * The other variant is a non-noise shaped version for 13586eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * S32 coefficients (noise shaping doesn't gain much). 13686eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * 1376582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * Caution: No bounds saturation is applied, but isn't needed in this case. 13886eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * 13986eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * @param x is the value to round. 14086eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * 14186eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * @param maxval is the maximum integer scale factor expressed as an int64 (for headroom). 14286eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * Typically this may be the maximum positive integer+1 (using the fact that double precision 14386eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * FIR coefficients generated here are never that close to 1.0 to pose an overflow condition). 14486eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * 14586eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * @param err is the previous error (actual - rounded) for the previous rounding op. 1466582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * For 16b coefficients this can improve stopband dB performance by up to 2dB. 1476582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 1486582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * Many variants exist for the noise shaping: http://en.wikipedia.org/wiki/Noise_shaping 14986eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * 15086eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung */ 15186eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung 15286eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hungstatic inline int64_t toint(double x, int64_t maxval, double& err) { 15386eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung double val = x * maxval; 1546582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung double ival = floor(val + 0.5 + err*0.2); 15586eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung err = val - ival; 15686eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung return static_cast<int64_t>(ival); 15786eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung} 15886eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung 15986eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hungstatic inline int64_t toint(double x, int64_t maxval) { 16086eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung return static_cast<int64_t>(floor(x * maxval + 0.5)); 16186eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung} 16286eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung 16386eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung/* 16486eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * Modified Bessel function of the first kind 16586eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * http://en.wikipedia.org/wiki/Bessel_function 16686eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * 1676582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * The formulas are taken from Abramowitz and Stegun, 1686582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * _Handbook of Mathematical Functions_ (links below): 16986eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * 17086eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * http://people.math.sfu.ca/~cbm/aands/page_375.htm 17186eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * http://people.math.sfu.ca/~cbm/aands/page_378.htm 17286eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * 17386eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * http://dlmf.nist.gov/10.25 17486eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * http://dlmf.nist.gov/10.40 17586eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * 17686eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * Note we assume x is nonnegative (the function is symmetric, 17786eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * pass in the absolute value as needed). 17886eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * 17986eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * Constants are compile time derived with templates I0Term<> and 18086eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * I0ATerm<> to the precision of the compiler. The series can be expanded 18186eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * to any precision needed, but currently set around 24b precision. 18286eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * 18386eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * We use a bit of template math here, constexpr would probably be 18486eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * more appropriate for a C++11 compiler. 18586eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * 1866582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * For the intermediate range 3.75 < x < 15, we use minimax polynomial fit. 1876582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 18886eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung */ 18986eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung 19086eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hungtemplate <int N> 19186eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hungstruct I0Term { 19236802bd18b7b4e8c87fa019c7e3068bee330d174Dan Albert static const CONSTEXPR double value = I0Term<N-1>::value / (4. * N * N); 19386eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung}; 19486eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung 19586eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hungtemplate <> 19686eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hungstruct I0Term<0> { 19736802bd18b7b4e8c87fa019c7e3068bee330d174Dan Albert static const CONSTEXPR double value = 1.; 19886eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung}; 19986eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung 20086eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hungtemplate <int N> 20186eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hungstruct I0ATerm { 20236802bd18b7b4e8c87fa019c7e3068bee330d174Dan Albert static const CONSTEXPR double value = I0ATerm<N-1>::value * (2.*N-1.) * (2.*N-1.) / (8. * N); 20386eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung}; 20486eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung 20586eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hungtemplate <> 20686eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hungstruct I0ATerm<0> { // 1/sqrt(2*PI); 207b187de1ada34a9023c05d020a4592686ba761278Glenn Kasten static const CONSTEXPR double value = 208b187de1ada34a9023c05d020a4592686ba761278Glenn Kasten 0.398942280401432677939946059934381868475858631164934657665925; 20986eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung}; 21086eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung 2116582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung#if USE_HORNERS_METHOD 2126582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung/* Polynomial evaluation of A + Bx + Cx^2 + Dx^3 + ... 2136582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * using Horner's Method: http://en.wikipedia.org/wiki/Horner's_method 2146582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 2156582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * This has fewer multiplications than Estrin's method below, but has back to back 2166582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * floating point dependencies. 2176582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 2186582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * On ARM this appears to work slower, so USE_HORNERS_METHOD is not default enabled. 2196582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung */ 2206582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung 2216582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hunginline double Poly2(double A, double B, double x) { 2226582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung return A + x * B; 2236582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung} 2246582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung 2256582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hunginline double Poly4(double A, double B, double C, double D, double x) { 2266582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung return A + x * (B + x * (C + x * (D))); 2276582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung} 2286582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung 2296582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hunginline double Poly7(double A, double B, double C, double D, double E, double F, double G, 2306582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung double x) { 2316582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung return A + x * (B + x * (C + x * (D + x * (E + x * (F + x * (G)))))); 2326582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung} 2336582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung 2346582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hunginline double Poly9(double A, double B, double C, double D, double E, double F, double G, 2356582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung double H, double I, double x) { 2366582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung return A + x * (B + x * (C + x * (D + x * (E + x * (F + x * (G + x * (H + x * (I)))))))); 2376582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung} 2386582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung 2396582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung#else 2406582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung/* Polynomial evaluation of A + Bx + Cx^2 + Dx^3 + ... 2416582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * using Estrin's Method: http://en.wikipedia.org/wiki/Estrin's_scheme 2426582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 2436582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * This is typically faster, perhaps gains about 5-10% overall on ARM processors 2446582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * over Horner's method above. 2456582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung */ 2466582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung 2476582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hunginline double Poly2(double A, double B, double x) { 2486582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung return A + B * x; 2496582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung} 2506582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung 2516582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hunginline double Poly3(double A, double B, double C, double x, double x2) { 2526582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung return Poly2(A, B, x) + C * x2; 2536582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung} 2546582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung 2556582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hunginline double Poly3(double A, double B, double C, double x) { 2566582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung return Poly2(A, B, x) + C * x * x; 2576582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung} 2586582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung 2596582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hunginline double Poly4(double A, double B, double C, double D, double x, double x2) { 2606582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung return Poly2(A, B, x) + Poly2(C, D, x) * x2; // same as poly2(poly2, poly2, x2); 2616582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung} 2626582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung 2636582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hunginline double Poly4(double A, double B, double C, double D, double x) { 2646582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung return Poly4(A, B, C, D, x, x * x); 2656582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung} 2666582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung 2676582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hunginline double Poly7(double A, double B, double C, double D, double E, double F, double G, 2686582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung double x) { 2696582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung double x2 = x * x; 2706582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung return Poly4(A, B, C, D, x, x2) + Poly3(E, F, G, x, x2) * (x2 * x2); 2716582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung} 2726582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung 2736582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hunginline double Poly8(double A, double B, double C, double D, double E, double F, double G, 2746582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung double H, double x, double x2, double x4) { 2756582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung return Poly4(A, B, C, D, x, x2) + Poly4(E, F, G, H, x, x2) * x4; 2766582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung} 2776582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung 2786582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hunginline double Poly9(double A, double B, double C, double D, double E, double F, double G, 2796582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung double H, double I, double x) { 2806582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung double x2 = x * x; 2816582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung#if 1 2826582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung // It does not seem faster to explicitly decompose Poly8 into Poly4, but 2836582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung // could depend on compiler floating point scheduling. 2846582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung double x4 = x2 * x2; 2856582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung return Poly8(A, B, C, D, E, F, G, H, x, x2, x4) + I * (x4 * x4); 2866582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung#else 2876582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung double val = Poly4(A, B, C, D, x, x2); 2886582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung double x4 = x2 * x2; 2896582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung return val + Poly4(E, F, G, H, x, x2) * x4 + I * (x4 * x4); 2906582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung#endif 2916582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung} 2926582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung#endif 2936582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung 29486eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hungstatic inline double I0(double x) { 2956582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung if (x < 3.75) { 2966582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung x *= x; 2976582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung return Poly7(I0Term<0>::value, I0Term<1>::value, 2986582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung I0Term<2>::value, I0Term<3>::value, 2996582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung I0Term<4>::value, I0Term<5>::value, 3006582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung I0Term<6>::value, x); // e < 1.6e-7 3016582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung } 3026582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung if (1) { 3036582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung /* 3046582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * Series expansion coefs are easy to calculate, but are expanded around 0, 3056582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * so error is unequal over the interval 0 < x < 3.75, the error being 3066582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * significantly better near 0. 3076582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 3086582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * A better solution is to use precise minimax polynomial fits. 3096582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 3106582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * We use a slightly more complicated solution for 3.75 < x < 15, based on 3116582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * the tables in Blair and Edwards, "Stable Rational Minimax Approximations 3126582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * to the Modified Bessel Functions I0(x) and I1(x)", Chalk Hill Nuclear Laboratory, 3136582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * AECL-4928. 3146582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 3156582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * http://www.iaea.org/inis/collection/NCLCollectionStore/_Public/06/178/6178667.pdf 3166582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 3176582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * See Table 11 for 0 < x < 15; e < 10^(-7.13). 3186582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 3196582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * Note: Beta cannot exceed 15 (hence Stopband cannot exceed 144dB = 24b). 3206582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 3216582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * This speeds up overall computation by about 40% over using the else clause below, 3226582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * which requires sqrt and exp. 3236582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 3246582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung */ 3256582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung 32686eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung x *= x; 3276582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung double num = Poly9(-0.13544938430e9, -0.33153754512e8, 3286582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung -0.19406631946e7, -0.48058318783e5, 3296582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung -0.63269783360e3, -0.49520779070e1, 3306582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung -0.24970910370e-1, -0.74741159550e-4, 3316582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung -0.18257612460e-6, x); 3326582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung double y = x - 225.; // reflection around 15 (squared) 3336582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung double den = Poly4(-0.34598737196e8, 0.23852643181e6, 3346582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung -0.70699387620e3, 0.10000000000e1, y); 3356582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung return num / den; 3366582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung 3376582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung#if IO_EXTENDED_BETA 3386582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung /* Table 42 for x > 15; e < 10^(-8.11). 3396582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * This is used for Beta>15, but is disabled here as 3406582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * we never use Beta that high. 3416582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 3426582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * NOTE: This should be enabled only for x > 15. 3436582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung */ 3446582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung 3456582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung double y = 1./x; 3466582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung double z = y - (1./15); 3476582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung double num = Poly2(0.415079861746e1, -0.5149092496e1, z); 3486582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung double den = Poly3(0.103150763823e2, -0.14181687413e2, 3496582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung 0.1000000000e1, z); 3506582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung return exp(x) * sqrt(y) * num / den; 3516582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung#endif 3526582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung } else { 3536582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung /* 3546582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * NOT USED, but reference for large Beta. 3556582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 3566582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * Abramowitz and Stegun asymptotic formula. 3576582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * works for x > 3.75. 3586582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung */ 3596582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung double y = 1./x; 3606582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung return exp(x) * sqrt(y) * 3616582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung // note: reciprocal squareroot may be easier! 3626582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung // http://en.wikipedia.org/wiki/Fast_inverse_square_root 3636582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung Poly9(I0ATerm<0>::value, I0ATerm<1>::value, 3646582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung I0ATerm<2>::value, I0ATerm<3>::value, 3656582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung I0ATerm<4>::value, I0ATerm<5>::value, 3666582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung I0ATerm<6>::value, I0ATerm<7>::value, 3676582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung I0ATerm<8>::value, y); // (... e) < 1.9e-7 36886eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung } 36986eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung} 37086eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung 371bafa561d0c9363c5307b6b1daa498bd3ae36089dAndy Hung/* A speed optimized version of the Modified Bessel I0() which incorporates 372bafa561d0c9363c5307b6b1daa498bd3ae36089dAndy Hung * the sqrt and numerator multiply and denominator divide into the computation. 373bafa561d0c9363c5307b6b1daa498bd3ae36089dAndy Hung * This speeds up filter computation by about 10-15%. 374bafa561d0c9363c5307b6b1daa498bd3ae36089dAndy Hung */ 375bafa561d0c9363c5307b6b1daa498bd3ae36089dAndy Hungstatic inline double I0SqrRat(double x2, double num, double den) { 376bafa561d0c9363c5307b6b1daa498bd3ae36089dAndy Hung if (x2 < (3.75 * 3.75)) { 377bafa561d0c9363c5307b6b1daa498bd3ae36089dAndy Hung return Poly7(I0Term<0>::value, I0Term<1>::value, 378bafa561d0c9363c5307b6b1daa498bd3ae36089dAndy Hung I0Term<2>::value, I0Term<3>::value, 379bafa561d0c9363c5307b6b1daa498bd3ae36089dAndy Hung I0Term<4>::value, I0Term<5>::value, 380bafa561d0c9363c5307b6b1daa498bd3ae36089dAndy Hung I0Term<6>::value, x2) * num / den; // e < 1.6e-7 381bafa561d0c9363c5307b6b1daa498bd3ae36089dAndy Hung } 382bafa561d0c9363c5307b6b1daa498bd3ae36089dAndy Hung num *= Poly9(-0.13544938430e9, -0.33153754512e8, 383bafa561d0c9363c5307b6b1daa498bd3ae36089dAndy Hung -0.19406631946e7, -0.48058318783e5, 384bafa561d0c9363c5307b6b1daa498bd3ae36089dAndy Hung -0.63269783360e3, -0.49520779070e1, 385bafa561d0c9363c5307b6b1daa498bd3ae36089dAndy Hung -0.24970910370e-1, -0.74741159550e-4, 386bafa561d0c9363c5307b6b1daa498bd3ae36089dAndy Hung -0.18257612460e-6, x2); // e < 10^(-7.13). 387bafa561d0c9363c5307b6b1daa498bd3ae36089dAndy Hung double y = x2 - 225.; // reflection around 15 (squared) 388bafa561d0c9363c5307b6b1daa498bd3ae36089dAndy Hung den *= Poly4(-0.34598737196e8, 0.23852643181e6, 389bafa561d0c9363c5307b6b1daa498bd3ae36089dAndy Hung -0.70699387620e3, 0.10000000000e1, y); 390bafa561d0c9363c5307b6b1daa498bd3ae36089dAndy Hung return num / den; 391bafa561d0c9363c5307b6b1daa498bd3ae36089dAndy Hung} 392bafa561d0c9363c5307b6b1daa498bd3ae36089dAndy Hung 39386eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung/* 39486eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * calculates the transition bandwidth for a Kaiser filter 39586eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * 3966582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * Formula 3.2.8, Vaidyanathan, _Multirate Systems and Filter Banks_, p. 48 3976582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * Formula 7.76, Oppenheim and Schafer, _Discrete-time Signal Processing, 3e_, p. 542 39886eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * 39986eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * @param halfNumCoef is half the number of coefficients per filter phase. 4006582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 40186eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * @param stopBandAtten is the stop band attenuation desired. 4026582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 40386eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * @return the transition bandwidth in normalized frequency (0 <= f <= 0.5) 40486eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung */ 40586eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hungstatic inline double firKaiserTbw(int halfNumCoef, double stopBandAtten) { 4066582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung return (stopBandAtten - 7.95)/((2.*14.36)*halfNumCoef); 40786eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung} 40886eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung 40986eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung/* 4106582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * calculates the fir transfer response of the overall polyphase filter at w. 4116582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 4126582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * Calculates the DTFT transfer coefficient H(w) for 0 <= w <= PI, utilizing the 4136582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * fact that h[n] is symmetric (cosines only, no complex arithmetic). 4146582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 4156582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * We use Goertzel's algorithm to accelerate the computation to essentially 4166582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * a single multiply and 2 adds per filter coefficient h[]. 41786eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * 4186582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * Be careful be careful to consider that h[n] is the overall polyphase filter, 4196582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * with L phases, so rescaling H(w)/L is probably what you expect for "unity gain", 4206582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * as you only use one of the polyphases at a time. 42186eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung */ 42286eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hungtemplate <typename T> 42386eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hungstatic inline double firTransfer(const T* coef, int L, int halfNumCoef, double w) { 4246582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung double accum = static_cast<double>(coef[0])*0.5; // "center coefficient" from first bank 4256582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung coef += halfNumCoef; // skip first filterbank (picked up by the last filterbank). 4266582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung#if SLOW_FIRTRANSFER 4276582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung /* Original code for reference. This is equivalent to the code below, but slower. */ 42886eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung for (int i=1 ; i<=L ; ++i) { 42986eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung for (int j=0, ix=i ; j<halfNumCoef ; ++j, ix+=L) { 43086eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung accum += cos(ix*w)*static_cast<double>(*coef++); 43186eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung } 43286eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung } 4336582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung#else 4346582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung /* 4356582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * Our overall filter is stored striped by polyphases, not a contiguous h[n]. 4366582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * We could fetch coefficients in a non-contiguous fashion 4376582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * but that will not scale to vector processing. 4386582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 4396582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * We apply Goertzel's algorithm directly to each polyphase filter bank instead of 4406582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * using cosine generation/multiplication, thereby saving one multiply per inner loop. 4416582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 4426582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * See: http://en.wikipedia.org/wiki/Goertzel_algorithm 4436582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * Also: Oppenheim and Schafer, _Discrete Time Signal Processing, 3e_, p. 720. 4446582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 4456582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * We use the basic recursion to incorporate the cosine steps into real sequence x[n]: 4466582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * s[n] = x[n] + (2cosw)*s[n-1] + s[n-2] 4476582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 4486582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * y[n] = s[n] - e^(iw)s[n-1] 4496582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * = sum_{k=-\infty}^{n} x[k]e^(-iw(n-k)) 4506582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * = e^(-iwn) sum_{k=0}^{n} x[k]e^(iwk) 4516582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 4526582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * The summation contains the frequency steps we want multiplied by the source 4536582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * (similar to a DTFT). 4546582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 4556582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * Using symmetry, and just the real part (be careful, this must happen 4566582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * after any internal complex multiplications), the polyphase filterbank 4576582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * transfer function is: 4586582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 4596582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * Hpp[n, w, w_0] = sum_{k=0}^{n} x[k] * cos(wk + w_0) 4606582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * = Re{ e^(iwn + iw_0) y[n]} 4616582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * = cos(wn+w_0) * s[n] - cos(w(n+1)+w_0) * s[n-1] 4626582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 4636582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * using the fact that s[n] of real x[n] is real. 4646582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 4656582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung */ 4666582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung double dcos = 2. * cos(L*w); 4676582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung int start = ((halfNumCoef)*L + 1); 4686582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung SineGen cc((start - L) * w, w, true); // cosine 4696582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung SineGen cp(start * w, w, true); // cosine 4706582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung for (int i=1 ; i<=L ; ++i) { 4716582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung double sc = 0; 4726582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung double sp = 0; 4736582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung for (int j=0 ; j<halfNumCoef ; ++j) { 4746582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung double tmp = sc; 4756582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung sc = static_cast<double>(*coef++) + dcos*sc - sp; 4766582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung sp = tmp; 4776582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung } 4786582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung // If we are awfully clever, we can apply Goertzel's algorithm 4796582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung // again on the sc and sp sequences returned here. 4806582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung accum += cc.valueAdvance() * sc - cp.valueAdvance() * sp; 4816582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung } 4826582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung#endif 48386eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung return accum*2.; 48486eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung} 48586eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung 48686eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung/* 4876582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * evaluates the minimum and maximum |H(f)| bound in a band region. 4886582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 4896582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * This is usually done with equally spaced increments in the target band in question. 4906582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * The passband is often very small, and sampled that way. The stopband is often much 4916582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * larger. 4926582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 4936582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * We use the fact that the overall polyphase filter has an additional bank at the end 4946582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * for interpolation; hence it is overspecified for the H(f) computation. Thus the 4956582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * first polyphase is never actually checked, excepting its first term. 4966582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 4976582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * In this code we use the firTransfer() evaluator above, which uses Goertzel's 4986582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * algorithm to calculate the transfer function at each point. 4996582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 5006582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * TODO: An alternative with equal spacing is the FFT/DFT. An alternative with unequal 5016582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * spacing is a chirp transform. 50286eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * 50386eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * @param coef is the designed polyphase filter banks 50486eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * 50586eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * @param L is the number of phases (for interpolation) 50686eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * 50786eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * @param halfNumCoef should be half the number of coefficients for a single 50886eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * polyphase. 50986eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * 51086eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * @param fstart is the normalized frequency start. 51186eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * 51286eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * @param fend is the normalized frequency end. 51386eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * 51486eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * @param steps is the number of steps to take (sampling) between frequency start and end 51586eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * 51686eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * @param firMin returns the minimum transfer |H(f)| found 51786eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * 51886eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * @param firMax returns the maximum transfer |H(f)| found 51986eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * 52086eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * 0 <= f <= 0.5. 52186eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * This is used to test passband and stopband performance. 52286eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung */ 52386eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hungtemplate <typename T> 52486eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hungstatic void testFir(const T* coef, int L, int halfNumCoef, 52586eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung double fstart, double fend, int steps, double &firMin, double &firMax) { 52686eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung double wstart = fstart*(2.*M_PI); 52786eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung double wend = fend*(2.*M_PI); 52886eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung double wstep = (wend - wstart)/steps; 52986eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung double fmax, fmin; 53086eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung double trf = firTransfer(coef, L, halfNumCoef, wstart); 53186eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung if (trf<0) { 53286eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung trf = -trf; 53386eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung } 53486eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung fmin = fmax = trf; 53586eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung wstart += wstep; 53686eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung for (int i=1; i<steps; ++i) { 53786eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung trf = firTransfer(coef, L, halfNumCoef, wstart); 53886eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung if (trf<0) { 53986eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung trf = -trf; 54086eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung } 54186eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung if (trf>fmax) { 54286eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung fmax = trf; 54386eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung } 54486eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung else if (trf<fmin) { 54586eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung fmin = trf; 54686eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung } 54786eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung wstart += wstep; 54886eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung } 5496bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung // renormalize - this is needed for integer filter types, use 1 for float or double. 5506bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung constexpr int64_t integralShift = std::is_integral<T>::value ? (sizeof(T) * 8 - 1) : 0; 5516bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung const double norm = 1. / (L << integralShift); 55286eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung 55386eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung firMin = fmin * norm; 55486eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung firMax = fmax * norm; 55586eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung} 55686eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung 55786eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung/* 5586582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * evaluates the |H(f)| lowpass band characteristics. 5596582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 5606582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * This function tests the lowpass characteristics for the overall polyphase filter, 5616bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung * and is used to verify the design. 5626bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung * 5636bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung * For a polyphase filter (L > 1), typically fp should be set to the 5646582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * passband normalized frequency from 0 to 0.5 for the overall filter (thus it 5656582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * is the designed polyphase bank value / L). Likewise for fs. 5666bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung * Similarly the stopSteps should be L * passSteps for equivalent accuracy. 5676582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 5686582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * @param coef is the designed polyphase filter banks 5696582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 5706582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * @param L is the number of phases (for interpolation) 5716582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 5726582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * @param halfNumCoef should be half the number of coefficients for a single 5736582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * polyphase. 5746582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 5756582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * @param fp is the passband normalized frequency, 0 < fp < fs < 0.5. 5766582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 5776582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * @param fs is the stopband normalized frequency, 0 < fp < fs < 0.5. 5786582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 5796582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * @param passSteps is the number of passband sampling steps. 5806582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 5816582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * @param stopSteps is the number of stopband sampling steps. 5826582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 5836582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * @param passMin is the minimum value in the passband 5846582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 5856582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * @param passMax is the maximum value in the passband (useful for scaling). This should 5866582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * be less than 1., to avoid sine wave test overflow. 5876582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 5886582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * @param passRipple is the passband ripple. Typically this should be less than 0.1 for 5896582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * an audio filter. Generally speaker/headphone device characteristics will dominate 5906582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * the passband term. 5916582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 5926582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * @param stopMax is the maximum value in the stopband. 5936582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 5946582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * @param stopRipple is the stopband ripple, also known as stopband attenuation. 5956582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * Typically this should be greater than ~80dB for low quality, and greater than 5966582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * ~100dB for full 16b quality, otherwise aliasing may become noticeable. 5976582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 5986582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung */ 5996582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hungtemplate <typename T> 6006582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hungstatic void testFir(const T* coef, int L, int halfNumCoef, 6016582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung double fp, double fs, int passSteps, int stopSteps, 6026582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung double &passMin, double &passMax, double &passRipple, 6036582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung double &stopMax, double &stopRipple) { 6046582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung double fmin, fmax; 6056582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung testFir(coef, L, halfNumCoef, 0., fp, passSteps, fmin, fmax); 6066582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung double d1 = (fmax - fmin)/2.; 6076582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung passMin = fmin; 6086582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung passMax = fmax; 6096582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung passRipple = -20.*log10(1. - d1); // passband ripple 6106582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung testFir(coef, L, halfNumCoef, fs, 0.5, stopSteps, fmin, fmax); 6116582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung // fmin is really not important for the stopband. 6126582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung stopMax = fmax; 6136582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung stopRipple = -20.*log10(fmax); // stopband ripple/attenuation 6146582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung} 6156582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung 6166582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung/* 6176bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung * Estimate the windowed sinc minimum passband value. 6186bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung * 6196bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung * This is the minimum value for a windowed sinc filter in its passband, 6206bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung * which is identical to the scaling required not to cause overflow of a 0dBFS signal. 6216bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung * The actual value used to attenuate the filter amplitude should be slightly 6226bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung * smaller than this (suggest squaring) as this is just an estimate. 6236bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung * 6246bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung * As a windowed sinc has a passband ripple commensurate to the stopband attenuation 6256bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung * due to Gibb's phenomenon from truncating the sinc, we derive this value from 6266bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung * the design stopbandAttenuationDb (a positive value). 6276bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung */ 6286bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hungstatic inline double computeWindowedSincMinimumPassbandValue( 6296bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung double stopBandAttenuationDb) { 6306bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung return 1. - pow(10. /* base */, stopBandAttenuationDb * (-1. / 20.)); 6316bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung} 6326bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung 6336bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung/* 6346bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung * Compute the windowed sinc passband ripple from stopband attenuation. 6356bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung * 6366bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung * As a windowed sinc has an passband ripple commensurate to the stopband attenuation 6376bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung * due to Gibb's phenomenon from truncating the sinc, we derive this value from 6386bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung * the design stopbandAttenuationDb (a positive value). 6396bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung */ 6406bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hungstatic inline double computeWindowedSincPassbandRippleDb( 6416bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung double stopBandAttenuationDb) { 6426bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung return -20. * log10(computeWindowedSincMinimumPassbandValue(stopBandAttenuationDb)); 6436bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung} 6446bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung 6456bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung/* 6466bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung * Kaiser window Beta value 6476bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung * 6486bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung * Formula 3.2.5, 3.2.7, Vaidyanathan, _Multirate Systems and Filter Banks_, p. 48 6496bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung * Formula 7.75, Oppenheim and Schafer, _Discrete-time Signal Processing, 3e_, p. 542 6506bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung * 6516bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung * See also: http://melodi.ee.washington.edu/courses/ee518/notes/lec17.pdf 6526bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung * 6536bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung * Kaiser window and beta parameter 6546bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung * 6556bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung * | 0.1102*(A - 8.7) A > 50 6566bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung * Beta = | 0.5842*(A - 21)^0.4 + 0.07886*(A - 21) 21 < A <= 50 6576bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung * | 0. A <= 21 6586bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung * 6596bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung * with A is the desired stop-band attenuation in positive dBFS 6606bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung * 6616bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung * 30 dB 2.210 6626bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung * 40 dB 3.384 6636bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung * 50 dB 4.538 6646bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung * 60 dB 5.658 6656bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung * 70 dB 6.764 6666bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung * 80 dB 7.865 6676bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung * 90 dB 8.960 6686bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung * 100 dB 10.056 6696bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung * 6706bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung * For some values of stopBandAttenuationDb the function may be computed 6716bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung * at compile time. 6726bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung */ 6736bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hungstatic inline constexpr double computeBeta(double stopBandAttenuationDb) { 6746bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung if (stopBandAttenuationDb > 50.) { 6756bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung return 0.1102 * (stopBandAttenuationDb - 8.7); 6766bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung } 6776bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung const double offset = stopBandAttenuationDb - 21.; 6786bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung if (offset > 0.) { 6796bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung return 0.5842 * pow(offset, 0.4) + 0.07886 * offset; 6806bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung } 6816bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung return 0.; 6826bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung} 6836bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung 6846bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung/* 6856582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * Calculates the overall polyphase filter based on a windowed sinc function. 68686eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * 68786eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * The windowed sinc is an odd length symmetric filter of exactly L*halfNumCoef*2+1 68886eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * taps for the entire kernel. This is then decomposed into L+1 polyphase filterbanks. 68986eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * The last filterbank is used for interpolation purposes (and is mostly composed 69086eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * of the first bank shifted by one sample), and is unnecessary if one does 69186eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * not do interpolation. 69286eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * 6936582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * We use the last filterbank for some transfer function calculation purposes, 6946582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * so it needs to be generated anyways. 6956582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung * 69686eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * @param coef is the caller allocated space for coefficients. This should be 69786eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * exactly (L+1)*halfNumCoef in size. 69886eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * 69986eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * @param L is the number of phases (for interpolation) 70086eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * 70186eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * @param halfNumCoef should be half the number of coefficients for a single 70286eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * polyphase. 70386eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * 70486eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * @param stopBandAtten is the stopband value, should be >50dB. 70586eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * 70686eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * @param fcr is cutoff frequency/sampling rate (<0.5). At this point, the energy 70786eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * should be 6dB less. (fcr is where the amplitude drops by half). Use the 70886eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * firKaiserTbw() to calculate the transition bandwidth. fcr is the midpoint 70986eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * between the stop band and the pass band (fstop+fpass)/2. 71086eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * 71186eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung * @param atten is the attenuation (generally slightly less than 1). 71286eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung */ 71386eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung 71486eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hungtemplate <typename T> 71586eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hungstatic inline void firKaiserGen(T* coef, int L, int halfNumCoef, 71686eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung double stopBandAtten, double fcr, double atten) { 71786eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung const int N = L * halfNumCoef; // non-negative half 7186bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung const double beta = computeBeta(stopBandAtten); 7196582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung const double xstep = (2. * M_PI) * fcr / L; 72086eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung const double xfrac = 1. / N; 7216582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung const double yscale = atten * L / (I0(beta) * M_PI); 722bafa561d0c9363c5307b6b1daa498bd3ae36089dAndy Hung const double sqrbeta = sqr(beta); 7236582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung 7246582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung // We use sine generators, which computes sines on regular step intervals. 7256582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung // This speeds up overall computation about 40% from computing the sine directly. 7266582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung 7276582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung SineGenGen sgg(0., xstep, L*xstep); // generates sine generators (one per polyphase) 7286582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung 72986eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung for (int i=0 ; i<=L ; ++i) { // generate an extra set of coefs for interpolation 7306582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung 7316582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung // computation for a single polyphase of the overall filter. 7326582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung SineGen sg = sgg.valueAdvance(); // current sine generator for "j" inner loop. 7336582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung double err = 0; // for noise shaping on int16_t coefficients (over each polyphase) 7346582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung 73586eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung for (int j=0, ix=i ; j<halfNumCoef ; ++j, ix+=L) { 7366582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung double y; 7376582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung if (CC_LIKELY(ix)) { 7386582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung double x = static_cast<double>(ix); 7396582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung 7406582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung // sine generator: sg.valueAdvance() returns sin(ix*xstep); 741bafa561d0c9363c5307b6b1daa498bd3ae36089dAndy Hung // y = I0(beta * sqrt(1.0 - sqr(x * xfrac))) * yscale * sg.valueAdvance() / x; 742bafa561d0c9363c5307b6b1daa498bd3ae36089dAndy Hung y = I0SqrRat(sqrbeta * (1.0 - sqr(x * xfrac)), yscale * sg.valueAdvance(), x); 7436582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung } else { 7446582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung y = 2. * atten * fcr; // center of filter, sinc(0) = 1. 7456582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung sg.advance(); 7466582f2b14a21e630654c5522ef9ad64e80d5058dAndy Hung } 74786eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung 7486bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung if (std::is_same<T, int16_t>::value) { // int16_t needs noise shaping 74986eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung *coef++ = static_cast<T>(toint(y, 1ULL<<(sizeof(T)*8-1), err)); 7506bd378feab93c5f975b9afa09650cdaa1d921b90Andy Hung } else if (std::is_same<T, int32_t>::value) { 75186eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung *coef++ = static_cast<T>(toint(y, 1ULL<<(sizeof(T)*8-1))); 752430b61c72094882bc48693dfc10c256a6ae36ee9Andy Hung } else { // assumed float or double 753430b61c72094882bc48693dfc10c256a6ae36ee9Andy Hung *coef++ = static_cast<T>(y); 75486eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung } 75586eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung } 75686eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung } 75786eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung} 75886eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung 75963238efb0d674758902918e3cdaac322126484b7Glenn Kasten} // namespace android 76086eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung 76186eae0e5931103e040ac2cdd023ef5db252e09f6Andy Hung#endif /*ANDROID_AUDIO_RESAMPLER_FIR_GEN_H*/ 762