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); }
/// <summary> /// Linear Convolution x ** h を計算。 /// </summary> /// <param name="h">コンボリューションカーネル。左右反転される。</param> /// <param name="x">入力数列。</param> public WWComplex[] ConvolutionBruteForce(WWComplex[] h, WWComplex[] x) { var r = new WWComplex[h.Length + x.Length - 1]; Parallel.For(0, r.Length, i => { WWComplex v = WWComplex.Zero(); for (int j = 0; j < h.Length; ++j) { int hPos = h.Length - j - 1; int xPos = i + j - (h.Length - 1); v = WWComplex.Add(v, WWComplex.Mul(Get(h, hPos), Get(x, xPos))); } r[i] = v; }); return r; }
Mul(FirstOrderComplexRationalPolynomial lhs, FirstOrderComplexRationalPolynomial rhs) { // 分子の項 p 分子の項 var n2 = WWComplex.Mul(lhs.N(1), rhs.N(1)); var n1 = WWComplex.Add(WWComplex.Mul(lhs.N(1), rhs.N(0)), WWComplex.Mul(lhs.N(0), rhs.N(1))); var n0 = WWComplex.Mul(lhs.N(0), rhs.N(0)); // 分母の項 p 分母の項 var d2 = WWComplex.Mul(lhs.D(1), rhs.D(1)); var d1 = WWComplex.Add(WWComplex.Mul(lhs.D(1), rhs.D(0)), WWComplex.Mul(lhs.D(0), rhs.D(1))); var d0 = WWComplex.Mul(lhs.D(0), rhs.D(0)); return(new SecondOrderComplexRationalPolynomial(n2, n1, n0, d2, d1, d0)); }
Add(FirstOrderComplexRationalPolynomial L, FirstOrderComplexRationalPolynomial R) { /* nL nR * p+2 p+4 * ─── + ──── * p+1 p+3 * dL dR * * nL dR nR dL * 分子 = (p+2)(p+3) + (p+4)(p+1) = (p^2+5x+6) + (p^2+5x+4) = 2x^2+10x+10 * 分子の次数 = 2 * * dL dR * 分母 = (p+1)(p+3) = p^2 + (1+3)p + 1*3 * 分母の次数 = 2 */ // 分子の項 var n2 = WWComplex.Add( WWComplex.Mul(L.N(1), R.D(1)), WWComplex.Mul(R.N(1), L.D(1))); var n1 = WWComplex.Add( WWComplex.Add(WWComplex.Mul(L.N(0), R.D(1)), WWComplex.Mul(L.N(1), R.D(0))), WWComplex.Add(WWComplex.Mul(R.N(0), L.D(1)), WWComplex.Mul(R.N(1), L.D(0)))); var n0 = WWComplex.Add( WWComplex.Mul(L.N(0), R.D(0)), WWComplex.Mul(R.N(0), L.D(0))); // 分母の項 var d2 = WWComplex.Mul(L.D(1), R.D(1)); var d1 = WWComplex.Add(WWComplex.Mul(L.D(0), R.D(1)), WWComplex.Mul(L.D(1), R.D(0))); var d0 = WWComplex.Mul(L.D(0), R.D(0)); return(new SecondOrderComplexRationalPolynomial(n2, n1, n0, d2, d1, d0)); }
MulComplexConjugatePair(FirstOrderComplexRationalPolynomial p0) { if (p0.NumerDegree() != 1 || p0.DenomDegree() != 1) { throw new ArgumentException("p0"); } var n0 = p0.NumerCoeffs(); var d0 = p0.DenomCoeffs(); // n1, d1 : 共役の多項式の係数 var n1 = new WWComplex[2]; for (int i = 0; i < 2; ++i) { n1[i] = n0[i].ComplexConjugate(); } var d1 = new WWComplex[2]; for (int i = 0; i < 2; ++i) { d1[i] = d0[i].ComplexConjugate(); } var n = new double[3]; n[0] = WWComplex.Mul(n0[0], n1[0]).real; n[1] = WWComplex.Add(WWComplex.Mul(n0[0], n1[1]), WWComplex.Mul(n0[1], n1[0])).real; n[2] = WWComplex.Mul(n0[1], n1[1]).real; var d = new double[3]; d[0] = WWComplex.Mul(d0[0], d1[0]).real; d[1] = WWComplex.Add(WWComplex.Mul(d0[0], d1[1]), WWComplex.Mul(d0[1], d1[0])).real; d[2] = WWComplex.Mul(d0[1], d1[1]).real; return(new RealRationalPolynomial(n, d)); }
public WWComplex Evaluate(WWComplex x) { WWComplex n = WWComplex.Zero(); WWComplex xN = WWComplex.Unity(); for (int i = 0; i < numer.Length; ++i) { n = WWComplex.Add(n, WWComplex.Mul(xN, numer[i])); xN = WWComplex.Mul(xN, x); } WWComplex d = WWComplex.Zero(); xN = WWComplex.Unity(); for (int i = 0; i < denom.Length; ++i) { d = WWComplex.Add(d, WWComplex.Mul(xN, denom[i])); xN = WWComplex.Mul(xN, x); } WWComplex y = WWComplex.Div(n, d); return(y); }
private void FftStageN(int stageNr, LargeArray <WWComplex> x, LargeArray <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: */ long nRepeat = Pow2(mNumStage - stageNr - 1); long nSubRepeat = mNumPoints / nRepeat; for (long i = 0; i < nRepeat; ++i) { long offsBase = i * nSubRepeat; bool allZero = true; for (long j = 0; j < nSubRepeat / 2; ++j) { long offs = offsBase + (j % (nSubRepeat / 2)); if (Double.Epsilon < x.At(offs).Magnitude()) { allZero = false; break; } if (Double.Epsilon < x.At(offs + nSubRepeat / 2).Magnitude()) { allZero = false; break; } } if (allZero) { for (long j = 0; j < nSubRepeat; ++j) { y.Set(j + offsBase, WWComplex.Zero()); } } else { for (long j = 0; j < nSubRepeat; ++j) { long offs = offsBase + (j % (nSubRepeat / 2)); var t = x.At(offs); var t2 = WWComplex.Mul(mWn.At(j * nRepeat), x.At(offs + nSubRepeat / 2)); var t3 = WWComplex.Add(t, t2); y.Set(j + offsBase, t3); } } } }
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 = WWComplex.Zero(); 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; ++j) { y[j + offsBase] = WWComplex.Zero(); } } else { for (int j = 0; j < nSubRepeat; ++j) { int offs = offsBase + (j % (nSubRepeat / 2)); var v1 = x[offs]; var v2 = WWComplex.Mul(mWn[j * nRepeat], x[offs + nSubRepeat / 2]); y[j + offsBase] = WWComplex.Add(v1, v2); } } } }
/// <summary> /// 窓関数をかけた周波数ドメイン値X^m(q)を戻す。 /// 同様の処理をWWGoertzel (Stable Goertzel algorithm)で行うこともできるだろう。 /// </summary> public WWComplex[] FilterWithWindow(double x, WWWindowFunc.WindowType wt) { var r = Filter(x); var rW = new WWComplex[r.Length]; var w = WWWindowFunc.FreqDomainWindowCoeffs(wt); switch (w.Length) { case 2: for (int i = 0; i < mN; ++i) { int il = i - 1; if (il < 0) { il = mN - 1; } int ir = i + 1; if (mN <= ir) { ir = 0; } rW[i] = WWComplex.Add( WWComplex.Mul(r[il], w[1] * 0.5), WWComplex.Mul(r[i], w[0]), WWComplex.Mul(r[ir], w[1] * 0.5)); } return(rW); case 3: for (int i = 0; i < mN; ++i) { var pos = new int[5]; for (int offs = -2; offs <= 2; ++offs) { int p = i + offs; if (p < 0) { p += mN; } if (mN <= p) { p -= mN; } pos[offs + 2] = p; } rW[i] = WWComplex.Add( WWComplex.Mul(r[pos[0]], w[2] * 0.5), WWComplex.Mul(r[pos[1]], w[1] * 0.5), WWComplex.Mul(r[pos[2]], w[0]), WWComplex.Mul(r[pos[3]], w[1] * 0.5), WWComplex.Mul(r[pos[4]], w[2] * 0.5)); } return(rW); default: System.Diagnostics.Debug.Assert(false); return(null); } }
/// <summary> /// 連続FFT オーバーラップアド法でLinear convolution x ** hする。 /// </summary> /// <param name="h">コンボリューションカーネル。左右反転される。</param> /// <param name="x">入力数列。</param> /// <param name="fragmentSz">個々のFFTに入力するxの断片サイズ。</param> public WWComplex[] ConvolutionContinuousFft(WWComplex[] h, WWComplex[] x, int fragmentSz) { System.Diagnostics.Debug.Assert(2 <= fragmentSz); if (x.Length < h.Length) { // swap x and h var tmp = h; h = x; x = tmp; } // h.Len <= x.Len int fullConvLen = h.Length + x.Length - 1; var r = new WWComplex[fullConvLen]; for (int i = 0; i < r.Length; ++i) { r[i] = WWComplex.Zero(); } // hをFFTしてHを得る。 int fragConvLen = h.Length + fragmentSz - 1; int fftSize = Functions.NextPowerOf2(fragConvLen); var h2 = new WWComplex[fftSize]; Array.Copy(h, 0, h2, 0, h.Length); for (int i = h.Length; i < h2.Length; ++i) { h2[i] = WWComplex.Zero(); } var fft = new WWRadix2Fft(fftSize); var H = fft.ForwardFft(h2); for (int offs = 0; offs < x.Length; offs += fragmentSz) { // xFをFFTしてXを得る。 var xF = WWComplex.ZeroArray(fftSize); for (int i = 0; i < fragmentSz; ++i) { if (i + offs < x.Length) { xF[i] = x[offs + i]; } else { break; } } var X = fft.ForwardFft(xF); var Y = WWComplex.Mul(H, X); var y = fft.InverseFft(Y); // オーバーラップアド法。FFT結果を足し合わせる。 for (int i = 0; i < fragConvLen; ++i) { r[offs + i] = WWComplex.Add(r[offs + i], y[i]); } } return(r); }
private void FindRootCubic(double[] coeffs) { // 3次多項式 cubic poly equation // https://en.wikipedia.org/wiki/Cubic_function#Algebraic_solution System.Diagnostics.Debug.Assert(4 == coeffs.Length); double a = coeffs[3]; double b = coeffs[2]; double c = coeffs[1]; double d = coeffs[0]; double Δ = 18 * a * b * c * d - 4 * b * b * b * d + 1 * b * b * c * c - 4 * a * c * c * c - 27 * a * a * d * d; double Δ0 = b * b - 3 * a * c; // ↓ 雑な0かどうかの判定処理。 if (AlmostZero(Δ)) { if (AlmostZero(Δ0)) { // triple root double root = -b / (3 * a); mRoots.Add(new WWComplex(root, 0)); mRoots.Add(new WWComplex(root, 0)); mRoots.Add(new WWComplex(root, 0)); return; } // double root and a simple root double root2 = (9 * a * d - b * c) / (2 * Δ0); double root1 = (4 * a * b * c - 9 * a * a * d - b * b * b); mRoots.Add(new WWComplex(root2, 0)); mRoots.Add(new WWComplex(root2, 0)); mRoots.Add(new WWComplex(root1, 0)); return; } { double Δ1 = +2 * b * b * b - 9 * a * b * c + 27 * a * a * d; WWComplex C; { WWComplex c1 = new WWComplex(Δ1 / 2, 0); double c2Sqrt = Math.Sqrt(Math.Abs(-27 * a * a * Δ) / 4); WWComplex c2; if (Δ < 0) { //C2 is real number c2 = new WWComplex(c2Sqrt, 0); } else { //C2 is imaginary number c2 = new WWComplex(0, c2Sqrt); } WWComplex c1c2; WWComplex c1Pc2 = WWComplex.Add(c1, c2); WWComplex c1Mc2 = WWComplex.Sub(c1, c2); if (c1Mc2.Magnitude() <= c1Pc2.Magnitude()) { c1c2 = c1Pc2; } else { c1c2 = c1Mc2; } // 3乗根 = 大きさが3分の1乗で角度が3分の1. double magnitude = c1c2.Magnitude(); double phase = c1c2.Phase(); double cMag = Math.Pow(magnitude, 1.0 / 3.0); double cPhase = phase / 3; C = new WWComplex(cMag * Math.Cos(cPhase), cMag * Math.Sin(cPhase)); } var ζ = new WWComplex(-1.0 / 2.0, 1.0 / 2.0 * Math.Sqrt(3.0)); var ζ2 = new WWComplex(-1.0 / 2.0, -1.0 / 2.0 * Math.Sqrt(3.0)); var r3a = new WWComplex(-1.0 / 3.0 / a, 0); WWComplex root0 = WWComplex.Mul(r3a, WWComplex.Add( WWComplex.Add(new WWComplex(b, 0), C), WWComplex.Div(new WWComplex(Δ0, 0), C))); WWComplex root1 = WWComplex.Mul(r3a, WWComplex.Add( WWComplex.Add(new WWComplex(b, 0), WWComplex.Mul(ζ, C)), WWComplex.Div(new WWComplex(Δ0, 0), WWComplex.Mul(ζ, C)))); WWComplex root2 = WWComplex.Mul(r3a, WWComplex.Add( WWComplex.Add(new WWComplex(b, 0), WWComplex.Mul(ζ2, C)), WWComplex.Div(new WWComplex(Δ0, 0), WWComplex.Mul(ζ2, C)))); mRoots.Add(root0); mRoots.Add(root1); mRoots.Add(root2); return; } }