示例#1
0
        /*
        private static void Butterfly(WWComplex vFrom0, WWComplex vFrom1, WWComplex wn, WWComplex[] vTo, int toPos) {
            vTo[toPos].CopyFrom(vFrom0);
            var t = new WWComplex(vFrom1);
            t.Mul(wn);
            vTo[toPos].Mul(t);

            vTo[toPos + 1].CopyFrom(vFrom0);
            t.Mul(-1);
            vTo[toPos + 1].Mul(t);
        }
        */
        public WWComplex[] ForwardFft(WWComplex[] aFrom)
        {
            if (aFrom == null || aFrom.Length != mNumPoints) {
                throw new ArgumentOutOfRangeException("aFrom");
            }
            var aTo = new WWComplex[aFrom.Length];

            var aTmp0 = new WWComplex[mNumPoints];
            for (int i=0; i < aTmp0.Length; ++i) {
                aTmp0[i] = new WWComplex(aFrom[mBitReversalTable[i]]);
            }
            var aTmp1 = new WWComplex[mNumPoints];
            for (int i=0; i < aTmp1.Length; ++i) {
                aTmp1[i] = new WWComplex();
            }

            var aTmps = new WWComplex[2][];
            aTmps[0] = aTmp0;
            aTmps[1] = aTmp1;

            for (int i=0; i < mNumStage - 1; ++i) {
                FftStageN(i, aTmps[((i & 1) == 1) ? 1 : 0], aTmps[((i & 1) == 0) ? 1 : 0]);
            }
            FftStageN(mNumStage - 1, aTmps[(((mNumStage - 1) & 1) == 1) ? 1 : 0], aTo);

            return aTo;
        }
示例#2
0
        public WWRadix2Fft(int numPoints)
        {
            if (!WWUtil.IsPowerOfTwo(numPoints) || numPoints < 2) {
                throw new ArgumentException("numPoints must be power of two integer and larger than 2");
            }
            mNumPoints = numPoints;

            mWn = new WWComplex[mNumPoints];
            for (int i=0; i < mNumPoints; ++i) {
                double angle = -2.0 * Math.PI * i / mNumPoints;
                mWn[i] = new WWComplex(Math.Cos(angle), Math.Sin(angle));
            }

            // mNumStage == log_2(mNumPoints)
            int t = mNumPoints;
            for (int i=0; 0 < t; ++i) {
                t >>= 1;
                mNumStage = i;
            }

            mBitReversalTable = new uint[mNumPoints];
            for (uint i=0; i < mNumPoints; ++i) {
                mBitReversalTable[i] = BitReversal(mNumStage, i);
            }
        }
示例#3
0
        public WWComplex[] ForwardFft(double[] timeDomain)
        {
            double[] timeFull = null;
            timeFull = new double[mFftLength];
            if (mFirstTime) {
                System.Diagnostics.Debug.Assert(timeDomain.Length == mFftLength*3/4);
                Array.Copy(timeDomain, 0, timeFull, mFftLength / 4, mFftLength * 3/4);
            } else {
                System.Diagnostics.Debug.Assert(timeDomain.Length == mFftLength / 2);
                Array.Copy(mOverlapInputSamples, 0, timeFull, 0, mFftLength / 2);
                Array.Copy(timeDomain, 0, timeFull, mFftLength / 2, mFftLength / 2);
            }

            // store last half part of input samples for later processing
            Array.Copy(timeFull, mFftLength / 2, mOverlapInputSamples, 0, mFftLength / 2);

            for (int i = 0; i < mFftLength / 4; ++i) {
                timeFull[mFftLength / 4 - i - 1] *= mWindow[mFftLength / 4 + i -1];
                timeFull[mFftLength * 3 / 4 + i] *= mWindow[mFftLength / 4 + i - 1];
            }

            var timeComplex = new WWComplex[mFftLength];
            for (int i = 0; i < timeComplex.Length; ++i) {
                timeComplex[i] = new WWComplex(timeFull[i], 0);
            }

            return mFft.ForwardFft(timeComplex);
        }
示例#4
0
 public static WWComplex[] From(double[] from)
 {
     var to = new WWComplex[from.Length];
     for (int i = 0; i < from.Length; ++i) {
         to[i].real = from[i];
     }
     return to;
 }
示例#5
0
 public static double[] ExtractRealPart(WWComplex[] from)
 {
     var to = new double[from.Length];
     for (int i = 0; i < from.Length; ++i) {
         to[i] = from[i].real;
     }
     return to;
 }
示例#6
0
        public static WWComplex[] Add(WWComplex[] a, WWComplex[] b)
        {
            if (a.Length != b.Length) {
                throw new ArgumentException("input array length mismatch");
            }

            var c = new WWComplex[a.Length];
            for (int i = 0; i < a.Length; ++i) {
                c[i] = WWComplex.Add(a[i], b[i]);
            }
            return c;
        }
示例#7
0
        public static double AverageDistance(WWComplex[] a, WWComplex[] b)
        {
            if (a.Length != b.Length) {
                throw new ArgumentException("input array length mismatch");
            }

            double d = 0.0;
            for (int i=0; i<a.Length; ++i) {
                var s = WWComplex.Sub(a[i], b[i]);
                d += s.Magnitude();
            }

            d /= a.Length;
            return d;
        }
 public WWComplex Mul(WWComplex rhs)
 {
     #if false
     // straightforward but slow
     double tR = real * rhs.real      - imaginary * rhs.imaginary;
     double tI = real * rhs.imaginary + imaginary * rhs.real;
     real      = tR;
     imaginary = tI;
     #else
     // more efficient way
     double k1 = real          * (rhs.real  + rhs.imaginary);
     double k2 = rhs.imaginary * (real      + imaginary);
     double k3 = rhs.real      * (imaginary - real);
     real      = k1 - k2;
     imaginary = k1 + k3;
     #endif
     return this;
 }
        private void InspectFilter(double[] coeff, int nFFT)
        {
            var time = new WWComplex[nFFT];
            for (int i = 0; i < time.Count(); ++i) {
                if (i < coeff.Count()) {
                    time[i] = new WWComplex(coeff[i], 0.0);
                } else {
                    time[i] = new WWComplex(0.0, 0.0);
                }
            }

            var freq = new WWComplex[nFFT];

            var fft = new WWRadix2Fft(nFFT);
            fft.ForwardFft(time, freq);

            for (int i = 0; i < freq.Count()/2; ++i) {
                Console.WriteLine("{0}, {1}, {2}", i, freq[i].Magnitude(), freq[i].Phase());
            }
        }
示例#10
0
        public double[] InverseFft(WWComplex[] freqDomain)
        {
            System.Diagnostics.Debug.Assert(freqDomain.Length == mFftLength);

            var timeComplex = mFft.InverseFft(freqDomain);

            #if false
            for (int i = 0; i < mFftLength; ++i) {
                System.Console.WriteLine("{0},{1}", i + mOffset, timeComplex[i].real);
            }
            #endif
            mOffset += mFftLength / 2;

            // 逆FFT結果の時間ドメインの真ん中データを戻す。
            var result = new double[mFftLength / 2];
            for (int i = 0; i < mFftLength / 2; ++i) {
                result[i] = timeComplex[i + mFftLength / 4].real;
            }

            if (!mFirstTime) {
                // 出力結果の最初の部分を、最後の出力結果の対応部分とミックスする。
                // ブチっという音を抑制するため。
                for (int i = 0; i < mFftLength / mSpliceDenominator; ++i) {
                    double secondGain = (double)i / ( mFftLength / mSpliceDenominator);
                    double firstGain = 1.0 - secondGain;
                    result[i] = firstGain * mLastOutputSamplesTail[i] + secondGain * result[i];
                }
            }

            // 出力結果最後の先の部分を次回処理で使用するため保存する。
            for (int i = 0; i < mFftLength / mSpliceDenominator; ++i) {
                mLastOutputSamplesTail[i] = timeComplex[i + mFftLength *3 / 4].real;
            }

            mFirstTime = false;
            return result;
        }
示例#11
0
 public WWComplex Sub(WWComplex rhs)
 {
     real      -= rhs.real;
     imaginary -= rhs.imaginary;
     return this;
 }
示例#12
0
 public static WWComplex Sub(WWComplex a, WWComplex b)
 {
     var r = new WWComplex(a);
     r.Sub(b);
     return r;
 }
示例#13
0
        private void UpdateZ()
        {
            double scale = mPoleZeroScale[comboBoxPoleZeroScale.SelectedIndex];
            PoleZeroDispMode dispMode = (PoleZeroDispMode)comboBoxPoleZeroDispMode.SelectedIndex;

            var im = new Image();

            var bm = new WriteableBitmap(
                512,
                512,
                96,
                96,
                dispMode == PoleZeroDispMode.Magnitude ? PixelFormats.Gray32Float : PixelFormats.Bgra32,
                null);
            var pxF = new float[bm.PixelHeight * bm.PixelWidth];
            var pxBgra = new int[bm.PixelHeight * bm.PixelWidth];

            im.Source = bm;
            im.Stretch = Stretch.None;
            im.HorizontalAlignment = HorizontalAlignment.Left;
            im.VerticalAlignment   = VerticalAlignment.Top;

            int pos = 0;
            for (int yI = 0; yI < bm.PixelHeight; yI++) {
                for (int xI = 0; xI < bm.PixelWidth; xI++) {
                    double y = 2.6666666666666 / scale * (bm.PixelHeight / 2 - yI) / bm.PixelHeight;
                    double x = 2.6666666666666 / scale * (xI - bm.PixelWidth / 2) / bm.PixelHeight;
                    var z = new WWComplex(x, y);

                    var h = EvalH(z);
                    var hM = h.Magnitude();

                    if (hM < 0.1) {
                        hM = 0.1;
                    }
                    float hL = (float)((Math.Log10(hM) + 1.0f) / 5.0f);
                    pxF[pos] = hL;
                    pxBgra[pos] = PhaseToBgra(h.Phase());
                    ++pos;
                }
            }

            switch (dispMode) {
            case PoleZeroDispMode.Magnitude:
                bm.WritePixels(new Int32Rect(0, 0, bm.PixelWidth, bm.PixelHeight), pxF, bm.BackBufferStride, 0);
                break;
            case PoleZeroDispMode.Phase:
                bm.WritePixels(new Int32Rect(0, 0, bm.PixelWidth, bm.PixelHeight), pxBgra, bm.BackBufferStride, 0);
                break;
            }

            canvasZ.Children.Clear();
            canvasZ.Children.Add(im);

            double circleRadius = 192.0 * scale;
            Ellipse unitCircle = new Ellipse { Width = circleRadius * 2, Height = circleRadius * 2, Stroke = new SolidColorBrush {Color = Colors.Black} };
            unitCircle.Width = circleRadius * 2;
            unitCircle.Height = circleRadius * 2;
            canvasZ.Children.Add(unitCircle);
            Canvas.SetLeft(unitCircle, 256.0 - circleRadius);
            Canvas.SetTop(unitCircle, 256.0 - circleRadius);
            Canvas.SetZIndex(unitCircle, 1);
        }
 public void CopyFrom(WWComplex rhs)
 {
     real      = rhs.real;
     imaginary = rhs.imaginary;
 }
 public WWComplex(WWComplex rhs)
 {
     this.real      = rhs.real;
     this.imaginary = rhs.imaginary;
 }
        public override double[] FilterDo(double[] inPcm)
        {
            System.Diagnostics.Debug.Assert(inPcm.LongLength == NumOfSamplesNeeded());

            var inPcmR = new double[FftLength];
            if (mFirst) {
                Array.Copy(inPcm, 0, inPcmR, OverlapLength, inPcm.LongLength);

                mFirst = false;
            } else {
                System.Diagnostics.Debug.Assert(mOverlapSamples != null
                        && mOverlapSamples.LongLength == OverlapLength*2);

                Array.Copy(mOverlapSamples, 0, inPcmR, 0, OverlapLength * 2);
                mOverlapSamples = null;
                Array.Copy(inPcm, 0, inPcmR, OverlapLength * 2, inPcm.LongLength);
            }

            // inPcmTをFFTしてinPcmFを得る。
            var inPcmT = new WWComplex[FftLength];
            for (int i=0; i < inPcmT.Length; ++i) {
                inPcmT[i] = new WWComplex(inPcmR[i], 0);
            }

            var inPcmF = new WWComplex[FftLength];
            {
                var fft = new WWRadix2Fft(FftLength);
                fft.ForwardFft(inPcmT, inPcmF);
            }
            inPcmT = null;

            // inPcmFを0で水増ししたデータoutPcmFを作って逆FFTしoutPcmTを得る。

            var UPSAMPLE_FFT_LENGTH = Factor * FftLength;

            var outPcmF = new WWComplex[UPSAMPLE_FFT_LENGTH];
            for (int i=0; i < outPcmF.Length; ++i) {
                if (i <= FftLength / 2) {
                    outPcmF[i].CopyFrom(inPcmF[i]);
                    if (i == FftLength / 2) {
                        outPcmF[i].Mul(0.5);
                    }
                } else if (UPSAMPLE_FFT_LENGTH - FftLength / 2 <= i) {
                    int pos = i + FftLength - UPSAMPLE_FFT_LENGTH;
                    outPcmF[i].CopyFrom(inPcmF[pos]);
                    if (outPcmF.Length - FftLength / 2 == i) {
                        outPcmF[i].Mul(0.5);
                    }
                } else {
                    // do nothing
                }
            }
            inPcmF = null;
            var outPcmT = new WWComplex[UPSAMPLE_FFT_LENGTH];
            {
                var fft = new WWRadix2Fft(UPSAMPLE_FFT_LENGTH);
                fft.InverseFft(outPcmF, outPcmT, 1.0 / FftLength);
            }
            outPcmF = null;

            // outPcmTの実数成分を戻り値とする。
            var outPcm = new double[Factor * (FftLength - OverlapLength*2)];
            for (int i=0; i < outPcm.Length; ++i) {
                outPcm[i] = outPcmT[i + Factor * OverlapLength].real;
            }
            outPcmT = null;

            // 次回計算に使用するオーバーラップ部分のデータをmOverlapSamplesに保存。
            mOverlapSamples = new double[OverlapLength * 2];
            Array.Copy(inPcm, inPcm.LongLength - OverlapLength * 2, mOverlapSamples, 0, OverlapLength * 2);

            return outPcm;
        }
示例#17
0
        public override double[] FilterDo(double[] inPcm)
        {
            System.Diagnostics.Debug.Assert(inPcm.LongLength <= NumOfSamplesNeeded());
            var fft = new WWRadix2Fft(FFT_LEN);

            // Overlap and add continuous FFT

            var inTime = new WWComplex[FFT_LEN];

            for (int i = 0; i < inPcm.Length; ++i)
            {
                inTime[i].real = inPcm[i];
            }

            // FFTでinTimeをinFreqに変換
            var inFreq = fft.ForwardFft(inTime);

            inTime = null;

            // FFT後、フィルターHの周波数ドメインデータを掛ける
            var mulFreq = WWComplex.Mul(inFreq, mFilterFreq);

            inFreq = null;

            // inFreqをIFFTしてoutTimeに変換
            var outTime = fft.InverseFft(mulFreq);

            mulFreq = null;

            double [] outReal;
            if (mFirstFilterDo)
            {
                // 最初のFilterDo()のとき、フィルタの遅延サンプル数だけ先頭サンプルを削除する。
                outReal = new double[NumOfSamplesNeeded() - FILTER_DELAY];
                for (int i = 0; i < outReal.Length; ++i)
                {
                    outReal[i] = outTime[i + FILTER_DELAY].real;
                }
                mFirstFilterDo = false;
            }
            else
            {
                outReal = new double[NumOfSamplesNeeded()];
                for (int i = 0; i < outReal.Length; ++i)
                {
                    outReal[i] = outTime[i].real;
                }
            }

            // 前回のIFFT結果の最後のFILTER_LENGTH-1サンプルを先頭に加算する
            if (null != mIfftAddBuffer)
            {
                for (int i = 0; i < mIfftAddBuffer.Length; ++i)
                {
                    outReal[i] += mIfftAddBuffer[i];
                }
            }

            // 今回のIFFT結果の最後のFILTER_LENGTH-1サンプルをmIfftAddBufferとして保存する
            mIfftAddBuffer = new double[FILTER_LENP1];
            for (int i = 0; i < mIfftAddBuffer.Length; ++i)
            {
                mIfftAddBuffer[i] = outTime[outTime.Length - mIfftAddBuffer.Length + i].real;
            }
            outTime = null;

            return(outReal);
        }
示例#18
0
        private void DesignCutoffFilter()
        {
            var fromF = new WWComplex[FILTER_LENP1];

            // バターワースフィルター
            // 1次 = 6dB/oct
            // 2次 = 12dB/oct

            double orderX2 = 2.0 * (FilterSlopeDbOct / 6.0);

            double cutoffRatio = CutoffFrequency / (SampleRate / 2);

            // フィルタのF特
            fromF[0].real = 1.0f;
            for (int i = 1; i <= FILTER_LENP1 / 2; ++i)
            {
                double omegaRatio = i * (1.0 / (FILTER_LENP1 / 2));
                double v          = Math.Sqrt(1.0 / (1.0 + Math.Pow(omegaRatio / cutoffRatio, orderX2)));
                if (Math.Abs(v) < Math.Pow(0.5, 24))
                {
                    v = 0.0;
                }
                fromF[i].real = v;
            }
            for (int i = 1; i < FILTER_LENP1 / 2; ++i)
            {
                fromF[FILTER_LENP1 - i].real = fromF[i].real;
            }

            // IFFTでfromFをfromTに変換
            var fromT = new WWComplex[FILTER_LENP1];

            {
                var fft = new WWRadix2Fft(FILTER_LENP1);
                fft.ForwardFft(fromF, fromT);

                double compensation = 1.0 / (FILTER_LENP1 * cutoffRatio);
                for (int i = 0; i < FILTER_LENP1; ++i)
                {
                    fromT[i].Set(
                        fromT[i].real * compensation,
                        fromT[i].imaginary * compensation);
                }
            }
            fromF = null;

            // fromTの中心がFILTER_LENGTH/2番に来るようにする。
            // delayT[0]のデータはfromF[FILTER_LENGTH/2]だが、非対称になるので入れない
            // このフィルタの遅延はFILTER_LENGTH/2サンプルある

            var delayT = new WWComplex[FILTER_LENP1];

            for (int i = 1; i < FILTER_LENP1 / 2; ++i)
            {
                delayT[i] = fromT[i + FILTER_LENP1 / 2];
            }
            for (int i = 0; i < FILTER_LENP1 / 2; ++i)
            {
                delayT[i + FILTER_LENP1 / 2] = fromT[i];
            }
            fromT = null;

            // Kaiser窓をかける
            var w = WWWindowFunc.KaiserWindow(FILTER_LENP1 + 1, 9.0);

            for (int i = 0; i < FILTER_LENP1; ++i)
            {
                delayT[i].Mul(w[i]);
            }

            var delayTL = new WWComplex[FFT_LEN];

            for (int i = 0; i < delayT.Length; ++i)
            {
                delayTL[i] = delayT[i];
            }
            delayT = null;

            // できたフィルタをFFTする
            var delayF = new WWComplex[FFT_LEN];

            {
                var fft = new WWRadix2Fft(FFT_LEN);
                fft.ForwardFft(delayTL, delayF);

                for (int i = 0; i < FFT_LEN; ++i)
                {
                    delayF[i].Mul(cutoffRatio);
                }
            }
            delayTL = null;

            mFilterFreq = delayF;
        }
示例#19
0
        public override double[] FilterDo(double[] inPcm)
        {
            System.Diagnostics.Debug.Assert(inPcm.LongLength == NumOfSamplesNeeded());

            var inPcmR = new double[FftLength];

            if (mFirst)
            {
                Array.Copy(inPcm, 0, inPcmR, OverlapLength, inPcm.LongLength);

                mFirst = false;
            }
            else
            {
                System.Diagnostics.Debug.Assert(mOverlapSamples != null &&
                                                mOverlapSamples.LongLength == OverlapLength * 2);

                Array.Copy(mOverlapSamples, 0, inPcmR, 0, OverlapLength * 2);
                mOverlapSamples = null;
                Array.Copy(inPcm, 0, inPcmR, OverlapLength * 2, inPcm.LongLength);
            }

            // inPcmTをFFTしてinPcmFを得る。
            var inPcmT = new WWComplex[FftLength];

            for (int i = 0; i < inPcmT.Length; ++i)
            {
                inPcmT[i] = new WWComplex(inPcmR[i], 0);
            }

            var inPcmF = new WWComplex[FftLength];

            {
                var fft = new WWRadix2Fft(FftLength);
                fft.ForwardFft(inPcmT, inPcmF);
            }
            inPcmT = null;

            // inPcmFを0で水増ししたデータoutPcmFを作って逆FFTしoutPcmTを得る。

            var UPSAMPLE_FFT_LENGTH = Factor * FftLength;

            var outPcmF = new WWComplex[UPSAMPLE_FFT_LENGTH];

            for (int i = 0; i < outPcmF.Length; ++i)
            {
                if (i <= FftLength / 2)
                {
                    outPcmF[i].CopyFrom(inPcmF[i]);
                    if (i == FftLength / 2)
                    {
                        outPcmF[i].Mul(0.5);
                    }
                }
                else if (UPSAMPLE_FFT_LENGTH - FftLength / 2 <= i)
                {
                    int pos = i + FftLength - UPSAMPLE_FFT_LENGTH;
                    outPcmF[i].CopyFrom(inPcmF[pos]);
                    if (outPcmF.Length - FftLength / 2 == i)
                    {
                        outPcmF[i].Mul(0.5);
                    }
                }
                else
                {
                    // do nothing
                }
            }
            inPcmF = null;
            var outPcmT = new WWComplex[UPSAMPLE_FFT_LENGTH];

            {
                var fft = new WWRadix2Fft(UPSAMPLE_FFT_LENGTH);
                fft.InverseFft(outPcmF, outPcmT, 1.0 / FftLength);
            }
            outPcmF = null;

            // outPcmTの実数成分を戻り値とする。
            var outPcm = new double[Factor * (FftLength - OverlapLength * 2)];

            for (int i = 0; i < outPcm.Length; ++i)
            {
                outPcm[i] = outPcmT[i + Factor * OverlapLength].real;
            }
            outPcmT = null;

            // 次回計算に使用するオーバーラップ部分のデータをmOverlapSamplesに保存。
            mOverlapSamples = new double[OverlapLength * 2];
            Array.Copy(inPcm, inPcm.LongLength - OverlapLength * 2, mOverlapSamples, 0, OverlapLength * 2);

            return(outPcm);
        }
        private WWComplex EvalH(WWComplex z)
        {
            var zRecip = new WWComplex(z).Reciprocal();

            var zRecip2 = new WWComplex(zRecip).Mul(zRecip);
            var zRecip3 = new WWComplex(zRecip2).Mul(zRecip);
            var zRecip4 = new WWComplex(zRecip3).Mul(zRecip);
            var zRecip5 = new WWComplex(zRecip4).Mul(zRecip);
            var zRecip6 = new WWComplex(zRecip5).Mul(zRecip);
            var zRecip7 = new WWComplex(zRecip6).Mul(zRecip);
            var zRecip8 = new WWComplex(zRecip7).Mul(zRecip);

            var hDenom0 = new WWComplex(mDenominators[0], 0.0f);
            var hDenom1 = new WWComplex(mDenominators[1], 0.0f).Mul(zRecip);
            var hDenom2 = new WWComplex(mDenominators[2], 0.0f).Mul(zRecip2);
            var hDenom3 = new WWComplex(mDenominators[3], 0.0f).Mul(zRecip3);
            var hDenom4 = new WWComplex(mDenominators[4], 0.0f).Mul(zRecip4);
            var hDenom5 = new WWComplex(mDenominators[5], 0.0f).Mul(zRecip5);
            var hDenom6 = new WWComplex(mDenominators[6], 0.0f).Mul(zRecip6);
            var hDenom7 = new WWComplex(mDenominators[7], 0.0f).Mul(zRecip7);
            var hDenom8 = new WWComplex(mDenominators[8], 0.0f).Mul(zRecip8);
            var hDenom = new WWComplex(hDenom0).Add(hDenom1).Add(hDenom2).Add(hDenom3).Add(hDenom4).Add(hDenom5).Add(hDenom6).Add(hDenom7).Add(hDenom8).Reciprocal();

            var hNumer0 = new WWComplex(mNumerators[0], 0.0f);
            var hNumer1 = new WWComplex(mNumerators[1], 0.0f).Mul(zRecip);
            var hNumer2 = new WWComplex(mNumerators[2], 0.0f).Mul(zRecip2);
            var hNumer3 = new WWComplex(mNumerators[3], 0.0f).Mul(zRecip3);
            var hNumer4 = new WWComplex(mNumerators[4], 0.0f).Mul(zRecip4);
            var hNumer5 = new WWComplex(mNumerators[5], 0.0f).Mul(zRecip5);
            var hNumer6 = new WWComplex(mNumerators[6], 0.0f).Mul(zRecip6);
            var hNumer7 = new WWComplex(mNumerators[7], 0.0f).Mul(zRecip7);
            var hNumer8 = new WWComplex(mNumerators[8], 0.0f).Mul(zRecip8);
            var hNumer = new WWComplex(hNumer0).Add(hNumer1).Add(hNumer2).Add(hNumer3).Add(hNumer4).Add(hNumer5).Add(hNumer6).Add(hNumer7).Add(hNumer8);
            var h = new WWComplex(hNumer).Mul(hDenom);

            // 孤立特異点や極で起きる異常を適当に除去する
            if (double.IsNaN(h.Magnitude())) {
                return new WWComplex(0.0f, 0.0f);
            }
            return h;
        }
        private void UpdateFR()
        {
            foreach (var item in mLineList) {
                canvasFR.Children.Remove(item);
            }
            mLineList.Clear();

            // calc frequency response

            double [] frMagnitude = new double[FR_LINE_NUM];
            double [] frPhase     = new double[FR_LINE_NUM];
            double maxMagnitude = 0.0f;

            for (int i=0; i < FR_LINE_NUM; ++i) {
                double theta = AngleFrequency(i);
                var z = new WWComplex(Math.Cos(theta), Math.Sin(theta));
                var h = EvalH(z);
                frMagnitude[i] = h.Magnitude();
                if (maxMagnitude < frMagnitude[i]) {
                    maxMagnitude = frMagnitude[i];
                }
                frPhase[i] = h.Phase();
            }

            if (maxMagnitude < float.Epsilon) {
                maxMagnitude = 1.0f;
            }

            // draw result
            int sampleFrequency = SAMPLE_FREQS[comboBoxSampleFreq.SelectedIndex];

            double phaseShift = 0;

            // 配列に入れてデータ化するといい。
            switch (comboBoxPhaseShift.SelectedIndex) {
            case (int)PhaseShiftType.Zero:
                phaseShift = 0;
                labelPhase180.Content = "180";
                labelPhase90.Content = "90";
                labelPhase0.Content = "0";
                labelPhaseM90.Content = "-90";
                labelPhaseM180.Content = "-180";
                break;
            case (int)PhaseShiftType.P45:
                phaseShift = -1.0 * Math.PI / 4.0;
                labelPhase180.Content = "225";
                labelPhase90.Content = "135";
                labelPhase0.Content = "45";
                labelPhaseM90.Content = "-45";
                labelPhaseM180.Content = "-135";
                break;
            case (int)PhaseShiftType.P90:
                phaseShift = -2.0 * Math.PI / 4.0;
                labelPhase180.Content = "270";
                labelPhase90.Content = "180";
                labelPhase0.Content = "90";
                labelPhaseM90.Content = "00";
                labelPhaseM180.Content = "-90";
                break;
            case (int)PhaseShiftType.P135:
                phaseShift = -3.0 * Math.PI / 4.0;
                labelPhase180.Content = "315";
                labelPhase90.Content = "225";
                labelPhase0.Content = "135";
                labelPhaseM90.Content = "45";
                labelPhaseM180.Content = "-45";
                break;
            case (int)PhaseShiftType.P180:
                phaseShift = -4.0 * Math.PI / 4.0;
                labelPhase180.Content = "360";
                labelPhase90.Content = "270";
                labelPhase0.Content = "180";
                labelPhaseM90.Content = "90";
                labelPhaseM180.Content = "0";
                break;
            case (int)PhaseShiftType.M45:
                phaseShift = 1.0 * Math.PI / 4.0;
                labelPhase180.Content = "135";
                labelPhase90.Content = "45";
                labelPhase0.Content = "-45";
                labelPhaseM90.Content = "-135";
                labelPhaseM180.Content = "-225";
                break;
            case (int)PhaseShiftType.M90:
                phaseShift = 2.0 * Math.PI / 4.0;
                labelPhase180.Content = "90";
                labelPhase90.Content = "0";
                labelPhase0.Content = "-90";
                labelPhaseM90.Content = "-180";
                labelPhaseM180.Content = "-270";
                break;
            case (int)PhaseShiftType.M135:
                phaseShift = 3.0 * Math.PI/4.0;
                labelPhase180.Content = "45";
                labelPhase90.Content = "-45";
                labelPhase0.Content = "-135";
                labelPhaseM90.Content = "-225";
                labelPhaseM180.Content = "-315";
                break;
            case (int)PhaseShiftType.M180:
                phaseShift = Math.PI;
                labelPhase180.Content = "0";
                labelPhase90.Content = "-90";
                labelPhase0.Content = "-180";
                labelPhaseM90.Content = "-270";
                labelPhaseM180.Content = "-360";
                break;
            }

            switch (comboBoxFreqScale.SelectedIndex) {
            case (int)FreqScaleType.Linear:
                labelFRMin.Visibility = Visibility.Visible;
                labelFRMax.Visibility = System.Windows.Visibility.Hidden;
                labelFRMin.Content = SampleFreqString(0);
                labelFR0.Content = SampleFreqString(sampleFrequency * 1.0f / 8);
                labelFR1.Content = SampleFreqString(sampleFrequency * 2.0f / 8);
                labelFR2.Content = SampleFreqString(sampleFrequency * 3.0f / 8);
                labelFR3.Content = SampleFreqString(sampleFrequency * 4.0f / 8);
                lineFR0.X1 = lineFR0.X2 = FR_LINE_LEFT + FR_LINE_NUM * 1 / 4;
                lineFR1.X1 = lineFR1.X2 = FR_LINE_LEFT + FR_LINE_NUM * 2 / 4;
                lineFR2.X1 = lineFR2.X2 = FR_LINE_LEFT + FR_LINE_NUM * 3 / 4;
                lineFR3.Visibility = System.Windows.Visibility.Hidden;
                Canvas.SetLeft(labelFRMin, FR_LINE_LEFT - 10);
                Canvas.SetLeft(labelFR0, lineFR0.X1 - 20);
                Canvas.SetLeft(labelFR1, lineFR1.X1 - 20);
                Canvas.SetLeft(labelFR2, lineFR2.X1 - 20);
                Canvas.SetLeft(labelFR3, FR_LINE_LEFT + FR_LINE_NUM - 20);
                break;
            case (int)FreqScaleType.Logarithmic:
                labelFRMin.Visibility = Visibility.Visible;
                labelFRMax.Visibility = System.Windows.Visibility.Visible;
                labelFRMin.Content = SampleFreqString(LogSampleFreq(sampleFrequency, 0));
                labelFR0.Content = SampleFreqString(LogSampleFreq(sampleFrequency, 1));
                labelFR1.Content = SampleFreqString(LogSampleFreq(sampleFrequency, 2));
                labelFR2.Content = SampleFreqString(LogSampleFreq(sampleFrequency, 3));
                labelFR3.Content = SampleFreqString(LogSampleFreq(sampleFrequency, 4));
                labelFRMax.Content = SampleFreqString(sampleFrequency/2);
                lineFR0.X1 = lineFR0.X2 = FR_LINE_LEFT + FR_LINE_NUM * LogFrequencyX(LogSampleFreq(sampleFrequency, 1));
                lineFR1.X1 = lineFR1.X2 = FR_LINE_LEFT + FR_LINE_NUM * LogFrequencyX(LogSampleFreq(sampleFrequency, 2));
                lineFR2.X1 = lineFR2.X2 = FR_LINE_LEFT + FR_LINE_NUM * LogFrequencyX(LogSampleFreq(sampleFrequency, 3));
                lineFR3.X1 = lineFR3.X2 = FR_LINE_LEFT + FR_LINE_NUM * LogFrequencyX(LogSampleFreq(sampleFrequency, 4));
                lineFR3.Visibility = System.Windows.Visibility.Visible;
                Canvas.SetLeft(labelFRMin, FR_LINE_LEFT - 20);
                Canvas.SetLeft(labelFR0, lineFR0.X1 - 20);
                Canvas.SetLeft(labelFR1, lineFR1.X1 - 20);
                Canvas.SetLeft(labelFR2, lineFR2.X1 - 20);
                Canvas.SetLeft(labelFR3, lineFR3.X1 - 25);
                Canvas.SetLeft(labelFRMax, FR_LINE_LEFT + FR_LINE_NUM);
                break;
            default:
                System.Diagnostics.Debug.Assert(false);
                break;
            }

            switch (comboBoxMagScale.SelectedIndex) {
            case (int)MagScaleType.Linear:
                labelMagnitude.Content = "Magnitude";
                labelFRMagMax.Content = string.Format("{0:0.00}", maxMagnitude);
                labelFRMag2.Content   = string.Format("{0:0.00}", maxMagnitude*0.75);
                labelFRMag1.Content   = string.Format("{0:0.00}", maxMagnitude*0.5);
                labelFRMag0.Content   = string.Format("{0:0.00}", maxMagnitude*0.25);
                labelFRMagMin.Content = string.Format("{0:0.00}", 0);

                lineFRMag0.Y1 = lineFRMag0.Y2 = FR_LINE_BOTTOM - FR_LINE_HEIGHT *1 / 4;
                lineFRMag1.Y1 = lineFRMag1.Y2 = FR_LINE_BOTTOM - FR_LINE_HEIGHT *2 / 4;
                lineFRMag2.Y1 = lineFRMag2.Y2 = FR_LINE_BOTTOM - FR_LINE_HEIGHT *3 / 4;
                break;
            case (int)MagScaleType.Logarithmic:
                labelMagnitude.Content = "Magnitude (dB)";
                labelFRMagMax.Content = string.Format("{0:0.00}", 20.0 * Math.Log10(maxMagnitude));
                labelFRMag2.Content   = string.Format("{0:0.00}", 20.0 * Math.Log10(maxMagnitude / 16));
                labelFRMag1.Content   = string.Format("{0:0.00}", 20.0 * Math.Log10(maxMagnitude / 256));
                labelFRMag0.Content   = string.Format("{0:0.00}", 20.0 * Math.Log10(maxMagnitude / 4096));
                labelFRMagMin.Content = string.Format("{0:0.00}", 20.0 * Math.Log10(maxMagnitude / 65536));

                lineFRMag0.Y1 = lineFRMag0.Y2 = FR_LINE_BOTTOM - FR_LINE_HEIGHT *1 / 4;
                lineFRMag1.Y1 = lineFRMag1.Y2 = FR_LINE_BOTTOM - FR_LINE_HEIGHT *2 / 4;
                lineFRMag2.Y1 = lineFRMag2.Y2 = FR_LINE_BOTTOM - FR_LINE_HEIGHT *3 / 4;
                break;
            default:
                System.Diagnostics.Debug.Assert(false);
                break;
            }

            var lastPosM = new Point();
            var lastPosP = new Point();

            for (int i=0; i < FR_LINE_NUM; ++i) {
                Point posM = new Point();
                Point posP = new Point();

                double phase = frPhase[i] + phaseShift;
                while (phase <= -Math.PI) {
                    phase += 2.0 * Math.PI;
                }
                while (Math.PI < phase) {
                    phase -= 2.0f * Math.PI;
                }

                posP = new Point(FR_LINE_LEFT + i, FR_LINE_YCENTER - FR_LINE_HEIGHT * phase / (2.0f * Math.PI));

                switch (comboBoxMagScale.SelectedIndex) {
                case (int)MagScaleType.Linear:
                    posM = new Point(FR_LINE_LEFT + i, FR_LINE_BOTTOM - FR_LINE_HEIGHT * frMagnitude[i] / maxMagnitude);
                    break;
                case (int)MagScaleType.Logarithmic:
                    posM = new Point(FR_LINE_LEFT + i,
                        FR_LINE_TOP + FR_LINE_HEIGHT * 20.0 * Math.Log10(frMagnitude[i] / maxMagnitude) / (20.0 * Math.Log10(1.0/65536)) );
                    break;
                default:
                    System.Diagnostics.Debug.Assert(false);
                    break;
                }

                if (1 <= i) {
                    bool bDraw = true;
                    switch (comboBoxMagScale.SelectedIndex) {
                    case (int)MagScaleType.Logarithmic:
                        if (FR_LINE_BOTTOM < posM.Y || FR_LINE_BOTTOM < lastPosM.Y) {
                            bDraw = false;
                        }
                        break;
                    }

                    if (bDraw) {
                        var lineM = new Line();
                        lineM.Stroke = Brushes.Blue;
                        LineSetX1Y1X2Y2(lineM, lastPosM.X, lastPosM.Y, posM.X, posM.Y);
                        mLineList.Add(lineM);
                        canvasFR.Children.Add(lineM);
                    }
                }

                if (2 <= i) {
                    var lineP = new Line();
                    lineP.Stroke = Brushes.Red;
                    LineSetX1Y1X2Y2(lineP, lastPosP.X, lastPosP.Y, posP.X, posP.Y);
                    mLineList.Add(lineP);
                    canvasFR.Children.Add(lineP);
                }
                lastPosP = posP;
                lastPosM = posM;
            }
        }
示例#22
0
        private void FftStageN(int stageNr, WWComplex[] x, WWComplex[] y)
        {
            /*
             * stage0: 2つの入力データにバタフライ演算 (length=8の時) 4回 (nRepeat=4, nSubRepeat=2)
             * y[0] = x[0] + w_n^(0*4) * x[1]
             * y[1] = x[0] + w_n^(1*4) * x[1]
             *
             * y[2] = x[2] + w_n^(0*4) * x[3]
             * y[3] = x[2] + w_n^(1*4) * x[3]
             *
             * y[4] = x[4] + w_n^(0*4) * x[5]
             * y[5] = x[4] + w_n^(1*4) * x[5]
             *
             * y[6] = x[6] + w_n^(0*4) * x[7]
             * y[7] = x[6] + w_n^(1*4) * x[7]
             */

            /*
             * stage1: 4つの入力データにバタフライ演算 (length=8の時) 2回 (nRepeat=2, nSubRepeat=4)
             * y[0] = x[0] + w_n^(0*2) * x[2]
             * y[1] = x[1] + w_n^(1*2) * x[3]
             * y[2] = x[0] + w_n^(2*2) * x[2]
             * y[3] = x[1] + w_n^(3*2) * x[3]
             *
             * y[4] = x[4] + w_n^(0*2) * x[6]
             * y[5] = x[5] + w_n^(1*2) * x[7]
             * y[6] = x[4] + w_n^(2*2) * x[6]
             * y[7] = x[5] + w_n^(3*2) * x[7]
             */

            /*
             * stage2: 8つの入力データにバタフライ演算 (length=8の時) 1回 (nRepeat=1, nSubRepeat=8)
             * y[0] = x[0] + w_n^(0*1) * x[4]
             * y[1] = x[1] + w_n^(1*1) * x[5]
             * y[2] = x[2] + w_n^(2*1) * x[6]
             * y[3] = x[3] + w_n^(3*1) * x[7]
             * y[4] = x[0] + w_n^(4*1) * x[4]
             * y[5] = x[1] + w_n^(5*1) * x[5]
             * y[6] = x[2] + w_n^(6*1) * x[6]
             * y[7] = x[3] + w_n^(7*1) * x[7]
             */

            /*
             * stageN:
             */

            int nRepeat    = Pow2(mNumStage - stageNr - 1);
            int nSubRepeat = mNumPoints / nRepeat;
            var t = new WWComplex();

            for (int i=0; i<nRepeat; ++i) {
                int offsBase = i * nSubRepeat;

                bool allZero = true;
                for (int j=0; j < nSubRepeat/2; ++j) {
                    int offs = offsBase + (j % (nSubRepeat/2));
                    if (Double.Epsilon < x[offs].Magnitude()) {
                        allZero = false;
                        break;
                    }
                    if (Double.Epsilon < x[offs + nSubRepeat / 2].Magnitude()) {
                        allZero = false;
                        break;
                    }
                }

                if (allZero) {
                    for (int j=0; j < nSubRepeat / 2; ++j) {
                        y[j + offsBase].Set(0, 0);
                    }
                } else {
                    for (int j=0; j < nSubRepeat; ++j) {
                        int offs = offsBase + (j % (nSubRepeat / 2));
                        y[j + offsBase].CopyFrom(x[offs]);

                        t.CopyFrom(mWn[j * nRepeat]);
                        t.Mul(x[offs + nSubRepeat / 2]);

                        y[j + offsBase].Add(t);
                    }
                }
            }
        }
示例#23
0
        void CalcFilterCompleted(object sender, RunWorkerCompletedEventArgs e)
        {
            mMainPanel.IsEnabled             = true;
            mTextBlockCalculating.Visibility = System.Windows.Visibility.Collapsed;
            mTextBoxLog.Clear();

            var r = e.Result as BackgroundCalcResult;

            if (!r.success)
            {
                MessageBox.Show(r.message);
                return;
            }

            AddLog(string.Format("Order={0}\n", mAfd.Order()));

            // 伝達関数の式をログに出力。
            AddLog("Transfer function (factorized, normalized) H(s) = ");
            AddLog(string.Format("{0:G4}", mAfd.NumeratorConstant()));
            for (int i = 0; i < mAfd.NumOfZeroes(); ++i)
            {
                AddLog(string.Format(" (s + {0})", WWComplex.Minus(mAfd.ZeroNth(i))));
            }
            AddLog(" /");
            for (int i = 0; i < mAfd.NumOfPoles(); ++i)
            {
                AddLog(string.Format(" (s + {0})", WWComplex.Minus(mAfd.PoleNth(i))));
            }
            AddLog("\n");

            {   // Show expanded Transfer function
                var zeroList = new WWComplex[mAfd.NumOfZeroes()];
                for (int i = 0; i < mAfd.NumOfZeroes(); ++i)
                {
                    zeroList[i] = mAfd.ZeroNth(i);
                }

                var poleList = new WWComplex[mAfd.NumOfPoles()];
                for (int i = 0; i < mAfd.NumOfPoles(); ++i)
                {
                    poleList[i] = mAfd.PoleNth(i);
                }

                var numer = WWPolynomial.RootListToCoeffList(zeroList,
                                                             new WWComplex(mAfd.NumeratorConstant(), 0));
                var denom = WWPolynomial.RootListToCoeffList(poleList, WWComplex.Unity());

                AddLog("Transfer function (expanded, normalized) H(s) = {");
                AddLog(new ComplexPolynomial(numer).ToString("s"));
                AddLog(" } / {");
                AddLog(new ComplexPolynomial(denom).ToString("s"));
                AddLog(" }\n");
            }

            AddLog(string.Format("Transfer function with real coefficients H(s) = "));
            for (int i = 0; i < mAfd.RealPolynomialCount(); ++i)
            {
                AddLog(mAfd.RealPolynomialNth(i).ToString("(s/ωc)"));
                if (i != mAfd.RealPolynomialCount() - 1)
                {
                    AddLog(" + ");
                }
            }
            AddLog("\n");

            // インパルス応答の式をログに出力。
            var H_s = new List <FirstOrderComplexRationalPolynomial>();

            AddLog(("Impulse Response (frequency normalized): h(t) = "));
            for (int i = 0; i < mAfd.HPfdCount(); ++i)
            {
                var p = mAfd.HPfdNth(i);
                H_s.Add(p);

                if (!p.N(1).EqualValue(WWComplex.Zero()))
                {
                    throw new System.NotImplementedException();
                }

                if (p.D(1).EqualValue(WWComplex.Zero()) &&
                    p.D(0).EqualValue(WWComplex.Unity()))
                {
                    // 1 → δ(t)
                    AddLog(string.Format("{0:G4} * δ(t)", p.N(0)));
                }
                else if (p.D(0).EqualValue(WWComplex.Zero()))
                {
                    // b/s → b*u(t)
                    AddLog(string.Format("{0:G4} * u(t)", p.N(0)));
                }
                else
                {
                    // b/(s-a) ⇒ b * exp(t * a)
                    AddLog(string.Format("({0}) * e^ {{ t * ({1}) }}", p.N(0), WWComplex.Minus(p.D(0))));
                }

                if (i != mAfd.HPfdCount() - 1)
                {
                    AddLog(" + ");
                }
            }
            AddLog("\n");

            // 周波数応答グラフに伝達関数をセット。
            mFrequencyResponseS.TransferFunction = mAfd.TransferFunction;
            mFrequencyResponseS.Update();

            // Pole-Zeroプロットに極と零の位置をセット。
            mPoleZeroPlotS.ClearPoleZero();

            double scale = mAfd.PoleNth(0).Magnitude();

            if (0 < mAfd.NumOfZeroes() && scale < mAfd.ZeroNth(0).Magnitude())
            {
                scale = mAfd.ZeroNth(0).Magnitude();
            }
            mPoleZeroPlotS.SetScale(scale);
            for (int i = 0; i < mAfd.NumOfPoles(); ++i)
            {
                var p = mAfd.PoleNth(i);
                mPoleZeroPlotS.AddPole(p);
            }
            for (int i = 0; i < mAfd.NumOfZeroes(); ++i)
            {
                var p = mAfd.ZeroNth(i);
                mPoleZeroPlotS.AddZero(p);
            }
            mPoleZeroPlotS.TransferFunction = mAfd.PoleZeroPlotTransferFunction;
            mPoleZeroPlotS.Update();

            // 時間ドメインプロットの更新。
            mTimeDomainPlot.ImpulseResponseFunction = mAfd.ImpulseResponseFunction;
            mTimeDomainPlot.StepResponseFunction    = mAfd.UnitStepResponseFunction;
            mTimeDomainPlot.TimeScale = mAfd.TimeDomainFunctionTimeScale;
            mTimeDomainPlot.Update();

            // アナログ回路表示。
            AddLog(string.Format("Analog Filter Stages = {0}\n", mAfd.RealPolynomialCount()));

            mAnalogFilterCircuit.Clear();

            {
                groupBoxAFC.Visibility = System.Windows.Visibility.Visible;
                mAnalogFilterCircuit.CutoffFrequencyHz = mFc;
                for (int i = 0; i < mAfd.RealPolynomialCount(); ++i)
                {
                    mAnalogFilterCircuit.Add(mAfd.RealPolynomialNth(i));
                }
                mAnalogFilterCircuit.AddFinished();
                mAnalogFilterCircuit.Update();
            }
        }
示例#24
0
 public static WWComplex Add(WWComplex a, WWComplex b)
 {
     var r = new WWComplex(a);
     r.Add(b);
     return r;
 }
示例#25
0
 public WWComplex(WWComplex rhs)
 {
     this.real      = rhs.real;
     this.imaginary = rhs.imaginary;
 }
示例#26
0
 public void CopyFrom(WWComplex rhs)
 {
     real      = rhs.real;
     imaginary = rhs.imaginary;
 }
示例#27
0
        public WWComplex[] InverseFft(WWComplex[] aFrom, double? compensation = null)
        {
            for (int i=0; i < aFrom.LongLength; ++i) {
                aFrom[i].imaginary *= -1.0;
            }

            var aTo = ForwardFft(aFrom);

            double c = 1.0 / mNumPoints;
            if (compensation != null) {
                c = (double)compensation;
            }

            for (int i=0; i < aTo.LongLength; ++i) {
                aTo[i].real      *= c;
                aTo[i].imaginary *= -1.0 * c;
            }

            return aTo;
        }
        private void UpdateZ()
        {
            canvasZ.Children.Clear();

            var im = new Image();

            var bm = new WriteableBitmap(
                512,
                512,
                96,
                96,
                PixelFormats.Gray32Float,
                null);
            im.Source = bm;
            im.Stretch = Stretch.None;
            im.HorizontalAlignment = HorizontalAlignment.Left;
            im.VerticalAlignment   = VerticalAlignment.Top;

            var px = new float[bm.PixelHeight*bm.PixelWidth];

            int pos = 0;
            for (int yI = 0; yI < bm.PixelHeight; yI++) {
                for (int xI = 0; xI < bm.PixelWidth; xI++) {
                    double y = 2.6666666666666 * (bm.PixelHeight / 2 - yI) / bm.PixelHeight;
                    double x = 2.6666666666666 * (xI - bm.PixelWidth / 2) / bm.PixelHeight;
                    var z = new WWComplex(x, y);

                    var h = EvalH(z);
                    var hM = h.Magnitude();

                    if (hM < 0.1) {
                        hM = 0.1;
                    }
                    float hL = (float)((Math.Log10(hM) + 1.0f) / 5.0f);
                    px[pos] = hL;
                    ++pos;
                }
            }

            bm.WritePixels(new Int32Rect(0, 0, bm.PixelWidth, bm.PixelHeight), px, bm.BackBufferStride, 0);

            canvasZ.Children.Add(im);
        }
示例#29
0
 public static WWComplex Mul(WWComplex a, WWComplex b)
 {
     var r = new WWComplex(a);
     r.Mul(b);
     return r;
 }
示例#30
0
        /// <summary>
        /// バターワースフィルターのFFT coeffs
        /// </summary>
        /// <param name="filterSlopeDbOct">1次 = 6(dB/oct), 2次 = 12(dB/oct)</param>
        /// <returns></returns>
        public static WWComplex[] Design(int sampleRate, double cutoffFrequency, int fftLength, int filterSlopeDbOct)
        {
            int filterLenP1  = fftLength / 4;
            int filterLength = filterLenP1 - 1;

            var fromF = new WWComplex[filterLenP1];

            double orderX2 = 2.0 * (filterSlopeDbOct / 6.0);

            double cutoffRatio = cutoffFrequency / (sampleRate / 2);

            // フィルタのF特
            fromF[0].real = 1.0f;
            for (int i = 1; i <= filterLenP1 / 2; ++i)
            {
                double omegaRatio = i * (1.0 / (filterLenP1 / 2));
                double v          = Math.Sqrt(1.0 / (1.0 + Math.Pow(omegaRatio / cutoffRatio, orderX2)));
                if (Math.Abs(v) < Math.Pow(0.5, 24))
                {
                    v = 0.0;
                }
                fromF[i].real = v;
            }
            for (int i = 1; i < filterLenP1 / 2; ++i)
            {
                fromF[filterLenP1 - i].real = fromF[i].real;
            }

            // IFFTでfromFをfromTに変換
            WWComplex[] fromT;
            {
                var fft = new WWRadix2Fft(filterLenP1);
                fromT = fft.ForwardFft(fromF);

                double compensation = 1.0 / (filterLenP1 * cutoffRatio);
                for (int i = 0; i < filterLenP1; ++i)
                {
                    fromT[i].Set(
                        fromT[i].real * compensation,
                        fromT[i].imaginary * compensation);
                }
            }
            fromF = null;

            // fromTの中心がFILTER_LENGTH/2番に来るようにする。
            // delayT[0]のデータはfromF[FILTER_LENGTH/2]だが、非対称になるので入れない
            // このフィルタの遅延はFILTER_LENGTH/2サンプルある

            var delayT = new WWComplex[filterLenP1];

            for (int i = 1; i < filterLenP1 / 2; ++i)
            {
                delayT[i] = fromT[i + filterLenP1 / 2];
            }
            for (int i = 0; i < filterLenP1 / 2; ++i)
            {
                delayT[i + filterLenP1 / 2] = fromT[i];
            }
            fromT = null;

            // Kaiser窓をかける
            var w = WWWindowFunc.KaiserWindow(filterLenP1 + 1, 9.0);

            for (int i = 0; i < filterLenP1; ++i)
            {
                delayT[i].Mul(w[i]);
            }

            var delayTL = new WWComplex[fftLength];

            for (int i = 0; i < delayT.Length; ++i)
            {
                delayTL[i] = delayT[i];
            }
            delayT = null;

            // できたフィルタをFFTする
            WWComplex[] delayF;
            {
                var fft = new WWRadix2Fft(fftLength);
                delayF = fft.ForwardFft(delayTL);

                for (int i = 0; i < fftLength; ++i)
                {
                    delayF[i].Mul(cutoffRatio);
                }
            }
            delayTL = null;

            return(delayF);
        }
示例#31
0
        public override double[] FilterDo(double[] inPcm)
        {
            System.Diagnostics.Debug.Assert(inPcm.Length == NumOfSamplesNeeded());

            var inPcmR = new double[FftLength];
            if (mFirst) {
                Array.Copy(inPcm, 0, inPcmR, HalfOverlapLength, inPcm.Length);
            } else {
                System.Diagnostics.Debug.Assert(mOverlapSamples != null);
                System.Diagnostics.Debug.Assert(mOverlapSamples.Length == HalfOverlapLength * 2);
                Array.Copy(mOverlapSamples, 0, inPcmR, 0, HalfOverlapLength * 2);
                mOverlapSamples = null;
                Array.Copy(inPcm, 0, inPcmR, HalfOverlapLength * 2, inPcm.Length);
            }

            var inPcmT = new WWComplex[FftLength];
            for (int i=0; i < inPcmT.Length; ++i) {
                inPcmT[i] = new WWComplex(inPcmR[i], 0);
            }

            {
                // inPcmTの出力されず捨てられる領域に窓関数を掛ける。
                // Kaiser窓(α==9)をかける
                var w = WWWindowFunc.KaiserWindow(HalfOverlapLength * 2, 9.0);
                for (int i = 0; i < HalfOverlapLength; ++i) {
                    inPcmT[i].Mul(w[i]);
                    inPcmT[FftLength - i - 1].Mul(w[i]);
                }
            }

            // inPcmTをFFTしてinPcmFを得る。
            WWComplex[] inPcmF;
            {
                var fft = new WWRadix2Fft(FftLength);
                inPcmF = fft.ForwardFft(inPcmT);
            }
            inPcmT = null;

            // inPcmFを0で水増ししたデータoutPcmFを作ってローパスフィルターを通し逆FFTしoutPcmTを得る。

            var outPcmF = new WWComplex[UpsampleFftLength];
            for (int i=0; i < outPcmF.Length; ++i) {
                if (i <= FftLength / 2) {
                    outPcmF[i].CopyFrom(inPcmF[i]);
                } else if (UpsampleFftLength - FftLength / 2 <= i) {
                    int pos = i + FftLength - UpsampleFftLength;
                    outPcmF[i].CopyFrom(inPcmF[pos]);
                } else {
                    // do nothing
                }
                outPcmF[i].Mul(mFreqFilter[i]);
            }
            inPcmF = null;

            WWComplex[] outPcmT;
            {
                var fft = new WWRadix2Fft(UpsampleFftLength);
                outPcmT = fft.InverseFft(outPcmF, 1.0 / FftLength);
            }
            outPcmF = null;

            // outPcmTの実数成分を戻り値とする。
            var outPcm = new double[Factor * (FftLength - HalfOverlapLength * 2)];
            for (int i=0; i < outPcm.Length; ++i) {
                outPcm[i] = outPcmT[i + Factor * HalfOverlapLength].real;
            }
            outPcmT = null;

            // 次回計算に使用するオーバーラップ部分のデータをmOverlapSamplesに保存。
            // オーバラップ部分==inPcmRの最後の方
            mOverlapSamples = new double[HalfOverlapLength * 2];
            Array.Copy(inPcmR, inPcmR.Length - HalfOverlapLength * 2, mOverlapSamples, 0, HalfOverlapLength * 2);

            if (mFirst) {
                mFirst = false;
            }

            return outPcm;
        }
        public override double[] FilterDo(double[] inPcm)
        {
            System.Diagnostics.Debug.Assert(inPcm.LongLength <= NumOfSamplesNeeded());

            // Overlap and add continuous FFT

            var inTime = new WWComplex[FFT_LEN];
            for (int i=0; i < inPcm.LongLength; ++i) {
                inTime[i] = new WWComplex(inPcm[i], 0.0);
            }

            // FFTでinTimeをinFreqに変換
            var inFreq = new WWComplex[FFT_LEN];
            {
                var fft = new WWRadix2Fft(FFT_LEN);
                fft.ForwardFft(inTime, inFreq);
            }
            inTime = null;

            // FFT後、フィルターHの周波数ドメインデータを掛ける
            for (int i=0; i < FFT_LEN; ++i) {
                inFreq[i].Mul(mFilterFreq[i]);
            }

            // inFreqをIFFTしてoutTimeに変換
            var outTime = new WWComplex[FFT_LEN];
            {
                var outTimeS = new WWComplex[FFT_LEN];
                var fft = new WWRadix2Fft(FFT_LEN);
                fft.InverseFft(inFreq, outTime);
            }
            inFreq = null;

            double [] outReal;
            if (mFirstFilterDo) {
                // 最初のFilterDo()のとき、フィルタの遅延サンプル数だけ先頭サンプルを削除する。
                outReal = new double[NumOfSamplesNeeded() - FILTER_DELAY];
                for (int i=0; i < outReal.Length; ++i) {
                    outReal[i] = outTime[i+FILTER_DELAY].real;
                }
                mFirstFilterDo = false;
            } else {
                outReal = new double[NumOfSamplesNeeded()];
                for (int i=0; i < outReal.Length; ++i) {
                    outReal[i] = outTime[i].real;
                }
            }

            // 前回のIFFT結果の最後のFILTER_LENGTH-1サンプルを先頭に加算する
            if (null != mIfftAddBuffer) {
                for (int i=0; i < mIfftAddBuffer.Length; ++i) {
                    outReal[i] += mIfftAddBuffer[i];
                }
            }

            // 今回のIFFT結果の最後のFILTER_LENGTH-1サンプルをmIfftAddBufferとして保存する
            mIfftAddBuffer = new double[FILTER_LENP1];
            for (int i=0; i < mIfftAddBuffer.Length; ++i) {
                mIfftAddBuffer[i] = outTime[outTime.Length - mIfftAddBuffer.Length + i].real;
            }
            outTime = null;

            return outReal;
        }
示例#33
0
 public WWComplex Sub(WWComplex rhs)
 {
     real      -= rhs.real;
     imaginary -= rhs.imaginary;
     return(this);
 }
        private void DesignCutoffFilter()
        {
            var fromF = new WWComplex[FILTER_LENP1];

            // バターワースフィルター
            // 1次 = 6dB/oct
            // 2次 = 12dB/oct

            double orderX2 = 2.0 * (FilterSlopeDbOct / 6.0);

            double cutoffRatio = CutoffFrequency / (SampleRate/2);

            // フィルタのF特
            fromF[0].real = 1.0f;
            for (int i=1; i <= FILTER_LENP1 / 2; ++i) {
                double omegaRatio = i * (1.0 / (FILTER_LENP1 / 2));
                double v = Math.Sqrt(1.0 / (1.0 + Math.Pow(omegaRatio / cutoffRatio, orderX2)));
                if (Math.Abs(v) < Math.Pow(0.5, 24)) {
                    v = 0.0;
                }
                fromF[i].real = v;
            }
            for (int i=1; i < FILTER_LENP1 / 2; ++i) {
                fromF[FILTER_LENP1 - i].real = fromF[i].real;
            }

            // IFFTでfromFをfromTに変換
            var fromT   = new WWComplex[FILTER_LENP1];
            {
                var fft = new WWRadix2Fft(FILTER_LENP1);
                fft.ForwardFft(fromF, fromT);

                double compensation = 1.0 / (FILTER_LENP1 * cutoffRatio);
                for (int i=0; i < FILTER_LENP1; ++i) {
                    fromT[i].Set(
                            fromT[i].real      * compensation,
                            fromT[i].imaginary * compensation);
                }
            }
            fromF = null;

            // fromTの中心がFILTER_LENGTH/2番に来るようにする。
            // delayT[0]のデータはfromF[FILTER_LENGTH/2]だが、非対称になるので入れない
            // このフィルタの遅延はFILTER_LENGTH/2サンプルある

            var delayT = new WWComplex[FILTER_LENP1];
            for (int i=1; i < FILTER_LENP1 / 2; ++i) {
                delayT[i] = fromT[i + FILTER_LENP1 / 2];
            }
            for (int i=0; i < FILTER_LENP1 / 2; ++i) {
                delayT[i + FILTER_LENP1 / 2] = fromT[i];
            }
            fromT = null;

            // Kaiser窓をかける
            var w = WWWindowFunc.KaiserWindow(FILTER_LENP1 + 1, 9.0);
            for (int i=0; i < FILTER_LENP1; ++i) {
                delayT[i].Mul(w[i]);
            }

            var delayTL = new WWComplex[FFT_LEN];
            for (int i=0; i < delayT.Length; ++i) {
                delayTL[i] = delayT[i];
            }
            delayT = null;

            // できたフィルタをFFTする
            var delayF = new WWComplex[FFT_LEN];
            {
                var fft = new WWRadix2Fft(FFT_LEN);
                fft.ForwardFft(delayTL, delayF);

                for (int i=0; i < FFT_LEN; ++i) {
                    delayF[i].Mul(cutoffRatio);
                }
            }
            delayTL = null;

            mFilterFreq = delayF;
        }
示例#35
0
 public WWComplex Add(WWComplex rhs)
 {
     real      += rhs.real;
     imaginary += rhs.imaginary;
     return(this);
 }
示例#36
0
        private double[] FFTFir(double[] inPcm, double[] coef, int fftLength)
        {
            var fft = new WWRadix2Fft(fftLength);
            var inTime = new WWComplex[fftLength];

            for (int i = 0; i < mNumSamples; ++i) {
                inTime[i].real = inPcm[i];
            }

            var inFreq = fft.ForwardFft(inTime);
            inTime = null;

            var coefTime = new WWComplex[fftLength];
            for (int i = 0; i < mCoeffs[mChannelId * 2].Length; ++i) {
                coefTime[i].real = coef[i];
            }

            var coefFreq = fft.ForwardFft(coefTime);
            coefTime = null;

            var mulFreq = Mul(inFreq, coefFreq);
            inFreq = null;
            coefFreq = null;

            var mulTime = fft.InverseFft(mulFreq);
            mulFreq = null;

            var result = new double[inPcm.Length];
            for (int i = 0; i < inPcm.Length; ++i) {
                result[i] = mulTime[i].real;
            }
            mulTime = null;

            return result;
        }
示例#37
0
        private void FftStageN(int stageNr, WWComplex[] x, WWComplex[] y)
        {
            /*
             * stage0: 2つの入力データにバタフライ演算 (length=8の時) 4回 (nRepeat=4, nSubRepeat=2)
             * y[0] = x[0] + w_n^(0*4) * x[1]
             * y[1] = x[0] + w_n^(1*4) * x[1]
             *
             * y[2] = x[2] + w_n^(0*4) * x[3]
             * y[3] = x[2] + w_n^(1*4) * x[3]
             *
             * y[4] = x[4] + w_n^(0*4) * x[5]
             * y[5] = x[4] + w_n^(1*4) * x[5]
             *
             * y[6] = x[6] + w_n^(0*4) * x[7]
             * y[7] = x[6] + w_n^(1*4) * x[7]
             */

            /*
             * stage1: 4つの入力データにバタフライ演算 (length=8の時) 2回 (nRepeat=2, nSubRepeat=4)
             * y[0] = x[0] + w_n^(0*2) * x[2]
             * y[1] = x[1] + w_n^(1*2) * x[3]
             * y[2] = x[0] + w_n^(2*2) * x[2]
             * y[3] = x[1] + w_n^(3*2) * x[3]
             *
             * y[4] = x[4] + w_n^(0*2) * x[6]
             * y[5] = x[5] + w_n^(1*2) * x[7]
             * y[6] = x[4] + w_n^(2*2) * x[6]
             * y[7] = x[5] + w_n^(3*2) * x[7]
             */

            /*
             * stage2: 8つの入力データにバタフライ演算 (length=8の時) 1回 (nRepeat=1, nSubRepeat=8)
             * y[0] = x[0] + w_n^(0*1) * x[4]
             * y[1] = x[1] + w_n^(1*1) * x[5]
             * y[2] = x[2] + w_n^(2*1) * x[6]
             * y[3] = x[3] + w_n^(3*1) * x[7]
             * y[4] = x[0] + w_n^(4*1) * x[4]
             * y[5] = x[1] + w_n^(5*1) * x[5]
             * y[6] = x[2] + w_n^(6*1) * x[6]
             * y[7] = x[3] + w_n^(7*1) * x[7]
             */

            /*
             * stageN:
             */

            int nRepeat    = Pow2(mNumStage - stageNr - 1);
            int nSubRepeat = mNumPoints / nRepeat;
            var t          = new WWComplex();

            for (int i = 0; i < nRepeat; ++i)
            {
                int offsBase = i * nSubRepeat;

                bool allZero = true;
                for (int j = 0; j < nSubRepeat / 2; ++j)
                {
                    int offs = offsBase + (j % (nSubRepeat / 2));
                    if (Double.Epsilon < x[offs].Magnitude())
                    {
                        allZero = false;
                        break;
                    }
                    if (Double.Epsilon < x[offs + nSubRepeat / 2].Magnitude())
                    {
                        allZero = false;
                        break;
                    }
                }

                if (allZero)
                {
                    for (int j = 0; j < nSubRepeat / 2; ++j)
                    {
                        y[j + offsBase].Set(0, 0);
                    }
                }
                else
                {
                    for (int j = 0; j < nSubRepeat; ++j)
                    {
                        int offs = offsBase + (j % (nSubRepeat / 2));
                        y[j + offsBase].CopyFrom(x[offs]);

                        t.CopyFrom(mWn[j * nRepeat]);
                        t.Mul(x[offs + nSubRepeat / 2]);

                        y[j + offsBase].Add(t);
                    }
                }
            }
        }
示例#38
0
        private WWComplex[] Mul(WWComplex[] a, WWComplex[] b)
        {
            if (a.Length != b.Length) {
                return null;
            }

            var result = new WWComplex[a.Length];
            for (int i = 0; i < a.Length; ++i) {
                result[i] = a[i].Mul(b[i]);
            }

            return result;
        }
 public WWComplex Add(WWComplex rhs)
 {
     real      += rhs.real;
     imaginary += rhs.imaginary;
     return this;
 }
示例#40
0
 public WWComplex Div(WWComplex rhs)
 {
     var denom = new WWComplex(rhs).Reciprocal();
     return Mul(denom);
 }
示例#41
0
        public override double[] FilterDo(double[] inPcm)
        {
            System.Diagnostics.Debug.Assert(inPcm.Length == NumOfSamplesNeeded());

            var inPcmR = new double[FftLength];

            if (mFirst)
            {
                Array.Copy(inPcm, 0, inPcmR, HalfOverlapLength, inPcm.Length);
            }
            else
            {
                System.Diagnostics.Debug.Assert(mOverlapSamples != null);
                System.Diagnostics.Debug.Assert(mOverlapSamples.Length == HalfOverlapLength * 2);
                Array.Copy(mOverlapSamples, 0, inPcmR, 0, HalfOverlapLength * 2);
                mOverlapSamples = null;
                Array.Copy(inPcm, 0, inPcmR, HalfOverlapLength * 2, inPcm.Length);
            }

            var inPcmT = new WWComplex[FftLength];

            for (int i = 0; i < inPcmT.Length; ++i)
            {
                inPcmT[i] = new WWComplex(inPcmR[i], 0);
            }

            {
                // inPcmTの出力されず捨てられる領域に窓関数を掛ける。
                // Kaiser窓(α==9)をかける
                var w = WWWindowFunc.KaiserWindow(HalfOverlapLength * 2, 9.0);
                for (int i = 0; i < HalfOverlapLength; ++i)
                {
                    inPcmT[i].Mul(w[i]);
                    inPcmT[FftLength - i - 1].Mul(w[i]);
                }
            }

            // inPcmTをFFTしてinPcmFを得る。
            WWComplex[] inPcmF;
            {
                var fft = new WWRadix2Fft(FftLength);
                inPcmF = fft.ForwardFft(inPcmT);
            }
            inPcmT = null;

            // inPcmFを0で水増ししたデータoutPcmFを作ってローパスフィルターを通し逆FFTしoutPcmTを得る。

            var outPcmF = new WWComplex[UpsampleFftLength];

            for (int i = 0; i < outPcmF.Length; ++i)
            {
                if (i <= FftLength / 2)
                {
                    outPcmF[i].CopyFrom(inPcmF[i]);
                }
                else if (UpsampleFftLength - FftLength / 2 <= i)
                {
                    int pos = i + FftLength - UpsampleFftLength;
                    outPcmF[i].CopyFrom(inPcmF[pos]);
                }
                else
                {
                    // do nothing
                }
                outPcmF[i].Mul(mFreqFilter[i]);
            }
            inPcmF = null;

            WWComplex[] outPcmT;
            {
                var fft = new WWRadix2Fft(UpsampleFftLength);
                outPcmT = fft.InverseFft(outPcmF, 1.0 / FftLength);
            }
            outPcmF = null;

            // outPcmTの実数成分を戻り値とする。
            var outPcm = new double[Factor * (FftLength - HalfOverlapLength * 2)];

            for (int i = 0; i < outPcm.Length; ++i)
            {
                outPcm[i] = outPcmT[i + Factor * HalfOverlapLength].real;
            }
            outPcmT = null;

            // 次回計算に使用するオーバーラップ部分のデータをmOverlapSamplesに保存。
            // オーバラップ部分==inPcmRの最後の方
            mOverlapSamples = new double[HalfOverlapLength * 2];
            Array.Copy(inPcmR, inPcmR.Length - HalfOverlapLength * 2, mOverlapSamples, 0, HalfOverlapLength * 2);

            if (mFirst)
            {
                mFirst = false;
            }

            return(outPcm);
        }
示例#42
0
        /// <summary>
        /// バターワースフィルターのFFT coeffs
        /// </summary>
        /// <param name="filterSlopeDbOct">1次 = 6(dB/oct), 2次 = 12(dB/oct)</param>
        /// <returns></returns>
        public static WWComplex[] Design(int sampleRate, double cutoffFrequency, int fftLength, int filterSlopeDbOct)
        {
            int filterLenP1 = fftLength / 4;
            int filterLength = filterLenP1 - 1;

            var fromF = new WWComplex[filterLenP1];

            double orderX2 = 2.0 * (filterSlopeDbOct / 6.0);

            double cutoffRatio = cutoffFrequency / (sampleRate / 2);

            // フィルタのF特
            fromF[0].real = 1.0f;
            for (int i = 1; i <= filterLenP1 / 2; ++i) {
                double omegaRatio = i * (1.0 / (filterLenP1 / 2));
                double v = Math.Sqrt(1.0 / (1.0 + Math.Pow(omegaRatio / cutoffRatio, orderX2)));
                if (Math.Abs(v) < Math.Pow(0.5, 24)) {
                    v = 0.0;
                }
                fromF[i].real = v;
            }
            for (int i = 1; i < filterLenP1 / 2; ++i) {
                fromF[filterLenP1 - i].real = fromF[i].real;
            }

            // IFFTでfromFをfromTに変換
            WWComplex[] fromT;
            {
                var fft = new WWRadix2Fft(filterLenP1);
                fromT = fft.ForwardFft(fromF);

                double compensation = 1.0 / (filterLenP1 * cutoffRatio);
                for (int i = 0; i < filterLenP1; ++i) {
                    fromT[i].Set(
                            fromT[i].real * compensation,
                            fromT[i].imaginary * compensation);
                }
            }
            fromF = null;

            // fromTの中心がFILTER_LENGTH/2番に来るようにする。
            // delayT[0]のデータはfromF[FILTER_LENGTH/2]だが、非対称になるので入れない
            // このフィルタの遅延はFILTER_LENGTH/2サンプルある

            var delayT = new WWComplex[filterLenP1];
            for (int i = 1; i < filterLenP1 / 2; ++i) {
                delayT[i] = fromT[i + filterLenP1 / 2];
            }
            for (int i = 0; i < filterLenP1 / 2; ++i) {
                delayT[i + filterLenP1 / 2] = fromT[i];
            }
            fromT = null;

            // Kaiser窓をかける
            var w = WWWindowFunc.KaiserWindow(filterLenP1 + 1, 9.0);
            for (int i = 0; i < filterLenP1; ++i) {
                delayT[i].Mul(w[i]);
            }

            var delayTL = new WWComplex[fftLength];
            for (int i = 0; i < delayT.Length; ++i) {
                delayTL[i] = delayT[i];
            }
            delayT = null;

            // できたフィルタをFFTする
            WWComplex[] delayF;
            {
                var fft = new WWRadix2Fft(fftLength);
                delayF = fft.ForwardFft(delayTL);

                for (int i = 0; i < fftLength; ++i) {
                    delayF[i].Mul(cutoffRatio);
                }
            }
            delayTL = null;

            return delayF;
        }