- #include <math.h>
- #define M_PI 3.14159265358979323846
- #define FALSE 0
- #define TRUE 1
- #define BIG 1e10
- #define SMALL 1e-10
- typedef struct {
- float r, i;
- } complex;
- /* FAST CORRELATION OF X(0:L) AND Y(0:L). FINDS RXY(0) THRU RXY(NMAX). */
- /* L=LAST INDEX IN BOTH X AND Y. MUST BE (POWER OF 2)+1 AND AT LEAST 5. */
- /* ITYPE=TYPE OF CORRELATION=0 IF X AND Y ARE THE SAME VECTOR (AUTO- */
- /* CORRELATION), OR NOT 0 IF X AND Y ARE DIFFERENT VECTORS. */
- /* NMAX=MAXIMUM LAG OF INTEREST IN THE CORRELATION FUNCTION. */
- /* FFT LENGTH ,N, USED INTERNALLY, IS L-1. */
- /* LET K=INDEX OF FIRST NONZERO SAMPLE IN Y(0)---Y(N-1). THEN X(0) */
- /* 到 X(N-1) MUST INCLUDE PADDING OF AT LEAST NMAX-K ZEROS. */
- /* CORRELATION FUNCTION, RXY, REPLACES X(0) THRU X(NMAX). */
- /* Y(0) THRU Y(L) IS REPLACED BY ITS FFT, COMPUTED USING SPFFTR. */
- /* IERROR=0 NO ERROR DETECTED */
- /* 1 L-1 NOT A POWER OF 2 */
- /* 2 NMAX OUT OF RANGE */
- /* 3 INADEQUATE ZERO */
- void spcorr(float *x, float *y, long *l, long *type, long *nmax, long *error)
- /*
- x:序列X;
- y:序列Y;
- l:序列X与序列Y的长度,不小5,且要为2的幂次方;
- type:相关的类型,0:表示X与Y序列相同,其它值:X与Y序列不相同
- nmax:相关的最大时延;
- error:运行出错提示;0:无错;1:数据长度不是2的幂次方;2:时延超界;3:无足够零填充出错
- */
- {
- long j, k, m, n;//n:FFT长度;k:序列Y中的首个非零样本的位置序号;在序列Y中必须最少包含有(nmax-k)零填充。
- complex cx;
- float test;
- n = *l - 1;
- if (*nmax < 0 || *nmax >= n)
- {
- *error = 2;
- return;
- }
- test = (float) n;
- test /= 2.0;
- while ((test - 2.0) > 0.0)
- {
- test /= 2.0;
- }
- if ((test - 2.0) == 0)
- {
- for (k = 0 ; k < n && y[k] == 0.0 ; ++k) ;
- for (j = n - 1 ; j >= 0 && x[j] == 0.0 ; --j) ;
- if ((n - 1 - j) < (*nmax - k))
- {
- *error = 3;
- return;
- }
- spfftr(x, &n);//对X序列FFT变换
- if (*type != 0)
- {
- spfftr(y, &n);//如果X、Y是相同序列,则对Y序列也进行FFT
- }
- for (m = 0 ; m <= (n / 2) ; ++m)
- {
- cx.r = x[m * 2] * y[m * 2] - -x[(m * 2) + 1] * y[(m * 2) + 1];
- cx.i = x[m * 2] * y[(m * 2) + 1] + -x[(m * 2) + 1] * y[m * 2];
- x[m * 2] = cx.r / n;
- x[(m * 2) + 1] = cx.i / n;
- }
- spiftr(x, &n);
- *error = 0;
- }
- else if ((test - 2.0) < 0.0)
- {
- *error = 1;
- }
- return;
- } /* spcorr */
- /* SPFFTR 11/12/85 */
- /* FFT ROUTINE FOR REAL TIME SERIES (X) WITH N=2**K SAMPLES. */
- /* COMPUTATION IS IN PLACE, OUTPUT REPLACES INPUT. */
- /* INPUT: REAL VECTOR X(0:N+1) WITH REAL DATA SEQUENCE IN FIRST N */
- /* ELEMENTS; ANYTHING IN LAST 2. NOTE: X MAY BE DECLARED */
- /* REAL IN MAIN PROGRAM PROVIDED THIS ROUTINE IS COMPILED */
- /* SEPARATELY ... COMPLEX OUTPUT REPLACES REAL INPUT HERE. */
- /* OUTPUT: COMPLEX VECTOR XX(O:N/2), SUCH THAT X(0)=REAL(XX(0)),X(1)= */
- /* IMAG(XX(0)), X(2)=REAL(XX(1)), ..., X(N+1)=IMAG(XX(N/2). */
- /* IMPORTANT: N MUST BE AT LEAST 4 AND MUST BE A POWER OF 2. */
- //FFT计算函数
- void spfftr(complex *x, long *n)
- {
- /* Builtin functions */
- void r_cnjg();
- /* Local variables */
- void spfftc();
- long m, tmp_int;
- complex u, tmp, tmp_complex;
- float tpn, tmp_float;
- tpn = (float) (2.0 * M_PI / (double) *n);
- tmp_int = *n / 2;
- spfftc(x, &tmp_int, &neg_i1);
- x[*n / 2].r = x[0].r;
- x[*n / 2].i = x[0].i;
- for (m = 0 ; m <= (*n / 4) ; ++m)
- {
- u.r = (float) sin((double) m * tpn);
- u.i = (float) cos((double) m * tpn);
- r_cnjg(&tmp_complex, &x[*n / 2 - m]);
- tmp.r = (((1.0 + u.r) * x[m].r - u.i * x[m].i)
- + (1.0 - u.r) * tmp_complex.r - -u.i * tmp_complex.i) / 2.0;
- tmp.i = (((1.0 + u.r) * x[m].i + u.i * x[m].r)
- + (1.0 - u.r) * tmp_complex.i + -u.i * tmp_complex.r) / 2.0;
- tmp_float = ((1.0 - u.r) * x[m].r - -u.i * x[m].i
- + (1.0 + u.r) * tmp_complex.r - u.i * tmp_complex.i) / 2.0;
- x[m].i = ((1.0 - u.r) * x[m].i + -u.i * x[m].r
- + (1.0 + u.r) * tmp_complex.i + u.i * tmp_complex.r) / 2.0;
- x[m].r = tmp_float;
- r_cnjg(&x[*n / 2 - m], &tmp);
- }
- return;
- } /* spfftr */
- /* SPIFTR 02/20/87 */
- /* INVERSE FFT OF THE COMPLEX SPECTRUM OF A REAL TIME SERIES. */
- /* X AND N ARE THE SAME AS IN SPFFTR. IMPORTANT: N MUST BE A POWER */
- /* OF 2 AND X MUST BE DIMENSIONED X(0:N+1) (REAL ARRAY, NOT COMPLEX). */
- /* THIS ROUTINE TRANSFORMS THE OUTPUT OF SPFFTR BACK INTO THE INPUT, */
- /* SCALED BY N. COMPUTATION IS IN PLACE, AS IN SPFFTR. */
- //逆FFT变换函数
- void spiftr(complex *x, long *n)
- {
- long m, tmp_int;
- complex u, tmp_complex, tmp;
- float tpn, tmp_float;
- tpn = (float) (2.0 * M_PI / (double) *n);
- for (m = 0 ; m <= (*n / 4) ; ++m)
- {
- u.r = (float) sin((double) m * tpn);
- u.i = (float) -cos((double) m * tpn);
- r_cnjg(&tmp_complex, &x[*n / 2 - m]);
- tmp.r = ((1.0 + u.r) * x[m].r - u.i * x[m].i)
- + ((1.0 - u.r) * tmp_complex.r - -u.i * tmp_complex.i);
- tmp.i = ((1.0 + u.r) * x[m].i + u.i * x[m].r)
- + ((1.0 - u.r) * tmp_complex.i + -u.i * tmp_complex.r);
- r_cnjg(&tmp_complex, &x[*n / 2 - m]);
- tmp_float = ((1.0 - u.r) * x[m].r - -u.i * x[m].i)
- + ((1.0 + u.r) * tmp_complex.r - u.i * tmp_complex.i);
- x[m].i = ((1.0 - u.r) * x[m].i + -u.i * x[m].r)
- + ((1.0 + u.r) * tmp_complex.i + u.i * tmp_complex.r);
- x[m].r = tmp_float;
- r_cnjg(&x[*n / 2 - m], &tmp);
- }
- tmp_int = *n / 2;
- spfftc(x, &tmp_int, &pos_i1);
- return;
- } /* spiftr *
- void r_cnjg(complex *r, complex *z)
- {
- r->r = z->r;
- r->i = -z->i;
- }
复制代码 |