matrix_functions.c

来自「f16的非线性模型设计」· C语言 代码 · 共 119 行 · 第 1/5 页

void matrix_sub(double a[3][3], double b[3][3], double result[3][3])
{
   int i, j;
   for(i=0; i<3; i++)
   {
       for (j=0; j<3; j++)
       {
		  result[i][j] = a[i][j] - b[i][j];
       }
   }
}

void matrix_inverse(double a[3][3], double ainv[3][3])
{
    double det;
    
    det = a[0][0]*(a[1][1]*a[2][2]-a[2][1]*a[1][2])
        - a[0][1]*(a[1][0]*a[2][2]-a[2][0]*a[1][2])
        + a[0][2]*(a[1][0]*a[2][1]-a[2][0]*a[1][1]);
    
    ainv[0][0] = (a[1][1]*a[2][2]-a[2][1]*a[1][2])/det;
    ainv[1][0] = (a[1][2]*a[2][0]-a[2][2]*a[1][0])/det;
    ainv[2][0] = (a[1][0]*a[2][1]-a[2][0]*a[1][1])/det;