自寫的C語言矩陣簡易運算庫

因爲機器人相關的基本運算中很多都是矩陣運算，雖然C++有現成的Eigen庫，ROS中的矩陣運算也是基於Eigen庫的，但是我目前想自己做一做這個底層驅動，涉及正逆運動學、關節速度規劃、空間姿態插補算法等，並且我現有的單片機不支持這個Eigen庫，所以就寫了一個簡單的基於Ｃ語言的矩陣運算庫，滿足基本的矩陣運算需求。不過缺陷還是很明顯的，只適用於學習交流。
我主要寫了如下幾個功能的子函數：

• 創建矩陣；
• 初始化矩陣(將所有元素初始化爲０)；
• 釋放申請的矩陣空間；
• 給矩陣每個元素賦值；
• 打印矩陣；
• 矩陣加減法；
• 矩陣轉置；
• 矩陣乘法；
• 創建單位矩陣；
• 取出矩陣第n行第m列的元素；
• 順序高斯消去法；
• 列主元高斯消去法；
• 向量叉乘；
• 矩陣求逆，利用矩陣LU分解求逆（直接三角分解）；
• 矩陣求逆，利用初等行變換求逆；
• 求矩陣行列式的值，通過行變換（列主元消去法）將矩陣變換成上三角矩陣；
• 對一個矩陣進行復制；

未來還會根據需要繼續更新……

頭文件如下：

/*
 * MyMatrix.h
 *
 *  Created on: Jul 13, 2019
 *      Author: xuuyann
 */

#ifndef HEADER_MYMATRIX_H_
#define HEADER_MYMATRIX_H_
/*
 * 問題記錄：
 *　* 存在內存濫用的現象，因爲每次創建矩陣都會申請一塊新內存，並沒有free掉
 *　* LU分解求逆不太熟
 */
typedef struct MNode *PtrToMNode;
struct MNode
{
	int row;
	int column;
	double **data;
};
typedef PtrToMNode Matrix;
// 釋放申請的矩陣空間
void free_Matrix(Matrix mat);
//　創建矩陣
Matrix Create_Matrix(int row, int column);
// 初始化矩陣(將所有元素初始化爲０)
void Init_Matrix(Matrix mat);
// 給矩陣每個元素賦值
void SetData_Matrix(Matrix mat, double data[]);
//　打印矩陣
void Show_Matrix(Matrix mat, char *s);
//　矩陣加減法
Matrix AddorSub_Matrix(Matrix mat_1, Matrix mat_2, int flag);
//　矩陣轉置
Matrix Trans_Matrix(Matrix mat);
// 矩陣乘法
Matrix Mult_Matrix(Matrix mat_1, Matrix mat_2);
//　創建單位矩陣
Matrix eye(int n);
//　取出矩陣某行某列的元素
double PickInMat(Matrix mat, int r, int c);
//　順序高斯消去法
Matrix Gauss_shunxu(Matrix A, Matrix b);
//　列主元高斯消去法
Matrix Gauss_lie(Matrix A, Matrix b);
// 向量叉乘
Matrix Cross(Matrix a, Matrix b);

/*
 * 矩陣求逆，利用矩陣LU分解求逆（直接三角分解）
 * 該方法來源於順序高斯消元法，沒有進行選主元，可能產生數值不穩定的現象
 */
Matrix LUInverse_Matrix(Matrix mat);

// 矩陣求逆，利用初等行變換求逆
Matrix EleTransInv_Matrix(Matrix mat);
// 矩陣第n行與第m行互換
void Swap_row(Matrix mat, int n, int m);

/*
 * 求矩陣行列式的值，通過行變換（列主元消去法）將矩陣變換成上三角矩陣
 * 注意行變換會改變行列式的符號
 */
double Det_Matrix(Matrix mat);
//　伴隨矩陣(還沒寫)
Matrix Adj_Matrix(Matrix mat);
//　對一個矩陣進行復制
Matrix Copy_Matrix(Matrix mat);

/*
 * 利用旋轉矩陣的公式來計算齊次變換矩陣的逆
 * 機器人學專用(目前還沒寫)
 */
Matrix Rot_inv(Matrix T);


#endif /* HEADER_MYMATRIX_H_ */

MyMatrix.c如下：

/*
 * MyMatrix.c
 *
 *  Created on: Jul 13, 2019
 *      Author: xuuyann
 */

#include "../header/MyMatrix.h"
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>

void Init_Matrix(Matrix mat)
{
	int i, j;
	for (i = 0; i < mat->row; i++){
		for (j = 0; j < mat->column; j++){
			mat->data[i][j] = 0;
		}
	}
}

// 釋放申請的矩陣空間
void Free_Matrix(Matrix mat)
{
	for (int i = 0; i < mat->row; i++)
		free(mat->data[i]); // 釋放行指針
	free(mat->data); // 釋放頭指針
	free(mat); // 釋放結構體指針
}

Matrix Create_Matrix(int row, int col)
{
	Matrix mat;
	mat = (Matrix)malloc(sizeof(struct MNode)); //　分配結構體指針
	if (row <= 0 || col <= 0){
		printf("ERROR, in creat_Matrix the row or col <= 0\n");
		exit(1);
	}
	if (row > 0 && col >0){
		mat->row = row;
		mat->column = col;
		mat->data = (double **)malloc(row*sizeof(double *));// 分配頭指針
		if (mat->data == NULL){
			printf("ERROR, in creat_Matrix the mat->data == NULL\n");
			exit(1);
		}
		int i;
		for (i = 0; i < row; i++ ){
			*(mat->data + i) = (double *)malloc(col*sizeof(double)); //　分配每行的指針
			if (mat->data[i] == NULL){
				printf("ERROR, in create_Matrix the mat->data[i] == NULL\n");
				exit(1);
			}
		}
		Init_Matrix(mat);
	}
	return mat;
}

void Show_Matrix(Matrix mat, char *s)
{
	int i, j;
	printf("%s\n", s);
	for (i = 0; i < mat->row; i++){
		for (j = 0; j < mat->column; j++)
			printf("%.6f\t", mat->data[i][j]);
		printf("\n");
	}
	printf("\n");
}

void SetData_Matrix(Matrix mat, double data[])
{
	int i, j;
	for (i = 0; i < mat->row; i++){
		for (j = 0; j < mat->column; j++){
			mat->data[i][j] = data[i*mat->column + j];
		}
	}
}

//flag = 0, add; flag = 1, sub
Matrix AddorSub_Matrix(Matrix mat_1, Matrix mat_2, int flag)
{
	Matrix rst_mat;
	if (mat_1->column != mat_2->column){
		printf("ERROR in AddorSub, column !=\n");
		exit(1);
	}
	if (mat_1->row != mat_2->row){
		printf("ERROR in AddorSub, row !=\n");
		exit(1);
	}
	int i, j;
	rst_mat = Create_Matrix(mat_1->row, mat_1->column);
	for (i = 0; i < mat_1->row; i++){
		for (j = 0; j < mat_1->column; j++)
			rst_mat->data[i][j] = mat_1->data[i][j] + pow(-1, flag)*mat_2->data[i][j];
	}
	return rst_mat;
}

//轉置
Matrix Trans_Matrix(Matrix mat)
{
	Matrix mat_;
	int i, j;
	mat_ = Create_Matrix(mat->row, mat->column);
	for (i = 0; i < mat->row; i ++){
		for (j = 0; j < mat->column; j++)
			mat_->data[i][j] = mat->data[i][j];
	}
	return mat_;
}

Matrix Mult_Matrix(Matrix mat_1, Matrix mat_2)
{
	Matrix rst_mat;
	int i, j, m;
	if (mat_1->column != mat_2->row){
		printf("ERROR in Mult_Matrix, column != row\n");
		exit(1);
	}else{
		rst_mat = Create_Matrix(mat_1->row, mat_2->column);
		for (i = 0; i < mat_1->row; i++){
			for (j = 0; j < mat_2->column; j++){
				for (m = 0; m < mat_1->column; m++)
					rst_mat->data[i][j] += mat_1->data[i][m] * mat_2->data[m][j];
			}
		}
	}
	return rst_mat;
}

Matrix eye(int n)
{
	Matrix E;
	int i, j;
	if (n <= 0){
		printf("ERROR in eye\n");
		exit(1);
	}
	E = Create_Matrix(n, n);
	for (i = 0; i < n; i++){
		for (j = 0; j < n; j++){
			if (i == j)
				E->data[i][j] = 1;
			else
				E->data[i][j] = 0;
		}
	}
	return E;
}

double PickInMat(Matrix mat, int r, int c)
{
	double rst;
	rst = mat->data[r - 1][c - 1];
	return rst;
}

//　順序高斯消去法
Matrix Gauss_shunxu(Matrix A, Matrix b)
{
	int n;
	n = b->row;
	Matrix x;
	x = Create_Matrix(b->row, b->column);
	for (int i = 0; i < n; i++){
	        for (int j = 0; j < n; j++)
	            if (A->data[i][j] == 0){
	                printf("can't use the Gauss_shunxu\n");
	                exit(1);
	            }
	    }
	    // 消元
	    double L[n];
	    for (int k = 0; k < n - 1; k++){
	        for (int i = k + 1; i < n; i++){
	            L[i] = A->data[i][k] / A->data[k][k];
	            for (int j = k + 1; j < n; j++)
	            	A->data[i][j] = A->data[i][j] - L[i]*A->data[k][j];
	            b->data[i][0] = b->data[i][0] - L[i] * b->data[k][0];
	        }
	    }
	    for (int i = 0; i < n; i++){
	    	if (A->data[i][i] == 0){
	    		printf("can't use the Gauss_shunxu\n");
	    		exit(1);
	    	}
	    }
	    // 回代
	    x->data[n - 1][0] = b->data[n - 1][0] / A->data[n - 1][n - 1];
	    for (int i = n - 2; i >= 0; i--){
	        double sum_a = 0;
	        for (int j = i + 1; j < n; j ++)
	            sum_a += A->data[i][j] * x->data[j][0];
	        x->data[i][0] = (b->data[i][0] - sum_a) / A->data[i][i];
	    }
	    return x;
}

//　列主元高斯消去法
Matrix Gauss_lie(Matrix A, Matrix b)
{
	int n;
	n = b->row;
	Matrix x;
	x = Create_Matrix(b->row, b->column);
	double L[n];
	    for (int k = 0; k < n - 1; k++){
	        int cnt = k;
	        double a_max = fabs(A->data[k][k]);
	        for (int i = k; i < n; i++){
	            if (fabs(A->data[i][k]) > a_max){
	                a_max = fabs(A->data[i][k]);
	                cnt = i; // 確定下標i
	            }
	        }
	        if (A->data[cnt][k] == 0){
	            printf("Gauss_lie: no unique solution\n");
	            exit(1);
	        }
	        // 換行
	        if (cnt != k){
	            double t = 0, s = 0;
	            for (int j = k; j < n; j++){
	                t = A->data[k][j];
	                A->data[k][j] = A->data[cnt][j];
	                A->data[cnt][j] = t;
	                s = b->data[cnt][0];
	                b->data[cnt][0] = b->data[k][0];
	                b->data[k][0] = s;
	            }
	        }
	        // step 5
	        for (int i = k + 1; i < n; i++){
	            L[i] = A->data[i][k] / A->data[k][k];
	            for (int j = k + 1; j < n; j++)
	            	A->data[i][j] = A->data[i][j] - L[i]*A->data[k][j];
	            b->data[i][0] = b->data[i][0] - L[i] * b->data[k][0];
	        }
	    }
	    for (int i = 0; i < n; i++){
	    	if (A->data[i][i] == 0.0){
	    		printf("Gauss_lie: no unique solution\n");
	            exit(1);
	    	}
	    }
	    // 回代
	    x->data[n - 1][0] = b->data[n - 1][0] / A->data[n - 1][n - 1];
	    for (int i = n - 2; i >= 0; i--){
	        double sum_a = 0;
	        for (int j = i + 1; j < n; j ++)
	            sum_a += A->data[i][j] * x->data[j][0];
	        x->data[i][0] = (b->data[i][0] - sum_a) / A->data[i][i];
	    }
	    return x;
}

// 3×3向量叉乘
Matrix Cross(Matrix a, Matrix b)
{
	Matrix C;
	C = Create_Matrix(a->row, a->column);
	double ax, ay, az;
	double bx, by, bz;
	double c[a->row];
	ax = PickInMat(a, 1, a->column);
	ay = PickInMat(a, 2, a->column);
	az = PickInMat(a, 3, a->column);
	bx = PickInMat(b, 1, b->column);
	by = PickInMat(b, 2, b->column);
	bz = PickInMat(b, 3, b->column);
	c[0] = ay*bz - az*by;
	c[1] = az*bx - ax*bz;
	c[2] = ax*by - ay*bx;
	SetData_Matrix(C, c);
	return C;
}

// 矩陣求逆，利用矩陣LU分解求逆（直接三角分解）
Matrix LUInverse_Matrix(Matrix mat)
{
	Matrix inv;
	if (mat->column != mat->row){
		printf("ERROR in inverse, the row != the column\n");
		exit(1);
	}
	if (Det_Matrix(mat) == 0){
		printf("The Matrix is not invertible\n");
		exit(1);
	}
	// Ｌ矩陣和Ｕ矩陣
	Matrix L, U;
	int n = mat->column;
	inv = Create_Matrix(n, n);
	L = Create_Matrix(n, n);
	U = Create_Matrix(n, n);
	// 計算Ｕ的第一行元素
	for (int j = 0; j < n; j++)
		U->data[0][j] = mat->data[0][j];
	//　計算Ｌ的第一列元素
	for (int i = 0; i < n; i++){
		L->data[i][0] = mat->data[i][0] / U->data[0][0];
		L->data[i][i] = 1.0;
	}
	double sum_u = 0, sum_l = 0;
	//　分別計算Ｕ和Ｌ的第２到ｎ行、列元素
	for (int k = 1; k < n; k++){
		// 求U，U矩陣從第２行迭代到第n行，且U矩陣先於L矩陣一個節拍
		for (int j = k; j < n; j++){
			sum_u = 0;
			for (int m = 0; m <= k - 1; m++)
				sum_u += L->data[k][m] * U->data[m][j];
			U->data[k][j] = mat->data[k][j] - sum_u;
		}
		//　求Ｌ，L矩陣從第２列迭代到第n列
		for (int i = k + 1; i < n; i++){
			sum_l = 0;
			for (int m = 0; m <= k - 1; m++)
				sum_l += L->data[i][m] * U->data[m][k];
			L->data[i][k] = (mat->data[i][k] - sum_l) / U->data[k][k];
		}
	}
	Show_Matrix(U, "U");
	Show_Matrix(L, "L");
	// 分別求下三角矩陣Ｌ和上三角矩陣Ｕ的逆矩陣
	Matrix L_, U_;
	L_ = Create_Matrix(n ,n);
	U_ = Create_Matrix(n, n);
	// 求矩陣Ｕ的逆
	double sum_u_;
	for (int i = 0; i < n; i++){
		U_->data[i][i] = 1.0 / U->data[i][i];// 對角線元素的值直接取倒數
		for (int j = i - 1; j >= 0; j--){
			sum_u_ = 0;
			for (int k = j + 1; k <= i; k++)
				sum_u_ += U->data[j][k] * U_->data[k][i];
			U_->data[j][i] = -sum_u_ / U->data[j][j]; // 迭代計算，按列倒序依次得到每一個值
		}
	}
	// 求Ｌ的逆
	for (int i = 0; i < n; i++){
		L_->data[i][i] = 1; // 對角線元素的值直接取倒數，這裏爲１
		for (int k = i + 1; k < n; k++){
			for (int j = i; j <= k - 1; j++)
				L_->data[k][i] -= L->data[k][j] * L_->data[j][i]; // 迭代計算，按列順序依次得到每一個值
		}
	}
	//Show_Matrix(U_, "U_");
	//Show_Matrix(L_, "L_");
	// 已經得到Ｌ和Ｕ的逆矩陣
	inv = Mult_Matrix(U_, L_);

	Free_Matrix(L_);
	Free_Matrix(U_);
	Free_Matrix(L);
	Free_Matrix(U);
	return inv;
}

// 矩陣第n行與第m行互換
void Swap_row(Matrix mat, int n, int m)
{
	double temp;
	for (int i = 0; i < mat->column; i++){
		temp = mat->data[n - 1][i];
		mat->data[n - 1][i] = mat->data[m - 1][i];
		mat->data[m - 1][i] = temp;
	}
}

// 矩陣求逆，利用初等行變換求逆
Matrix EleTransInv_Matrix(Matrix mat)
{
	if (mat->column != mat->row){
		printf("ERROR in inverse, the row != the column\n");
		exit(1);
	}
	if (Det_Matrix(mat) == 0){
		printf("The Matrix is not invertible\n");
		exit(1);
	}
	int n = mat->row;
	// 爲防止改變原矩陣，此處進行復制處理
	Matrix mat_ = Copy_Matrix(mat);
	//　創建單位矩陣
	Matrix inv_eye = eye(n);
	double e, a_max;
	int i, j, k, t, cnt;
	for (k = 0; k < n - 1; k++){
		a_max = fabs(mat_->data[k][k]);
		cnt = k;
		// 選主元
		for (i = k; i < n; i++){
			if (fabs(mat_->data[i][k]) > a_max){
				a_max = fabs(mat_->data[i][k]);
				cnt = i;
			}
		}
		//　換行，原矩陣換行的同時，單位矩陣也換行
		double temp, temp_e;
		if (cnt != k){
			for (j = 0; j < n; j++){
				temp = mat_->data[k][j];
				mat_->data[k][j] = mat_->data[cnt][j];
				mat_->data[cnt][j] = temp;
				temp_e = inv_eye->data[k][j];
				inv_eye->data[k][j] = inv_eye->data[cnt][j];
				inv_eye->data[cnt][j] = temp_e;
			}
		}
		// 消元
		for (i = k + 1; i < n; i++){
			e = mat_->data[i][k] / mat_->data[k][k];
			for (j = 0; j < n; j++){
				mat_->data[i][j] = mat_->data[i][j] - e * mat_->data[k][j];
				inv_eye->data[i][j] = inv_eye->data[i][j] - e * inv_eye->data[k][j];
			}
		}
	}
	// 將矩陣每行的行首元素化爲１
	for (i = 0; i < n; i++){
		e = 1.0 / mat_->data[i][i];
		for (j = 0; j < n; j++){
			mat_->data[i][j] = mat_->data[i][j] * e;
			inv_eye->data[i][j] = inv_eye->data[i][j] * e;
		}
	}
	// 從最後一排開始消元，把增廣矩陣左邊的矩陣化爲單位矩陣
	for (i = n - 1; i > 0; i--){
		for (j = i - 1; j >= 0; j--){
			e = mat_->data[j][i] / mat_->data[i][i];
			for (t = 0; t < n; t++){
				mat_->data[j][t] = mat_->data[j][t] - e*mat_->data[i][t];
				inv_eye->data[j][t] = inv_eye->data[j][t] - e*inv_eye->data[i][t];
			}
		}
	}
	Free_Matrix(mat_);
	return inv_eye;
}

/*
 * 通過行變換（列主元消去法）將矩陣變換成上三角矩陣
 * 注意行變換會改變行列式的符號
 */
double Det_Matrix(Matrix mat)
{
	double det = 1.0;
	if (mat->row != mat->column){
		printf("ERROR in Det_Matrix, the row != the column\n");
		exit(1);
	}
	if (mat->row == 1 && mat->column == 1)
		return mat->data[0][0];
	// 爲防止改變原矩陣，此處進行復制處理
	Matrix mat_ = Copy_Matrix(mat);
	int n = mat_->row, s = 0, cnt;
	double L, a_max;
	for (int k = 0; k < n - 1; k++){
		cnt = k;
		a_max = fabs(mat_->data[k][k]);
		for (int i = k; i < n; i++){
			if (fabs(mat_->data[i][k]) > a_max){
				a_max = fabs(mat_->data[i][k]);
				cnt = i; // 確定下標i
			}
		}
		//　換行
		double temp;
		if (cnt != k){
			s++; // 換行次數
			for (int j = 0; j < n; j++){
				temp = mat_->data[cnt][j];
				mat_->data[cnt][j] = mat_->data[k][j];
				mat_->data[k][j] = temp;
			}
		}
		// 消元計算
		for (int i = k + 1; i < n; i++){
			L = mat_->data[i][k] / mat_->data[k][k];
			for (int j = k + 1; j < n; j++)
				mat_->data[i][j] = mat_->data[i][j] - L*mat_->data[k][j];
		}
	}
	if (s % 2 == 0)
		s = 1;
	else
		s = -1;
	for (int i = 0; i < n; i++)
		det *= mat_->data[i][i];
	det *= s;
	Free_Matrix(mat_); //釋放掉複製矩陣的內存
	return det;
}

//　伴隨矩陣
Matrix Adj_Matrix(Matrix mat)
{
	Matrix adj;

	return adj;
}

//　對一個矩陣進行復制
Matrix Copy_Matrix(Matrix mat)
{
	Matrix copy_mat = Create_Matrix(mat->row, mat->column);
	for (int i = 0; i < mat->row; i++){
		for (int j = 0; j < mat->column; j++)
			copy_mat->data[i][j] = mat->data[i][j];
	}
	return copy_mat;
}

測試main.c和測試結果如下:

#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include "../header/MyMatrix.h"
#define PI 3.141592653

int main()
{
	//測試矩陣運算庫
	Matrix A, A_, A__, b;
	A = Create_Matrix(3, 3);
	b = Create_Matrix(3, 1);
	double coef[9] = {0.0120, 0.01, 0.167,
				 	  1.0, 0.8334, 5.91,
					  3200, 1200, 4.2};
	double b_[3] = {0.6781, 12.1, 983.3};
	SetData_Matrix(A, coef);
	SetData_Matrix(b, b_);
	A_ = Copy_Matrix(A);
	A__ = Copy_Matrix(A);
	Show_Matrix(A, "A = ");
	Show_Matrix(b, "b = ");
	Show_Matrix(A_, "A_ = ");
	Show_Matrix(A__, "A__ = ");
	printf("A(1, 1) = %f, A(2, 3) = %f, A(3, 3) = %f\n", PickInMat(A, 1, 1), PickInMat(A, 2, 3), PickInMat(A, 3, 3));
	Matrix x, x_;
	x = Create_Matrix(b->row, b->column);
	x_ = Create_Matrix(b->row, b->column);
	x = Gauss_lie(A_, b);
	x_ = Gauss_shunxu(A__, b);
	Show_Matrix(x, "Gauss_lie solution x = ");
	Show_Matrix(x, "Gauss_shunxu solution x = ");
	double det = Det_Matrix(A);
	printf("det = %f\n", det);
	Matrix inv = Create_Matrix(3, 3);
//	inv = LUInverse_Matrix(A);
	inv = EleTransInv_Matrix(A);
	Show_Matrix(inv, "inv:");
	Free_Matrix(A);
	Free_Matrix(A_);
	Free_Matrix(A__);
	Free_Matrix(b);
	Free_Matrix(inv);
	Free_Matrix(x);
	Free_Matrix(x_);
	return 0;
}

測試結果：

參考：
基於C語言的矩陣運算庫
矩陣求逆-高斯消元法介紹及其實現
C語言求矩陣的行列式、伴隨矩陣、逆矩陣
矩陣LU分解求逆詳細分析與C語言實現