matrix.cpp

一些矩阵运算的函数

#include "matrix.h"
#include "math.h"
#include "stdlib.h"
#include "iostream.h"

void matrix_add(double *matrixA,double *matrixB,double *matrixC,int m,int n)
{
	//两个矩阵相加（m*n矩阵）
	//其中前两个矩阵为加数，第三个矩阵为得到的结果
	for(int i=0;i<m;i++)
		for(int j=0;j<n;j++)
			*(matrixC+i*m+j)=*(matrixA+i*m+j)+*(matrixB+i*m+j);
}

void matrix_add(double *matrixA,double *matrixB,double *matrixC,int m)
{
	//两个矩阵相加（m*m矩阵）
	//其中前两个矩阵为加数，第三个矩阵为得到的结果
	for(int i=0;i<m;i++)
		for(int j=0;j<m;j++)
			*(matrixC+i*m+j)=*(matrixA+i*m+j)+*(matrixB+i*m+j);
}

void matrix_add(double *matrixA,double *matrixB,int m,int n)
{
	//两个矩阵相加（m*n矩阵）
	//其中前两个矩阵为加数，得到的结果存放在第一矩阵里
	for(int i=0;i<m;i++)
		for(int j=0;j<n;j++)
			*(matrixA+i*m+j)=*(matrixA+i*m+j)+*(matrixB+i*m+j);
}

void matrix_add(double *matrixA,double *matrixB,int m)
{
	//两个矩阵相加（m*m矩阵）
	//其中前两个矩阵为加数，得到的结果存放在第一矩阵里
	for(int i=0;i<m;i++)
		for(int j=0;j<m;j++)
			*(matrixA+i*m+j)=*(matrixA+i*m+j)+*(matrixB+i*m+j);
}

void matrix_minus(double *matrixA,double *matrixB,double *matrixC,int m,int n)
{
	//两个矩阵相减（m*n矩阵）
	//其中前两个矩阵为减数，第三个矩阵为得到的结果
	for(int i=0;i<m;i++)
		for(int j=0;j<n;j++)
			*(matrixC+i*m+j)=*(matrixA+i*m+j)-*(matrixB+i*m+j);
}

void matrix_minus(double *matrixA,double *matrixB,double *matrixC,int m)
{
	//两个矩阵相减（m*m矩阵）
	//其中前两个矩阵为减数，第三个矩阵为得到的结果
	for(int i=0;i<m;i++)
		for(int j=0;j<m;j++)
			*(matrixC+i*m+j)=*(matrixA+i*m+j)-*(matrixB+i*m+j);
}

void matrix_minus(double *matrixA,double *matrixB,int m,int n)
{
	//两个矩阵相减（m*n矩阵）
	//其中前两个矩阵为减数，得到的结果存放在第一矩阵里
	for(int i=0;i<m;i++)
		for(int j=0;j<n;j++)
			*(matrixA+i*m+j)=*(matrixA+i*m+j)-*(matrixB+i*m+j);
}

void matrix_minus(double *matrixA,double *matrixB,int m)
{
	//两个矩阵相减（m*m矩阵）
	//其中前两个矩阵为减数，得到的结果存放在第一矩阵里
	for(int i=0;i<m;i++)
		for(int j=0;j<m;j++)
			*(matrixA+i*m+j)=*(matrixA+i*m+j)-*(matrixB+i*m+j);
}

void matrix_mul(double *matrixA,double *matrixB,double *matrixC
				,int l,int m,int n)
{
	//两个矩阵相乘(l*m矩阵和m*n矩阵,结果为l*n矩阵)
	//其中前两个矩阵为乘数，得到的结果存放在第三个矩阵中
	int i,j,k;
	for(i=0;i<l;i++)
		for(j=0;j<n;j++)
			*(matrixC+i*l+j)=0;//对matrixC初始化置零
	for(i=0;i<l;i++)
		for(j=0;j<n;j++)
			for(k=0;k<m;k++)
				*(matrixC+i*l+j)+=*(matrixA+i*l+k)**(matrixB+k*m+j);
}

void matrix_mul(double *matrixA,double *matrixB,double *matrixC,int n)				
{
	//两个矩阵相乘(均为n*n方阵)
	//其中前两个矩阵为乘数，得到的结果存放在第三个矩阵中
	int i,j,k;
	for(i=0;i<n;i++)
		for(j=0;j<n;j++)
			*(matrixC+i*n+j)=0;//对matrixC初始化置零
	for(i=0;i<n;i++)
		for(j=0;j<n;j++)
			for(k=0;k<n;k++)
				*(matrixC+i*n+j)+=*(matrixA+i*n+k)**(matrixB+k*n+j);
}

void matrix_mul(double *matrixA,double *matrixB,int n)				
{
	//注意，这个函数无法应用！！
	//两个矩阵相乘(均为n*n方阵)
	//其中前两个矩阵为乘数，得到的结果存放在第一个矩阵中
	//指针(数组）的初始化问题还不明白
	int i,j,k;
	double *matrixC=0;
	for(i=0;i<n;i++)
		for(j=0;j<n;j++)
			for(k=0;k<n;k++)
				*(matrixC+i*n+j)+=*(matrixA+i*n+k)**(matrixB+k*n+j);
	//matrix_copy(matrixA,matrixC,n,n);
	for(i=0;i<n;i++)
		for(j=0;j<n;j++)
			*(matrixA+i*n+j)=*(matrixC+i*n+j);
}

void matrix_copy(double *matrixA,double *matrixB,int m,int n)
{
	//矩阵复制(m*n矩阵)
	//把第二个矩阵的值传给第一个矩阵
	for(int i=0;i<m;i++)
		for(int j=0;j<n;j++)
			*(matrixA+i*m+j)=*(matrixB+i*m+j);
}

void matrix_copy(double *matrixA,double *matrixB,int n)
{
	//矩阵复制(n*n矩阵)
	//把第二个矩阵的值传给第一个矩阵
	for(int i=0;i<n;i++)
		for(int j=0;j<n;j++)
			*(matrixA+i*n+j)=*(matrixB+i*n+j);
}

int matrix_qiuni(double *matrixA,int n)
{
	//矩阵的高斯-约当法求逆
	//结果直接保存到原来的矩阵，应用时注意对原来数据的保存
	int i,j,k,course1,course2;
	double compare1,compare2;
	int linenum[20],rownum[20];//矩阵运算的最大阶数为20
	                           //无奈的初始化，继续查资料完善
	for(k=0;k<n;k++)
	{
		compare1=0.0;
		for(i=k;i<n;i++)
			for(j=k;j<n;j++)
			{
				course1=i*n+j;
				compare2=fabs(*(matrixA+course1));
				if(compare2>compare1)
				{
					compare1=compare2;
					linenum[k]=i;
					rownum[k]=j;
				}
			}
			if(compare1+1.0==1.0)
			{
				cout<<"err in computing!\n";
				return -1;
			}
			if(linenum[k]!=k)
				for(j=0;j<n;j++)
				{
					course1=k*n+j;
					course2=linenum[k]*n+j;
					compare2=*(matrixA+course1);
					*(matrixA+course1)=*(matrixA+course2);
					*(matrixA+course2)=compare2;
				}
			if(rownum[k]!=k)
				for(i=0;i<n;i++)
				{
					course1=i*n+k;
					course2=rownum[k]+i*n;
					compare2=*(matrixA+course1);
					*(matrixA+course1)=*(matrixA+course2);
					*(matrixA+course2)=compare2;
				}
			course1=k*n+k;
			*(matrixA+course1)=1.0/(0+*(matrixA+course1));
			for(j=0;j<n;j++)
				if(j!=k)
				{
					course2=k*n+j;
					*(matrixA+course2)=*(matrixA+course2)
						**(matrixA+course1);
				}
			for(i=0;i<n;i++)
				if(i!=k)
				{
					for(j=0;j<n;j++)
						if(j!=k)
						{
							course2=i*n+j;
							*(matrixA+course2)=*(matrixA+course2)
								-*(matrixA+i*n+k)**(matrixA+k*n+j);
						}
				}
			for(i=0;i<n;i++)
				if(i!=k)
				{
					course2=i*n+k;
					*(matrixA+course2)=-*(matrixA+course2)**(matrixA+course1);
				}
	}
	for(k=n-1;k>=0;k--)
	{
		if(rownum[k]!=k)
			for(j=0;j<n;j++)
			{
				course2=k*n+j;
				course1=rownum[k]*n+j;
				compare2=*(matrixA+course2);
				*(matrixA+course2)=*(matrixA+course1);
				*(matrixA+course1)=compare2;
			}
		if(linenum[k]!=k)
			for(i=0;i<n;i++)
			{
				course2=i*n+k;
				course1=i*n+linenum[k];
				compare2=*(matrixA+course2);
				*(matrixA+course2)=*(matrixA+course1);
				*(matrixA+course1)=compare2;
			}
	}	
	return 1;
}

int dzmatrix_qiuni(double *matrixA,int n)
{
	//正定对称矩阵求逆
	//输出结果直接放在原来的矩阵中，如果要用原来的矩阵，需要先保护数据
	int i,j,k,m;
	double w,g,b[20];
	for(k=0;k<n;k++)
	{
		w=*matrixA;
		if(fabs(w)+1.0==1.0)
		{
			cout<<"fail!\n";
			return -1;
		}
		m=n-k-1;
		for(i=1;i<n;i++)
		{
			g=*(matrixA+i*n);
			b[i]=g/w;
			if(i<=m)
				b[i]=-b[i];
			for(j=1;j<=i;j++)
				*(matrixA+(i-1)*n+j-1)=*(matrixA+i*n+j)+g*b[j];
		}
		*(matrixA+n*n-1)=1.0/w;
		for(i=1;i<n;i++)
			*(matrixA+(n-1)*n+i-1)=b[i];
	}
	for(i=0;i<n;i++)
		for(j=i+1;j<n;j++)
			*(matrixA+i*n+j)=*(matrixA+j*n+i);
	return 1;
}

void matrix_fenjie(double *matrixA,double *matrixL,double *matrixU,int n)
{
	int i,j,k,course,w,v;
	for(k=0;k<n-1;k++)
	{
		w=k*n+k;
		if(fabs(*(matrixA+w))+1.0==1.0)
		{
			cout<<"matrix fenjie fail!\n";
			return ;
		}
		for(i=k+1;i<n;i++)
		{
			course=i*n+k;
			*(matrixA+course)=*(matrixA+course)/(0+*(matrixA+w));
		}
		for(i=k+1;i<n;i++)
		{
			course=i*n+k;
			for(j=k+1;j<n;j++)
			{
				v=i*n+j;
				*(matrixA+v)=*(matrixA+v)-*(matrixA+course)**(matrixA+k*n+j);
			}
		}
	}
	for(i=0;i<n;i++)
	{
		for(j=0;j<i;j++)
		{
			course=i*n+j;
			*(matrixL+course)=*(matrixA+course);
			*(matrixU+course)=0.0;
		}
		course=i*n+i;
		*(matrixL+course)=1.0;
		*(matrixU+course)=*(matrixA+course);
		for(j=i+1;j<n;j++)
		{
			course=i*n+j;
			*(matrixL+course)=0.0;
			*(matrixU+course)=*(matrixA+course);
		}
	}
	return ;
}

void matrix_qrfenjie(double *a,double *q,int m,int n)
{
	//对矩阵进行QR分解
	//其中得到的正交矩阵存放在矩阵Q中，
	//右上三角阵存放在矩阵A中
	//矩阵a(m*n),q(m*m),r(m*n)
	int i,j,k,l,nn,p,jj;
	double u,alpha,w,t;
	if(m<n)
	{ 
		cout<<"QR fenjie fail!\n";
		return ;
	}
	for(i=0;i<=m-1;i++)
		for(j=0;j<=m-1;j++)
		{
			l=i*m+j;
			*(q+l)=0.0;
			if(i==j)
				*(q+l)=1.0;
		}
	nn=n;
	if(m==n)
		nn=m-1;
	for(k=0;k<=nn-1;k++)
	{
		u=0.0;
		l=k*n+k;
		for(i=k;i<=m-1;i++)
		{
			w=fabs(*(a+i*n+k));
			if(w>u)
				u=w;
		}
		alpha=0.0;
		for(i=k;i<=m-1;i++)
		{
			t=*(a+i*n+k)/u;
			alpha=alpha+t*t;
		}
		if(*(a+l)>0.0)
			u=-u;
		alpha=u*sqrt(alpha);
		if((fabs(alpha)+1.0)==1.0)
		{
			cout<<"QR fenjie fail!\n";
			return;
		}
		u=sqrt(2.0*alpha*(alpha-*(a+l)));
		if((u+1.0)!=1.0)
		{
			*(a+l)=(*(a+l)-alpha)/u;
			for(i=k+1;i<=m-1;i++)
			{
				p=i*n+k;
				*(a+p)=*(a+p)/u;
			}
			for(j=0;j<=m-1;j++)
			{
				t=0.0;
				for(jj=k;jj<=m-1;jj++)
					t=t+*(a+jj*n+k)**(q+jj*m+j);
				for(i=k;i<=m-1;i++)
				{
					p=i*m+j;
					*(q+p)=*(q+p)-2*t**(a+i*n+k);
				}
			}
			for(j=k+1;j<=n-1;j++)
			{
				t=0.0;
				for(jj=k;jj<=m-1;jj++)
					t=t+*(a+jj*n+k)**(a+jj*n+j);
				for(i=k;i<=m-1;i++)
				{
					p=i*n+j;
					*(a+p)=*(a+p)-2*t**(a+i*n+k);
				}
			}
			*(a+l)=alpha;
			for(i=k+1;i<=m-1;i++)
				*(a+i*n+k)=0.0;
		}
	}
	for(i=0;i<=m-2;i++)
		for(j=i+1;j<=m-1;j++)
		{
			p=i*m+j;
			l=j*m+i;
			t=*(q+p);
			*(q+p)=*(q+l);
			*(q+l)=t;
		}
	return ;
}

int matrix_hqr(double *a,double *u,double *v,double eps,int n,int jt)
{
	//求Hessenberg矩阵全部特征根的QR法
	int m,it,i,j,k,l,ii,jj,kk,ll;
	double b,c,w,g,xy,p,q,r,x,s,e,f,z,y;
	it=0;
	m=n;
	while(m!=0)
	{
		l=m-1;
		while((l>0)&&(fabs(*(a+l*n+l-1))>eps*
			(fabs(*(a+(l-1)*n+l-1))+fabs(*(a+l*n+l)))))
			l=l-1;
		//用来中断while循环,对角线元素为零，则可以判断其化成一、二阶
		//的对角阵了。从后面开始化简
		ii=(m-1)*n+m-1;
		jj=(m-1)*n+m-2;
		kk=(m-2)*n+m-1;
		ll=(m-2)*n+m-2;
		if(l==m-1)//化成一阶矩阵时的根（一个）
		{
			*(u+m-1)=*(a+(m-1)*n+m-1);
			*(v+m-1)=0.0;
			m=m-1;
			it=0;
		}
		else if(l==m-2)//化成二阶矩阵时的根，可能共轭
		{
			b=-(*(a+ii)+*(a+ll));
			c=*(a+ii)**(a+ll)-*(a+jj)**(a+kk);
			w=b*b-4.0*c;
			y=sqrt(fabs(w));
			if(w>0.0)
			{
				xy=1.0;
				if(b<0.0)
					xy=-1.0;
				*(u+m-1)=(-b-xy*y)/2.0;
				*(u+m-2)=c/(*(u+m-1));
				*(v+m-1)=0.0;
				*(v+m-2)=0.0;
			}
			else
			{
				*(u+m-1)=-b/2;
				*(u+m-2)=*(u+m-1);
				*(v+m-1)=y/2;
				*(v+m-2)=-*(v+m-1);
			}
			m=m-2;
			it=0;
		}
		else
		{
			if(it>=jt)
			{
				cout<<"fail!\n";
				return (-1);
			}
			it=it+1;
			for(j=l+2;j<m;j++)
				*(a+j*n+j-2)=0.0;
			for(j=l+3;j<m;j++)
				*(a+j*n+j-3)=0.0;
			for(k=l;k<m-1;k++)
			{
				if(k!=l)
				{
					p=*(a+k*n+k-1);
					q=*(a+(k+1)*n+k-1);
					r=0.0;
					if(k!=m-2)
						r=*(a+(k+2)*n+k-1);
				}
				else
				{
					x=*(a+ii)+*(a+ll);
					y=*(a+ll)**(a+ii)-*(a+kk)**(a+jj);
					ii=l*n+l;
					jj=l*n+l+1;
					kk=(l+1)*n+l;
					ll=(l+1)*n+l+1;
					p=*(a+ii)*(*(a+ii)-x)+*(a+jj)**(a+kk)+y;
					q=*(a+kk)*(*(a+ii)+*(a+ll)-x);
					r=*(a+kk)**(a+(l+2)*n+l+1);
				}
				if((fabs(p)+fabs(q)+fabs(r))!=0.0)
				{
					xy=1.0;
					if(p<0.0)
						xy=-1.0;
					s=xy*sqrt(p*p+q*q+r*r);
					if(k!=l)
						*(a+k*n+k-1)=-s;
					e=-q/s;
					f=-r/s;
					x=-p/s;
					y=-x-f*r/(p+s);
					g=e*r/(p+s);
					z=-x-e*q/(p+s);
					for(j=k;j<m;j++)
					{
						ii=k*n+j;
						jj=(k+1)*n+j;
						p=x**(a+ii)+e**(a+jj);
						q=e**(a+ii)+y**(a+jj);
						r=f**(a+ii)+g**(a+jj);
						if(k!=m-2)
						{
							kk=(k+2)*n+j;
							p=p+f**(a+kk);
							q=q+g**(a+kk);
							r=r+z**(a+kk);
							*(a+kk)=r;
						}
						*(a+jj)=q;
						*(a+ii)=p;
					}
					j=k+3;
					if(j>=m-1)
						j=m-1;
					for(i=l;i<=j;i++)
					{
						ii=i*n+k;
						jj=i*n+k+1;
						p=x**(a+ii)+e**(a+jj);
						q=e**(a+ii)+y**(a+jj);
						r=f**(a+ii)+g**(a+jj);
						if(k!=m-2)
						{
							kk=i*n+k+2;
							p=p+f**(a+kk);
							q=q+g**(a+kk);
							r=r+z**(a+kk);
							*(a+kk)=r;
						}
						*(a+jj)=q;
						*(a+ii)=p;
					}
				}
			}
		}
	}
	return 1;
}

void matrix_hessen(double *a,int n)
{
	int i,j,k,u,v;
	double d,t;
	for(k=1;k<n-1;k++)
	{
		d=0.0;
		for(j=k;j<n;j++)
		{
			u=j*n+k-1;
			t=*(a+u);
			if(fabs(t)>fabs(d))
			{
				d=t;
				i=j;
			}
		}
	    if(fabs(d)+1.0!=1.0)
		{
			if(i!=k)
			{
				for(j=k-1;j<n;j++)
				{
					u=i*n+j;
					v=k*n+j;
					t=*(a+u);
					*(a+u)=*(a+v);
					*(a+v)=t;
				}
				for(j=0;j<n;j++)
				{
					u=j*n+i;
					v=j*n+k;
					t=*(a+u);
					*(a+u)=*(a+v);
					*(a+v)=t;
				}
			}
			for(i=k+1;i<n;i++)
			{
				u=i*n+k-1;
				t=*(a+u)/d;
				*(a+u)=0.0;
				for(j=k;j<n;j++)
				{
					v=i*n+j;
					*(a+v)=*(a+v)-t**(a+k*n+j);
				}
				for(j=0;j<n;j++)
				{
					v=j*n+k;
					*(a+v)=*(a+v)+t**(a+j*n+i);
				}
			}
		}
	}
	return ;
}