genburg.c

speech signal process tools

	return;
}

freesp(p_p_block)
short **p_p_block;
{
	char *p_char;

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

#include <stdio.h>
char *sprintf ();

/*
Given the range xmin to xmax and the number of points N on the range
( including the end points ) and the function name for computing the
value of the function, find the minimum value of the function and the
x that produces that minimum.
Assume only one minimum in range and no maxima except at the ends of the range.
*/
get_min(xmin,xmax,N,function,min_x,min_y,level,flag)
int N,level,flag;
double xmin,xmax,*min_x,*min_y;
double (*function)();
{
	double x,del_x,y;
	double new_xmin,new_xmax;
	double n_min_x,n_min_y;
	int i,s_i;

	del_x=(xmax-xmin)/(N-1);
	*min_y= 1e30;
	for(i=0;i<N;i++)
		{
		x=xmin+i*del_x;
		y=(*function)(x);
		printf("x=%g y=%g\n",x,y);
		if(*min_y>y)
			{
			/* function decreasing */
			*min_y=y;
			*min_x=x;
			s_i=i;/* save position in table of minimum */
			}
		else
			{
			if(i && !flag)
				/* function increasing */
				break;
			}
		}
	if(level>1)
		{
		if(s_i==0)
			{
			new_xmin=xmin;
			new_xmax=xmin+del_x;
			}
		else if(s_i==(N-1))
			{
			new_xmin=xmax-del_x;
			new_xmax=xmax;
			}
		else
			{
			new_xmin=xmin+(s_i-1)*del_x;
			new_xmax=xmin+(s_i+1)*del_x;
			}
	get_min(new_xmin,new_xmax,N,function,&n_min_x,&n_min_y,level-1,flag);
		if(n_min_y<*min_y)
			{
			*min_y=n_min_y;
			*min_x=n_min_x;
			}
		}
	return;
}
double hdmtrace(M1,iM1,M2,iM2,n)
/* M1 and M2 are hermitian matrices, hdmtrace produces the trace of the
product of the two matrices, which is real */
double M1[],M2[],iM1[],iM2[];
int n;
{
	int j,k,jM,kM;
	double sum;
	sum=0;
	for(j=0,jM=0;j<n;j++,jM+=n)
		{
		for(k=0,kM=0;k<n;k++,kM+=n)
			{
			sum+=(M1[jM+k]*M2[kM+j]-iM1[jM+k]*iM2[kM+j]);
			}
		}
	return(sum);
}

lctoepinv(R,iR,Rinv,iRinv,pdet,n)
	/* natural log of determinant is returned */
	/* R a vector of length n, Rinv a matrix of rank n */
register double R[],iR[],Rinv[],iRinv[],*pdet;
register int n;
{
	register int l,i,iM,j,jM,ss;

	int N,rtflag,toggle;
	static double *a,*ia,*p,Rt,iRt;
	register double det,pt,dtemp;
	double log();

	N=n-1;

	if(allocdp(&a,n*n))
		{
		printf("ctoepinv not possible due to storage limitations\n");
		exit(1);
		}
	if(allocdp(&ia,n*n))
		{
		freedp(&a);
		printf("ctoepinv not pollsible due to storage limitations\n");
		exit(1);
		}
	if(allocdp(&p,n))
		{
		freedp(&a);
		freedp(&ia);
		printf("ctoepinv not possible due to storage limitations\n");
		exit(1);
		}
	
	rtflag=cnormal(R,iR,a,ia,p,n);
	if(rtflag==1)
		{
		freedp(&a);
		freedp(ia);
		freedp(&p);
		*pdet=0;
		return(1);
		}
	
	/* if(rtflag==2) printf("ctoep non-pos-definite\n"); */

	det=0;
	if(rtflag==0)
		{
		for(i=0;i<n;i++)
			det+=log(p[i]);
		}
	if(rtflag==2)
		{
		toggle=1;
		for(i=0;i<n;i++)
			{
			if((pt=p[i])<0)
				{
				toggle= -toggle;
				pt= -pt;
				}
			det+=log(pt);
			}
		if(toggle!=1)
			det=0;
		}
	*pdet=det;

	for(i=0,iM=0;i<n;i++,iM+=n)
		{
		for(j=i,jM=i*n;j<n;j++,jM+=n)
			{
			Rt=0;
			iRt=0;
			for(l=0,ss=N*n;l<=i;l++,ss-=n)
				toepmult(a[ss+i-l],-ia[ss+i-l],a[ss+j-l],ia[ss+j-l],&Rt,&iRt,p[N-l]);
			Rinv[jM+i]=Rinv[iM+j]=Rt;
			iRinv[iM+j]=iRt;
			if(i!=j)
				{
				dtemp= -iRt;
				iRinv[jM+i]=dtemp;
				}
			}
		}
	
	freedp(&a);
	freedp(&ia);
	freedp(&p);
	return(rtflag);
}
ldmsyminv(M1,M2,pdet,n,flag)
/* this routine appears to give the inverse for non symmetric matrices */
/* produces the inverse of the symmetric matrix M1 in M2 */
/* M2 can =M1, M1 is destroyed unless flag=1 */
/* returns 0 for non-singular matrix, 1 for singular matrix */
/* return 2 for storage allocation problems */
/* the determinant is returned as the natural log */
/* det is zero for singular case and npd case */
register double M1[],M2[],*pdet;
register int n,flag;
{
	register int rtflag,nxn;
	static double *M3,*M4;
	double det,t0,t2,t3,dtemp;
	double log();

	if(n==1)
		{
		if(M1[0]<=0)
			*pdet=0;
		else *pdet=log(M1[0]);
		if(M1[0]==0)
			return(1);
		dtemp=1/M1[0];
		M2[0]=dtemp;
		return(0);
		}
	if(n==2)
		{
		det=M1[0]*M1[3]-M1[2]*M1[1];
		if(det<=0)
			*pdet=0;
		else *pdet=log(det);
		if(det==0)
			return(1);
		t0=M1[3]/det;
		t3=M1[0]/det;
		t2= -M1[1]/det;
		M2[3]=t3;
		M2[0]=t0;
		M2[2]=M2[1]=t2;
		return(0);
		}
	nxn=n*n;
	if( (flag==1) || M1==M2 )
		{
		if(allocdp(&M3,nxn) || allocdp(&M4,nxn))
			{
			printf("ldmsyminv not possible due to storage allocation problems\n");
			return(2);
			}
		dmmove(M1,M4,n);/* move M1 to M4 */
		rtflag=lorthogin(M4,M2,M3,pdet,n);
		freedp(&M3);
		freedp(&M4);
		if(rtflag)
			return(1);
		return(0);
		}
	if(allocdp(&M3,nxn))
		{
		printf("ldmsyminv not possible due to storage allocation problems\n");
		return(2);
		}
	rtflag=lorthogin(M1,M2,M3,pdet,n);
		freedp(&M3);
	if(rtflag)
		return(1);
	return(0);
}
lorthogin(M,Minv,Tinv,pdet,n)
register double *M,*Minv,*Tinv,*pdet;
register int n;
/* returns log(det) , 0 if negative definite or singular */
{
	register int i,j,toggle;
	register double Mt,*s,*s1;
	double log(),dot();

	if(orthog(M,Minv,n))
		{
		*pdet=0;
		return(1);
		}
	/* orthog returns an orthogonal matrix O in M and an upper triagonal
	matrix T in Minv such that M=O*T */

	*pdet=0;
	toggle=1;
	j=n+1;
	for(i=0,s=Minv;i<n;i++,s+=j)
		{
		if((Mt= *s)<0)
			{
			toggle= -toggle;
			Mt= -Mt;
			}
		if(!Mt)
			{
			toggle= -1;
			break;
			}
		*pdet+=log(Mt);
		}
	if(toggle!=1)
		*pdet=0;

	if(triinv(Minv,Tinv,n))
		return(1);
	for(i=0,s=Minv;i<n;i++,Tinv+=n)
		{
		for(j=0,s1=M;j<n;j++,s1+=n,s++)
			*s=dot(Tinv,1,s1,1,n);
		}
	return(0);
}
lsyminv(M,Minv,Temp,n,pdet)	/* M symmetric (toeplitz?) positive definite */
				/* Minv=Ainv*Ainvt */
		/* cholesky finds A, such that M=At*A, A upper triangular */
		/* triinv finds Ainv */
		/* return 0 normal, 1 M singular, 2 M negative definite (???) */
		/* returns ln det of M in pdet */
register int n;
double M[],Minv[],Temp[],*pdet;
{
	register double sum;
	register int i,j,k,iM,jM;
	int rtflag;
	double log();

	for(i=iM=0;i<n;i++,iM+=n)
		{
		for(j=0;j<n;j++)
			Temp[iM+j]=M[iM+j];
		}
	if((rtflag=cholesky(Temp,Minv,n)))
		{
		*pdet=0;
		return(rtflag);
		}
	/* Minv contains A, Temp scribbled */
	if(triinv(Minv,Temp,n))
		return(1);
		/* Temp==Ainv, Minv==A not scribbled, do det */
	sum=0;
	for(i=iM=0;i<n;i++,iM+=n)
		sum+=log(Minv[iM+i]);
	*pdet=2*sum;/* return ln det M */
	for(i=iM=0;i<n;i++,iM+=n)
		{
		for(j=i,jM=i*n;j<n;j++,jM+=n)
			{
			sum=0;
			for(k=0;k<n;k++)
				sum+=Temp[iM+k]*Temp[jM+k];
			Minv[jM+i]=Minv[iM+j]=sum;
			}
		}
	return(0);
}
ltoepinv(R,Rinv,pdet,n)
	/* natural log of determinant is returned */
double R[],Rinv[],*pdet;
register int n;
{
	register int l,i,iM,j,jM,ss;
	int N;
	int rtflag,toggle;
	double det,Rt,pt,log();
	static double *a,*p;

	N=n-1;

	if(allocdp(&a,n*n))
		{
		printf("ltoepinv not possible due to storage limitations\n");
		exit(1);
		}
	if(allocdp(&p,n))
		{
		freedp(&a);
		printf("ltoepinv not possible due to storage limitations\n");
		exit(1);
		}

	rtflag=normal(R,a,p,n);
	/* rtflag=1 singular, rtflag=2 non-pos-def */

	if(rtflag==1)
		{
		freedp(&a);
		freedp(&p);
		*pdet=0;
		return(1);
		}

	/* if(rtflag==2) printf("non-minimum phase\n"); */

	det=0;
	if(rtflag==0)
		{
		for(i=0;i<n;i++)
			det+=log(p[i]);
		}
	if(rtflag==2)
		{
		toggle=1;
		for(i=0;i<n;i++)
			{
			if((pt=p[i])<0)
				{
				toggle= -toggle;
				pt= -pt;
				}
			det+=log(pt);
			}
		if(toggle!=1)
			det=0;
		}
	*pdet=det;

	for(i=0,iM=0;i<n;i++,iM+=n)
		{
		for(j=i,jM=i*n;j<n;j++,jM+=n)
			{
			Rt=0;
			for(l=0,ss=N*n;l<=i;l++,ss-=n)
				Rt+= a[ss+i-l]*a[ss+j-l]/p[N-l];
			Rinv[jM+i]=Rinv[iM+j]=Rt;
			}
		}

	freedp(&a);
	freedp(&p);
	return(rtflag);
}
normal(R,a,p,n)
	/* rtflag=1 if singular, rtflag=2 if non-pos-definite */
double R[],a[],p[];
register int n;
{
	register int i,j,ss,s,iM;
	int rtflag;
	register double ci,po,pf,temp;

	rtflag=0;

	for(i=0,iM=0;i<n;i++,iM+=n)
		{
		a[iM]=1;
		for(j=i+1;j<n;j++)
			a[iM+j]=0;
		}
	p[0]=R[0];
	for(i=1,s=n;i<n;i++,s+=n)
		{
		ss=s-n;
		ci=R[i];
		for(j=1;j<i;j++)
			ci+= a[ss+j]*R[i-j];
		if((po=p[i-1])==0)return(1);
		ci= -ci/po;
		if(ci<-1 || ci>1)
			rtflag++;
		a[s+i]=ci;
		for(j=1;j<i;j++)
			{
			temp=a[ss+j]+ci*a[ss+i-j];
			a[s+j]=temp;
			}
		pf=(1-ci*ci)*po;
		p[i]=pf;
		if(pf==0)return(1);
		}
	if(rtflag>0)return(2);
	return(0);
}
orthog(M,T,n)
/* routine returns in M an orthogonal matrix O
and in T an upper triangular matrix T such that M=O*T */
/* if M non-singular, T non-singular */
register double *M,*T;
register int n;
{
	register int i,j,k;
	register double *s,*c,*sT,*s1T,*sM;
	double sqrt(),dot();

	for(k=0,s1T=T,c=M;k<n;k++,s1T+=n+1,c++)
		{
		if(k>0)
			{
			for(j=0,sT=T+k,sM=M;j<k;j++,sT+=n,sM++)
				{
				*sT=dot(sM,n,c,n,n);
				sp1(*sT,sM,c,n);
				}
			}
		(*s1T)=dot(c,n,c,n,n);
		if((*s1T)<=0)
			return(1);

		*s1T=sqrt(*s1T);
		for(i=0,s=c;i<n;i++,s+=n)
			*s/= *s1T;
		}
	return(0);
}
sp1(p1,p2,p3,n)
register double p1,*p2,*p3;
register int n;
{
	register int i;

	for(i=0;i<n;i++,p2+=n,p3+=n)
		*p3 -= p1 * *p2;
	return;
}
posdefsol(M,A,B,x,y,t,t1,n)/* solves M*x=y, M pos definite,
A, B temp matrices, t and t1 temp vectors */
register double A[],B[],M[],x[],y[],t[],t1[];
register int n;
{
	register int rtflag,i;

	dmmove(M,B,n);/* save M, move M to B which is destroyed */
	rtflag=cholesky(B,A,n);	/* routine destroys B but finds A such that B=At*A */
	if(rtflag)
		return(rtflag);
	for(i=0;i<n;i++)
		t1[i]= -y[i];
	solchol(A,t1,x,t,n);
	/* A upper triangular from cholesky, t1=b constants, x unknowns */
	/* Mx+b=0   M=At*A */
	return(rtflag);
}
solchol(R,b,x,t,n)
/* R upper triangular from cholesky, b constants, x unknowns */
/* t is a temporary vector */
/* Mx+b=0   M=Rt*R */
register double R[],b[],*x,t[];
register int n;
{
	register int k,np1,i,iM;
	register double *M,dtemp;
	register double S;
	double dot();

	np1=n+1;

/* forward substitution */

	for(k=0,M=R;k<n;k++,M+=np1)
		{
		dtemp= -(b[k]+dot(R+k,n,t,1,k))/(*M);
		t[k]=dtemp;
		}
	/*
	for(k=0,kM=0;k<n;k++,kM+=n)
		{
		S=b[k];
		for(i=0,iM=0;i<k;i++,iM+=n)
			S+=R[iM+k]*t[i];
		dtemp= -S/R[kM+k];
		t[k]=dtemp;
		}
	*/

/* backward substitution */

	/*
	for(k=0,x=x+n,M=R+n*n;k<n;k++,M-=np1,x--)
		{
		dtemp=(t[n-1-k]-dot(M,1,x,1,k))/(*(M-1));
		x[k]=dtemp;
		}
	*/
	/*
	for(i=n-1,iM=(n-1)*n;i>=0;i--,iM-=n)
		{
		S=t[i];
		for(k=i+1;k<n;k++)
			S-=R[iM+k]*x[k];
		dtemp=S/R[iM+i];
		x[i]=dtemp;
		}
	*/
	for(i=n-1,iM=(n-1)*n;i>=0;i--,iM-=n)
		{
		S=t[i]-dot(R+iM+i+1,1,x+i+1,1,n-i-1);
		dtemp=S/R[iM+i];
		x[i]=dtemp;
		}
	return;
}
double
spdmtrace(M1,M2,n)/* trace of M1*M2, M1 double, M2 short */
register double *M1;
register short *M2;
register int n;
{
	register int i;
	register double trace;
	double spdot();

	trace=0;
	for(i=0;i<n;i++,M1+=n,M2++)
		trace+=spdot(M1,1,M2,n,n);
	return(trace);
}
double
spdot(a,na,b,nb,n)/* dot product of double and short */
/* short is one or zero or minus one */
register double *a;
register short *b;
register int n,na,nb;
{
	register int i;
	register double dot;

	dot=0;
	for(i=0;i<n;i++,a+=na,b+=nb)
		{
		if(*b)
			{
			if(*b>0)/* *b == 1 */
				dot+=(*a);
			else/* *b == -1 */
				dot-=(*a);
			}
		}
	return(dot);
}
toepmult(a,ia,b,ib,c,ic,p)
double a,ia,b,ib,*c,*ic,p;
{
	*c+= (a*b-ia*ib)/p;
	*ic+= (a*ib+ia*b)/p;
	return;
}
double 
trace(M,n)/* return trace of double matrix */
register double *M;
register int n;
{
	register int i,con;
	register double sum;

	sum=0;
	con=n+1;
	for(i=0;i<n;i++,M+=con)
		sum+= *M;
	return(sum);
}
triinv(T,Tinv,n)	/* T and Tinv upper triangular */
		/* 0 normal return, 1 T singular */
register int n;
register double *T,*Tinv;
{
	register int i,j,k,iT,kT;
	register double sum,dtemp,*sT,*sTinv;

	j=n+1;
	for(i=0,sT=T,sTinv=Tinv;i<n;i++,sT+=j,sTinv+=j)
		{
		if(!*sT)return(1);
		*sTinv=1/(*sT);
		}
	for(j=n-1;j>=0;j--)
		{
		for(i=j-1,iT=(j-1)*n;i>=0;i--,iT-=n)
			{
			sum=0;
			for(k=i+1,kT=(i+1)*n;k<j+1;k++,kT+=n)
				sum+=T[iT+k]*Tinv[kT+j];
			dtemp= -sum*Tinv[iT+i];
			Tinv[iT+j]=dtemp;
			}
		}
	for(i=0;i<n;i++,Tinv+=n)/* clear lower triangle */
		{
		for(j=0,sT=Tinv;j<i;j++,sT++)
			*sT=0;
		}
	return(0);
}