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