matops.c

来自「speech signal process tools」· C语言 代码 · 共 267 行

/*	

 This material contains proprietary software of Entropic Processing, Inc.
 Any reproduction, distribution, or publication without the the prior	   
 written permission of Entropic Processing, Inc. is strictly prohibited.
 Any public distribution of copies of this work authorized in writing by
 Entropic Processing, Inc. must bear the notice			
       
	Copyright 1986, Entropic Proccessing, Inc
	(C) 1985, Entropic Processing, Inc.
         */

#ifdef SCCS
static char *sccsid = "@(#)matops.c	1.2 4/28/86";
#endif
	
copy (inp,out)
double out[2][2],inp[2][2];
{
	int i,j;
	for (i = 0; i < 2; i++)
		for (j = 0; j < 2; j++)
			out[i][j] = inp[i][j];
}

sign (x)
double x[2][2];
{
	int i,j;
	for (i = 0; i < 2; i++)
		for (j = 0; j < 2; j++)
			x[i][j] = - x[i][j];
}

transpose (inp,out)
double inp[2][2], out[2][2];
{
	out [0][0] = inp [0][0];
	out [0][1] = inp [1][0];
	out [1][0] = inp [0][1];
	out [1][1] = inp [1][1];
}

invert(inp,out)
double inp[2][2],out[2][2];
{
	double det;
	det = inp[0][0] * inp[1][1] - inp[0][1]*inp[1][0];
	out [0][0] = inp[1][1]/det;
	out [0][1] = -inp[0][1]/det;
	out [1][0] = - inp[1][0]/det;
	out [1][1] = inp[0][0] /det;
}

add (x,y,result)
double x[2][2],y[2][2], result[2][2];
{
	int i,j;
	for (i = 0; i < 2; i++)
		for (j = 0; j < 2; j++)
			result[i][j] = x[i][j] + y[i][j];
}

multiply (x,y,result)
double x[2][2],y[2][2], result[2][2];
{
	int i,j;
	for (i = 0; i < 2; i++)
		for (j = 0; j < 2; j++)
			result[i][j] = x[i][0]*y[0][j] + x[i][1]*y[1][j];
}
mac (x,y,result)
double x[2][2], y[2][2], result[2][2];
{
	int i,j,k;
	for (i = 0; i < 2; i++)
		for (j = 0; j < 2; j++)
			for (k = 0; k <2; k++)
				result[i][j] += x[i][k]*y[k][j];
}

/* Two channel Levinson algorithm */

mchlev(R,order,A,Ef)
double R[][2][2],Ef[2][2];
float A[][2][2];
int order;
{
	double	F[1000][2][2], B[1000][2][2], tmp[1000][2][2],
		inv_Ef[2][2], Eb[2][2],inv_Eb[2][2], Cf[2][2],Cb[2][2],
		delta[2][2], tr_delta[2][2];
	static double   I[2][2] = { 1.0, 0.0, 0.0, 1.0 },
			zero [2][2] = {0., 0., 0., 0.};
	int i,j,k,n;


		/* Initialization: B[0] = F[0] = I, Ef = Eb = R[0] */

	copy (R[0],Ef); copy (R[0],Eb);
	copy (I,B[0]); copy (I,F[0]);
	for ( i = 1; i <= order; i++)
	{
		copy (zero, B[i]);
		copy (zero, F[i]);
	}
	
	for (n = 1; n <= order; n++)
	{
		/*  delta = R(n) + R(n-1)*F(1) + ... + R(1)*F(n-1) */
		copy(R[n],delta);
		for (j = 1; j < n; j++)
			mac (R[n-j], F[j],delta);

		/* Cf = - inv_Eb * delta, Cb = - inv_Ef * tr_delta */
		
		invert (Eb, inv_Eb);
		multiply (inv_Eb, delta ,Cf); sign(Cf);

		/*
		printf("\tCf\n");
		for (j = 0; j < 2; j++)
		{
			for (k = 0; k < 2; k++)
				printf("%f\t", Cf[j][k]);
			printf("\n");
		}
		*/

		transpose (delta, tr_delta);
		invert (Ef, inv_Ef);
		multiply (inv_Ef, tr_delta, Cb); sign(Cb);

		/*
		printf("\tCb\n");
		for (j = 0; j < 2; j++)
		{
			for (k = 0; k < 2; k++)
				printf("%f\t", Cb[j][k]);
			printf("\n");
		}
		*/

		/* update F and B filters */

		for (i = 0; i <= n; i++) copy (F[i],tmp[i]);
		/*
		printf("\tF filter\n");
		*/
		for (i = 1; i <= n; i++)
		{
			mac (B[n-i], Cf, F[i]);
			/*
			for (j = 0; j < 2; j++)
				for (k = 0; k < 2; k++)
					printf("%f\t", F[i][j][k]);
			printf("\n");
			*/
		}

		/*
		printf("\tB Filter\n");
		*/
		for (i = 1; i <= n; i++)
		{
			mac (tmp[n-i], Cb, B[i]);
			/*
			for (j = 0; j < 2; j++)
				for (k = 0; k < 2; k++)
					printf("%f\t", B[i][j][k]);
			printf("\n");
			*/
		}

		/* Ef  = Ef( I - Cb * Cf), Eb = Eb( I - Cf*Cb) */

		multiply (Cb, Cf, tmp[0]);
		multiply (Ef, tmp[0], tmp[1]); sign(tmp[1]);
		add(Ef, tmp[1], Ef);

		/*
		printf("\tEf\n");
		for (j = 0; j < 2; j++)
		{
			for (k = 0; k < 2; k++)
				printf("%f\t", Ef[j][k]);
			printf("\n");
		}
		t = Ef[0][0]*Ef[1][1] - Ef[0][1]*Ef[1][0];
		printf("%f\n",t);
		*/
		multiply (Cf, Cb, tmp[0]);
		multiply (Eb, tmp[0], tmp[1]); sign(tmp[1]);
		add(Eb, tmp[1], Eb);

		/*
		printf("\tEb\n");
		for (j = 0; j < 2; j++)
		{
			for (k = 0; k < 2; k++)
				printf("%f\t", Eb[j][k]);
			printf("\n");
		}
		t = Eb[0][0]*Eb[1][1] - Eb[0][1]*Eb[1][0];
		printf("%f\n",t);
		*/
	}
	for (i = 0; i <= order; i++)
		for (j = 0; j < 2; j++)
			for (k = 0; k < 2; k++)
				A[i][j][k] = F[i][j][k];	

	/* compute R(order+1) */
	/*  R(n)  = - R(n-1)*F(1) - ... - R(1)*F(n-1) */
		copy(zero, R[order+1]);

		for (j = 0; j < order; j++)
			mac (R[order-j], F[j+1],R[order+1]);
		sign(R[order+1]);
}

scovar(x,order,nsmpls,pitch,S)
float x[];
int nsmpls, order, pitch;
double S[];
{
	int i,j,k,l,n;
	int i1,i2;
	int matsze;
	float *ptr1,*ptr2;
	double t;

	matsze = order + 1;
	for (i = 0; i < 4*matsze; i++) S[i] = 0.0;

	for (k = 0; k < 2; k++)

		for (j = 0; j < matsze; j++)
			for (l = 0; l < 2; l++)
			{
				t = 0.0;
				ptr1 = &x[k*pitch];
				ptr2 = &x[l*pitch+j];
				for (n = 0;  n < nsmpls; n++)
				{
					t += (*ptr1++) * (*ptr2++);
				}
				S[2*matsze * k + 2*j + l] = t;
				S[2*matsze * (2*j+l) + k] = t;
			}

	for (i = 0; i < matsze - 1; i++)
		for (k = 0; k < 2; k++)
			{
			i1 = (2*i + k) * 2 * matsze;
			for (j = 0; j < matsze - 1; j++)
				for (l = 0; l < 2; l++)
				{
					i2 = i1 + 2*j + l;
					S[i2+4*matsze+2] = S[i2]
					- x[i+k*pitch] * x[j+l*pitch]
					+ x[nsmpls+i+k*pitch] * x[nsmpls+j+l*pitch];
				}
			}
	i = 4 * (order+1) *(order+1);
	for (j = 0; j < i; j++) S[j] /= nsmpls;
}