矩陣變換是機器人學的基礎，所以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;
}