shoes_project/motion_version/motion_03_14/matrixOperations.c

#include "matrixOperations.h"

void matrix_inverse(float (*a)[N],float (*b)[N])
{
    int i,j,k;
    float max,temp;
    // 定义一个临时矩阵t
    float t[N][N];
    // 将a矩阵临时存放在矩阵t[n][n]中
    for(i=0;i<N;i++)
    {
        for(j=0;j<N;j++)
        {
            t[i][j] = a[i][j];
        }
    }
    // 初始化B矩阵为单位矩阵
    for(i=0;i<N;i++)
    {
        for(j=0;j<N;j++)
        {
            b[i][j] = (i == j)?(float)1:0;
        }
    }
    // 进行列主消元，找到每一列的主元
    for(i=0;i<N;i++)
    {
        max = t[i][i];
        // 用于记录每一列中的第几个元素为主元
        k = i;
        // 寻找每一列中的主元元素
        for(j=i+1;j<N;j++)
        {
            if(fabsf(t[j][i]) > fabsf(max))
            {
                max = t[j][i];
                k = j;
            }
        }
        //cout<<"the max number is "<<max<<endl;
        // 如果主元所在的行不是第i行，则进行行交换
        if(k != i)
        {
            // 进行行交换
            for(j=0;j<N;j++)
            {
                temp = t[i][j];
                t[i][j] = t[k][j];
                t[k][j] = temp;
                // 伴随矩阵B也要进行行交换
                temp = b[i][j];
                b[i][j] = b[k][j];
                b[k][j] = temp;
            }
        }
        if(t[i][i] == 0)
        {
            break;
        }
        // 获取列主元素
        temp = t[i][i];
        // 将主元所在的行进行单位化处理
        //cout<<"\nthe temp is "<<temp<<endl;
        for(j=0;j<N;j++)
        {
            t[i][j] = t[i][j]/temp;
            b[i][j] = b[i][j]/temp;
        }
        for(j=0;j<N;j++)
        {
            if(j != i)
            {
                temp = t[j][i];
                //消去该列的其他元素
                for(k=0;k<N;k++)
                {
                    t[j][k] = t[j][k] - temp*t[i][k];
                    b[j][k] = b[j][k] - temp*b[i][k];
                }
            }

        }

    }
}