00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016 #include <stdio.h>
00017 #include <stdlib.h>
00018 #include <string.h>
00019 #include <math.h>
00020 #include <fft_prep.h>
00021 #include <fft99.h>
00022 #include <transfer.h>
00023
00024 #define MAXLINE 80
00025 static int Debug = 0;
00026
00027
00028 static void drop(PZNum *, int *, int);
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041 void response(long nfft, double deltat, ResponseStruct *pRS,
00042 double *tfr, double *tfi)
00043 {
00044 double delomg, omega, mag2;
00045 double sr, si, srn, sin, srd, sid, sr0, si0;
00046 long i, j, ntr;
00047
00048 ntr = nfft / 2 + 1;
00049 delomg = 2.0 * PI / (nfft * deltat);
00050
00051
00052
00053 srn = 1.0;
00054 sin = 0.0;
00055 omega = delomg * 0.001;
00056 for (j = 0; j < pRS->iNumZeros; j++)
00057 {
00058 sr = - pRS->Zeros[j].dReal;
00059 si = omega - pRS->Zeros[j].dImag;
00060 sr0 = srn * sr - sin * si;
00061 si0 = srn * si + sin * sr;
00062 srn = sr0;
00063 sin = si0;
00064 }
00065
00066
00067 srd = 1.0;
00068 sid = 0.0;
00069
00070 for (j = 0; j < pRS->iNumPoles; j++)
00071 {
00072 sr = - pRS->Poles[j].dReal;
00073 si = omega - pRS->Poles[j].dImag;
00074 sr0 = srd * sr - sid * si;
00075 si0 = srd * si + sid * sr;
00076 srd = sr0;
00077 sid = si0;
00078 }
00079
00080
00081 mag2 = pRS->dGain / (srd * srd + sid * sid);
00082 tfr[0] = mag2 * (srn * srd + sin * sid);
00083 tfi[0] = 0.0;
00084
00085
00086 for (i = 1; i < ntr; i++)
00087 {
00088
00089 srn = 1.0;
00090 sin = 0.0;
00091 omega = delomg * i;
00092 for (j = 0; j < pRS->iNumZeros; j++)
00093 {
00094 sr = - pRS->Zeros[j].dReal;
00095 si = omega - pRS->Zeros[j].dImag;
00096 sr0 = srn * sr - sin * si;
00097 si0 = srn * si + sin * sr;
00098 srn = sr0;
00099 sin = si0;
00100 }
00101
00102
00103 srd = 1.0;
00104 sid = 0.0;
00105
00106 for (j = 0; j < pRS->iNumPoles; j++)
00107 {
00108 sr = - pRS->Poles[j].dReal;
00109 si = omega - pRS->Poles[j].dImag;
00110 sr0 = srd * sr - sid * si;
00111 si0 = srd * si + sid * sr;
00112 srd = sr0;
00113 sid = si0;
00114 }
00115
00116
00117 mag2 = pRS->dGain / (srd * srd + sid * sid);
00118 tfr[i] = mag2 * (srn * srd + sin * sid);
00119 tfi[i] = mag2 * (sin * srd - srn * sid);
00120 }
00121 return;
00122 }
00123
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139
00140
00141
00142 int readPZ( char *pzfile, ResponseStruct *pRS )
00143 {
00144 FILE *pzFILE;
00145 int retval = 0, status = 0;
00146 int i, nz = 0, np = 0;
00147 char line[MAXLINE], word[21];
00148 enum states {Key, Pole, Zero};
00149 enum states state = Key;
00150
00151 if ( pzfile == (char *)NULL || strlen(pzfile) == 0)
00152 {
00153
00154 return -3;
00155 }
00156
00157 if ( (pzFILE = fopen(pzfile, "r")) == (FILE *)NULL)
00158 {
00159
00160 return -4;
00161 }
00162
00163 while ( fgets( line, MAXLINE, pzFILE) != (char *)NULL)
00164 {
00165
00166
00167
00168 switch (state)
00169 {
00170 case Key:
00171 if (sscanf(line, "%20s", word) == 0)
00172 continue;
00173 if ( word[0] == '*' || word[0] == '#') continue;
00174
00175 if ( strcmp(word, "CONSTANT") == 0)
00176 {
00177 if (sscanf(line, "%*s %lf", &pRS->dGain) == 0 )
00178 {
00179 retval = -2;
00180 goto abort;
00181 }
00182 status |= 1;
00183 }
00184 else if ( strcmp(word, "ZEROS") == 0)
00185 {
00186 if (sscanf(line, "%*s %d", &pRS->iNumZeros) == 0 || pRS->iNumZeros < 0)
00187 {
00188
00189 retval = -2;
00190 goto abort;
00191 }
00192 if (pRS->iNumZeros > 0)
00193 {
00194 if ( (pRS->Zeros = (PZNum *)malloc(pRS->iNumZeros * sizeof(PZNum)))
00195 == (PZNum *)0 )
00196 {
00197 retval = -1;
00198 goto abort;
00199 }
00200 for (i = 0; i < pRS->iNumZeros; i++)
00201 {
00202 pRS->Zeros[i].dReal = 0.0;
00203 pRS->Zeros[i].dImag = 0.0;
00204 }
00205
00206 state = Zero;
00207 continue;
00208 }
00209 status |= 2;
00210 }
00211 else if ( strcmp(word, "POLES") == 0)
00212 {
00213 if (sscanf(line, "%*s %d", &pRS->iNumPoles) == 0 || pRS->iNumPoles < 0)
00214 {
00215
00216 retval = -2;
00217 goto abort;
00218 }
00219 if (pRS->iNumPoles > 0)
00220 {
00221 if ( (pRS->Poles = (PZNum *)malloc(pRS->iNumPoles * sizeof(PZNum)))
00222 == (PZNum *)0 )
00223 {
00224 retval = -1;
00225 goto abort;
00226 }
00227 for (i = 0; i < pRS->iNumPoles; i++)
00228 {
00229 pRS->Poles[i].dReal = 0.0;
00230 pRS->Poles[i].dImag = 0.0;
00231 }
00232
00233 state = Pole;
00234 continue;
00235 }
00236 status |= 4;
00237 }
00238 else
00239 {
00240
00241 retval = -2;
00242 goto abort;
00243 }
00244 break;
00245 case Zero:
00246
00247 if (nz >= pRS->iNumZeros)
00248 {
00249
00250 retval = -2;
00251 goto abort;
00252 }
00253 if (sscanf(line, "%lf %lf",
00254 &pRS->Zeros[nz].dReal, &pRS->Zeros[nz].dImag) != 2)
00255 {
00256
00257 retval = -2;
00258 goto abort;
00259 }
00260 if (++nz == pRS->iNumZeros)
00261 {
00262
00263 status |= 2;
00264 state = Key;
00265 continue;
00266 }
00267 break;
00268 case Pole:
00269
00270 if (np >= pRS->iNumPoles)
00271 {
00272
00273 retval = -2;
00274 goto abort;
00275 }
00276 if (sscanf(line, "%lf %lf",
00277 &pRS->Poles[np].dReal, &pRS->Poles[np].dImag) != 2)
00278 {
00279
00280 retval = -2;
00281 goto abort;
00282 }
00283 if (++np == pRS->iNumPoles)
00284 {
00285
00286 status |= 4;
00287 state = Key;
00288 continue;
00289 }
00290 break;
00291 }
00292 }
00293
00294 if (status != 7)
00295 {
00296
00297 retval = -2;
00298 }
00299
00300 abort:
00301 if (retval != 0)
00302 {
00303
00304 cleanPZ(pRS);
00305 }
00306 fclose(pzFILE);
00307
00308 return retval;
00309 }
00310
00311
00312
00313
00314
00315
00316
00317
00318
00319
00320
00321 double ftaper(double freq, double fon, double foff)
00322 {
00323 double t, pi = PI;
00324
00325 if (fon > foff)
00326 {
00327 if (freq < foff)
00328 t = 0.0;
00329 else if (freq > fon)
00330 t = 1.0;
00331 else
00332 t = 0.5 * (1.0 - cos(pi * (freq - foff) / (fon - foff)));
00333 }
00334 else if (fon < foff)
00335 {
00336 if (freq < fon)
00337 t = 1.0;
00338 else if (freq > foff)
00339 t = 0.0;
00340 else
00341 t = 0.5 * (1.0 + cos(pi * (freq - fon) / (foff - fon)));
00342 }
00343 else
00344 t = 1.0;
00345
00346 return t;
00347 }
00348
00349
00350
00351
00352
00353
00354
00355
00356
00357
00358
00359
00360
00361
00362
00363
00364
00365
00366
00367
00368
00369
00370
00371
00372
00373
00374
00375
00376
00377
00378
00379
00380
00381
00382
00383
00384
00385
00386
00387
00388
00389
00390
00391
00392
00393
00394
00395
00396 int convertWave(double input[], long npts, double deltat,
00397 ResponseStruct *origRS, ResponseStruct *finalRS,
00398 double freq[4], int retFD, long *pPadLen, long *pnfft,
00399 double output[], long outBufLen, double *work, double *workFFT)
00400 {
00401 ResponseStruct rs;
00402 FACT *pfact;
00403 long i, ii, retval = 0;
00404 int nz = 0, np = 0;
00405 long nfft, nfreq, trial_nfft;
00406 double delfreq, tpr, dre, dim, f, delomg, omega;
00407 double *fre, *fim;
00408
00409
00410 if (origRS == (ResponseStruct *)NULL || finalRS == (ResponseStruct *)NULL ||
00411 npts < 2 || deltat <= 0.0 || freq == (double *)NULL || outBufLen < npts)
00412 {
00413 return -3;
00414 }
00415 if (freq[0] > freq[1] || freq[1] >= freq[2] || freq[2] > freq[3] ||
00416 origRS->dGain == 0.0 || finalRS->dGain == 0.0)
00417 {
00418 return -3;
00419 }
00420
00421
00422
00423 rs.dGain = finalRS->dGain / origRS->dGain;
00424 rs.iNumPoles = finalRS->iNumPoles + origRS->iNumZeros;
00425 rs.iNumZeros = finalRS->iNumZeros + origRS->iNumPoles;
00426 if ( (rs.Poles = (PZNum *)malloc(sizeof(PZNum) * rs.iNumPoles)) ==
00427 (PZNum *)NULL ||
00428 (rs.Zeros = (PZNum *)malloc(sizeof(PZNum) *rs.iNumZeros)) ==
00429 (PZNum *)NULL)
00430 {
00431 retval = -1;
00432 goto exit;
00433 }
00434
00435 for (i = 0; i < origRS->iNumPoles; i++)
00436 rs.Zeros[nz++] = origRS->Poles[i];
00437 for (i = 0; i < origRS->iNumZeros; i++)
00438 rs.Poles[np++] = origRS->Zeros[i];
00439 for (i = 0; i < finalRS->iNumPoles; i++)
00440 rs.Poles[np++] = finalRS->Poles[i];
00441 for (i = 0; i < finalRS->iNumZeros; i++)
00442 rs.Zeros[nz++] = finalRS->Zeros[i];
00443
00444 if (Debug & TR_DBG_PZG)
00445 {
00446 printf("Input response function: gain %10.3e\n", origRS->dGain);
00447 printf("Poles: %d\n", origRS->iNumPoles);
00448 for (i = 0; i < origRS->iNumPoles; i++)
00449 printf("%10.3e %10.3e\n", origRS->Poles[i].dReal,
00450 origRS->Poles[i].dImag);
00451 printf("\nZeros: %d\n", origRS->iNumZeros);
00452 for (i = 0; i < origRS->iNumZeros; i++)
00453 printf("%10.3e %10.3e\n", origRS->Zeros[i].dReal,
00454 origRS->Zeros[i].dImag);
00455 printf("\nOutput response function: gain %10.3e\n", finalRS->dGain);
00456 printf("Poles: %d\n", finalRS->iNumPoles);
00457 for (i = 0; i < finalRS->iNumPoles; i++)
00458 printf("%10.3e %10.3e\n", finalRS->Poles[i].dReal,
00459 finalRS->Poles[i].dImag);
00460 printf("\nZeros: %d\n", finalRS->iNumZeros);
00461 for (i = 0; i < finalRS->iNumZeros; i++)
00462 printf("%10.3e %10.3e\n", finalRS->Zeros[i].dReal,
00463 finalRS->Zeros[i].dImag);
00464 }
00465
00466
00467 if ( *pPadLen < 0)
00468 {
00469 if ( (*pPadLen = respLen( &rs, deltat, freq)) < 0)
00470 {
00471
00472 retval = *pPadLen;
00473 if (Debug)
00474 printf("\nrespLen error: %ld\n", *pPadLen);
00475 goto exit;
00476 }
00477 if (Debug)
00478 printf("estimated pad length: %ld\n", *pPadLen);
00479 }
00480
00481
00482
00483 trial_nfft = *pPadLen + npts;
00484 while ( (nfft = prepFFT(trial_nfft, &pfact)) > outBufLen)
00485 {
00486 if (nfft < 0)
00487 {
00488
00489 retval = nfft;
00490 goto exit;
00491 }
00492 trial_nfft -= 100;
00493 }
00494 if (nfft - *pPadLen < npts)
00495 npts = nfft - *pPadLen;
00496
00497
00498
00499 nfreq = nfft / 2 + 1;
00500 fre = work;
00501 fim = work + 1 + outBufLen/2;
00502
00503 for (i = 0; i < npts; i++)
00504 output[i] = input[i];
00505
00506
00507
00508 for (i = outBufLen + FFT_EXTRA; i < nfft; i++)
00509 output[i] = 0.0;
00510
00511
00512 fft991(output, workFFT, pfact->trigs, pfact->ifax, 1, nfft, nfft, 1, -1);
00513
00514 response(nfft, deltat, &rs, fre, fim);
00515
00516
00517
00518
00519
00520 delfreq = 1.0 / (nfft * deltat);
00521 output[0] = 0.0;
00522 for (i = 1; i < nfreq - 1; i++)
00523 {
00524 ii = i+i;
00525 f = i * delfreq;
00526 tpr = ftaper(f, freq[1], freq[0]) * ftaper(f, freq[2], freq[3]);
00527 dre = output[ii];
00528 dim = output[ii+1];
00529 output[ii] = (dre * fre[i] - dim * fim[i]) * tpr;
00530 output[ii+1] = (dre * fim[i] + dim * fre[i]) * tpr;
00531 }
00532 f = i * delfreq;
00533 tpr = ftaper(f, freq[1], freq[0]) * ftaper(f, freq[2], freq[3]);
00534 dre = output[nfft];
00535 output[nfft] = dre * fre[i] * tpr;
00536
00537 if (Debug & TR_DBG_ARS)
00538 {
00539 delomg = 2.0 * PI / (nfft * deltat);
00540 printf(" omega tapered\n");
00541 printf("%4ld %10.3e %10.3e %10.3e\n", 0L, 0.0, 0.0, 0.0);
00542 for (i = 1; i < nfreq - 1; i++ )
00543 {
00544 omega = i * delomg;
00545 printf("%4ld %10.3e %10.3e %10.3e\n", i, omega, fre[i], fim[i]);
00546 }
00547 }
00548
00549
00550 if (!retFD)
00551 fft991(output, workFFT, pfact->trigs, pfact->ifax, 1, nfft, nfft, 1, +1);
00552
00553
00554 *pnfft = nfft;
00555
00556 exit:
00557
00558 cleanPZ( &rs );
00559 return retval;
00560 }
00561
00562
00563
00564 #define NFFT_TEST 1024
00565
00566
00567
00568
00569
00570 #define ALIAS_CUTOFF 0.01
00571
00572
00573
00574
00575
00576
00577
00578
00579
00580
00581
00582
00583
00584
00585
00586
00587
00588
00589
00590
00591 int respLen( ResponseStruct *rs, double deltat, double freq[4])
00592 {
00593 FACT *pfact;
00594 double fre[NFFT_TEST/2+1], fim[NFFT_TEST/2+1], data[NFFT_TEST + 2];
00595 double work[NFFT_TEST+1];
00596 long i, ii, nf_test, nfreq;
00597 double imp_max, thresh, delfreq, f, tpr, delomg, omega;
00598 long imax, left_lim, right_lim;
00599
00600
00601
00602
00603
00604
00605
00606 if ( (nf_test = prepFFT( NFFT_TEST, &pfact)) < 0)
00607 {
00608
00609 return -1;
00610 }
00611 else if (nf_test != NFFT_TEST)
00612 {
00613 fprintf(stderr, "respLen fatal error: NFFT_TEST (%d) not factorable by 2, 3, 5\n",
00614 NFFT_TEST);
00615 return -1;
00616 }
00617 nfreq = nf_test/2+1;
00618
00619 response(nf_test, deltat, rs, fre, fim);
00620
00621 delfreq = 1.0 / (nf_test * deltat);
00622
00623 if (Debug & TR_DBG_TRS)
00624 {
00625 delomg = 2.0 * PI / (nf_test * deltat);
00626 printf("\nTest response function in Frequency Domain\n");
00627 printf(" i omega raw tapered\n");
00628 f = 0.0;
00629 tpr = ftaper(f, freq[1], freq[0]);
00630 data[0] = fre[0] * tpr;
00631 data[1] = 0.0;
00632 omega = 0.001 * delomg;
00633 printf("%4ld %10.3e %10.3e %10.3e %10.3e %10.3e\n", 0L, omega, fre[0],
00634 0.0, data[0], 0.0);
00635 for (i = 1; i < nfreq - 1; i++ )
00636 {
00637 ii = i+i;
00638 f = i * delfreq;
00639 tpr = ftaper(f, freq[1], freq[0]) * ftaper(f, freq[2], freq[3]);
00640 data[ii] = fre[i] * tpr;
00641 data[ii+1] = fim[i] * tpr;
00642 omega = i * delomg;
00643 printf("%4ld %10.3e %10.3e %10.3e %10.3e %10.3e\n", i, omega,
00644 fre[i], fim[i], data[ii], data[ii+1]);
00645 }
00646 data[nf_test] = 0.0;
00647 data[nf_test+1] = 0.0;
00648 }
00649 else
00650 {
00651 f = 0.0;
00652 tpr = ftaper(f, freq[1], freq[0]);
00653 data[0] = fre[0] * tpr;
00654 data[1] = 0.0;
00655 for (i = 1; i < nfreq - 1; i++ )
00656 {
00657 ii = i+i;
00658 f = i * delfreq;
00659 tpr = ftaper(f, freq[1], freq[0]) * ftaper(f, freq[2], freq[3]);
00660 data[ii] = fre[i] * tpr;
00661 data[ii+1] = fim[i] * tpr;
00662 }
00663 data[nf_test] = 0.0;
00664 data[nf_test+1] = 0.0;
00665 }
00666
00667
00668 fft991(data, work, pfact->trigs, pfact->ifax, 1, nf_test, nf_test, 1, +1);
00669
00670
00671 if (Debug & TR_DBG_TRS)
00672 {
00673 printf("\nTest response function in TD\n");
00674 for (i = 0; i < nf_test; i++)
00675 {
00676 printf("%5ld %10.3e\n", i, data[i]);
00677 }
00678 }
00679
00680
00681
00682
00683
00684
00685
00686
00687
00688 imp_max = 0.0;
00689 for (i = 0; i < nf_test; i++)
00690 {
00691 if (fabs(data[i]) > imp_max)
00692 {
00693 imp_max = fabs(data[i]);
00694 imax = i;
00695 }
00696 }
00697
00698 if (imp_max > 0.001 * rs->dGain)
00699 {
00700 thresh = imp_max * ALIAS_CUTOFF;
00701
00702
00703
00704
00705
00706 right_lim = left_lim = imax;
00707 for (i = imax; i >= 0; i--)
00708 {
00709 if ( fabs(data[i]) < thresh)
00710 {
00711 left_lim = i;
00712 break;
00713 }
00714 }
00715 if (left_lim == imax)
00716 {
00717 for (i = nf_test - 1; i > imax; i--)
00718 {
00719 if ( fabs(data[i]) < thresh)
00720 {
00721 left_lim = i;
00722 break;
00723 }
00724 }
00725 if (left_lim == imax)
00726 {
00727 return NFFT_TEST;
00728 }
00729 }
00730
00731 for (i = imax; i < nf_test; i++)
00732 {
00733 if ( fabs(data[i]) < thresh)
00734 {
00735 right_lim = i;
00736 break;
00737 }
00738 }
00739 if (right_lim == imax)
00740 {
00741 for (i = 0; i < imax; i++)
00742 {
00743 if ( fabs(data[i]) < thresh)
00744 {
00745 right_lim = i;
00746 break;
00747 }
00748 }
00749 if (right_lim == imax)
00750 {
00751 return -NFFT_TEST;
00752 }
00753 }
00754 if (left_lim < right_lim)
00755 return (right_lim - left_lim + 1);
00756 else
00757 return (right_lim - left_lim + nf_test + 1);
00758 }
00759
00760 return -2;
00761 }
00762
00763
00764
00765
00766
00767
00768
00769
00770 void pzCancel(ResponseStruct *rs, double tol)
00771 {
00772 int ip, iz, mz;
00773
00774 if (rs->iNumZeros == 0 || rs->iNumPoles == 0)
00775 return;
00776
00777 for (ip = 0; ip < rs->iNumPoles; ip++)
00778 {
00779 mz = -1;
00780 for (iz = 0; iz < rs->iNumZeros; iz++)
00781 {
00782 if (fabs(rs->Poles[ip].dReal - rs->Zeros[iz].dReal) < tol &&
00783 fabs(rs->Poles[ip].dImag - rs->Zeros[iz].dImag) < tol)
00784 {
00785 mz = iz;
00786 break;
00787 }
00788 }
00789 if (mz != -1)
00790 {
00791
00792
00793 drop(rs->Poles, &rs->iNumPoles, ip);
00794 drop(rs->Zeros, &rs->iNumZeros, mz);
00795 ip--;
00796 iz--;
00797 }
00798 }
00799 return;
00800 }
00801
00802
00803
00804
00805
00806 static void drop(PZNum *pPZ, int *pNumPZ, int ipz)
00807 {
00808 int i;
00809
00810 (*pNumPZ)--;
00811 for (i = ipz; i < *pNumPZ; i++)
00812 {
00813 pPZ[i] = pPZ[i+1];
00814 }
00815 return;
00816 }
00817
00818
00819
00820
00821
00822
00823
00824
00825
00826
00827
00828
00829
00830
00831
00832 void taper(double *data, long npts, long tLen)
00833 {
00834 long i, jb, je;
00835 double tap, omega;
00836
00837 tLen++;
00838 if (tLen < 2 || tLen > npts / 2)
00839 return;
00840
00841 omega = PI / tLen;
00842
00843 for (i = 1; i < tLen; i++)
00844 {
00845 jb = i - 1;
00846 je = npts - i;
00847 tap = 0.5 * (1.0 - cos(omega * i));
00848 data[jb] *= tap;
00849 data[je] *= tap;
00850 }
00851 return;
00852 }
00853
00854
00855 void deMean( double *data, long npts, double *mean)
00856 {
00857 long i;
00858 double sum = 0.0;
00859
00860 if (npts < 1)
00861 {
00862 *mean = 0.0;
00863 return;
00864 }
00865
00866 for (i = 0; i < npts; i++)
00867 sum += data[i];
00868
00869 sum /= (double)npts;
00870 for (i = 0; i < npts; i++)
00871 data[i] -= sum;
00872
00873 *mean = sum;
00874 return;
00875 }
00876
00877
00878 void cleanPZ( ResponseStruct *pRS)
00879 {
00880 if (pRS->Zeros != (PZNum *)NULL)
00881 {
00882 free( pRS->Zeros );
00883 pRS->Zeros = (PZNum *)NULL;
00884 }
00885 if (pRS->Poles != (PZNum *)NULL)
00886 {
00887 free( pRS->Poles );
00888 pRS->Poles = (PZNum *)NULL;
00889 }
00890 pRS->iNumPoles = 0;
00891 pRS->iNumZeros = 0;
00892 return;
00893 }
00894
00895 void transferDebug( int level )
00896 {
00897 Debug = level;
00898 return;
00899 }