nr_utilities.c

来自「LastWave」· C语言 代码 · 共 262 行

#include "lastwave.h"
#include "signals.h"

#define ITMAX 100
#define EPS 3.0e-7
#define EPS2 1.0e-8
#define FPMIN 1.0e-30

LWFLOAT gammln(LWFLOAT xx) 
{
  double x,y,tmp,ser; 
  static double cof[6]={76.18009172947146,-86.50532032941677, 
    24.01409824083091,-1.231739572450155, 
    0.1208650973866179e-2,-0.5395239384953e-5}; 
  int j;
  y=x=xx; 
  tmp=x+5.5; 
  tmp -= (x+0.5)*log(tmp); 
  ser=1.000000000190015; 
  for (j=0;j<=5;j++) ser += cof[j]/++y; 
  return -tmp+log(2.5066282746310005*ser/x); 
}

void gcf(LWFLOAT *gammcf, LWFLOAT a, LWFLOAT x, LWFLOAT *gln) 
{ 
  int i; 
  LWFLOAT an,b,c,d,del,h; 
  *gln=gammln(a); 
  b=x+1.0-a; 
  c=1.0/FPMIN; 
  d=1.0/b; 
  h=d; 
  for (i=1;i<=ITMAX;i++) {
    an = -i*(i-a); 
    b += 2.0; 
    d=an*d+b; 
    if (fabs(d) < FPMIN) d=FPMIN; 
    c=b+an/c; 
    if (fabs(c) < FPMIN) c=FPMIN; 
    d=1.0/d; del=d*c; h *= del; 
    if (fabs(del-1.0) < EPS2) break; 
  } 
  if (i > ITMAX) Errorf("a too large, ITMAX too small in gcf"); 
  *gammcf=exp(-x+a*log(x)-(*gln))*h;
}

void gser(LWFLOAT *gamser, LWFLOAT a, LWFLOAT x, LWFLOAT *gln) 
{ 
  int n; 
  LWFLOAT sum,del,ap; 
  *gln=gammln(a); 
  if (x <= 0.0) { 
    if (x < 0.0) Errorf("x less than 0 in routine gser"); 
    *gamser=0.0; 
    return; 
  } else { 
    ap=a; 
    del=sum=1.0/a; 
    for (n=1;n<=ITMAX;n++) { 
      ++ap; 
      del *= x/ap; 
      sum += del; 
      if (fabs(del) < fabs(sum)*EPS2) { 
        *gamser=sum*exp(-x+a*log(x)-(*gln)); 
        return; 
      } 
    } 
    Errorf("a too large, ITMAX too small in routine gser"); 
    return; 
  } 
}

LWFLOAT gammp(LWFLOAT a, LWFLOAT x) 
{ 
  LWFLOAT gamser,gammcf,gln; 
  if (x < 0.0 || a <= 0.0) Errorf("Invalid arguments in routine gammp"); 
  if (x < (a+1.0)) { 
    gser(&gamser,a,x,&gln); 
    return gamser; 
  } else { 
    gcf(&gammcf,a,x,&gln); 
    return 1.0-gammcf; 
  } 
}

LWFLOAT chi2(LWFLOAT x2, LWFLOAT n)
{
  if (x2>=0) {
    return gammp(n/2,x2/2);
  } else {
    return 0;
  }
}

//
// 
// Numerical recipee routine for solving Toeplitz system :  r * filter = corr
// 
//     r      : toeplitz matrix coded as a vector of size 2N-1
//     filter : unknown filter of size N
//     corr   : the right handside which is a vector of size N
//

#define FREERETURN {DeleteSignal(h);DeleteSignal(g);return;}

void Toepliz(SIGNAL r, SIGNAL filter, SIGNAL corr)
{ 
  int n = corr->size;
  int j,k,m,m1,m2;
  LWFLOAT pp,pt1,pt2,qq,qt1,qt2,sd,sgd,sgn,shn,sxn;
  SIGNAL g,h;
  
  SizeSignal(filter,n,YSIG);
  if (r->Y[n-1] == 0.0) Errorf("Toepliz(): toeplz-1 singular ppl minor");
  g=NewSignal();
  SizeSignal(g,n,YSIG);
  h=NewSignal();
  SizeSignal(h,n,YSIG);
  filter->Y[0]=corr->Y[0]/r->Y[n-1];
  if (n == 1) FREERETURN;
  g->Y[0]=r->Y[n-2]/r->Y[n-1];
  h->Y[0]=r->Y[n]/r->Y[n-1];
  for (m=1;m<=n;m++) {
    m1=m+1;
    sxn = -corr->Y[m1-1];
    sd =  -r->Y[n-1];
    for (j=1;j<=m;j++) {
	    sxn += r->Y[n+m1-j-1]*filter->Y[j-1];
	    sd += r->Y[n+m1-j-1]*g->Y[m-j];
    }
    if (sd == 0.0) Errorf("Toepliz(): toeplz-2 singular ppl minor");
    filter->Y[m1-1]=sxn/sd;
    for (j=1;j<=m;j++)
	    filter->Y[j-1] -= filter->Y[m1-1]*g->Y[m-j];
    if (m1 == n) FREERETURN;
    sgn = -r->Y[n-m1-1];
    shn = -r->Y[n+m1-1];
    sgd = -r->Y[n-1];
    for (j=1;j<=m;j++) {
	    sgn += r->Y[n+j-m1-1]*g->Y[j-1];
	    shn += r->Y[n+m1-j-1]*h->Y[j-1];
	    sgd += r->Y[n+j-m1-1]*h->Y[m-j];
    }
    if ((sd == 0.0) || (sgd == 0.0))
	    Errorf("Toepliz(): toeplz-3 singular ppl minor");
    g->Y[m1-1]=sgn/sgd;
    h->Y[m1-1]=shn/sd;
    k=m;
    m2=(m+1) >> 1;
    pp=g->Y[m1-1];
    qq=h->Y[m1-1];
    for (j=1;j<=m2;j++) {
	    pt1=g->Y[j-1];
	    pt2=g->Y[k-1];
	    qt1=h->Y[j-1];
	    qt2=h->Y[k-1];
	    g->Y[j-1]=pt1-pp*qt2;
	    g->Y[k-1]=pt2-pp*qt1;
	    h->Y[j-1]=qt1-qq*pt2;
	    h->Y[k-1]=qt2-qq*pt1;
      k--;
    }
  }
  Errorf("Toepliz(): Should not arrive here!");
}


/***** Root of a function using Newton's method *****/
#define JMAX 50
LWFLOAT rtnewtn(void (*funcd)(LWFLOAT, LWFLOAT, LWFLOAT, LWFLOAT, LWFLOAT *, LWFLOAT *), LWFLOAT p, LWFLOAT mu, LWFLOAT vv, LWFLOAT xacc)
{
  int j;
  LWFLOAT df,dx,f,rtn;
  LWFLOAT x1,x2;
  
  x1=-10;
  x2=10;
  rtn = 0.5*(x1+x2);
  for (j=1;j<=JMAX;j++) {
    (*funcd)(rtn,p,mu,vv,&f,&df);
    dx=f/df;
    rtn -= dx;
    if ((x1-rtn)*(rtn-x2) < 0.0){
      x2 = 0.5*(x1+x2);
      rtn = 0.5*(x1+x2);
    }
    if (fabs(dx) < xacc) return(rtn);
  }
  Errorf("rtnewtn(): Max. number of iterations exceeded");
  
  return(0);
} 

/*
 * Compute Gaussian Quadrature weights for computing integral
 */
#define PIM4 0.7511255444649425
#define MAXIT 10
#define EPS 3.0e-14


SIGNAL wwGauther = NULL;
SIGNAL xxGauther = NULL;

void Gauther()
{
#define NITER 30
  
  int n = NITER;
  int i,its,j,m;
  double p1,p2,p3,pp,z,z1;
  
  if (wwGauther != NULL) return;
  
  wwGauther = NewSignal();
  SizeSignal(wwGauther,NITER,YSIG);
  
  xxGauther = NewSignal();
  SizeSignal(xxGauther,NITER,YSIG);
  
  m=(n+1)/2;
  for(i=1;i<=m;i++) {
    if (i == 1) {
	    z=sqrt((double)(2*n+1))-1.85575*pow((double)(2*n+1),-0.16667);
    }
    else if (i==2) {
	    z -= 1.14*pow((double)n,0.426)/z;
    }
    else if (i==3) {
	    z=1.86*z-0.86*xxGauther->Y[0];
    }
    else if (i==4){
	    z=1.91*z-0.91*xxGauther->Y[1];
    }
    else {
	    z=2.0*z-xxGauther->Y[i-3];
    }
    for(its=1;its<=MAXIT;its++) {
	    p1=PIM4;
	    p2=0.0;
	    for(j=1;j<=n;j++){
        p3=p2;
        p2=p1;
        p1=z*sqrt(2.0/j)*p2-sqrt(((double)(j-1))/j)*p3;
	    }
	    pp=sqrt((double)2*n)*p2;
	    z1=z;
	    z=z1-p1/pp;
	    if(fabs(z-z1) <= EPS) break;
    }
    if (its > MAXIT) Errorf("Gauther: too many iterations");
    xxGauther->Y[i-1]=z;
    xxGauther->Y[n-i]= -z;
    wwGauther->Y[i-1]=2.0/(pp*pp);
    wwGauther->Y[n-i]=wwGauther->Y[i-1];
  }
}