cmatrix.cpp

一个高质量的矩阵运算源程序

#include <stdlib.h>
#include <malloc.h>
#include <memory.h>
#include <math.h>
#include "cmatrix.h"

#define	FALSE	0
#define	TRUE	1
#define	MATRIX_MAX	1024*1024
#define	E	1.0/2251799813685248.0  // E=2^-51

CMatrix::CMatrix()
{
	_SizeIJ=SizeI=SizeJ=0;
	_pMatrix=pMatrix=NULL;
}

CMatrix::~CMatrix()
{
	if(_pMatrix!=NULL)
		free(_pMatrix);
	if(pMatrix!=NULL)
		free(pMatrix);
}

double CMatrix::GetMean(void) const
{
	int ij;
	double mean;

	mean=0;
	for(ij=0;ij<SizeI*SizeJ;ij++)
		mean+=fabs(pMatrix[ij]);
	return mean/ij;
}

int CMatrix::SetSize(int i,int j)
{
	if(i==0 || j==0)
		return TRUE;
	if(i*j>MATRIX_MAX)
		return TRUE;
	if(pMatrix!=NULL)
		if(SizeI*SizeJ==i*j) {
			SizeI=i;SizeJ=j;
			return FALSE;
		}
		else {
			free(pMatrix);
			pMatrix=NULL;
		}
	SizeI=i;SizeJ=j;
	pMatrix=(double *)malloc(sizeof(double)*i*j);
	if(pMatrix==NULL) {
		SizeI=0;SizeJ=0;
		return TRUE;
	}
	return FALSE;
}

inline int CMatrix::GetSizeI(void) const
{
	if(pMatrix==NULL)
		return 0;
	else
		return SizeI;
}

inline int CMatrix::GetSizeJ(void) const
{
	if(pMatrix==NULL)
		return 0;
	else
		return SizeJ;
}

int CMatrix::_SetSize(int ij)
{
	if(ij>MATRIX_MAX)
		return TRUE;
	if(_pMatrix!=NULL)
		if(_SizeIJ==ij)
			return FALSE;
		else {
			free(_pMatrix);
			_pMatrix=NULL;
		}
	_SizeIJ=ij;
	_pMatrix=(double *)malloc(sizeof(double)*ij);
	if(_pMatrix==NULL) {
		_SizeIJ=0;
		return TRUE;
	}
	return FALSE;
}

int CMatrix::Copy(const CMatrix *pM)
{
	if(pM->pMatrix==NULL)
		return TRUE;
	if(SetSize(pM->SizeI,pM->SizeJ))
		return TRUE;
	memcpy(pMatrix,pM->pMatrix,sizeof(double)*SizeI*SizeJ);
	return FALSE;
}

void CMatrix::Set(int i,int j,double x)
{
	*(pMatrix+(SizeJ*(i-1)+j-1))=x;
}

double CMatrix::Get(int i,int j) const
{
	return *(pMatrix+(SizeJ*(i-1)+j-1));
}

int CMatrix::PL(int i,int j,double c)
{
	int k;
	double *pi,*pj;

	if(pMatrix==NULL)
		return TRUE;
	pi=pMatrix+SizeJ*(i-1);
	pj=pMatrix+SizeJ*(j-1);
	if(c==0)
		return FALSE;
	if(c==1) {
		for(k=SizeJ;k>0;k--,pi++,pj++)
			*pi+=*pj;
		return FALSE;
	}
	if(c==-1) {
		for(k=SizeJ;k>0;k--,pi++,pj++)
			*pi-=*pj;
		return FALSE;
	}
	for(k=SizeJ;k>0;k--,pi++,pj++)
		*pi=*pi+*pj*c;
	return FALSE;
}

int CMatrix::PR(int i,int j,double c)
{
	int k;
	double *pi,*pj;

	if(pMatrix==NULL)
		return TRUE;
	pi=pMatrix+i-1;
	pj=pMatrix+j-1;
	if(c==0)
		return FALSE;
	if(c==1) {
		for(k=SizeI;k>0;k--,pi+=SizeJ,pj+=SizeJ)
			*pj+=*pi;
		return FALSE;
	}
	if(c==-1) {
		for(k=SizeI;k>0;k--,pi+=SizeJ,pj+=SizeJ)
			*pj-=*pi;
		return FALSE;
	}
	for(k=SizeI;k>0;k--,pi+=SizeJ,pj+=SizeJ)
		*pj+=*pi*c;
	return FALSE;
}

int CMatrix::QL(int i,double c)
{
	int k;
	double *p;

	if(pMatrix==NULL)
		return TRUE;
	if(c==1)
		return FALSE;
	p=pMatrix+SizeJ*(i-1);
	if(c==0) {
		for(k=SizeJ;k>0;k--,p++)
			*p=0;
		return FALSE;
	}
	if(c==-1) {
		for(k=SizeJ;k>0;k--,p++)
			*p=-*p;
		return FALSE;
	}
	for(k=SizeJ;k>0;k--,p++)
		*p=*p*c;
	return FALSE;
}

int CMatrix::QR(int i,double c)
{
	int k;
	double *p;

	if(pMatrix==NULL)
		return TRUE;
	if(c==1)
		return FALSE;
	p=pMatrix+i-1;
	if(c==0) {
		for(k=SizeI;k>0;k--,p+=SizeJ)
			*p=0;
		return FALSE;
	}
	if(c==-1) {
		for(k=SizeI;k>0;k--,p+=SizeJ)
			*p=-*p;
		return FALSE;
	}
	for(k=SizeI;k>0;k--,p+=SizeJ)
		*p=*p*c;
	return FALSE;
}

int CMatrix::RL(int i,int j)
{
	int k;
	double c,*pi,*pj;

	if(pMatrix==NULL)
		return TRUE;
	pi=pMatrix+SizeJ*(i-1);
	pj=pMatrix+SizeJ*(j-1);
	for(k=SizeJ;k>0;k--,pi++,pj++) {
		c=*pi;
		*pi=*pj;
		*pj=c;
	}
	return FALSE;
}

int CMatrix::RR(int i,int j)
{
	int k;
	double c,*pi,*pj;

	if(pMatrix==NULL)
		return TRUE;
	pi=pMatrix+i-1;
	pj=pMatrix+j-1;
	for(k=SizeI;k>0;k--,pi+=SizeJ,pj+=SizeJ) {
		c=*pi;
		*pi=*pj;
		*pj=c;
	}
	return FALSE;
}

int CMatrix::Transpose(void)
{
	int i,j;
	double *p,*q,*r;

	if(pMatrix==NULL)
		return TRUE;
	if(_SetSize(SizeJ*SizeI))
		return TRUE;
	for(j=SizeJ,p=_pMatrix,r=pMatrix;j>0;j--,r++)
		for(i=SizeI,q=r;i>0;i--,p++,q+=SizeJ)
			*p=*q;
	p=_pMatrix;
	_pMatrix=pMatrix;
	pMatrix=p;
	i=SizeI;
	SizeI=SizeJ;
	SizeJ=i;
	return FALSE;
}

int CMatrix::Add(const CMatrix *pM)
{
	int i;
	double *p1,*p2;

	if(pMatrix==NULL)
		return TRUE;
	if(SizeI!=pM->SizeI || SizeJ!=pM->SizeJ)
		return TRUE;
	for(i=SizeI*SizeJ,p1=pMatrix,p2=pM->pMatrix;i>0;p1++,p2++,i--)
		*p1+=*p2;
	return FALSE;
}

int CMatrix::Sub(const CMatrix *pM)
{
	int i;
	double *p1,*p2;

	if(pMatrix==NULL)
		return TRUE;
	if(SizeI!=pM->SizeI || SizeJ!=pM->SizeJ)
		return TRUE;
	for(i=SizeI*SizeJ,p1=pMatrix,p2=pM->pMatrix;i>0;p1++,p2++,i--)
		*p1-=*p2;
	return FALSE;
}

int CMatrix::MulL(const CMatrix *pM)
{
	int i,j,k;
	double *p,*pl,*pr,*pl0,*pr0;

	if(pMatrix==NULL)
		return TRUE;
	if(pM->SizeJ!=SizeI)
		return TRUE;
	if(_SetSize(pM->SizeI*SizeJ))
		return TRUE;
	p=_pMatrix;
	for(i=pM->SizeI,pl0=pM->pMatrix;i>0;i--,pl0+=SizeI)
		for(j=SizeJ,pr0=pMatrix;j>0;j--,p++,pr0++)
			for(k=SizeI,*p=0,pl=pl0,pr=pr0;k>0;k--,pl++,pr+=SizeJ)
				*p+=*pl*(*pr);
	p=_pMatrix;
	_pMatrix=pMatrix;
	pMatrix=p;
	_SizeIJ=SizeI*SizeJ;
	SizeI=pM->SizeI;
	return FALSE;
}

int CMatrix::MulR(const CMatrix *pM)
{
	int i,j,k;
	double *p,*pl,*pr,*pl0,*pr0;

	if(pMatrix==NULL)
		return TRUE;
	if(SizeJ!=pM->SizeI)
		return TRUE;

	if(_SetSize(pM->SizeI*pM->SizeJ))
		return TRUE;
	p=_pMatrix;
	for(i=SizeI,pl0=pMatrix;i>0;i--,pl0+=SizeJ)
		for(j=pM->SizeJ,pr0=pM->pMatrix;j>0;j--,p++,pr0++)
			for(k=SizeJ,*p=0,pl=pl0,pr=pr0;k>0;k--,pl++,pr+=pM->SizeJ)
				*p+=*pl*(*pr);
	p=_pMatrix;
	_pMatrix=pMatrix;
	pMatrix=p;
	_SizeIJ=SizeI*SizeJ;
	SizeJ=pM->SizeJ;
	return FALSE;
}

int CMatrix::Mul(double a)
{
	int n;
	double *p;

	if(pMatrix==NULL)
		return TRUE;
	if(a==1)
		return FALSE;
	if(a==0) {
		for(n=SizeI*SizeJ,p=pMatrix;n>0;n--,p++)
			*p=0;
		return FALSE;
	}
	if(a==-1) {
		for(n=SizeI*SizeJ,p=pMatrix;n>0;n--,p++)
			*p=-*p;
		return FALSE;
	}
	for(n=SizeI*SizeJ,p=pMatrix;n>0;n--,p++)
		*p*=a;
	return FALSE;
}

int CMatrix::Power(int n)
{
	CMatrix M;
	int i,j;
	double *p;

	if(pMatrix==NULL)
		return TRUE;
	if(SizeI!=SizeJ)
		return TRUE;
	if(n==0) {
		for(i=SizeI-1,p=pMatrix;i>0;i--) {
			*(p++)=1;
			for(j=SizeI;j>0;j--)
				*(p++)=0;
		}
		*p=1;
		return FALSE;
	}
	if(n<0) {
		Inverse();
		n=-n;
	}
	if(n==1)
		return FALSE;
	M.Copy(this);
	for(n--;n>0;n--)
		MulR(&M);
	return FALSE;
}

int CMatrix::Lower(void)
{
	int i,j,n,max;
	double c,x,min;

	if(pMatrix==NULL)
		return TRUE;
	min=GetMean()*E;
	for(i=j=1;j<SizeJ && i<=SizeI;i++) {
		for(max=n=j,c=0;n<=SizeJ;n++) {
			x=fabs(Get(i,n));
			if(c<x) {
				c=x;
				max=n;
			}
		}
		if(fabs(c)<=min)
			continue;
		if(max!=j) {
			QR(j,-1);
			RR(max,j);
		}
		c=-Get(i,j);
		for(n=j+1;n<=SizeJ;n++)
			PR(j,n,Get(i,n)/c);
		j++;
	}
	return FALSE;
}

int CMatrix::Upper(void)
{
	int i,j,m,max;
	double c,x,min;

	if(pMatrix==NULL)
		return TRUE;
	min=GetMean()*E;
	for(i=j=1;i<SizeI && j<=SizeJ;j++) {
		for(max=m=i,c=0;m<=SizeI;m++) {
			x=fabs(Get(m,j));
			if(c<x) {
				c=x;
				max=m;
			}
		}
		if(fabs(c)<=min)
			continue;
		if(max!=i) {
			QL(i,-1);
			RL(i,max);
		}
		c=-Get(i,j);
		for(m=i+1;m<=SizeI;m++)
			PL(m,i,Get(m,j)/c);
		i++;
	}
	return FALSE;
}

int CMatrix::Lower(CMatrix *pM)
{
	int i,j,n,max;
	double c,x,min;

	if(pMatrix==NULL)
		return TRUE;
	if(pM->pMatrix==NULL)
		return TRUE;
	if(SizeJ!=pM->SizeJ)
		return TRUE;
	min=GetMean()*E;
	for(i=j=1;j<SizeJ && i<=SizeI;i++) {
		for(max=n=j,c=0;n<=SizeJ;n++) {
			x=fabs(Get(i,n));
			if(c<x) {
				c=x;
				max=n;
			}
		}
		if(fabs(c)<=min)
			continue;
		if(max!=j) {
			RR(max,j);	pM->RR(max,j);
		}
		c=-Get(i,j);
		for(n=j+1;n<=SizeJ;n++) {
			x=Get(i,n)/c;
			PR(j,n,x);	pM->PR(j,n,x);
		}
		j++;
	}
	return FALSE;
}

int CMatrix::Upper(CMatrix *pM)
{
	int i,j,m,max;
	double c,x,min;

	if(pMatrix==NULL)
		return TRUE;
	if(pM->pMatrix==NULL)
		return TRUE;
	if(SizeI!=pM->SizeI)
		return TRUE;
	if(pMatrix==NULL)
		return TRUE;
	min=GetMean()*E;
	for(i=j=1;i<SizeI && j<=SizeJ;j++) {
		for(max=m=i,c=0;m<=SizeI;m++) {
			x=fabs(Get(m,j));
			if(c<x) {
				c=x;
				max=m;
			}
		}
		if(fabs(c)<=min)
			continue;
		if(max!=i) {
			RL(i,max);	pM->RL(i,max);
		}
		c=-Get(i,j);
		for(m=i+1;m<=SizeI;m++) {
			x=Get(m,j)/c;
			PL(m,i,x);	pM->PL(m,i,x);
		}
		i++;
	}
	return FALSE;
}

int CMatrix::DiagonalU(void)
{
	int i,j,n;
	double c,min;

	if(pMatrix==NULL)
		return TRUE;
	min=GetMean()*E;
	for(i=1;i<=SizeI;i++) {
		for(j=i;j<=SizeJ;j++) {
			c=-Get(i,j);
			if(fabs(c)>min)
				break;
		}
		if(j>SizeJ)
			return FALSE;
		if(i<j) {
//			QR(i,-1);
			RR(i,j);
		}
		for(n=j+1;n<=SizeJ;n++)
			PR(j,n,Get(i,n)/c);
	}
	return FALSE;
}

int CMatrix::DiagonalL(void)
{
	int i,j,m;
	double c,min;

	if(pMatrix==NULL)
		return TRUE;
	min=GetMean()*E;
	for(j=1;j<=SizeJ;j++) {
		for(i=j;i<=SizeI;i++) {
			c=-Get(i,j);
			if(fabs(c)>min)
				break;
		}
		if(i>SizeI)
			return FALSE;
		if(i>j) {
//			QL(i,-1);
			RL(i,j);
		}
		for(m=i+1;m<=SizeI;m++)
			PL(m,j,Get(i,m)/c);
	}
	return FALSE;
}

int CMatrix::DiagonalU(CMatrix *pM)
{
	int i,j,n;
	double c,x,min;

	if(pMatrix==NULL)
		return TRUE;
	if(pM->pMatrix==NULL)
		return TRUE;
	if(SizeJ!=pM->SizeJ)
		return TRUE;
	min=GetMean()*E;
	for(i=1;i<=SizeI;i++) {
		for(j=i;j<=SizeJ;j++) {
			c=-Get(i,j);
			if(fabs(c)>min)
				break;
		}
		if(j>SizeJ)
			return FALSE;
		if(i<j) {
//			QR(i,-1);	pM->QR(i,-1);
			RR(i,j);	pM->RR(i,j);
		}
		for(n=j+1;n<=SizeJ;n++) {
			x=Get(i,n)/c;
			PR(j,n,x);	pM->PR(j,n,x);
		}
	}
	return FALSE;
}

int CMatrix::DiagonalL(CMatrix *pM)
{
	int i,j,m;
	double c,x,min;

	if(pMatrix==NULL)
		return TRUE;
	if(pM->pMatrix==NULL)
		return TRUE;
	if(SizeI!=pM->SizeI)
		return TRUE;
	if(pMatrix==NULL)
		return TRUE;
	min=GetMean()*E;
	for(j=1;j<=SizeJ;j++) {
		for(i=j;i<=SizeI;i++) {
			c=-Get(i,j);
			if(fabs(c)>min)
				break;
		}
		if(i>SizeI)
			return FALSE;
		if(i>j) {
//			QL(i,-1);	pM->QL(i,-1);
			RL(i,j);	pM->RL(i,j);
		}
		for(m=i+1;m<=SizeI;m++) {
			x=Get(i,m)/c;
			PL(m,j,x);	pM->PL(m,j,x);
		}
	}
	return FALSE;
}

int CMatrix::Inverse(void)
{
	int i;
	double c,min;
	CMatrix Mr,Ml;

	if(pMatrix==NULL)
		return TRUE;
	min=GetMean()*E;
	Mr.SetSize(SizeI,SizeI);
	Mr.Power(0);
	Ml.SetSize(SizeJ,SizeJ);
	Ml.Power(0);
	Upper(&Mr);
	DiagonalU(&Ml);
	for(i=1;i<=SizeI && i<=SizeJ;i++) {
		c=Get(i,i);
		if(fabs(c)>min)
			Set(i,i,1);	Mr.QL(i,1/c);
	}
	if(SizeI==SizeJ) {
		for(i=1;i<=SizeI;i++)
			if(Get(i,i)<=min)
				break;
		if(i>SizeI) {
			Copy(&Ml);
			MulR(&Mr);
			return FALSE;
		}
	}
	else
		Transpose();
	MulR(&Mr);
	MulL(&Ml);
	return -1;
}

int CMatrix::Det(void)
{
	int i;
	double det;

	if(SizeI!=SizeJ)
		return TRUE;
	if(Upper())
		return TRUE;
	det=1;
	for(i=1;i<=SizeI;i++)
		det*=Get(i,i);
	SetSize(1,1);
	Set(1,1,det);
	return FALSE;
}