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 #include <lal/LALStdlib.h>
00121 #include <lal/AVFactories.h>
00122 #include <lal/VectorOps.h>
00123 #include <lal/Sort.h>
00124 #include <math.h>
00125 #include <lal/ZPGFilter.h>
00126
00127 NRCSID(BILINEARTRANSFORMC,"$Id: BilinearTransform.c,v 1.13 2007/06/08 14:41:56 bema Exp $");
00128
00129
00130
00131
00132
00133
00134 static int CompareCOMPLEX8Abs( void *p, const void *a, const void *b )
00135 {
00136 REAL8 ar = ((const COMPLEX8 *)a)->re;
00137 REAL8 ai = ((const COMPLEX8 *)a)->im;
00138 REAL8 br = ((const COMPLEX8 *)b)->re;
00139 REAL8 bi = ((const COMPLEX8 *)b)->im;
00140 p = NULL;
00141 if ( (ar*ar+ai*ai) < (br*br+bi*bi) )
00142 return -1;
00143 return 1;
00144 }
00145
00146
00147
00148
00149
00150
00151 static int CompareCOMPLEX16Abs( void *p, const void *a, const void *b )
00152 {
00153 REAL8 ar = ((const COMPLEX16 *)a)->re;
00154 REAL8 ai = ((const COMPLEX16 *)a)->im;
00155 REAL8 br = ((const COMPLEX16 *)b)->re;
00156 REAL8 bi = ((const COMPLEX16 *)b)->im;
00157 p = NULL;
00158 if ( (ar*ar+ai*ai) < (br*br+bi*bi) )
00159 return -1;
00160 return 1;
00161 }
00162
00163 int XLALWToZCOMPLEX8ZPGFilter( COMPLEX8ZPGFilter *filter )
00164 {
00165 static const char *func = "XLALWToZCOMPLEX8ZPGFilter";
00166 INT4 i;
00167 INT4 j;
00168 INT4 num;
00169 INT4 numZeros;
00170 INT4 numPoles;
00171 COMPLEX8 *a;
00172 COMPLEX8 *b;
00173 COMPLEX8 *g;
00174 COMPLEX8Vector *z=NULL;
00175 COMPLEX8Vector *gain=NULL;
00176 COMPLEX8Vector null;
00177 INT4Vector *idx=NULL;
00178
00179
00180 if ( ! filter )
00181 XLAL_ERROR( func, XLAL_EFAULT );
00182
00183
00184
00185
00186 null.length=0;
00187 null.data=NULL;
00188 if(!filter->zeros)
00189 filter->zeros=&null;
00190 if(!filter->poles)
00191 filter->poles=&null;
00192
00193
00194
00195 numZeros=filter->zeros->length;
00196 if (numZeros<0)
00197 XLAL_ERROR(func,XLAL_EINVAL);
00198 if(numZeros>0)
00199 if (!filter->zeros->data)
00200 XLAL_ERROR(func,XLAL_EFAULT);
00201 numPoles=filter->poles->length;
00202 if (numPoles<0)
00203 XLAL_ERROR(func,XLAL_EINVAL);
00204 if(numPoles>0)
00205 if (!filter->poles->data)
00206 XLAL_ERROR(func,XLAL_EFAULT);
00207
00208
00209
00210 num = (numZeros>numPoles) ? numZeros : numPoles;
00211 numZeros=numPoles=num;
00212
00213
00214
00215
00216 if(num<=0){
00217 filter->zeros=NULL;
00218 filter->poles=NULL;
00219 return 0;
00220 }
00221
00222
00223
00224 for(i=0,a=filter->zeros->data;i<(INT4)filter->zeros->length;i++,a++)
00225 if((a->re==0.0)&&(a->im==-1.0))
00226 numZeros--;
00227 for(i=0,a=filter->poles->data;i<(INT4)filter->poles->length;i++,a++)
00228 if((a->re==0.0)&&(a->im==-1.0))
00229 numPoles--;
00230
00231
00232
00233 gain=XLALCreateCOMPLEX8Vector(filter->zeros->length+filter->poles->length);
00234 z=XLALCreateCOMPLEX8Vector(numZeros);
00235 if (!gain||!z)
00236 {
00237 XLALDestroyCOMPLEX8Vector(gain);
00238 XLALDestroyCOMPLEX8Vector(z);
00239 XLAL_ERROR(func,XLAL_EFUNC);
00240 }
00241 g=gain->data;
00242 b=z->data;
00243
00244
00245
00246
00247 for(i=0,j=0,a=filter->zeros->data;i<(INT4)filter->zeros->length;
00248 i++,a++,g++){
00249 REAL4 ar=a->re;
00250 REAL4 ai=a->im;
00251 if(ar==0.0){
00252 if(ai==-1.0){
00253
00254 g->re=0.0;
00255 g->im=2.0;
00256 }else{
00257
00258 b->re=(1.0-ai)/(1.0+ai);
00259 b->im=0.0;
00260 g->re=0.0;
00261 g->im=-(1.0+ai);
00262 b++;
00263 j++;
00264 }
00265 }else if(fabs(1.0+ai)>fabs(ar)){
00266 REAL8 ratio = -ar/(1.0+ai);
00267 REAL8 denom = 1.0+ai - ratio*ar;
00268
00269 b->re = (1.0-ai + ratio*ar)/denom;
00270 b->im = (ar - ratio*(1.0-ai))/denom;
00271 g->re = -ar;
00272 g->im = -(1.0+ai);
00273 b++;
00274 j++;
00275 }else{
00276 REAL8 ratio = -(1.0+ai)/ar;
00277 REAL8 denom = -ar + ratio*(1.0+ai);
00278
00279 b->re = ((1.0-ai)*ratio + ar)/denom;
00280 b->im = (ar*ratio - 1.0+ai)/denom;
00281 g->re = -ar;
00282 g->im = -(1.0+ai);
00283 b++;
00284 j++;
00285 }
00286 }
00287
00288 for(;j<numZeros;b++,j++){
00289 b->re = -1.0;
00290 b->im = 0.0;
00291 }
00292
00293 if(filter->zeros->length>0)
00294 XLALDestroyCOMPLEX8Vector(filter->zeros);
00295 filter->zeros=z;
00296 z=NULL;
00297
00298
00299 z=XLALCreateCOMPLEX8Vector(numPoles);
00300 if (!gain||!z)
00301 {
00302 XLALDestroyCOMPLEX8Vector(gain);
00303 XLAL_ERROR(func,XLAL_EFUNC);
00304 }
00305 b=z->data;
00306
00307
00308
00309 for(i=0,j=0,a=filter->poles->data;i<(INT4)filter->poles->length;
00310 i++,a++,g++){
00311 REAL4 ar=a->re;
00312 REAL4 ai=a->im;
00313 if(ar==0.0){
00314 if(ai==-1.0){
00315
00316 g->re=0.0;
00317 g->im=-0.5;
00318 }else{
00319
00320 b->re=(1.0-ai)/(1.0+ai);
00321 b->im=0.0;
00322 g->re=0.0;
00323 g->im=1.0/(1.0+ai);
00324 b++;
00325 j++;
00326 }
00327 }else if(fabs(1.0+ai)>fabs(ar)){
00328 REAL8 ratio = -ar/(1.0+ai);
00329 REAL8 denom = 1.0+ai - ratio*ar;
00330
00331 b->re = (1.0-ai + ratio*ar)/denom;
00332 b->im = (ar - ratio*(1.0-ai))/denom;
00333 g->re = ratio/denom;
00334 g->im = 1.0/denom;
00335 b++;
00336 j++;
00337 }else{
00338 REAL8 ratio = -(1.0+ai)/ar;
00339 REAL8 denom = -ar + ratio*(1.0+ai);
00340
00341 b->re = ((1.0-ai)*ratio + ar)/denom;
00342 b->im = (ar*ratio - 1.0+ai)/denom;
00343 g->re = ratio/denom;
00344 g->im = 1.0/denom;
00345 b++;
00346 j++;
00347 }
00348 }
00349
00350 for(;j<numPoles;b++,j++){
00351 b->re = -1.0;
00352 b->im = 0.0;
00353 }
00354
00355 if(filter->poles->length>0)
00356 XLALDestroyCOMPLEX8Vector(filter->poles);
00357 filter->poles=z;
00358 z=NULL;
00359
00360
00361
00362
00363
00364 idx=XLALCreateINT4Vector(gain->length);
00365 if(!idx||XLALHeapIndex(idx->data,gain->data,gain->length,sizeof(*gain->data),NULL,CompareCOMPLEX8Abs)<0)
00366 {
00367 XLALDestroyCOMPLEX8Vector(gain);
00368 XLALDestroyINT4Vector(idx);
00369 XLAL_ERROR(func,XLAL_EFUNC);
00370 }
00371
00372
00373
00374 for(i=0,j=gain->length-1;i<j;i++,j--){
00375
00376 REAL4 ar=gain->data[idx->data[i]].re;
00377 REAL4 ai=gain->data[idx->data[i]].im;
00378 REAL4 br=gain->data[idx->data[j]].re;
00379 REAL4 bi=gain->data[idx->data[j]].im;
00380 REAL4 cr=ar*br-ai*bi;
00381 REAL4 ci=ar*bi+ai*br;
00382
00383
00384 br=filter->gain.re;
00385 bi=filter->gain.im;
00386 filter->gain.re=br*cr-bi*ci;
00387 filter->gain.im=br*ci+bi*cr;
00388 }
00389 if(i==j){
00390
00391 REAL4 cr=gain->data[idx->data[i]].re;
00392 REAL4 ci=gain->data[idx->data[i]].im;
00393 REAL4 br=filter->gain.re;
00394 REAL4 bi=filter->gain.im;
00395
00396 filter->gain.re=br*cr-bi*ci;
00397 filter->gain.im=br*ci+bi*cr;
00398 }
00399
00400
00401 XLALDestroyCOMPLEX8Vector(gain);
00402 XLALDestroyINT4Vector(idx);
00403 return 0;
00404 }
00405
00406
00407 int XLALWToZCOMPLEX16ZPGFilter( COMPLEX16ZPGFilter *filter )
00408 {
00409 static const char *func = "XLALWToZCOMPLEX16ZPGFilter";
00410 INT4 i;
00411 INT4 j;
00412 INT4 num;
00413 INT4 numZeros;
00414 INT4 numPoles;
00415 COMPLEX16 *a;
00416 COMPLEX16 *b;
00417 COMPLEX16 *g;
00418 COMPLEX16Vector *z=NULL;
00419 COMPLEX16Vector *gain=NULL;
00420 COMPLEX16Vector null;
00421 INT4Vector *idx=NULL;
00422
00423
00424 if ( ! filter )
00425 XLAL_ERROR( func, XLAL_EFAULT );
00426
00427
00428
00429
00430 null.length=0;
00431 null.data=NULL;
00432 if(!filter->zeros)
00433 filter->zeros=&null;
00434 if(!filter->poles)
00435 filter->poles=&null;
00436
00437
00438
00439 numZeros=filter->zeros->length;
00440 if (numZeros<0)
00441 XLAL_ERROR(func,XLAL_EINVAL);
00442 if(numZeros>0)
00443 if (!filter->zeros->data)
00444 XLAL_ERROR(func,XLAL_EFAULT);
00445 numPoles=filter->poles->length;
00446 if (numPoles<0)
00447 XLAL_ERROR(func,XLAL_EINVAL);
00448 if(numPoles>0)
00449 if (!filter->poles->data)
00450 XLAL_ERROR(func,XLAL_EFAULT);
00451
00452
00453
00454 num = (numZeros>numPoles) ? numZeros : numPoles;
00455 numZeros=numPoles=num;
00456
00457
00458
00459
00460 if(num<=0){
00461 filter->zeros=NULL;
00462 filter->poles=NULL;
00463 return 0;
00464 }
00465
00466
00467
00468 for(i=0,a=filter->zeros->data;i<(INT4)filter->zeros->length;i++,a++)
00469 if((a->re==0.0)&&(a->im==-1.0))
00470 numZeros--;
00471 for(i=0,a=filter->poles->data;i<(INT4)filter->poles->length;i++,a++)
00472 if((a->re==0.0)&&(a->im==-1.0))
00473 numPoles--;
00474
00475
00476
00477 gain=XLALCreateCOMPLEX16Vector(filter->zeros->length+filter->poles->length);
00478 z=XLALCreateCOMPLEX16Vector(numZeros);
00479 if (!gain||!z)
00480 {
00481 XLALDestroyCOMPLEX16Vector(gain);
00482 XLALDestroyCOMPLEX16Vector(z);
00483 XLAL_ERROR(func,XLAL_EFUNC);
00484 }
00485 g=gain->data;
00486 b=z->data;
00487
00488
00489
00490
00491 for(i=0,j=0,a=filter->zeros->data;i<(INT4)filter->zeros->length;
00492 i++,a++,g++){
00493 REAL8 ar=a->re;
00494 REAL8 ai=a->im;
00495 if(ar==0.0){
00496 if(ai==-1.0){
00497
00498 g->re=0.0;
00499 g->im=2.0;
00500 }else{
00501
00502 b->re=(1.0-ai)/(1.0+ai);
00503 b->im=0.0;
00504 g->re=0.0;
00505 g->im=-(1.0+ai);
00506 b++;
00507 j++;
00508 }
00509 }else if(fabs(1.0+ai)>fabs(ar)){
00510 REAL8 ratio = -ar/(1.0+ai);
00511 REAL8 denom = 1.0+ai - ratio*ar;
00512
00513 b->re = (1.0-ai + ratio*ar)/denom;
00514 b->im = (ar - ratio*(1.0-ai))/denom;
00515 g->re = -ar;
00516 g->im = -(1.0+ai);
00517 b++;
00518 j++;
00519 }else{
00520 REAL8 ratio = -(1.0+ai)/ar;
00521 REAL8 denom = -ar + ratio*(1.0+ai);
00522
00523 b->re = ((1.0-ai)*ratio + ar)/denom;
00524 b->im = (ar*ratio - 1.0+ai)/denom;
00525 g->re = -ar;
00526 g->im = -(1.0+ai);
00527 b++;
00528 j++;
00529 }
00530 }
00531
00532 for(;j<numZeros;b++,j++){
00533 b->re = -1.0;
00534 b->im = 0.0;
00535 }
00536
00537 if(filter->zeros->length>0)
00538 XLALDestroyCOMPLEX16Vector(filter->zeros);
00539 filter->zeros=z;
00540 z=NULL;
00541
00542
00543 z=XLALCreateCOMPLEX16Vector(numPoles);
00544 if (!gain||!z)
00545 {
00546 XLALDestroyCOMPLEX16Vector(gain);
00547 XLAL_ERROR(func,XLAL_EFUNC);
00548 }
00549 b=z->data;
00550
00551
00552
00553 for(i=0,j=0,a=filter->poles->data;i<(INT4)filter->poles->length;
00554 i++,a++,g++){
00555 REAL8 ar=a->re;
00556 REAL8 ai=a->im;
00557 if(ar==0.0){
00558 if(ai==-1.0){
00559
00560 g->re=0.0;
00561 g->im=-0.5;
00562 }else{
00563
00564 b->re=(1.0-ai)/(1.0+ai);
00565 b->im=0.0;
00566 g->re=0.0;
00567 g->im=1.0/(1.0+ai);
00568 b++;
00569 j++;
00570 }
00571 }else if(fabs(1.0+ai)>fabs(ar)){
00572 REAL8 ratio = -ar/(1.0+ai);
00573 REAL8 denom = 1.0+ai - ratio*ar;
00574
00575 b->re = (1.0-ai + ratio*ar)/denom;
00576 b->im = (ar - ratio*(1.0-ai))/denom;
00577 g->re = ratio/denom;
00578 g->im = 1.0/denom;
00579 b++;
00580 j++;
00581 }else{
00582 REAL8 ratio = -(1.0+ai)/ar;
00583 REAL8 denom = -ar + ratio*(1.0+ai);
00584
00585 b->re = ((1.0-ai)*ratio + ar)/denom;
00586 b->im = (ar*ratio - 1.0+ai)/denom;
00587 g->re = ratio/denom;
00588 g->im = 1.0/denom;
00589 b++;
00590 j++;
00591 }
00592 }
00593
00594 for(;j<numPoles;b++,j++){
00595 b->re = -1.0;
00596 b->im = 0.0;
00597 }
00598
00599 if(filter->poles->length>0)
00600 XLALDestroyCOMPLEX16Vector(filter->poles);
00601 filter->poles=z;
00602 z=NULL;
00603
00604
00605
00606
00607
00608 idx=XLALCreateINT4Vector(gain->length);
00609 if(!idx||XLALHeapIndex(idx->data,gain->data,gain->length,sizeof(*gain->data),NULL,CompareCOMPLEX16Abs)<0)
00610 {
00611 XLALDestroyCOMPLEX16Vector(gain);
00612 XLALDestroyINT4Vector(idx);
00613 XLAL_ERROR(func,XLAL_EFUNC);
00614 }
00615
00616
00617
00618 for(i=0,j=gain->length-1;i<j;i++,j--){
00619
00620 REAL8 ar=gain->data[idx->data[i]].re;
00621 REAL8 ai=gain->data[idx->data[i]].im;
00622 REAL8 br=gain->data[idx->data[j]].re;
00623 REAL8 bi=gain->data[idx->data[j]].im;
00624 REAL8 cr=ar*br-ai*bi;
00625 REAL8 ci=ar*bi+ai*br;
00626
00627
00628 br=filter->gain.re;
00629 bi=filter->gain.im;
00630 filter->gain.re=br*cr-bi*ci;
00631 filter->gain.im=br*ci+bi*cr;
00632 }
00633 if(i==j){
00634
00635 REAL8 cr=gain->data[idx->data[i]].re;
00636 REAL8 ci=gain->data[idx->data[i]].im;
00637 REAL8 br=filter->gain.re;
00638 REAL8 bi=filter->gain.im;
00639
00640 filter->gain.re=br*cr-bi*ci;
00641 filter->gain.im=br*ci+bi*cr;
00642 }
00643
00644
00645 XLALDestroyCOMPLEX16Vector(gain);
00646 XLALDestroyINT4Vector(idx);
00647 return 0;
00648 }
00649
00650
00651
00652 void
00653 LALWToZCOMPLEX8ZPGFilter( LALStatus *stat,
00654 COMPLEX8ZPGFilter *filter )
00655 {
00656 INITSTATUS(stat,"LALWToZCOMPLEX8ZPGFilter",BILINEARTRANSFORMC);
00657
00658 if(XLALWToZCOMPLEX8ZPGFilter(filter)<0)
00659 {
00660 int code=xlalErrno;
00661 XLALClearErrno();
00662 switch(code){
00663 case XLAL_EFAULT:
00664 ABORT(stat,ZPGFILTERH_ENUL,ZPGFILTERH_MSGENUL);
00665 case XLAL_EINVAL:
00666 ABORT(stat,ZPGFILTERH_EBAD,ZPGFILTERH_MSGEBAD);
00667 default:
00668 ABORTXLAL(stat);
00669 }
00670 }
00671
00672 RETURN(stat);
00673 }
00674
00675
00676
00677 void
00678 LALWToZCOMPLEX16ZPGFilter( LALStatus *stat,
00679 COMPLEX16ZPGFilter *filter )
00680 {
00681 INITSTATUS(stat,"LALWToZCOMPLEX16ZPGFilter",BILINEARTRANSFORMC);
00682
00683 if(XLALWToZCOMPLEX16ZPGFilter(filter)<0)
00684 {
00685 int code=xlalErrno;
00686 XLALClearErrno();
00687 switch(code){
00688 case XLAL_EFAULT:
00689 ABORT(stat,ZPGFILTERH_ENUL,ZPGFILTERH_MSGENUL);
00690 case XLAL_EINVAL:
00691 ABORT(stat,ZPGFILTERH_EBAD,ZPGFILTERH_MSGEBAD);
00692 default:
00693 ABORTXLAL(stat);
00694 }
00695 }
00696
00697 RETURN(stat);
00698 }