intmatrixinv.cpp

亲手所编

#include <stdio.h>         /* Standard Libraries */
#include <stdlib.h>
#include <string.h>
#include <math.h>

FILE *fmatrix; 

typedef struct _Matrix{
    int nRows;
    int nCols;
    int **matr;
}Matrix;

typedef struct _Matrix64{
    int nRows;
    int nCols;
    unsigned int **matr;
}Matrix64;

void ToCompCode(unsigned int *scrData, unsigned int *desData)
{
	int i;

	for(i = 0; i < 4; i++)
	{
		desData[i] = scrData[i] ^ 0x0000ffff;
	} 

	desData[0] = desData[0] + 0x00000001;
	for(i = 0; i < 3; i++)
	{
		desData[i+1] = (desData[i] >> 16) + desData[i+1];
		desData[i] = desData[i] & 0x0000ffff;
	}
	desData[i] = desData[i] & 0x0000ffff; /* cut the overflow bit */
}

void printMatrix(Matrix m, FILE *fp, char *Mname)
{
	int i, j;
	
	fprintf(fp,"%s:\n", Mname);
	for (i = 0; i < m.nRows; i++)
	{
		for (j = 0; j < m.nCols; j++)
		{
			fprintf(fp, "%e ", m.matr[i][j]/1024.);
		}
		fprintf(fp, "\n");
	}
}

void printMatrix64(Matrix64 m64, int Q, FILE *fp, char *Mname)
{
	int i, j;
	double val;
	unsigned int flag;
	unsigned int bits[2];
	unsigned int matr[4];

	fprintf(fp,"%s:\n", Mname);
	for (i = 0; i < m64.nRows; i++)
	{
		for (j = 0; j < m64.nRows; j++)
		{
		
			flag = m64.matr[i][j*4+3] & 0x00008000;

			if(flag != 0) /* negative */
			{
				ToCompCode(&m64.matr[i][j*4], matr);	
				bits[0] = (matr[1] << 16) | matr[0];
				bits[1] = (matr[3] << 16) | matr[2];
				val = (double)bits[0] + bits[1] * pow(2,32);
				fprintf(fp, "-%e ",val/pow(2, Q));
			}
			else
			{
				bits[0] = (m64.matr[i][j*4+1] << 16) | m64.matr[i][j*4];
				bits[1] = (m64.matr[i][j*4+3] << 16) | m64.matr[i][j*4+2];
				val = (double)bits[0] + bits[1] * pow(2,32);
				fprintf(fp, "%e ",val/pow(2, Q));
			}
		}
		fprintf(fp, "\n");
	}
}

void CreateMatrix(int nrows, int ncols, Matrix *m)
{
	int i; 
	/*Matrix m;*/
	m->nRows = nrows; 
	m->nCols = ncols; 
	
	m->matr = (int **)malloc(nrows * sizeof(int *)); 
	if (m->matr == NULL)
	{
		printf( "Insufficient memory available\n" );
		exit(0);
	}
	
	for(i = 0; i < nrows; i++)
	{
		m->matr[i] = (int *)malloc(ncols * sizeof(int)); 
		if (m->matr[i] == NULL)
		{
			printf( "Insufficient memory available\n" );
			exit(0);
		}
	}
}
void FreeMatrix(Matrix *m)
{
	int i;
	/*printf("test_m %d\n",m->nRows);*/
	
	for (i = 0; i < m->nRows; i++)
	{
		free(m->matr[i]);
	}
	free(m->matr);
}

void CreateMatrix64(int nrows, int ncols, Matrix64 *m)
{
	int i; 
	/*Matrix m;*/
	m->nRows = nrows; 
	m->nCols = ncols; 
	
	m->matr = (unsigned int **)malloc(nrows * sizeof(unsigned int *)); 
	if (m->matr == NULL)
	{
		printf( "Insufficient memory available\n" );
		exit(0);
	}
	
	for(i = 0; i < nrows; i++)
	{
		m->matr[i] = (unsigned int *)calloc(4 * ncols, sizeof(unsigned int)); 
		/* assign memories to 0 */
		if (m->matr[i] == NULL)
		{
			printf( "Insufficient memory available\n" );
			exit(0);
		}
	}
}
void FreeMatrix64(Matrix64 *m)
{
	int i;
	for (i = 0; i < m->nRows; i++)
	{
		free(m->matr[i]);
	}
	free(m->matr);
}

void MatrTOMatr64(Matrix *m, int Qm, Matrix64 *m64, int Qm64)
{
	int i,j;
	int n;
	unsigned int bits[2];

	n = m->nRows;
	
	for(i = 0; i < n; i++)
	{
		for(j = 0; j < 4*n; j = j+4)
		{
			bits[0] = m->matr[i][j/4] << (Qm64 - Qm);
			bits[1] = m->matr[i][j/4] >> (32-Qm64+Qm); /* Q10 -> Q24 */
			m64->matr[i][j] = bits[0] & 0x0000ffff; /* divide 64bits to 4 parts */
			m64->matr[i][j+1] = bits[0] >> 16;
			m64->matr[i][j+2] = bits[1] & 0x0000ffff;
			m64->matr[i][j+3] = bits[1] >> 16;
		}
	}
}

void Add64_Q(unsigned int *x, unsigned int *y, unsigned int *z) 
{
	/* here no check of overflow */
	int i;
	unsigned int carry = 0x00;
	
	for(i = 0; i < 4; i++)
	{
		z[i] =  x[i] + y[i] + carry;
		carry = z[i] >> 16;
		z[i] = z[i] & 0x0000ffff;
	}
}


unsigned int Add32_carry(unsigned int *x, unsigned int *y, unsigned int *z)
{
	/* x,y,z are all 32bits，return carry */
	unsigned int carry;
	
	if ((*x + *y) <= 0x0000ffff)
	{
		carry = 0;
		*z = *x + *y;
	}
	else
	{
		carry = 1;
		*z = *x + *y - 0x00010000;
	}
	return carry;
}
void Multiply64_Q(unsigned int *x, unsigned int *y, unsigned int *z, int Q)
{
	int ixx,iyy,ixy = 0;
	unsigned int carry = 0, carryAll = 0, carrytmp;
	unsigned int xx[4], yy[4], xy[20], zz[8];
	unsigned int signx = 0x00;
	unsigned int signy = 0x00;

	xx[0] = x[0];
	xx[1] = x[1];
	xx[2] = x[2];
	xx[3] = x[3];
	yy[0] = y[0];
	yy[1] = y[1];
	yy[2] = y[2];
	yy[3] = y[3];

	if ((x[3] & 0x00008000) != 0) 
	{
		ToCompCode(x, xx); /* x is positive, so convert negative value to its positive value */
		signx = 0x01;
	}
	if ((y[3] & 0x00008000) != 0) 
	{
		ToCompCode(y, yy);
		signy = 0x01;
	}
	signx = signx ^ signy;

	for (iyy = 0; iyy < 4; iyy++)
	{
		for (ixx = 0; ixx < 4; ixx++)
		{
			if (xx[ixx] * yy[iyy] + carry <= 0x0000ffff)
			{
				xy[ixy++] = (xx[ixx] * yy[iyy] + carry);
				carry = 0;
			}
			else
			{
				xy[ixy++] = (xx[ixx] * yy[iyy] + carry) & 0x0000ffff;
				carry = (xx[ixx] * yy[iyy] + carry) >> 16; /* 溢出 */
			}
		}
		xy[ixy++] = carry;
	}
	zz[0] = xy[0];   /* 算出z[0] */
	carryAll = Add32_carry(&xy[1], &xy[5], &zz[1]); /* 算出z[1] */
	carrytmp = carryAll;
	
	carryAll = Add32_carry(&xy[2], &xy[6], &zz[2]); /* 算出z[2] */
	carryAll += Add32_carry(&zz[2], &xy[10], &zz[2]);
	carryAll += Add32_carry(&zz[2], &carrytmp, &zz[2]);
	carrytmp = carryAll;
	
	carryAll = Add32_carry(&xy[3], &xy[7], &zz[3]); /* 算出z[3] */
	carryAll += Add32_carry(&zz[3], &xy[11], &zz[3]);
	carryAll += Add32_carry(&zz[3], &xy[15], &zz[3]);
	carryAll += Add32_carry(&zz[3], &carrytmp, &zz[3]);
	carrytmp = carryAll;
	
	carryAll = Add32_carry(&xy[4], &xy[8], &zz[4]); /* 算出z[4] */
	carryAll += Add32_carry(&zz[4], &xy[12], &zz[4]); 
	carryAll += Add32_carry(&zz[4], &xy[16], &zz[4]); 
	carryAll += Add32_carry(&zz[4], &carrytmp, &zz[4]); 
	carrytmp = carryAll;
	
	carryAll = Add32_carry(&xy[9], &xy[13], &zz[5]); /* 算出z[5] */
	carryAll += Add32_carry(&zz[5], &xy[17], &zz[5]); 
	carryAll += Add32_carry(&zz[5], &carrytmp, &zz[5]); 
	carrytmp = carryAll;
	
	carryAll = Add32_carry(&xy[14], &xy[18], &zz[6]); /* 算出z[6] */
	carryAll += Add32_carry(&zz[6], &carrytmp, &zz[6]); 
	
	zz[7] = xy[19] + carryAll; /* 算出z[7] */
	
	zz[0] = (zz[1] << 16) | zz[0]; /* 先合并一下，方便移位 */
	zz[1] = (zz[3] << 16) | zz[2];
	zz[2] = (zz[5] << 16) | zz[4];
	zz[3] = (zz[7] << 16) | zz[6];
	
	/* 对zz[3]->zz[0]右移Q位，将值赋给z */
	zz[0] = zz[0] >> Q;
	zz[0] = (zz[1] << (32-Q)) | zz[0];
	
	zz[1] = zz[1] >> Q;
	zz[1] = (zz[2] << (32-Q)) | zz[1];
	
	if(zz[3] != 0 || (zz[2] >> Q) != 0) /* 乘法结果溢出64位，可以去掉试试 */
	{
		z[0] = 0x0000ffff; /* 溢出保护 */
		z[1] = 0x0000ffff; 
		z[2] = 0x0000ffff;
		z[3] = 0x00007fff;
	}
	else 
	{
		z[0] = zz[0] & 0x0000ffff;	/* 低位 */
		z[1] = zz[0] >> 16;	        /* 高位 */
		z[2] = zz[1] & 0x0000ffff;
		z[3] = zz[1] >> 16;
	}
	if (signx == 0x01) /* the sign is 1, then transfer to complement code */
		ToCompCode(z, z);
}

void Divide64_Q(unsigned int *x, unsigned int *y, unsigned int *z, int Q)
{
	unsigned int xtmp[4], ytmp[4]; /* 用于将x,y的符号位去掉 */
	unsigned int xx[3], yy[2], zz[3], dividend[2];
	unsigned int yytmp[4], dividendtmp[4];

	unsigned int flag = (x[3] & 0x00008000) ^ (y[3] & 0x00008000);
	
	int idx = 0;
	int Flag_start = 0;
	
	if ((x[3] & 0x00008000) != 0)
	{
		ToCompCode(x, xtmp);
		xtmp[0] = xtmp[0] | (xtmp[1] << 16); 
		xtmp[1] = xtmp[2] | (xtmp[3] << 16);	
	}
	else
	{
		xtmp[0] = x[0] | (x[1] << 16); 
		xtmp[1] = x[2] | (x[3] << 16);
	}
	if ((y[3] & 0x00008000) != 0)
	{
		ToCompCode(y, ytmp);
		ytmp[0] = ytmp[0] | (ytmp[1] << 16);
		ytmp[1] = ytmp[2] | (ytmp[3] << 16);
	}
	else
	{
		ytmp[0] = y[0] | (y[1] << 16); 
		ytmp[1] = y[2] | (y[3] << 16);