00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163
00164
00165
00166
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210 #ifndef fft_SOURCES
00211 #define fft_SOURCES
00212
00213 #include "integrate.h"
00214
00217 namespace PLib {
00218
00219 template <class T>
00220 void bitrv2(int n, BasicArray<T> &a)
00221 {
00222 int j, j1, k, k1, l, m, m2, n2;
00223 T xr, xi;
00224
00225 m = n >> 2;
00226 m2 = m << 1;
00227 n2 = n - 2;
00228 k = 0;
00229 for (j = 0; j <= m2 - 4; j += 4) {
00230 if (j < k) {
00231 xr = a[j];
00232 xi = a[j + 1];
00233 a[j] = a[k];
00234 a[j + 1] = a[k + 1];
00235 a[k] = xr;
00236 a[k + 1] = xi;
00237 } else if (j > k) {
00238 j1 = n2 - j;
00239 k1 = n2 - k;
00240 xr = a[j1];
00241 xi = a[j1 + 1];
00242 a[j1] = a[k1];
00243 a[j1 + 1] = a[k1 + 1];
00244 a[k1] = xr;
00245 a[k1 + 1] = xi;
00246 }
00247 k1 = m2 + k;
00248 xr = a[j + 2];
00249 xi = a[j + 3];
00250 a[j + 2] = a[k1];
00251 a[j + 3] = a[k1 + 1];
00252 a[k1] = xr;
00253 a[k1 + 1] = xi;
00254 l = m;
00255 while (k >= l) {
00256 k -= l;
00257 l >>= 1;
00258 }
00259 k += l;
00260 }
00261 }
00262
00263
00264 template <class T>
00265 void cdft(int n, T wr, T wi, BasicArray<T> &a)
00266 {
00267 int i, j, k, l, m;
00268 T wkr, wki, wdr, wdi, ss, xr, xi;
00269
00270 m = n;
00271 while (m > 4) {
00272 l = m >> 1;
00273 wkr = 1;
00274 wki = 0;
00275 wdr = 1 - 2 * wi * wi;
00276 wdi = 2 * wi * wr;
00277 ss = 2 * wdi;
00278 wr = wdr;
00279 wi = wdi;
00280 for (j = 0; j <= n - m; j += m) {
00281 i = j + l;
00282 xr = a[j] - a[i];
00283 xi = a[j + 1] - a[i + 1];
00284 a[j] += a[i];
00285 a[j + 1] += a[i + 1];
00286 a[i] = xr;
00287 a[i + 1] = xi;
00288 xr = a[j + 2] - a[i + 2];
00289 xi = a[j + 3] - a[i + 3];
00290 a[j + 2] += a[i + 2];
00291 a[j + 3] += a[i + 3];
00292 a[i + 2] = wdr * xr - wdi * xi;
00293 a[i + 3] = wdr * xi + wdi * xr;
00294 }
00295 for (k = 4; k <= l - 4; k += 4) {
00296 wkr -= ss * wdi;
00297 wki += ss * wdr;
00298 wdr -= ss * wki;
00299 wdi += ss * wkr;
00300 for (j = k; j <= n - m + k; j += m) {
00301 i = j + l;
00302 xr = a[j] - a[i];
00303 xi = a[j + 1] - a[i + 1];
00304 a[j] += a[i];
00305 a[j + 1] += a[i + 1];
00306 a[i] = wkr * xr - wki * xi;
00307 a[i + 1] = wkr * xi + wki * xr;
00308 xr = a[j + 2] - a[i + 2];
00309 xi = a[j + 3] - a[i + 3];
00310 a[j + 2] += a[i + 2];
00311 a[j + 3] += a[i + 3];
00312 a[i + 2] = wdr * xr - wdi * xi;
00313 a[i + 3] = wdr * xi + wdi * xr;
00314 }
00315 }
00316 m = l;
00317 }
00318 if (m > 2) {
00319 for (j = 0; j <= n - 4; j += 4) {
00320 xr = a[j] - a[j + 2];
00321 xi = a[j + 1] - a[j + 3];
00322 a[j] += a[j + 2];
00323 a[j + 1] += a[j + 3];
00324 a[j + 2] = xr;
00325 a[j + 3] = xi;
00326 }
00327 }
00328 if (n > 4) {
00329 bitrv2(n, a);
00330 }
00331 }
00332
00333
00334 template <class T>
00335 void rdft(int n, T wr, T wi, BasicArray<T> &a)
00336 {
00337 int j, k;
00338 T wkr, wki, wdr, wdi, ss, xr, xi, yr, yi;
00339
00340 if (n > 4) {
00341 wkr = 0;
00342 wki = 0;
00343 wdr = wi * wi;
00344 wdi = wi * wr;
00345 ss = 4 * wdi;
00346 wr = 1 - 2 * wdr;
00347 wi = 2 * wdi;
00348 if (wi >= 0) {
00349 cdft(n, wr, wi, a);
00350 xi = a[0] - a[1];
00351 a[0] += a[1];
00352 a[1] = xi;
00353 }
00354 for (k = (n >> 1) - 4; k >= 4; k -= 4) {
00355 j = n - k;
00356 xr = a[k + 2] - a[j - 2];
00357 xi = a[k + 3] + a[j - 1];
00358 yr = wdr * xr - wdi * xi;
00359 yi = wdr * xi + wdi * xr;
00360 a[k + 2] -= yr;
00361 a[k + 3] -= yi;
00362 a[j - 2] += yr;
00363 a[j - 1] -= yi;
00364 wkr += ss * wdi;
00365 wki += ss * (T(0.5) - wdr);
00366 xr = a[k] - a[j];
00367 xi = a[k + 1] + a[j + 1];
00368 yr = wkr * xr - wki * xi;
00369 yi = wkr * xi + wki * xr;
00370 a[k] -= yr;
00371 a[k + 1] -= yi;
00372 a[j] += yr;
00373 a[j + 1] -= yi;
00374 wdr += ss * wki;
00375 wdi += ss * (T(0.5) - wkr);
00376 }
00377 j = n - 2;
00378 xr = a[2] - a[j];
00379 xi = a[3] + a[j + 1];
00380 yr = wdr * xr - wdi * xi;
00381 yi = wdr * xi + wdi * xr;
00382 a[2] -= yr;
00383 a[3] -= yi;
00384 a[j] += yr;
00385 a[j + 1] -= yi;
00386 if (wi < 0) {
00387 a[1] = T(0.5) * (a[0] - a[1]);
00388 a[0] -= a[1];
00389 cdft(n, wr, wi, a);
00390 }
00391 } else {
00392 if (wi < 0) {
00393 a[1] = T(0.5) * (a[0] - a[1]);
00394 a[0] -= a[1];
00395 }
00396 if (n > 2) {
00397 xr = a[0] - a[2];
00398 xi = a[1] - a[3];
00399 a[0] += a[2];
00400 a[1] += a[3];
00401 a[2] = xr;
00402 a[3] = xi;
00403 }
00404 if (wi >= 0) {
00405 xi = a[0] - a[1];
00406 a[0] += a[1];
00407 a[1] = xi;
00408 }
00409 }
00410 }
00411
00412
00413 template <class T>
00414 void ddct(int n, T wr, T wi, BasicArray<T> &a)
00415 {
00416 int j, k, m;
00417 T wkr, wki, wdr, wdi, ss, xr;
00418
00419 if (n > 2) {
00420 wkr = T(0.5);
00421 wki = T(0.5);
00422 wdr = T(0.5) * (wr - wi);
00423 wdi = T(0.5) * (wr + wi);
00424 ss = 2 * wi;
00425 if (wi < 0) {
00426 xr = a[n - 1];
00427 for (k = n - 2; k >= 2; k -= 2) {
00428 a[k + 1] = a[k] - a[k - 1];
00429 a[k] += a[k - 1];
00430 }
00431 a[1] = 2 * xr;
00432 a[0] *= 2;
00433 rdft(n, 1 - ss * wi, ss * wr, a);
00434 xr = wdr;
00435 wdr = wdi;
00436 wdi = xr;
00437 ss = -ss;
00438 }
00439 m = n >> 1;
00440 for (k = 1; k <= m - 3; k += 2) {
00441 j = n - k;
00442 xr = wdi * a[k] - wdr * a[j];
00443 a[k] = wdr * a[k] + wdi * a[j];
00444 a[j] = xr;
00445 wkr -= ss * wdi;
00446 wki += ss * wdr;
00447 xr = wki * a[k + 1] - wkr * a[j - 1];
00448 a[k + 1] = wkr * a[k + 1] + wki * a[j - 1];
00449 a[j - 1] = xr;
00450 wdr -= ss * wki;
00451 wdi += ss * wkr;
00452 }
00453 k = m - 1;
00454 j = n - k;
00455 xr = wdi * a[k] - wdr * a[j];
00456 a[k] = wdr * a[k] + wdi * a[j];
00457 a[j] = xr;
00458 a[m] *= wki + ss * wdr;
00459 if (wi >= 0) {
00460 rdft(n, 1 - ss * wi, ss * wr, a);
00461 xr = a[1];
00462 for (k = 2; k <= n - 2; k += 2) {
00463 a[k - 1] = a[k] - a[k + 1];
00464 a[k] += a[k + 1];
00465 }
00466 a[n - 1] = xr;
00467 }
00468 } else {
00469 if (wi >= 0) {
00470 xr = T(0.5) * (wr + wi) * a[1];
00471 a[1] = a[0] - xr;
00472 a[0] += xr;
00473 } else {
00474 xr = a[0] - a[1];
00475 a[0] += a[1];
00476 a[1] = T(0.5) * (wr - wi) * xr;
00477 }
00478 }
00479 }
00480
00481
00482 template <class T>
00483 void ddst(int n, T wr, T wi, BasicArray<T> &a)
00484 {
00485 int j, k, m;
00486 T wkr, wki, wdr, wdi, ss, xr;
00487
00488 if (n > 2) {
00489 wkr = T(0.5);
00490 wki = T(0.5);
00491 wdr = T(0.5) * (wr - wi);
00492 wdi = T(0.5) * (wr + wi);
00493 ss = 2 * wi;
00494 if (wi < 0) {
00495 xr = a[n - 1];
00496 for (k = n - 2; k >= 2; k -= 2) {
00497 a[k + 1] = a[k] + a[k - 1];
00498 a[k] -= a[k - 1];
00499 }
00500 a[1] = -2 * xr;
00501 a[0] *= 2;
00502 rdft(n, 1 - ss * wi, ss * wr, a);
00503 xr = wdr;
00504 wdr = -wdi;
00505 wdi = xr;
00506 wkr = -wkr;
00507 }
00508 m = n >> 1;
00509 for (k = 1; k <= m - 3; k += 2) {
00510 j = n - k;
00511 xr = wdi * a[j] - wdr * a[k];
00512 a[k] = wdr * a[j] + wdi * a[k];
00513 a[j] = xr;
00514 wkr -= ss * wdi;
00515 wki += ss * wdr;
00516 xr = wki * a[j - 1] - wkr * a[k + 1];
00517 a[k + 1] = wkr * a[j - 1] + wki * a[k + 1];
00518 a[j - 1] = xr;
00519 wdr -= ss * wki;
00520 wdi += ss * wkr;
00521 }
00522 k = m - 1;
00523 j = n - k;
00524 xr = wdi * a[j] - wdr * a[k];
00525 a[k] = wdr * a[j] + wdi * a[k];
00526 a[j] = xr;
00527 a[m] *= wki + ss * wdr;
00528 if (wi >= 0) {
00529 rdft(n, 1 - ss * wi, ss * wr, a);
00530 xr = a[1];
00531 for (k = 2; k <= n - 2; k += 2) {
00532 a[k - 1] = a[k + 1] - a[k];
00533 a[k] += a[k + 1];
00534 }
00535 a[n - 1] = -xr;
00536 }
00537 } else {
00538 if (wi >= 0) {
00539 xr = T(0.5) * (wr + wi) * a[1];
00540 a[1] = xr - a[0];
00541 a[0] += xr;
00542 } else {
00543 xr = a[0] + a[1];
00544 a[0] -= a[1];
00545 a[1] = T(0.5) * (wr - wi) * xr;
00546 }
00547 }
00548 }
00549
00550
00551 template <class T>
00552 void bitrv(int n, BasicArray<T> &a)
00553 {
00554 int j, k, l, m, m2, n1;
00555 T xr;
00556
00557 if (n > 2) {
00558 m = n >> 2;
00559 m2 = m << 1;
00560 n1 = n - 1;
00561 k = 0;
00562 for (j = 0; j <= m2 - 2; j += 2) {
00563 if (j < k) {
00564 xr = a[j];
00565 a[j] = a[k];
00566 a[k] = xr;
00567 } else if (j > k) {
00568 xr = a[n1 - j];
00569 a[n1 - j] = a[n1 - k];
00570 a[n1 - k] = xr;
00571 }
00572 xr = a[j + 1];
00573 a[j + 1] = a[m2 + k];
00574 a[m2 + k] = xr;
00575 l = m;
00576 while (k >= l) {
00577 k -= l;
00578 l >>= 1;
00579 }
00580 k += l;
00581 }
00582 }
00583 }
00584
00585
00586 template <class T>
00587 void dfct(int n, T wr, T wi, BasicArray<T> &a)
00588 {
00589 int j, k, m, mh;
00590 T xr, xi, an;
00591
00592 m = n >> 1;
00593 for (j = 0; j <= m - 1; j++) {
00594 k = n - j;
00595 xr = a[j] + a[k];
00596 a[j] -= a[k];
00597 a[k] = xr;
00598 }
00599 an = a[n];
00600 while (m >= 2) {
00601 ddct(m, wr, wi, a);
00602 xr = 1 - 2 * wi * wi;
00603 wi *= 2 * wr;
00604 wr = xr;
00605 bitrv(m, a);
00606 mh = m >> 1;
00607 xi = a[m];
00608 a[m] = a[0];
00609 a[0] = an - xi;
00610 an += xi;
00611 for (j = 1; j <= mh - 1; j++) {
00612 k = m - j;
00613 xr = a[m + k];
00614 xi = a[m + j];
00615 a[m + j] = a[j];
00616 a[m + k] = a[k];
00617 a[j] = xr - xi;
00618 a[k] = xr + xi;
00619 }
00620 xr = a[mh];
00621 a[mh] = a[m + mh];
00622 a[m + mh] = xr;
00623 m = mh;
00624 }
00625 xi = a[1];
00626 a[1] = a[0];
00627 a[0] = an + xi;
00628 a[n] = an - xi;
00629 bitrv(n, a);
00630 }
00631
00632
00633 template <class T>
00634 void dfst(int n, T wr, T wi, BasicArray<T> &a)
00635 {
00636 int j, k, m, mh;
00637 T xr, xi;
00638
00639 m = n >> 1;
00640 for (j = 1; j <= m - 1; j++) {
00641 k = n - j;
00642 xr = a[j] - a[k];
00643 a[j] += a[k];
00644 a[k] = xr;
00645 }
00646 a[0] = a[m];
00647 while (m >= 2) {
00648 ddst(m, wr, wi, a);
00649 xr = 1 - 2 * wi * wi;
00650 wi *= 2 * wr;
00651 wr = xr;
00652 bitrv(m, a);
00653 mh = m >> 1;
00654 for (j = 1; j <= mh - 1; j++) {
00655 k = m - j;
00656 xr = a[m + k];
00657 xi = a[m + j];
00658 a[m + j] = a[j];
00659 a[m + k] = a[k];
00660 a[j] = xr + xi;
00661 a[k] = xr - xi;
00662 }
00663 a[m] = a[0];
00664 a[0] = a[m + mh];
00665 a[m + mh] = a[mh];
00666 m = mh;
00667 }
00668 a[1] = a[0];
00669 a[0] = 0;
00670 bitrv(n, a);
00671 }
00672
00673 }
00674
00675 #endif