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