matrixprocess.cpp

一些基本的矩阵操作

#include "matrixProcess.h"
#include "math.h"

//---------------------------------------------------------------------------
// name: matrixRank2
// Input:	
// 			double* squareMatrix	二维矩阵
// 			int		dimension		2
// preconditions: 
// process: 求二维矩阵的秩
// output:	resultRank	二维矩阵的秩，取值为0，1，2
// postcondition:
// function_index:
// author:	chenjing
// created:	
//---------------------------------------------------------------------------
int matrixRank2(double* squareMatrix,int dimension)
{
	int resultRank = 2;	
	if (squareMatrix[0]*squareMatrix[3]-squareMatrix[1]*squareMatrix[2]+1.0==1.0)
	{
		resultRank = 1;
		if (squareMatrix[0] + 1.0 == 1.0
		&& squareMatrix[1] + 1.0 == 1.0
		&& squareMatrix[2] + 1.0 == 1.0
		&& squareMatrix[3] + 1.0 == 1.0)
			resultRank = 0;			
	}
	return resultRank;
}

//---------------------------------------------------------------------------
// name: columnCross3
// Input: 
// 		double* column1		 列1（或行1）
// 		double* column2		 列2（或行2）
// 		double* resultColumn 列3（或行3）
// preconditions: 
// process: 求与两个正交列相正交的列（或行）
// output:	0
// postcondition:
// function_index:
// author:	chenjing
// created:	
//---------------------------------------------------------------------------
int columnCross3(double* column1,double* column2,double* resultColumn)
{
	double a1 = column1[0];
	double a2 = column1[1];
	double a3 = column1[2];
	double b1 = column2[0];
	double b2 = column2[1];
	double b3 = column2[2];
	double x,y,z;
	if ( a1*b3 - b1*a3 + 1.0 == 1.0)
	{
		y = 0;
		if (a1 == 0)
		{
			z = 0;
			x = 1;
		}
		z = sqrt(a1*a1/(a1*a1+a3*a3));
		x = a3 /a1 * z;
	}
	else
	{
		double temp1 = (a3*b2 - a2*b3)/(a1*b3 - a3*b1);	
		double temp2 = (a1*b2 - a2*b1)/(a3*b1 - a1*b3);
		y = -sqrt( 1 + temp1*temp1 + temp2*temp2 );
//		y = fabs(y);
		y = 1 / y;
		x = temp1 * y;
		z = temp2 * y;
	}
	resultColumn[0] = x;
	resultColumn[1] = y;
	resultColumn[2] = z;
	return 0;
}

//---------------------------------------------------------------------------
// name: columnMax
// Input: 
// 			double* columnData		输入的列（或行）
// 			int		col				列数（或行数）
// 			double	&MaxData		列中的最大值（或行）
// 			int		&serialNumber	最大值在列中的序号（从0开始）
// preconditions: 
// process: 求一列或一行中的最大值，并返回最大值和序号（序号是从0开始）
// output:	0
// postcondition:
// function_index:
// author:	chenjing
// created:	
//---------------------------------------------------------------------------
int columnMax(double* columnData,int col,
			  double &MaxData,
			  int &serialNumber)
{
	int i;
	MaxData = columnData[0];
	serialNumber = 0;
	for (i=1;i<col;i++)
	{
		if (MaxData<columnData[i])
		{
			MaxData = columnData[i];
			serialNumber = i;
		}
	}
	return 0;
}


//---------------------------------------------------------------------------
// name: matrixLeftDivide
// Input: 
// 				double*	leftMatrix		左矩阵
// 				int		leftRow			左矩阵的行数
// 				int		leftCol			左矩阵的列数
// 				double* righMatrix		右矩阵
// 				int		righRow			右矩阵的行数
// 				int		righCol			右矩阵的列数
// 				double* resultDivide	左除的结果
// preconditions: 
// process:	左除，求Ax=B的x，即：x=A\B
// output:		flag	标志位			0表示求解过程无误
// postcondition:					   -1表示左矩阵的列数与右矩阵的行数不相等，无法左除
// function_index:
// author:	chenjing
// created:	
//---------------------------------------------------------------------------
int matrixLeftDivide(
					 double* leftMatrix,int leftRow,int leftCol,
					 double* righMatrix,int righRow,int righCol,
					 double* resultLeftDivide)
{
	int i,j;
	if ( leftRow != righRow )
	{
		cout<<"matrixLeftDivide error!"<<endl;
		cout<<"The col of left matrix is not equal to col of right matrix!"<<endl;
		return -2;
	}
	if (leftRow == leftCol)
	{
		double* leftMatInverse = new double[leftCol*leftCol];
		for (i=0;i<leftCol*leftCol;i++)
			leftMatInverse[i] = leftMatrix[i];
		if (matrixInverseGaussJordan(leftMatInverse,leftCol) == -1)
			return -1;
		matrixMultiply(leftMatInverse,leftCol,leftCol,
					   righMatrix,righRow,righCol,
					   resultLeftDivide);
	}
	else
	{
		double* leftMatTranspose = new double[leftCol*leftRow];
		for (i=0;i<leftRow;i++)
			for (j=0;j<leftCol;j++)
				leftMatTranspose[j*leftRow+i] = leftMatrix[i*leftCol+j];
		double* leftMatTranleftMatMul = new double[leftCol*leftCol];
		double* leftMatTranrighMatMul = new double[leftCol*righCol];
		matrixMultiply(leftMatTranspose,leftCol,leftRow,
					   leftMatrix,leftRow,leftCol,
					   leftMatTranleftMatMul);
		matrixMultiply(leftMatTranspose,leftCol,leftRow,
					   righMatrix,righRow,righCol,
					   leftMatTranrighMatMul);
		if(matrixInverseGaussJordan(leftMatTranleftMatMul,leftCol)==-1)
			return -1;
		matrixMultiply(leftMatTranleftMatMul,leftCol,leftCol,
					   leftMatTranrighMatMul,leftCol,righCol,
					   resultLeftDivide);
	}
	return 0;
}

//---------------------------------------------------------------------------
// name: matrixMultiply
// Input: 
// 			float*		leftMatrix		乘号左边的矩阵
// 			int			lRow			左矩阵的行数
// 			int			lCol			左矩阵的列数
// 			float*		rightMatrix		乘号右边的矩阵
// 			int			rRow			右矩阵的行数
// 			int			rCol			右矩阵的列数
// 			float*		result			两个矩阵相乘的结果
// preconditions: 
// process: 两个矩阵相乘
// output:	
// postcondition:
// function_index:
// author:	chenjing
// created:	2007-12-17
//---------------------------------------------------------------------------
int matrixMultiply(
				   double* leftMatrix,int leftRow,int leftCol,
				   double* righMatrix,int righRow,int righCol,
				   double* resultMultiply)
{
	if (leftCol!=righRow)
	{
		cout<<"matrixMultiply error!"<<endl;
		cout<<"The col of left matrix is not equal to row of right matrix!"<<endl;
		return -1;
	}
	int lColRRow = leftCol;
	int i,j,k;
	for (i=0;i<leftRow;i++)
	{
		for (j=0;j<righCol;j++)
		{
			resultMultiply[i*righCol+j] = 0;	
			for (k=0;k<lColRRow;k++)
			{
				resultMultiply[i*righCol+j] = 
					resultMultiply[i*righCol+j] + leftMatrix[i*leftCol+k] * righMatrix[k*righCol+j];
			}
		}
	}
	return 0;
}

//---------------------------------------------------------------------------
// name: matrixInverseGaussJordan
// Input: 
// 			float*		a	输入的方阵，求得方阵的逆存储其中
// 			int			n	方阵的行数
// preconditions: 
// process: 利用高斯约当法求方阵的逆
// output:	
// postcondition:
// function_index:
// author:	chenjing
// created:	
//---------------------------------------------------------------------------
int matrixInverseGaussJordan(double* squareMatrix, int dimension)
{ 
	double* a = new double[dimension*dimension];
	int n = dimension;
	int *is,*js,i,j,k,l,u,v; 
	double d,p; 
	for (i=0;i<dimension;i++)
		for (j=0;j<dimension;j++)
			a[i*dimension+j] = squareMatrix[i*dimension+j];
		
	is=new int[n]; 
	js=new int[n]; 
	for (k=0; k<=n-1; k++) 
	{ 
		d=0.0; 
		for (i=k; i<=n-1; i++) 
			for (j=k; j<=n-1; j++) 
			{ 
				l=i*n+j; p=fabs(a[l]); 
				if (p>d) { d=p; is[k]=i; js[k]=j;} 
			} 
		if (d+1.0==1.0) 
		{ 
			free(is); free(js); 
//			printf("err**not inv\n"); 
			return -1; 
		} 
		if (is[k]!=k) 
			for (j=0; j<=n-1; j++) 
			{ 
				u=k*n+j; v=is[k]*n+j; 
				p=a[u]; a[u]=a[v]; a[v]=p; 
			} 
		if (js[k]!=k)
			for (i=0; i<=n-1; i++) 
			{ 
				u=i*n+k; v=i*n+js[k]; 
				p=a[u]; a[u]=a[v]; a[v]=p; 
			} 
		l=k*n+k; 
		a[l]=1.0/a[l]; 
		for (j=0; j<=n-1; j++) 
			if (j!=k) 
			{ u=k*n+j; a[u]=a[u]*a[l];} 
		for (i=0; i<=n-1; i++) 
			if (i!=k) 
				for (j=0; j<=n-1; j++) 
					if (j!=k) 
					{ u=i*n+j; a[u]=a[u]-a[i*n+k]*a[k*n+j]; } 
		for (i=0; i<=n-1; i++) 
			if (i!=k) 
			{ u=i*n+k; a[u]=-a[u]*a[l];	} 
	} 
	for (k=n-1; k>=0; k--) 
	{ 
		if (js[k]!=k) 
			for (j=0; j<=n-1; j++) 
			{ 
				u=k*n+j; v=js[k]*n+j; 
				p=a[u]; a[u]=a[v]; a[v]=p; 
			} 
		if (is[k]!=k) 
			for (i=0; i<=n-1; i++) 
			{ 
				u=i*n+k; v=i*n+is[k]; 
				p=a[u]; a[u]=a[v]; a[v]=p; 
			} 
	} 
	for (i=0;i<dimension;i++)
		for (j=0;j<dimension;j++)
			squareMatrix[i*dimension+j] = a[i*dimension+j];
	free(is); 
	free(js); 
	return 0; 
} 

//---------------------------------------------------------------------------
// name: brmul
// Input: double* a, double* b,int m,int n,int k,double* c
// preconditions: 
// process: 
// output:	
// postcondition:
// function_index:
// author:	chenjing
// created:	
//---------------------------------------------------------------------------
void brmul(double* a, double* b,int m,int n,int k,double* c) 
{ 
	int i,j,l,u; 
	for (i=0; i<=m-1; i++) 
		for (j=0; j<=k-1; j++) 
		{ 
			u=i*k+j; c[u]=0.0; 
			for (l=0; l<=n-1; l++) 
				c[u]=c[u]+a[i*n+l]*b[l*k+j];
		} 
	return; 
} 

//---------------------------------------------------------------------------
// name: matrixRank
// Input: 
// 			double* squareMatrix	输入方阵
// 			int		dimension		输入方阵的行列数
// preconditions: 
// process: 求方阵的秩
// output:	resultRank	方阵的秩
// postcondition:
// function_index:
// author:	chenjing
// created:	
//---------------------------------------------------------------------------
// int matrixRank(double* squareMatrix, int dimension)
// {
// 	mat* test=new mat(dimension, dimension);
// 	test->setmat(squareMatrix);// 设置矩阵元素
// 	int resultRank = test->done();
//     return resultRank;		
// }