/*
private static void Butterfly(WWDecimalComplex vFrom0, WWDecimalComplex vFrom1, WWDecimalComplex wn, WWDecimalComplex[] vTo, int toPos) {
vTo[toPos].CopyFrom(vFrom0);
var t = new WWDecimalComplex(vFrom1);
t.Mul(wn);
vTo[toPos].Mul(t);
vTo[toPos + 1].CopyFrom(vFrom0);
t.Mul(-1);
vTo[toPos + 1].Mul(t);
}
*/
public WWDecimalComplex[] ForwardFft(WWDecimalComplex[] aFrom, decimal scale = 1M)
{
if (aFrom == null || aFrom.Length != mNumPoints) {
throw new ArgumentOutOfRangeException("aFrom");
}
var aTo = new WWDecimalComplex[aFrom.Length];
var aTmp0 = new WWDecimalComplex[mNumPoints];
for (int i=0; i < aTmp0.Length; ++i) {
aTmp0[i] = new WWDecimalComplex(aFrom[mBitReversalTable[i]]);
}
var aTmp1 = new WWDecimalComplex[mNumPoints];
for (int i=0; i < aTmp1.Length; ++i) {
aTmp1[i] = new WWDecimalComplex();
}
var aTmps = new WWDecimalComplex[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);
if (scale != 1M) {
for (int i=0; i < aTo.Length; ++i) {
aTo[i].Mul(scale);
}
}
return aTo;
}
public WWDecimalFft(int numPoints)
{
if (!IsPowerOfTwo(numPoints) || numPoints < 2) {
throw new ArgumentException("numPoints must be power of two integer and larger than 2");
}
mNumPoints = numPoints;
mWn = new WWDecimalComplex[mNumPoints];
for (int i=0; i < mNumPoints; ++i) {
decimal angle = -2.0M * WWDecimalMath.M_PI * i / mNumPoints;
mWn[i] = new WWDecimalComplex(WWDecimalMath.Cos(angle), WWDecimalMath.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 static WWDecimalComplex Mul(WWDecimalComplex a, WWDecimalComplex b)
{
var r = new WWDecimalComplex(a);
r.Mul(b);
return(r);
}
public static WWDecimalComplex Add(WWDecimalComplex a, WWDecimalComplex b)
{
var r = new WWDecimalComplex(a);
r.Add(b);
return(r);
}
public WWDecimalFft(int numPoints)
{
if (!IsPowerOfTwo(numPoints) || numPoints < 2)
{
throw new ArgumentException("numPoints must be power of two integer and larger than 2");
}
mNumPoints = numPoints;
mWn = new WWDecimalComplex[mNumPoints];
for (int i = 0; i < mNumPoints; ++i)
{
decimal angle = -2.0M * WWDecimalMath.M_PI * i / mNumPoints;
mWn[i] = new WWDecimalComplex(WWDecimalMath.Cos(angle), WWDecimalMath.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 static WWDecimalComplex Sub(WWDecimalComplex a, WWDecimalComplex b)
{
var r = new WWDecimalComplex(a);
r.Sub(b);
return(r);
}
public static WWDecimalComplex[] From(decimal[] from)
{
var to = new WWDecimalComplex[from.Length];
for (int i = 0; i < from.Length; ++i) {
to[i].real = from[i];
}
return to;
}
public static decimal[] ExtractRealPart(WWDecimalComplex[] from)
{
var to = new decimal[from.Length];
for (int i = 0; i < from.Length; ++i) {
to[i] = from[i].real;
}
return to;
}
public static WWDecimalComplex[] From(decimal[] from)
{
var to = new WWDecimalComplex[from.Length];
for (int i = 0; i < from.Length; ++i)
{
to[i].real = from[i];
}
return(to);
}
public static WWDecimalComplex[] Add(WWDecimalComplex[] a, WWDecimalComplex[] b)
{
if (a.Length != b.Length) {
throw new ArgumentException("input array length mismatch");
}
var c = new WWDecimalComplex[a.Length];
for (int i = 0; i < a.Length; ++i) {
c[i] = WWDecimalComplex.Add(a[i], b[i]);
}
return c;
}
public static decimal AverageDistance(WWDecimalComplex[] a, WWDecimalComplex[] b)
{
if (a.Length != b.Length) {
throw new ArgumentException("input array length mismatch");
}
decimal d = 0M;
for (int i=0; i<a.Length; ++i) {
var s = WWDecimalComplex.Sub(a[i], b[i]);
d += s.Magnitude();
}
d /= a.Length;
return d;
}
public static WWDecimalComplex[] Mul(WWDecimalComplex[] a, WWDecimalComplex[] b)
{
if (a.Length != b.Length)
{
throw new ArgumentException("input array length mismatch");
}
var c = new WWDecimalComplex[a.Length];
for (int i = 0; i < a.Length; ++i)
{
c[i] = WWDecimalComplex.Mul(a[i], b[i]);
}
return(c);
}
void Run(ValueType t)
{
// 1024 samples of 44100Hz PCM contains 23 periods of 998.5Hz sine wave
decimal frequency = 23 * WWDecimalMath.M_2PI;
var signalTime = new WWDecimalComplex[LENGTH];
for (int i = 0; i < LENGTH; ++i)
{
decimal r = WWDecimalMath.Cos(frequency * i / LENGTH);
switch (t)
{
case ValueType.VT_Int16:
r = ((int)(r * 32767)) / 32767M;
break;
case ValueType.VT_Int24:
r = (decimal)((int)(r * 8388607) / 8388607M);
break;
case ValueType.VT_Int32:
r = (decimal)((int)(r * Int32.MaxValue) / ((decimal)(Int32.MaxValue)));
break;
case ValueType.VT_Float32:
r = (decimal)((float)r);
break;
case ValueType.VT_Float64:
r = (decimal)((double)r);
break;
case ValueType.VT_Int64:
r = (decimal)(((long)(r * Int64.MaxValue)) / ((decimal)(Int64.MaxValue)));
break;
case ValueType.VT_Decimal:
break;
}
signalTime[i] = new WWDecimalComplex(r, 0);
}
FftSpectrum(signalTime);
Console.WriteLine("Done.");
}
public static decimal AverageDistance(WWDecimalComplex[] a, WWDecimalComplex[] b)
{
if (a.Length != b.Length)
{
throw new ArgumentException("input array length mismatch");
}
decimal d = 0M;
for (int i = 0; i < a.Length; ++i)
{
var s = WWDecimalComplex.Sub(a[i], b[i]);
d += s.Magnitude();
}
d /= a.Length;
return(d);
}
public WWDecimalComplex Mul(WWDecimalComplex rhs)
{
#if false
// straightforward but slow
decimal tR = real * rhs.real - imaginary * rhs.imaginary;
decimal tI = real * rhs.imaginary + imaginary * rhs.real;
real = tR;
imaginary = tI;
#else
// more efficient way
decimal k1 = real * (rhs.real + rhs.imaginary);
decimal k2 = rhs.imaginary * (real + imaginary);
decimal k3 = rhs.real * (imaginary - real);
real = k1 - k2;
imaginary = k1 + k3;
#endif
return(this);
}
void FftSpectrum(WWDecimalComplex[] signalTime)
{
WWDecimalFft fft = new WWDecimalFft(LENGTH);
var signalFreq = fft.ForwardFft(signalTime, 1M/LENGTH);
var result = new Dictionary<double, double>();
Parallel.For(0, LENGTH / 2, i => {
decimal magnitude = signalFreq[i].Magnitude();
double db = (double)(20M * WWDecimalMath.Log10(magnitude));
lock (result) {
result.Add((double)(SAMPLERATE * i / LENGTH), db);
}
});
foreach (var item in result) {
Console.WriteLine("{0},{1}", item.Key, item.Value);
}
}
/*
* private static void Butterfly(WWDecimalComplex vFrom0, WWDecimalComplex vFrom1, WWDecimalComplex wn, WWDecimalComplex[] vTo, int toPos) {
* vTo[toPos].CopyFrom(vFrom0);
* var t = new WWDecimalComplex(vFrom1);
* t.Mul(wn);
* vTo[toPos].Mul(t);
*
* vTo[toPos + 1].CopyFrom(vFrom0);
* t.Mul(-1);
* vTo[toPos + 1].Mul(t);
* }
*/
public WWDecimalComplex[] ForwardFft(WWDecimalComplex[] aFrom, decimal scale = 1M)
{
if (aFrom == null || aFrom.Length != mNumPoints)
{
throw new ArgumentOutOfRangeException("aFrom");
}
var aTo = new WWDecimalComplex[aFrom.Length];
var aTmp0 = new WWDecimalComplex[mNumPoints];
for (int i = 0; i < aTmp0.Length; ++i)
{
aTmp0[i] = new WWDecimalComplex(aFrom[mBitReversalTable[i]]);
}
var aTmp1 = new WWDecimalComplex[mNumPoints];
for (int i = 0; i < aTmp1.Length; ++i)
{
aTmp1[i] = new WWDecimalComplex();
}
var aTmps = new WWDecimalComplex[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);
if (scale != 1M)
{
for (int i = 0; i < aTo.Length; ++i)
{
aTo[i].Mul(scale);
}
}
return(aTo);
}
public WWDecimalComplex Sub(WWDecimalComplex rhs)
{
real -= rhs.real;
imaginary -= rhs.imaginary;
return(this);
}
/*
* /// <summary>
* /// Phase in radians
* /// </summary>
* /// <returns>radians, -π to +π</returns>
* public decimal Phase() {
* if (WWDecimalMath.IsTooSmall(Magnitude())) {
* return 0;
* }
*
* return WWDecimalMath.Atan2(imaginary, real);
* }
*/
public WWDecimalComplex Add(WWDecimalComplex rhs)
{
real += rhs.real;
imaginary += rhs.imaginary;
return(this);
}
public static WWDecimalComplex Sub(WWDecimalComplex a, WWDecimalComplex b)
{
var r = new WWDecimalComplex(a);
r.Sub(b);
return r;
}
/*
/// <summary>
/// Phase in radians
/// </summary>
/// <returns>radians, -π to +π</returns>
public decimal Phase() {
if (WWDecimalMath.IsTooSmall(Magnitude())) {
return 0;
}
return WWDecimalMath.Atan2(imaginary, real);
}
*/
public WWDecimalComplex Add(WWDecimalComplex rhs)
{
real += rhs.real;
imaginary += rhs.imaginary;
return this;
}
public void CopyFrom(WWDecimalComplex rhs)
{
real = rhs.real;
imaginary = rhs.imaginary;
}
public WWDecimalComplex(WWDecimalComplex rhs)
{
this.real = rhs.real;
this.imaginary = rhs.imaginary;
}
void Run(ValueType t)
{
// 1024 samples of 44100Hz PCM contains 23 periods of 998.5Hz sine wave
decimal frequency = 23 * WWDecimalMath.M_2PI;
var signalTime = new WWDecimalComplex[LENGTH];
for (int i=0; i < LENGTH; ++i) {
decimal r = WWDecimalMath.Cos(frequency * i / LENGTH);
switch (t) {
case ValueType.VT_Int16:
r = ((int)(r * 32767))/32767M;
break;
case ValueType.VT_Int24:
r = (decimal)((int)(r * 8388607) / 8388607M);
break;
case ValueType.VT_Int32:
r = (decimal)((int)(r * Int32.MaxValue) / ((decimal)(Int32.MaxValue)));
break;
case ValueType.VT_Float32:
r = (decimal)((float)r);
break;
case ValueType.VT_Float64:
r = (decimal)((double)r);
break;
case ValueType.VT_Int64:
r = (decimal)(((long)(r * Int64.MaxValue)) / ((decimal)(Int64.MaxValue)));
break;
case ValueType.VT_Decimal:
break;
}
signalTime[i] = new WWDecimalComplex(r, 0);
}
FftSpectrum(signalTime);
Console.WriteLine("Done.");
}
public WWDecimalComplex Sub(WWDecimalComplex rhs)
{
real -= rhs.real;
imaginary -= rhs.imaginary;
return this;
}
public WWDecimalComplex(WWDecimalComplex rhs)
{
this.real = rhs.real;
this.imaginary = rhs.imaginary;
}
public static WWDecimalComplex Add(WWDecimalComplex a, WWDecimalComplex b)
{
var r = new WWDecimalComplex(a);
r.Add(b);
return r;
}
public static WWDecimalComplex Mul(WWDecimalComplex a, WWDecimalComplex b)
{
var r = new WWDecimalComplex(a);
r.Mul(b);
return r;
}
private void FftStageN(int stageNr, WWDecimalComplex[] x, WWDecimalComplex[] 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 WWDecimalComplex();
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 (!WWDecimalMath.IsExtremelySmall(x[offs].Magnitude())) {
allZero = false;
break;
}
if (!WWDecimalMath.IsExtremelySmall(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);
}
}
}
}
public WWDecimalComplex[] InverseFft(WWDecimalComplex[] aFrom, decimal? compensation = null)
{
for (int i=0; i < aFrom.LongLength; ++i) {
aFrom[i].imaginary *= -1.0M;
}
var aTo = ForwardFft(aFrom);
decimal c = 1.0M / mNumPoints;
if (compensation != null) {
c = (decimal)compensation;
}
for (int i=0; i < aTo.LongLength; ++i) {
aTo[i].real *= c;
aTo[i].imaginary *= -1.0M * c;
}
return aTo;
}
private void FftStageN(int stageNr, WWDecimalComplex[] x, WWDecimalComplex[] 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 WWDecimalComplex();
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 (!WWDecimalMath.IsExtremelySmall(x[offs].Magnitude()))
{
allZero = false;
break;
}
if (!WWDecimalMath.IsExtremelySmall(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);
}
}
}
}
public void CopyFrom(WWDecimalComplex rhs)
{
real = rhs.real;
imaginary = rhs.imaginary;
}
public WWDecimalComplex Mul(WWDecimalComplex rhs)
{
#if false
// straightforward but slow
decimal tR = real * rhs.real - imaginary * rhs.imaginary;
decimal tI = real * rhs.imaginary + imaginary * rhs.real;
real = tR;
imaginary = tI;
#else
// more efficient way
decimal k1 = real * (rhs.real + rhs.imaginary);
decimal k2 = rhs.imaginary * (real + imaginary);
decimal k3 = rhs.real * (imaginary - real);
real = k1 - k2;
imaginary = k1 + k3;
#endif
return this;
}
