/*
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;
}
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);
}
}
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);
}
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;
}
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;
}
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;
}
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());
}
}
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;
}
public WWComplex Sub(WWComplex rhs)
{
real -= rhs.real;
imaginary -= rhs.imaginary;
return this;
}
public static WWComplex Sub(WWComplex a, WWComplex b)
{
var r = new WWComplex(a);
r.Sub(b);
return r;
}
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;
}
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);
}
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;
}
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;
}
}
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);
}
}
}
}
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();
}
}
public static WWComplex Add(WWComplex a, WWComplex b)
{
var r = new WWComplex(a);
r.Add(b);
return r;
}
public WWComplex(WWComplex rhs)
{
this.real = rhs.real;
this.imaginary = rhs.imaginary;
}
public void CopyFrom(WWComplex rhs)
{
real = rhs.real;
imaginary = rhs.imaginary;
}
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);
}
public static WWComplex Mul(WWComplex a, WWComplex b)
{
var r = new WWComplex(a);
r.Mul(b);
return r;
}
/// <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);
}
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;
}
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;
}
public WWComplex Add(WWComplex rhs)
{
real += rhs.real;
imaginary += rhs.imaginary;
return(this);
}
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;
}
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);
}
}
}
}
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;
}
public WWComplex Div(WWComplex rhs)
{
var denom = new WWComplex(rhs).Reciprocal();
return Mul(denom);
}
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);
}
/// <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;
}
