public OverlappedFft(int fftLength)
{
mFftLength = fftLength;
mFft = new WWRadix2Fft(mFftLength);
mOverlapInputSamples = new double[mFftLength / 2];
mWindow = WWWindowFunc.BlackmanWindow(mFftLength / 2 - 1);
mLastOutputSamplesTail = new double[mFftLength / mSpliceDenominator];
mFirstTime = true;
}
public OverlappedFft(int fftLength)
{
mFftLength = fftLength;
mFft = new WWRadix2Fft(mFftLength);
mOverlapInputSamples = new double[mFftLength / 2];
mWindow = WWWindowFunc.BlackmanWindow(mFftLength / 2 - 1);
mLastOutputSamplesTail = new double[mFftLength / mSpliceDenominator];
mFirstTime = true;
}
private double[] FFTFir(double[] inPcm, double[] coef, int fftLength)
{
var inTime = new WWComplex[fftLength];
for (int i = 0; i < mNumSamples; ++i)
{
inTime[i].real = inPcm[i];
}
var inFreq = new WWComplex[fftLength];
{
var fft = new WWRadix2Fft(fftLength);
fft.ForwardFft(inTime, inFreq);
}
inTime = null;
var coefTime = new WWComplex[fftLength];
for (int i = 0; i < mCoeffs[mChannelId * 2].Length; ++i)
{
coefTime[i].real = coef[i];
}
var coefFreq = new WWComplex[fftLength];
{
var fft = new WWRadix2Fft(fftLength);
fft.ForwardFft(coefTime, coefFreq);
}
coefTime = null;
var mulFreq = Mul(inFreq, coefFreq);
inFreq = null;
coefFreq = null;
var mulTime = new WWComplex[fftLength];
{
var fft = new WWRadix2Fft(fftLength);
fft.InverseFft(mulFreq, mulTime);
}
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);
}
public WWComplex[] ConvolutionContinuousFft(WWComplex[] h, WWComplex[] x)
{
int fftSize = WWRadix2Fft.NextPowerOf2(h.Length * 4);
int fragmentSize = fftSize - h.Length + 1;
if (x.Length <= fragmentSize)
{
// 1回のFFTで計算する。
return(ConvolutionFft(h, x));
}
var fft = new WWRadix2Fft(fftSize);
var r = new WWComplex[h.Length + x.Length - 1];
var h2 = new WWComplex[fftSize];
Array.Copy(h, 0, h2, 0, h.Length);
var Hf = fft.ForwardFft(h2);
// x(n)をfragmentSize要素ごとのデータ列に分解し、
// それぞれ長さfftSizeになるように末尾に0を追加してx0(n)、x1(n)を得る。
//Parallel.For(0, x.Length / fragmentSize, i => {
for (int i = 0; i < (x.Length + fragmentSize - 1) / fragmentSize; ++i)
{
var xf = new WWComplex[fftSize];
int count = fragmentSize;
if (x.Length < (i + 1) * fragmentSize)
{
count = x.Length - i * fragmentSize;
}
Array.Copy(x, i * fragmentSize, xf, 0, count);
var Xf = fft.ForwardFft(xf);
var Yf = WWComplex.Mul(Hf, Xf);
var yf = fft.InverseFft(Yf);
for (int j = 0; j < fftSize; ++j)
{
if (r.Length <= i * fragmentSize + j)
{
break;
}
r[i * fragmentSize + j].Add(yf[j]);
}
}
;
return(r);
}
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 override double[] FilterDo(double[] inPcm)
{
// この計算で求めるのは、mChannelId==0のとき左耳, mChannelId==1のとき右耳の音。mChannelIdは耳のチャンネル番号。
// 入力データとしてmPcmAllChannelsが使用できる。mPcmAllChannels[0]==左スピーカーの音、mPcmAllChannels[1]==右スピーカーの音。
int fftLength = ((int)mNumSamples < mCoeffs[0].Length) ? mCoeffs[0].Length : (int)mNumSamples;
fftLength = WWRadix2Fft.NextPowerOf2(fftLength);
// 左スピーカーの音=mPcmAllChannels[0]
// 左スピーカーと耳chの相互作用のCoeff==mCoeffs[ch]
var leftSpeaker = FFTFir(mPcmAllChannels[0], mCoeffs[mChannelId + 0], fftLength);
// 右スピーカーの音=mPcmAllChannels[1]
// 右スピーカーと耳chの相互作用のCoeff==mCoeffs[ch+2]
var rightSpeaker = FFTFir(mPcmAllChannels[1], mCoeffs[mChannelId + 2], fftLength);
var mixed = Add(leftSpeaker, rightSpeaker);
return(mixed);
}
public WWComplex[] ConvolutionFft(WWComplex[] h, WWComplex[] x)
{
var r = new WWComplex[h.Length + x.Length - 1];
int fftSize = WWRadix2Fft.NextPowerOf2(r.Length);
var h2 = new WWComplex[fftSize];
Array.Copy(h, 0, h2, 0, h.Length);
var x2 = new WWComplex[fftSize];
Array.Copy(x, 0, x2, 0, x.Length);
var fft = new WWRadix2Fft(fftSize);
var H = fft.ForwardFft(h2);
var X = fft.ForwardFft(x2);
var Y = WWComplex.Mul(H, X);
var y = fft.InverseFft(Y);
Array.Copy(y, 0, r, 0, r.Length);
return(r);
}
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);
}
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 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());
// 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 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 void Clear()
{
mFft = null;
mOverlapInputSamples = null;
mWindow = null;
}
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 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 void Clear()
{
mFft = null;
mOverlapInputSamples = null;
mWindow = null;
}
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;
}
/// <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.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);
}
/// <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;
}
