|
发表于 2014-12-14 14:52:19
|
显示全部楼层
Gauss消元法啊。
- CAL_STATUS MATRIX_Inv(MATRIX_TypeDef* A, MATRIX_TypeDef* res)
- {
- int i, j, k;
- float m, det = 1, tempBuf[A->nCol*A->nRow];
- if((A->nRow != res->nRow) || (A->nCol != res->nCol) || (A->nCol != A->nRow))
- return CAL_ERROR;
- /*init Buf*/
- for(i = 0; i < A->nCol*A->nCol; i++)
- *(res->pData) = 0.0;
- for(i = 0; i< A->nCol; i++)
- *(res->pData + i + i*A->nRow) = 1;
- for(i = 0; i < A->nCol*A->nCol; i++)
- tempBuf[i] = *(A->pData + i);
- /*Gauss*/
- for(i = 0; (i < A->nCol-1); i++)
- {
- for(k = i+1; k < A->nCol; k++ )
- {
- /*Calculate M*/
- m = *(A->pData + i + k*A->nCol) / *(A->pData + i + i*A->nCol);
- /*Updata Row*/
- for(j = i; j < A->nRow; j++)
- *(A->pData + j + k*A->nCol) -= m * *(A->pData + j + i*A->nCol);
- for(j = 0; j < A->nRow; j++)
- *(res->pData + j + k*A->nCol) -= m * *(res->pData + j + i*A->nCol);
- }
- }
- /*Check det*/
- for(i = 0; i< A->nCol; i++)
- det *= *(A->pData + i + i*A->nRow);
- if(det < 0.00014)
- return CAL_ERROR;
- /*Call Back*/
- for(i = A->nCol-1; i > 0; i--)
- {
- for(k = i-1; k >= 0; k--)
- {
- m = *(A->pData + i + k*A->nCol) / *(A->pData + i + i*A->nCol);
- *(A->pData + i + k*A->nCol) -= m * *(A->pData + i + i*A->nCol);
- for(j = 0; j < A->nCol; j++)
- *(res->pData + j + k*A->nCol) -= m * *(res->pData + j + i*A->nCol);
- }
- }
- for(i = 0; i < A->nRow; i++)
- {
- for(k = 0; k < A->nRow; k++ )
- *(res->pData + k + i*A->nCol) /= *(A->pData + i + i*A->nCol);
- }
- for(i = 0; i < A->nCol*A->nCol; i++)
- *(A->pData + i) = tempBuf[i];
- return CAL_OK;
- }
复制代码
矩阵的定义是:
- typedef struct
- {
- uint8_t nRow; //行数
- uint8_t nCol; //列数
- float* pData; //数据
- } MATRIX_TypeDef;
复制代码
|
|