4Voice/j22330/old/FFT3.c

#define _USE_MATH_DEFINES

#include<stdio.h>
#include<math.h>
#include<stdlib.h>

#define LENGTH 512 /* 信号長N ただし，2の整数乗 */

typedef struct {
    double r; /* 実部 */
    double i; /* 虚部 */
} Complex; /* 複素数 */

/* プロトタイプ宣言 */
Complex getComplex(double r, double i);
void bit_reversal(Complex *x, int N);
void fft(Complex *x, int N, int inverse);
void genTwiddleTable(Complex*, int);
Complex calcTwiddlePower(Complex*, int, int);
void dft(Complex*, Complex*, int);
void applyWindow(Complex *x, int N);
Complex cexptheta(double theta);
Complex addComplex(Complex a, Complex b);
Complex subComplex(Complex a, Complex b);
Complex timeComplex(Complex a, Complex b);
Complex divComplexByReal(Complex z, double div);
Complex powComplex(Complex, int);
void printComplex(Complex z);
void printTable(char *name, char index, Complex *z, int N);

int main(int argc, char *argv[]) {
    /* 信号長 */
    int N = LENGTH;
    double amp;

    int i;

    FILE *fp = NULL;
    /* file open */
    if ( argc > 1 ) {
        fp = fopen ( argv[1], "rb" );
        if ( fp == NULL ) {
            fprintf ( stderr, "File open error (%s)\n", argv[1] );
            exit ( 1 );
        }
    } else {
        fprintf ( stderr, "Usage: %s input-filename\n", argv[0] );
        exit ( 1 );
    }

    /* 入力信号配列 */
    short input[LENGTH];
    Complex x[LENGTH];
    /* N点DFT係数 */
    Complex c[LENGTH];

    for(i = 0; i < N; i++) {
        c[i] = getComplex(0.0, 0.0);
    }

    /* 音声データを読み込み */
    if (fread(input, sizeof(short), N, fp) != (size_t)N) {
        fprintf(stderr, "File read error (%s)\n", argv[1]);
        fclose(fp);
        exit(1);
    }

    /* 音声データを複素数領域に変換 */
    for(int i = 0; i < N; i++) {
        x[i] = getComplex(input[i], 0);
    }

    applyWindow(x, N);

    fft(x, N, 0);
    
    for(int i = 0; i < N; i++) {
        amp = sqrt(x[i].r * x[i].r + x[i].i * x[i].i);
        printf("%f\n", amp);
    }
    

    fclose(fp);

    return 0;
}

/* 第一引数を実部，第二引数を虚部とする複素数を返す */
Complex getComplex(double r, double i) {
    Complex c;
    c.r = r;
    c.i = i;
    return c;
}

/**
 * @brief 配列の要素をビット逆順に並べ替える
 *
 * @param x 信号配列
 * @param N 信号長 ただし，2の整数乗
 */
void bit_reversal(Complex *x, int N) {
    int i, j, k;
    j = 0;
    Complex temp;
    for(i = 0; i < N; i++) {
        if (i < j) {
            temp = x[i];
            x[i] = x[j];
            x[j] = temp;
        }

        k = N >> 1;

        while (k <= j && k > 0) {
            j -= k;
            k >>= 1;
        }
        j += k;
    }

}

/**
 * @brief 高速フーリエ変換
 *
 * @param x 入力信号配列
 * @param N 信号長 ただし，2の整数乗
 * @param inverse 0ならDFT，1ならIDFT
 */
void fft(Complex *x, int N, int inverse) {
    bit_reversal(x, N);

    for (int s = 1; (1 << s) <= N; s++) {
        int m = 1 << s;           // 現在のブロックのサイズ
        int m2 = m >> 1;          // ブロックサイズの半分
        double theta = (inverse ? 2.0 : -2.0) * M_PI / m;
        Complex wm = cexptheta(theta); // 基本となる回転因子

        // 各ブロックの計算
        for (int k = 0; k < N; k += m) {
            Complex w = getComplex(1.0, 0);
            for (int j = 0; j < m2; j++) {
                // バタフライ演算の本体
                Complex u = x[k + j];
                Complex v = timeComplex(w, x[k + j + m2]);

                x[k + j] = addComplex(u, v);      // 上側
                x[k + j + m2] = subComplex(u, v); // 下側

                w = timeComplex(w, wm); // 回転因子を更新
            }
        }
    }

    // 3. 逆変換の場合、最後に n で割る
    if (inverse) {
        for (int i = 0; i < N; i++) {
            x[i] = divComplexByReal(x[i], N);
        }
    }
}

/* 回転子W_Nの0乗からN-1乗を計算 */
void genTwiddleTable(Complex *W, int N) {
    int i;
    for (i = 0; i < N; i++) {
        W[i] = getComplex(cos(i * 2.0 * M_PI / N), -sin(i * 2.0 * M_PI / N));
    }
}

/* 回転子W_Nのn乗のk乗を計算 */
Complex calcTwiddlePower(Complex *W, int n, int k) {
    return powComplex(W[n], k);
}

/* 離散フーリエ変換 */
void dft(Complex *x, Complex *c, int N) {
    int k, n;
    Complex W[LENGTH];

    genTwiddleTable(W, N);

    /* 信号長で反復 */
    for(k = 0; k < N; k++) {
        /* シグマループ */
        for(n = 0; n < N; n++) {
            c[k] = addComplex(
                c[k],
                timeComplex(
                    x[n],
                    calcTwiddlePower(
                        W, k, n
                    )
                )
            );
        }
    }
}

/* 配列にハニング窓を適用 */
void applyWindow(Complex *x, int N) {
    for (int i = 0; i < N; i++) {
        double w = 0.5 - 0.5 * cos(2 * M_PI * i / (N - 1)); // ハニング窓
        x[i].r *= w;
        x[i].i *= w;
    }
}

Complex cexptheta(double theta) {
    return getComplex(cos(theta), sin(theta));
}

Complex addComplex(Complex a, Complex b) {
    return getComplex(a.r + b.r, a.i + b.i);
}

Complex subComplex(Complex a, Complex b) {
    return getComplex(a.r - b.r, a.i - b.i);
}

Complex timeComplex(Complex a, Complex b) {
    return getComplex(a.r*b.r - a.i*b.i, a.r*b.i+a.i*b.r);
}

Complex divComplexByReal(Complex z, double div) {
    return getComplex(z.r / div, z.i / div);
}

/* 第一引数の第二引数乗を返す
 * 第二引数が負の場合は，第一引数の大きさが1のときのみ正しく計算される
 */
Complex powComplex(Complex z, int pow) {
    if(pow == 0) return getComplex(1.0, 0.0);

    int i;
    int p = (pow > 0) ? pow : -pow;
    Complex ret = z;
    for(i = 0; i < p-1; i++) {
        ret = timeComplex(ret, z);
    }

    /* |z|==1のとき，zの-1乗はzの共役複素数 */
    if(pow < 0)
        ret.i = -ret.i;

    return ret;
}

/* 複素数出力 */
void printComplex(Complex z) {
    if(z.i < 0) {
        printf("%f-j%f\n", z.r, -z.i);
    } else {
        printf("%f+j%f\n", z.r, z.i);
    }
}

/* 複素数テーブル出力 */
void printTable(char *name, char index, Complex *z, int N) {
    int i;
    printf("----- BEGIN %s TABLE -----\n", name);
    for(i = 0; i < N; i++) {
        printf("%c = %d : ", index, i);
        printComplex(z[i]);
    }
    printf("----- END %s TABLE -----\n", name);
}