00001
00002
00003
00004
00005
00006 #define WANT_MATH
00007
00008
00009 #include "include.h"
00010
00011 #include "newmatap.h"
00012
00013
00014
00015 #ifdef use_namespace
00016 namespace NEWMAT {
00017 #endif
00018
00019 #ifdef DO_REPORT
00020 #define REPORT { static ExeCounter ExeCount(__LINE__,19); ++ExeCount; }
00021 #else
00022 #define REPORT {}
00023 #endif
00024
00025 static void cossin(int n, int d, Real& c, Real& s)
00026
00027
00028 {
00029 REPORT
00030 long n4 = n * 4; int sector = (int)floor( (Real)n4 / (Real)d + 0.5 );
00031 n4 -= sector * d;
00032 if (sector < 0) { REPORT sector = 3 - (3 - sector) % 4; }
00033 else { REPORT sector %= 4; }
00034 Real ratio = 1.5707963267948966192 * (Real)n4 / (Real)d;
00035
00036 switch (sector)
00037 {
00038 case 0: REPORT c = cos(ratio); s = sin(ratio); break;
00039 case 1: REPORT c = -sin(ratio); s = cos(ratio); break;
00040 case 2: REPORT c = -cos(ratio); s = -sin(ratio); break;
00041 case 3: REPORT c = sin(ratio); s = -cos(ratio); break;
00042 }
00043 }
00044
00045 static void fftstep(ColumnVector& A, ColumnVector& B, ColumnVector& X,
00046 ColumnVector& Y, int after, int now, int before)
00047 {
00048 REPORT
00049 Tracer trace("FFT(step)");
00050
00051 const int gamma = after * before; const int delta = now * after;
00052
00053
00054 Real r_arg = 1.0; Real i_arg = 0.0;
00055 Real* x = X.Store(); Real* y = Y.Store();
00056 const int m = A.Nrows() - gamma;
00057
00058 for (int j = 0; j < now; j++)
00059 {
00060 Real* a = A.Store(); Real* b = B.Store();
00061 Real* x1 = x; Real* y1 = y; x += after; y += after;
00062 for (int ia = 0; ia < after; ia++)
00063 {
00064
00065
00066 cossin(-(j*after+ia), delta, r_arg, i_arg);
00067
00068 Real* a1 = a++; Real* b1 = b++; Real* x2 = x1++; Real* y2 = y1++;
00069 if (now==2)
00070 {
00071 REPORT int ib = before;
00072 if (ib) for (;;)
00073 {
00074 REPORT
00075 Real* a2 = m + a1; Real* b2 = m + b1; a1 += after; b1 += after;
00076 Real r_value = *a2; Real i_value = *b2;
00077 *x2 = r_value * r_arg - i_value * i_arg + *(a2-gamma);
00078 *y2 = r_value * i_arg + i_value * r_arg + *(b2-gamma);
00079 if (!(--ib)) break;
00080 x2 += delta; y2 += delta;
00081 }
00082 }
00083 else
00084 {
00085 REPORT int ib = before;
00086 if (ib) for (;;)
00087 {
00088 REPORT
00089 Real* a2 = m + a1; Real* b2 = m + b1; a1 += after; b1 += after;
00090 Real r_value = *a2; Real i_value = *b2;
00091 int in = now-1; while (in--)
00092 {
00093
00094
00095
00096 a2 -= gamma; b2 -= gamma; Real temp = r_value;
00097 r_value = r_value * r_arg - i_value * i_arg + *a2;
00098 i_value = temp * i_arg + i_value * r_arg + *b2;
00099 }
00100 *x2 = r_value; *y2 = i_value;
00101 if (!(--ib)) break;
00102 x2 += delta; y2 += delta;
00103 }
00104 }
00105
00106
00107
00108
00109
00110 }
00111 }
00112 }
00113
00114
00115 void FFTI(const ColumnVector& U, const ColumnVector& V,
00116 ColumnVector& X, ColumnVector& Y)
00117 {
00118
00119 Tracer trace("FFTI");
00120 REPORT
00121 FFT(U,-V,X,Y);
00122 const Real n = X.Nrows(); X /= n; Y /= (-n);
00123 }
00124
00125 void RealFFT(const ColumnVector& U, ColumnVector& X, ColumnVector& Y)
00126 {
00127
00128 Tracer trace("RealFFT");
00129 REPORT
00130 const int n = U.Nrows();
00131 const int n2 = n / 2;
00132 if (n != 2 * n2)
00133 Throw(ProgramException("Vector length not multiple of 2", U));
00134 ColumnVector A(n2), B(n2);
00135 Real* a = A.Store(); Real* b = B.Store(); Real* u = U.Store(); int i = n2;
00136 while (i--) { *a++ = *u++; *b++ = *u++; }
00137 FFT(A,B,A,B);
00138 int n21 = n2 + 1;
00139 X.ReSize(n21); Y.ReSize(n21);
00140 i = n2 - 1;
00141 a = A.Store(); b = B.Store();
00142 Real* an = a + i; Real* bn = b + i;
00143 Real* x = X.Store(); Real* y = Y.Store();
00144 Real* xn = x + n2; Real* yn = y + n2;
00145
00146 *x++ = *a + *b; *y++ = 0.0;
00147 *xn-- = *a++ - *b++; *yn-- = 0.0;
00148
00149 int j = -1; i = n2/2;
00150 while (i--)
00151 {
00152 Real c,s; cossin(j--,n,c,s);
00153 Real am = *a - *an; Real ap = *a++ + *an--;
00154 Real bm = *b - *bn; Real bp = *b++ + *bn--;
00155 Real samcbp = s * am + c * bp; Real sbpcam = s * bp - c * am;
00156 *x++ = 0.5 * ( ap + samcbp); *y++ = 0.5 * ( bm + sbpcam);
00157 *xn-- = 0.5 * ( ap - samcbp); *yn-- = 0.5 * (-bm + sbpcam);
00158 }
00159 }
00160
00161 void RealFFTI(const ColumnVector& A, const ColumnVector& B, ColumnVector& U)
00162 {
00163
00164 Tracer trace("RealFFTI");
00165 REPORT
00166 const int n21 = A.Nrows();
00167 if (n21 != B.Nrows() || n21 == 0)
00168 Throw(ProgramException("Vector lengths unequal or zero", A, B));
00169 const int n2 = n21 - 1; const int n = 2 * n2; int i = n2 - 1;
00170
00171 ColumnVector X(n2), Y(n2);
00172 Real* a = A.Store(); Real* b = B.Store();
00173 Real* an = a + n2; Real* bn = b + n2;
00174 Real* x = X.Store(); Real* y = Y.Store();
00175 Real* xn = x + i; Real* yn = y + i;
00176
00177 Real hn = 0.5 / n2;
00178 *x++ = hn * (*a + *an); *y++ = - hn * (*a - *an);
00179 a++; an--; b++; bn--;
00180 int j = -1; i = n2/2;
00181 while (i--)
00182 {
00183 Real c,s; cossin(j--,n,c,s);
00184 Real am = *a - *an; Real ap = *a++ + *an--;
00185 Real bm = *b - *bn; Real bp = *b++ + *bn--;
00186 Real samcbp = s * am - c * bp; Real sbpcam = s * bp + c * am;
00187 *x++ = hn * ( ap + samcbp); *y++ = - hn * ( bm + sbpcam);
00188 *xn-- = hn * ( ap - samcbp); *yn-- = - hn * (-bm + sbpcam);
00189 }
00190 FFT(X,Y,X,Y);
00191 U.ReSize(n); i = n2;
00192 x = X.Store(); y = Y.Store(); Real* u = U.Store();
00193 while (i--) { *u++ = *x++; *u++ = - *y++; }
00194 }
00195
00196 void FFT(const ColumnVector& U, const ColumnVector& V,
00197 ColumnVector& X, ColumnVector& Y)
00198 {
00199
00200
00201 Tracer trace("FFT");
00202 REPORT
00203 const int n = U.Nrows();
00204 if (n != V.Nrows() || n == 0)
00205 Throw(ProgramException("Vector lengths unequal or zero", U, V));
00206 if (n == 1) { REPORT X = U; Y = V; return; }
00207
00208
00209 if (!FFT_Controller::OnlyOldFFT && FFT_Controller::CanFactor(n))
00210 {
00211 REPORT
00212 X = U; Y = V;
00213 if ( FFT_Controller::ar_1d_ft(n,X.Store(),Y.Store()) ) return;
00214 }
00215
00216 ColumnVector B = V;
00217 ColumnVector A = U;
00218 X.ReSize(n); Y.ReSize(n);
00219 const int nextmx = 8;
00220 int prime[8] = { 2,3,5,7,11,13,17,19 };
00221 int after = 1; int before = n; int next = 0; bool inzee = true;
00222 int now = 0; int b1;
00223
00224 do
00225 {
00226 for (;;)
00227 {
00228 if (next < nextmx) { REPORT now = prime[next]; }
00229 b1 = before / now; if (b1 * now == before) { REPORT break; }
00230 next++; now += 2;
00231 }
00232 before = b1;
00233
00234 if (inzee) { REPORT fftstep(A, B, X, Y, after, now, before); }
00235 else { REPORT fftstep(X, Y, A, B, after, now, before); }
00236
00237 inzee = !inzee; after *= now;
00238 }
00239 while (before != 1);
00240
00241 if (inzee) { REPORT A.Release(); X = A; B.Release(); Y = B; }
00242 }
00243
00244
00245
00246
00247
00248 void DCT_II(const ColumnVector& U, ColumnVector& V)
00249 {
00250
00251 Tracer trace("DCT_II");
00252 REPORT
00253 const int n = U.Nrows();
00254 const int n2 = n / 2; const int n4 = n * 4;
00255 if (n != 2 * n2)
00256 Throw(ProgramException("Vector length not multiple of 2", U));
00257 ColumnVector A(n);
00258 Real* a = A.Store(); Real* b = a + n; Real* u = U.Store();
00259 int i = n2;
00260 while (i--) { *a++ = *u++; *(--b) = *u++; }
00261 ColumnVector X, Y;
00262 RealFFT(A, X, Y); A.CleanUp();
00263 V.ReSize(n);
00264 Real* x = X.Store(); Real* y = Y.Store();
00265 Real* v = V.Store(); Real* w = v + n;
00266 *v = *x;
00267 int k = 0; i = n2;
00268 while (i--)
00269 {
00270 Real c, s; cossin(++k, n4, c, s);
00271 Real xi = *(++x); Real yi = *(++y);
00272 *(++v) = xi * c + yi * s; *(--w) = xi * s - yi * c;
00273 }
00274 }
00275
00276 void DCT_II_inverse(const ColumnVector& V, ColumnVector& U)
00277 {
00278
00279 Tracer trace("DCT_II_inverse");
00280 REPORT
00281 const int n = V.Nrows();
00282 const int n2 = n / 2; const int n4 = n * 4; const int n21 = n2 + 1;
00283 if (n != 2 * n2)
00284 Throw(ProgramException("Vector length not multiple of 2", V));
00285 ColumnVector X(n21), Y(n21);
00286 Real* x = X.Store(); Real* y = Y.Store();
00287 Real* v = V.Store(); Real* w = v + n;
00288 *x = *v; *y = 0.0;
00289 int i = n2; int k = 0;
00290 while (i--)
00291 {
00292 Real c, s; cossin(++k, n4, c, s);
00293 Real vi = *(++v); Real wi = *(--w);
00294 *(++x) = vi * c + wi * s; *(++y) = vi * s - wi * c;
00295 }
00296 ColumnVector A; RealFFTI(X, Y, A);
00297 X.CleanUp(); Y.CleanUp(); U.ReSize(n);
00298 Real* a = A.Store(); Real* b = a + n; Real* u = U.Store();
00299 i = n2;
00300 while (i--) { *u++ = *a++; *u++ = *(--b); }
00301 }
00302
00303 void DST_II(const ColumnVector& U, ColumnVector& V)
00304 {
00305
00306 Tracer trace("DST_II");
00307 REPORT
00308 const int n = U.Nrows();
00309 const int n2 = n / 2; const int n4 = n * 4;
00310 if (n != 2 * n2)
00311 Throw(ProgramException("Vector length not multiple of 2", U));
00312 ColumnVector A(n);
00313 Real* a = A.Store(); Real* b = a + n; Real* u = U.Store();
00314 int i = n2;
00315 while (i--) { *a++ = *u++; *(--b) = -(*u++); }
00316 ColumnVector X, Y;
00317 RealFFT(A, X, Y); A.CleanUp();
00318 V.ReSize(n);
00319 Real* x = X.Store(); Real* y = Y.Store();
00320 Real* v = V.Store(); Real* w = v + n;
00321 *(--w) = *x;
00322 int k = 0; i = n2;
00323 while (i--)
00324 {
00325 Real c, s; cossin(++k, n4, c, s);
00326 Real xi = *(++x); Real yi = *(++y);
00327 *v++ = xi * s - yi * c; *(--w) = xi * c + yi * s;
00328 }
00329 }
00330
00331 void DST_II_inverse(const ColumnVector& V, ColumnVector& U)
00332 {
00333
00334 Tracer trace("DST_II_inverse");
00335 REPORT
00336 const int n = V.Nrows();
00337 const int n2 = n / 2; const int n4 = n * 4; const int n21 = n2 + 1;
00338 if (n != 2 * n2)
00339 Throw(ProgramException("Vector length not multiple of 2", V));
00340 ColumnVector X(n21), Y(n21);
00341 Real* x = X.Store(); Real* y = Y.Store();
00342 Real* v = V.Store(); Real* w = v + n;
00343 *x = *(--w); *y = 0.0;
00344 int i = n2; int k = 0;
00345 while (i--)
00346 {
00347 Real c, s; cossin(++k, n4, c, s);
00348 Real vi = *v++; Real wi = *(--w);
00349 *(++x) = vi * s + wi * c; *(++y) = - vi * c + wi * s;
00350 }
00351 ColumnVector A; RealFFTI(X, Y, A);
00352 X.CleanUp(); Y.CleanUp(); U.ReSize(n);
00353 Real* a = A.Store(); Real* b = a + n; Real* u = U.Store();
00354 i = n2;
00355 while (i--) { *u++ = *a++; *u++ = -(*(--b)); }
00356 }
00357
00358 void DCT_inverse(const ColumnVector& V, ColumnVector& U)
00359 {
00360
00361 Tracer trace("DCT_inverse");
00362 REPORT
00363 const int n = V.Nrows()-1;
00364 const int n2 = n / 2; const int n21 = n2 + 1;
00365 if (n != 2 * n2)
00366 Throw(ProgramException("Vector length not multiple of 2", V));
00367 ColumnVector X(n21), Y(n21);
00368 Real* x = X.Store(); Real* y = Y.Store(); Real* v = V.Store();
00369 Real vi = *v++; *x++ = vi; *y++ = 0.0;
00370 Real sum1 = vi / 2.0; Real sum2 = sum1; vi = *v++;
00371 int i = n2-1;
00372 while (i--)
00373 {
00374 Real vi2 = *v++; sum1 += vi2 + vi; sum2 += vi2 - vi;
00375 *x++ = vi2; vi2 = *v++; *y++ = vi - vi2; vi = vi2;
00376 }
00377 sum1 += vi; sum2 -= vi;
00378 vi = *v; *x = vi; *y = 0.0; vi /= 2.0; sum1 += vi; sum2 += vi;
00379 ColumnVector A; RealFFTI(X, Y, A);
00380 X.CleanUp(); Y.CleanUp(); U.ReSize(n+1);
00381 Real* a = A.Store(); Real* b = a + n; Real* u = U.Store(); v = u + n;
00382 i = n2; int k = 0; *u++ = sum1 / n2; *v-- = sum2 / n2;
00383 while (i--)
00384 {
00385 Real s = sin(1.5707963267948966192 * (++k) / n2);
00386 Real ai = *(++a); Real bi = *(--b);
00387 Real bz = (ai - bi) / 4 / s; Real az = (ai + bi) / 2;
00388 *u++ = az - bz; *v-- = az + bz;
00389 }
00390 }
00391
00392 void DCT(const ColumnVector& U, ColumnVector& V)
00393 {
00394
00395 Tracer trace("DCT");
00396 REPORT
00397 DCT_inverse(U, V);
00398 V *= (V.Nrows()-1)/2;
00399 }
00400
00401 void DST_inverse(const ColumnVector& V, ColumnVector& U)
00402 {
00403
00404 Tracer trace("DST_inverse");
00405 REPORT
00406 const int n = V.Nrows()-1;
00407 const int n2 = n / 2; const int n21 = n2 + 1;
00408 if (n != 2 * n2)
00409 Throw(ProgramException("Vector length not multiple of 2", V));
00410 ColumnVector X(n21), Y(n21);
00411 Real* x = X.Store(); Real* y = Y.Store(); Real* v = V.Store();
00412 Real vi = *(++v); *x++ = 2 * vi; *y++ = 0.0;
00413 int i = n2-1;
00414 while (i--) { *y++ = *(++v); Real vi2 = *(++v); *x++ = vi2 - vi; vi = vi2; }
00415 *x = -2 * vi; *y = 0.0;
00416 ColumnVector A; RealFFTI(X, Y, A);
00417 X.CleanUp(); Y.CleanUp(); U.ReSize(n+1);
00418 Real* a = A.Store(); Real* b = a + n; Real* u = U.Store(); v = u + n;
00419 i = n2; int k = 0; *u++ = 0.0; *v-- = 0.0;
00420 while (i--)
00421 {
00422 Real s = sin(1.5707963267948966192 * (++k) / n2);
00423 Real ai = *(++a); Real bi = *(--b);
00424 Real az = (ai + bi) / 4 / s; Real bz = (ai - bi) / 2;
00425 *u++ = az - bz; *v-- = az + bz;
00426 }
00427 }
00428
00429 void DST(const ColumnVector& U, ColumnVector& V)
00430 {
00431
00432 Tracer trace("DST");
00433 REPORT
00434 DST_inverse(U, V);
00435 V *= (V.Nrows()-1)/2;
00436 }
00437
00438
00439 void FFT2(const Matrix& U, const Matrix& V, Matrix& X, Matrix& Y)
00440 {
00441 Tracer trace("FFT2");
00442 REPORT
00443 int m = U.Nrows(); int n = U.Ncols();
00444 if (m != V.Nrows() || n != V.Ncols() || m == 0 || n == 0)
00445 Throw(ProgramException("Matrix dimensions unequal or zero", U, V));
00446 X = U; Y = V;
00447 int i; ColumnVector CVR; ColumnVector CVI;
00448 for (i = 1; i <= m; ++i)
00449 {
00450 FFT(X.Row(i).t(), Y.Row(i).t(), CVR, CVI);
00451 X.Row(i) = CVR.t(); Y.Row(i) = CVI.t();
00452 }
00453 for (i = 1; i <= n; ++i)
00454 {
00455 FFT(X.Column(i), Y.Column(i), CVR, CVI);
00456 X.Column(i) = CVR; Y.Column(i) = CVI;
00457 }
00458 }
00459
00460 void FFT2I(const Matrix& U, const Matrix& V, Matrix& X, Matrix& Y)
00461 {
00462
00463 Tracer trace("FFT2I");
00464 REPORT
00465 FFT2(U,-V,X,Y);
00466 const Real n = X.Nrows() * X.Ncols(); X /= n; Y /= (-n);
00467 }
00468
00469
00470 #ifdef use_namespace
00471 }
00472 #endif
00473
00474