genburg.c

speech signal process tools

					}
				}
			}
		}
	if(i!=Ni) { fprintf(stderr,"Ptab not ok\n");exit(1); }

	for(i=0;i<Ni;i++)
		{
		make(Pi,i);
		for(j=i;j<Ni;j++)
			{
			make(Pj,j);
			isum=0;
			for(k=0;k<qp;k++)
				{
				for(l=0;l<qp;l++)
					{
					if(!Pi[k*qp+l])continue;
					if(!Pj[l*qp+k])continue;
					isum++;
					}
				}
			if(isum!=0 && i!=j) { fprintf(stderr,"pij!=0\n");exit(1); }
			if(i!=j)continue;
			if(isum==0) { fprintf(stderr,"Pii=0\n"); }
			Pii[i]=isum;
			}
		}
	return;
}
makesigma(R,iR,sigma,isigma,Pi)
register double R[],iR[],sigma[],isigma[];
register short Pi[];
{
	register int i,m,n,nM,mM;
	register double Rt,iRt;

	dmclear(sigma,qp);
	if(cflag)
		dmclear(isigma,qp);

	for(i=0;i<Ni;i++)
		{
		make(Pi,i);
		Rt=R[i];
		if(cflag)iRt=iR[i];
		for(m=0,mM=0;m<qp;m++,mM+=qp)
			{
			for(n=m;n<qp;n++)
				{
				if(!Pi[mM+n])continue;
				sigma[mM+n]+=Rt;
				if(cflag)
					isigma[mM+n]+=iRt;/* iPi one's in upper triangle */
				}
			}
		}
	for(m=0,mM=0;m<qp;m++,mM+=qp)
		{
		for(n=m+1,nM=mM+qp;n<qp;n++,nM+=qp)
			{
			sigma[nM+m]=sigma[mM+n];
			if(cflag)
				isigma[nM+m]= -isigma[mM+n];
			}
		}
	return;
}
gb_free()
{
	freedp(&R);
	freedp(&R0);
	freedp(&Rnew);
	freedp(&del);
	freedp(&siginv);
	freedp(&sigtemp);
	freedp(&sig1temp);
	freedp(&sig2temp);
	freesp(&Pii);
	freesp(&Pi);
	freesp(&Pj);
	freesp(&Ptaba);
	freesp(&Ptabb);
	freesp(&Ptabc);
	freedp(&Aij);
	freedp(&Ainv);
	freedp(&M);
	freedp(&Bi);
	freedp(&tBi);
	freedp(&ttBi);
	freedp(&tttBi);
	if(cflag)
		{
		freedp(&iR);
		freedp(&iR0);
		freedp(&iRnew);
		freedp(&idel);
		freedp(&isiginv);
		freedp(&isigtemp);
		freedp(&isig1temp);
		freedp(&isig2temp);
		}
	return;
}
static do_alloc()
{
	if(allocsp(&Pii,Ni))return(1);
	if(allocsp(&Pi,qpxqp))return(1);
	if(allocsp(&Pj,qpxqp))return(1);
	if(allocsp(&Ptaba,Ni))return(1);
	if(allocsp(&Ptabb,Ni))return(1);
	if(mcflag)
		{ if(allocsp(&Ptabc,Ni))return(1); }
	if(allocdp(&R,Ni))return(1);
	if(allocdp(&R0,Ni))return(1);
	if(allocdp(&Rnew,Ni))return(1);
	if(allocdp(&del,Ni))return(1);
	if(allocdp(&siginv,qpxqp))return(1);
	if(allocdp(&Aij,tNi*tNi))return(1);
	if(allocdp(&Ainv,tNi*tNi))return(1);
	if(allocdp(&M,tNi*tNi))return(1);
	if(allocdp(&Bi,tNi))return(1);
	if(allocdp(&tBi,tNi))return(1);
	if(allocdp(&ttBi,tNi))return(1);
	if(allocdp(&tttBi,tNi))return(1);
	if(allocdp(&sigtemp,qpxqp))return(1);
	if(allocdp(&sig1temp,qpxqp))return(1);
	if(allocdp(&sig2temp,qpxqp))return(1);

	if(cflag)
		{
		if(allocdp(&iR,Ni))return(1);
		if(allocdp(&iR0,Ni))return(1);
		if(allocdp(&iRnew,Ni))return(1);
		if(allocdp(&idel,Ni))return(1);
		if(allocdp(&isiginv,qpxqp))return(1);
		if(allocdp(&isigtemp,qpxqp))return(1);
		if(allocdp(&isig1temp,qpxqp))return(1);
		if(allocdp(&isig2temp,qpxqp))return(1);
		}
	return(0);
}
setdel()/* set del vector for interpolation */
{
	if(anderson && (initflag==2 || initflag==3 || it_cnt>1) && rflag)
		dvmove(Rnew,del,Ni);
	else
		{
		dvsub(Rnew,R0,del,Ni);
		if(cflag)
			dvsub(iRnew,iR0,idel,Ni);
		}
	return;
}
interp()	/* long interpolation, by tenths of interval  */
{
	double Fact,lim;
	register double fact,ssfact,sfact;
	double dointerppoint();

	setdel();/* setup del vector */
	sfact=0;
	for(Fact=.1;sfact==0 && Fact>small;Fact/=10)
		{
		lim=10.1*Fact;
		for(fact=0;fact<=lim && !((fact+Fact)==fact);fact+=Fact)
			{
			/* check interpolated point for positive
			definitness and save in R0, and save in
			R if it has a lower measure */

			if((ssfact=dointerppoint(fact)))
				sfact=ssfact;
			}
		}
	if(sfact)update(sfact);/* update R0 and monitor if moflag */
	return(sfact?0:1);/* return indication of sucess or not */
}
update(sfact)/* maybe update R0 and monitor */
double sfact;
{
	R_R0();/* save best R in R0 */
	inv_det(R0,iR0,siginv,isiginv,&R0_det);/* get inverse and determinant */

	if(moflag)
		{
		fprintf(stderr,"maxfact=%g\n",sfact);
		fprintf(stderr,"R(%g)=\n",sfact);
		dvprintf(R,Ni);
		if(cflag)dvprintf(iR,Ni);
		fprintf(stderr,"ln|R(%g)|=%g\n",sfact,R_det);
		fprintf(stderr,"d(S,R(%g))=%.15g\n\n",sfact,R_measure);
		}
	return;
}
double
dointerppoint(fact)/* do interpolation point for fact */
register double fact;
{
	register int rtflag;

	spmult(R0,fact,del,Rnew,Ni);/* form Rnew[]=R0[]+fact*del[] */
	if(cflag)
		spmult(iR0,fact,idel,iRnew,Ni);

	rtflag=inv_det(Rnew,iRnew,siginv,isiginv,&Rnew_det);
		 /* 0 Rnew pos-def
		    1 Rnew singular, Rnew_det=0
		    2 Rnew non-pos-def, Rnew_det=0 */
	Rnew_measure=domeasure(siginv,isiginv,Rnew_det);

	if(moflag)
		{
		fprintf(stderr,"%g %.10g ",fact,Rnew_measure);
		if(rtflag==0)fprintf(stderr,"\n");
		if(rtflag==1)fprintf(stderr,"singular or negative def\n");
		if(rtflag==2)fprintf(stderr,"npd\n");
		}
	if((Rnew_measure<R_measure) && rtflag==0)
		{
		Rnew_R();/* update R[] with Rnew[] */
		return(fact);/* sucessful point */
		}
	else
		return((double)0);/* not a sucessful point */
}
Rnew_R()/* update R[] with Rnew[] */
{
	R_measure=Rnew_measure;
	R_det=Rnew_det;
	dvmove(Rnew,R,Ni);
	if(cflag)
		dvmove(iRnew,iR,Ni);
	return;
}
Rnew_R0()/* move Rnew[] to R0[] */
{
	R0_det=Rnew_det;
	R0_measure=Rnew_measure;
	dvmove(Rnew,R0,Ni);
	if(cflag)
		dvmove(iRnew,iR0,Ni);
	return;
}
R_R0()/* move R[] vector to R0[] vector */
{
	R0_det=R_det;
	R0_measure=R_measure;
	dvmove(R,R0,Ni);
	if(cflag)
		dvmove(iR,iR0,Ni);
	return;
}
iinterp()	/* interpolation by 1/2 */
{
	register double ssfact,sfact,Fact;
	double dointerppoint();

	setdel();/* setup del vector */
	sfact=0;
	for(Fact=.5;sfact==0 && Fact>small;Fact/=2)
		{
		if((ssfact=dointerppoint(Fact)))
			sfact=ssfact;
		}
	if(sfact)update(sfact);/* update R0 and maybe monitor */
	return(sfact?0:1);
}
spmult(p1,f,p2,p3,n)/* form the vector p3=p1+f*p2 */
register double *p1,f,*p2,*p3;
register int n;
{
	register int i;

	for(i=0;i<n;i++,p1++,p2++,p3++)
		*p3= *p1 + f*(*p2);
	return;
}
exR(sigma,isigma,R,iR,qp,Ni)
/* extract R from sigma */
register int qp,Ni;
register double sigma[],isigma[],R[],iR[];
{
	register int i;
	int n,m,nM,mM;
	register double temp,trace;
	double spdmtrace();

	for(i=0;i<Ni;i++)
		{
		make(Pi,i);
		trace=spdmtrace(sigma,Pi,qp);
		temp=trace/Pii[i];
		R[i]=temp;
		if(cflag)
			{
			for(m=0,mM=0;m<qp;m++,mM+=qp)
				{
				for(n=m+1,nM=mM+qp;n<qp;n++,nM+=qp)
					Pi[nM+m]= -Pi[mM+n];
				}
			trace=spdmtrace(isigma,Pi,qp);
			temp= -trace/Pii[i];
			iR[i]=temp;
			}
		}
	return;
}
static set_up()
{
	if(scflag)
		{
		pdim=1;
		Ni=qdim;
		qp=qdim;
		tNi=Ni;
		if(cflag)
			tNi= 2*Ni-1;
		}
	if(mdflag)
		{
		qp=qdim*pdim;
		Ni= 2*qp-qdim-pdim+1;
		tNi=Ni;
		if(cflag)
			tNi= 2*Ni-1;
		}
	if(mcflag)
		{
		pdim=2;
		qp=qdim*pdim;
		Ni= 2*qp-qdim-pdim+1;
		Ni+=qdim;	/* not general */
		tNi=Ni;
		if(cflag)
			tNi= 2*Ni-2;
		}
	qpxqp=qp*qp;
	return;
}


allocdp(p_p_block,n)	/* allocate a double pointer */
int n;
double **p_p_block;
{
	/*
	assume an 8 byte 64 bit double  
	program calls malloc to establish storage for a block of n doubles
	p_block is a pointer to the block
	p_p_block is a pointer to the pointer p_block

	return(0) for sucessful allocation
	return(1) for unsucessful allocation
	*/

	char *p_char,*malloc();
	unsigned size;

	if(*p_p_block!=0)
		{
		printf("allocdp expects a null p_block\n");
		exit(1);
		}
	size= 8*n;
	p_char=malloc(size);
	if(p_char==0)return(1);
	*p_p_block = (double *)p_char;
	return(0);
}

allocsp(p_p_block,n)	/* allocate a short pointer */
int n;
short **p_p_block;
{
	/*
	assume a 2 byte 16 bit short
	program calls malloc to establish storage for a block of n shorts
	p_block is a pointer to the block
	p_p_block is a pointer to the pointer p_block

	return(0) for sucessful allocation
	return(1) for unsucessful allocation
	*/

	char *p_char,*malloc();
	unsigned size;

	if(*p_p_block!=0)
		{
		printf("allocsp expects a null p_block\n");
		exit(1);
		}
	size= 2*n;
	p_char=malloc(size);
	if(p_char==0)return(1);
	*p_p_block =(short *) p_char;
	return(0);
}
cdmmult(A,iA,B,iB,C,iC,M,iM,n)/* multiply complex matrices */
/* M and iM are temporary matrices */
/* C=A*B */
/* A+iA' etc */
/* C=A*B - A'*B' */
/* iC=A*B' + A'*B */
register double *A,*iA,*B,*iB,*C,*iC,*M,*iM;
register int n;
{
	dmmult(A,B,M,n);
	dmmult(iA,iB,iM,n);
	dmsub(M,iM,C,n);
	dmmult(A,iB,M,n);
	dmmult(iA,B,iM,n);
	dmadd(M,iM,iC,n);
	return;
}
cholesky(M,A,n)	/* routine destroys M but finds A such that M=At*A */
		/* M symmetric (toeplitz?) A upper triangular */
		/* 0 normal return, 1 M singular, 2 M negative definite (???) */
register int n;
register double *M,*A;
{
	register int i,j,k,iM,jM;
	double Mii,Aii;
	register double dtemp,fact;
	double sqrt();

	for(i=iM=0;i<n;i++,iM+=n)
		{
		if((Mii=M[iM+i]) == 0 )return(1);
		if(Mii<0)return(2);
		Aii=sqrt(Mii);
		A[iM+i]=Aii;
		fact=1/Aii;
		for(j=i+1;j<n;j++)
			{
			dtemp=M[iM+j]*fact;
			A[iM+j]=dtemp;
			}
		for(j=i+1,jM=iM+n;j<n;j++,jM+=n)
			{
			fact=A[iM+j];
			for(k=j;k<n;k++)
				{
				dtemp=fact*A[iM+k];
				M[jM+k]-=dtemp;
				}
			}
		}

/* clear lower triangle */

	fact=0;
	for(i=0;i<n;i++,A+=n)
		{
		for(j=0;j<i;j++)
			*(A+j)=fact;
		}
	return(0);
}
cnormal(R,iR,a,ia,p,n)
	/* rtflag=1 singular, rtflag=2 non-pos-def */
double R[],iR[],a[],ia[],p[];
register int n;
{
	register int i,j,ss,s,iM;
	int rtflag;
	register double ci,ici,po,pf,Rt,iRt,at,iat,temp;

	rtflag=0;

	for(i=0,iM=0;i<n;i++,iM+=n)
		{
		a[iM]=1;
		ia[iM]=0;
		for(j=i+1;j<n;j++)
			a[iM+j]=ia[iM+j]=0;
		}
	p[0]=R[0];
	for(i=1,s=n;i<n;i++,s+=n)
		{
		ss=s-n;
		ci=R[i];
		ici=iR[i];
		for(j=1;j<i;j++)
			{
			Rt=R[i-j];
			iRt=iR[i-j];
			at=a[ss+j];
			iat=ia[ss+j];
			ci+= at*Rt-iat*iRt;
			ici+= at*iRt+iat*Rt;
			}
		if((po=p[i-1])==0)return(1);
		ci= -ci/po;
		ici= -ici/po;
		a[s+i]=ci;
		ia[s+i]=ici;
		for(j=1;j<i;j++)
			{
			temp=a[ss+j]+ci*a[ss+i-j]+ici*ia[ss+i-j];
			a[s+j]=temp;
			temp=ia[ss+j]-ci*ia[ss+i-j]+ici*a[ss+i-j];
			ia[s+j]=temp;
			}
		pf=(1-(ci*ci+ici*ici))*po;
		p[i]=pf;

		if(pf<0)rtflag++;

		if(pf==0)return(1);

		}
	if(rtflag>0)return(2);
	return(0);
}

dmadd(A,B,C,n)/* matrices C=A+B */
register double *A,*B,*C;
register int n;
{
	register int i,lim;

	lim=n*n;
	for(i=0;i<lim;i++,C++,A++,B++)
		*C= *A + *B;
	return;
}
dmclear(M,n)/* clear matrix */
register double *M;
register int n;
{
	register int i,lim;
	register double zero;

	lim=n*n;
	zero=0;
	for(i=0;i<lim;i++,M++)
		*M=zero;
	return;
}
dmmove(A,B,n)/* move matrix A to matrix B */
register double *A,*B;
register int n;
{
	register int i,lim;

	lim=n*n;
	for(i=0;i<lim;i++,A++,B++)
		*B= *A;
	return;
}
dmmult(a,b,c,n)/* multiply of matrices */
/* c=a*b */
register double *a,*b,*c;
register int n;
{
	register int i,j;
	register double *s;
	double dot();

	for(i=0;i<n;i++,a+=n)
		{
		for(j=0,s=b;j<n;j++,s++,c++)
			*c=dot(a,1,s,n,n);
		}
	return;
}
dmsub(A,B,C,n)/* matrices C=A-B */
register double *A,*B,*C;
register int n;
{
	register int i,lim;

	lim=n*n;
	for(i=0;i<lim;i++,C++,A++,B++)
		*C= *A - *B;
	return;
}
double dmtrace(M1,M2,n)/* returns trace of M1*M2, M1 and M2 matrices  */
register double *M1,*M2;
register int n;
{
	register int i;
	register double trace;
	double dot();

	trace=0;
	for(i=0;i<n;i++,M1+=n,M2++)
		trace+=dot(M1,1,M2,n,n);
	return(trace);
}
dojohnavg(M,Mout,n)/* John's averaging process for matrices */
register double *M,*Mout;
register int n;
{
	register int i,j,iM,jM;
	register double term,diag;
	double tr;
	double trace();

	tr=trace(M,n)*2/n;

	for(i=iM=0;i<n;i++,iM+=n)
		{
		diag=M[iM+i];
		for(j=jM=0;j<n;j++,jM+=n)
			{
			if(i==j)continue;
			term=M[iM+j]*tr/(diag+M[jM+j]);
			Mout[iM+j]=term;
			}
		}
	term=tr/2;
	for(i=0;i<n;i++,Mout+=n+1)
		*Mout=term;
	return;
}
double 
dot(a,na,b,nb,n)/* dot product a and b, na is a increment, mb is b increment */
register double *a,*b;
register int na,nb,n;
{
	register int i;
	register double sum;

	sum=0;
	for(i=0;i<n;i++,a+=na,b+=nb)
		sum+=(*a)*(*b);
	return(sum);
}
dvclear(M,n)/* clear vector */
register double *M;
register int n;
{
	register int i;
	register double zero;

	zero=0;
	for(i=0;i<n;i++,M++)
		*M=zero;
	return;
}
dvmove(x,y,n)/* move vector x to y */
register double *x,*y;
register int n;
{
	register int i;
	for(i=0;i<n;i++,y++,x++)
		*y= *x;
	return;
}
dvprintf(x,n)
double x[];
int n;
{
	int i;
	for(i=0;i<n;i++)
		printf("%f ",x[i]);
	printf("\n");
	return;
}

dvsub(A,B,C,n)/* vectors C=A-B */
register double *A,*B,*C;
register int n;
{
	register int i;

	for(i=0;i<n;i++,C++,A++,B++)
		*C= *A - *B;
	return;
}
freedp(p_p_block)
double **p_p_block;
{
	char *p_char;

	p_char =(char *) *p_p_block;
	if(p_char==0)return;
	free(p_char);
	*p_p_block=0;