C++设计矩阵，实现矩阵相乘和求逆矩阵

矩阵变换是机器人学的基础，所以Jungle把这一节内容划分到“工业机器人”栏目。这一节Jungle用C++设计了矩阵的类Matrix，并设计了3个方法：

• 矩阵相加add
• 矩阵相乘multiply
• 求矩阵的逆矩阵inverse

有了这三个方法，足以进行机器人正逆运动学分析了。
直接上代码：

#include<iostream>
#include<stdlib.h>
#include<math.h>

class Matrix
{
public:
	Matrix(double a11, double a12, double a13, double a14,
		   double a21, double a22, double a23, double a24,
		   double a31, double a32, double a33, double a34,
		   double a41, double a42, double a43, double a44);
	Matrix();
	~Matrix();

	//矩阵相乘
	static Matrix mutiply(Matrix A,Matrix B);
	//矩阵相加
	static Matrix add(Matrix A,Matrix B);
	//矩阵的逆
	static Matrix inverse(Matrix A);

	//二维数组，存储矩阵元素
	double M[4][4];
};

#include "Matrix.h"

Matrix::Matrix()
{
	for(int i=0;i<4;i++)
	{
		for(int j=0;j<4;j++)
			this->M[i][j] = 0.00;
	}
}

Matrix::Matrix(
			   double a11, double a12, double a13, double a14, 
			   double a21, double a22, double a23, double a24, 
			   double a31, double a32, double a33, double a34, 
			   double a41, double a42, double a43, double a44)
{
	M[0][0] = a11; M[0][1] = a12; M[0][2] = a13; M[0][3] = a14;
	M[1][0] = a21; M[1][1] = a22; M[1][2] = a23; M[1][3] = a24;
	M[2][0] = a31; M[2][1] = a32; M[2][2] = a33; M[2][3] = a34;
	M[3][0] = a41; M[3][1] = a42; M[3][2] = a43; M[3][3] = a44;
}

Matrix::~Matrix()
{

}

// Function:两个矩阵相乘
// para:A-矩阵A
// para:B-矩阵B
// return：矩阵AB
Matrix Matrix::mutiply(Matrix A, Matrix B)
{
	Matrix Result;
	for(int m=0;m<4;m++)
	{  
		for(int s=0;s<4;s++)
		{
			for(int n=0;n<4;n++)
			{  
				Result.M[m][s]+=A.M[m][n]*B.M[n][s];  
			}  
		}  
	}
	return Result;
}

// Function:两个矩阵相加
// para:A-矩阵A
// para:B-矩阵B
// return：矩阵AB
Matrix Matrix::add(Matrix A, Matrix B)
{
	Matrix Result;
	for(int m=0;m<4;m++)
	{  
		for(int s=0;s<4;s++)
		{
			Result.M[m][s]=A.M[m][s]+B.M[m][s]; 
		}  
	}
	return Result;
}

// Function:求矩阵的逆
// para:A-矩阵A
// return：矩阵A的逆矩阵
Matrix Matrix::inverse(Matrix A)
{
	double E_Matrix[4][4];
	double mik;
	for(int i = 0; i < 4; i++)
	{
		for(int j = 0; j < 4; j++)
		{
			if(i == j)
				E_Matrix[i][j] = 1.00;
			else
				E_Matrix[i][j] = 0.00;
		}
	}
	double CalcuMatrix[4][8];
	for(int i = 0; i < 4; i++)
	{
		for(int j = 0; j < 4; j++)
		{
			CalcuMatrix[i][j] = A.M[i][j];
		}
		for(int k = 4; k < 8; k++)
		{
			CalcuMatrix[i][k] = E_Matrix[i][k-4];
		}
	}

	for(int i = 1; i <= 4-1; i++)
	{
		for(int j = i+1; j <= 4; j++)
		{
			mik = CalcuMatrix[j-1][i-1]/CalcuMatrix[i-1][i-1];
			for(int k = i+1;k <= 8; k++)
			{
				CalcuMatrix[j-1][k-1] -= mik*CalcuMatrix[i-1][k-1];
			}
		}
	}
	for(int i=1;i<=4;i++)
	{
		double temp = CalcuMatrix[i-1][i-1];
		for(int j=1;j<=8;j++)
		{
			CalcuMatrix[i-1][j-1] = CalcuMatrix[i-1][j-1]/temp;
		}
	}
	for(int k=4-1;k>=1;k--)
	{
		for(int i=k;i>=1;i--)
		{
			mik = CalcuMatrix[i-1][k];
			for(int j=k+1;j<=8;j++)
			{
				CalcuMatrix[i-1][j-1] -= mik*CalcuMatrix[k][j-1];
			}
		}
	}
	double InverseMatrix[4][4];
	for(int i=0;i<4;i++)
	{
		for(int j=0;j<4;j++)
		{
			InverseMatrix[i][j] = CalcuMatrix[i][j+4];
		}
	}

	Matrix Result;
	for(int i=0;i<4;i++)
	{
		for(int j=0;j<4;j++)
		{
			if(fabs(InverseMatrix[i][j]) < 0.0000001)
				InverseMatrix[i][j] = 0.00;
			Result.M[i][j] = InverseMatrix[i][j];
		}
	}
	return Result;
}

欢迎关注知乎专栏：Jungle是一个用Qt的工业Robot

欢迎关注Jungle的微信公众号：Jungle笔记