toepsolve.c

speech signal process tools

toepsolve(Ma,G,D,Y,X,n0,n1,n2)
/* Ma is an n0 x n0 symmetric Toeplitz matrix, n0 odd, G and D are temporary
matrices of order n1=2n0-1 x n2=(n0+1)/2 and Y is a vector n0 long symmetric
about the mid value; X is n0 long and is the solution of 
Ma X = Y n0=2M+1 n1=4M+1 n2=M+1 */
register int n0,n1,n2;
register double Ma[],G[],D[],Y[],X[];
{
	register int k,lim;

	lim=n1*n2;
	for(k=0;k<lim;k++)
		G[k]=D[k]=0;
	G[(n0-1)*n2]=1;
	do_col(Ma,D,n0,n2);
	con1volve(Ma,G,D,n0,n1,n2);
	for(k=2;k<n2;k++)
		{
		con2volve(G,D,k-1,k,n1,n2);
		do_sub(G,D,k-2,k,n0,n1,n2);
		do_sub(G,D,k-1,k,n0,n1,n2);
		}
	domove(G,D,n0,n2);
	doX(G,D,Y,X,n0,n2);
	return;
}
static doX(G,D,Y,X,n0,n2)
register int n0,n2;
register double G[],D[],Y[],X[];
{
	register int i,iM,j,l,lM;
	register double sum,tsum,temp;
	double getPdiag();

	for(i=iM=0;i<n2;i++,iM+=n2)
		{
		sum=0;
		for(j=0;j<n2;j++)
			{
			if((temp=G[iM+j])==0)
				continue;
			tsum=0;
			for(l=0,lM=j;l<n0;l++,lM+=n2)
				tsum+=G[lM]*Y[l];
			tsum*=temp/getPdiag(G,D,j,n0,n2);
			sum+=tsum;
			}
		X[i]=X[n0-i-1]=sum;
		}
	return;
}
static domove(G,D,n0,n2)
register int n0,n2;
register double G[],D[];
{
	register int i,lim,c;

	lim=n0*n2;
	c=(n2-1)*n2;
	for(i=0;i<lim;i++,c++)
		{
		G[i]=G[c];
		D[i]=D[c];
		}
	return;
}
static double getPdiag(G,D,j,n0,n2)
register int j,n0,n2;
register double G[],D[];
{
	register int l,lM;
	register double sum;

	sum=0;
	for(l=0,lM=j;l<n0;l++,lM+=n2)
		sum+=G[lM]*D[lM];
	if(sum==0)
		printf("getPdiag is returning a zero\n");
	return(sum);
}
static do_sub(G,D,k1,k2,n0,n1,n2)
register int k1,k2,n0,n1,n2;
register double G[],D[];
{
	register int j,jM,c,lim;
	register double ratio,temp;

	c=(n0-k1-1)*n2;
	if((ratio=D[c+k1])==0)
		{
		printf("zero divide, do_sub from solveX\n");
		exit(1);
		}
	ratio=D[c+k2]/ratio;
	lim=n1-k2;
	for(j=k2,jM=k2*n2;j<lim;j++,jM+=n2)
		{
		temp=G[jM+k2]-ratio*G[jM+k1];
		G[jM+k2]=temp;
		temp=D[jM+k2]-ratio*D[jM+k1];
		D[jM+k2]=temp;
		}
	return;
}
static con2volve(G,D,k,ko,n1,n2)
register int k,ko,n1,n2;
register double G[],D[];
{
	register int j,iM,jMm,jMp,lim;
	register double temp;

	iM=ko*n2+ko;
	jMm=k*n2+k;
	jMp=jMm+2*n2;
	lim=n1-ko;
	for(j=ko;j<lim;j++)
		{
		temp=G[jMm]+G[jMp];
		G[iM]=temp;
		temp=D[jMm]+D[jMp];
		D[iM]=temp;
		iM+=n2;
		jMm+=n2;
		jMp+=n2;
		}
	return;
}
static con1volve(Ma,G,D,n0,n1,n2)
register int n0,n1,n2;
register double Ma[],G[],D[];
{
	register int j,iM,jM,jMm,jMp,lim;
	register double temp,term;

	term= -2*Ma[n0]/Ma[0];
	iM=n2+1;
	jMm=0;
	jM=n2;
	jMp=2*n2;
	lim=n1-1;
	for(j=1;j<lim;j++)
		{
		temp=G[jMm]+term*G[jM]+G[jMp];
		G[iM]=temp;
		temp=D[jMm]+term*D[jM]+D[jMp];
		D[iM]=temp;
		iM+=n2;
		jMm+=n2;
		jM+=n2;
		jMp+=n2;
		}
	return;
}
static do_col(Ma,D,n0,n2)
register int n0,n2;
register double Ma[],D[];
{
	register int i,iM1,iM2,iM3,n0m1;
	register double temp;

	n0m1=n0-1;
	for(i=iM1=iM2=0,iM3=(n0-1)*n2;i<n0;i++,iM1+=n0,iM2+=n2,iM3+=n2)
		{
		temp=Ma[iM1+n0m1];
		D[iM2]=temp;
		temp=Ma[iM1];
		D[iM3]=temp;
		}
	return;
}