inv_fixed.rar_c++ 矩阵相乘_fixed_inv_fixed_定点 求逆_矩阵求逆

#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); } xx[2] = xtmp[1] >> (32 - Q)