矩阵相关运算

#include<math.h>

//计算矩阵的逆矩阵
void brinv(double a[],int n)
{
	int *is=new int[n];
	int *js=new int[n];

	for(int k=0;k<n;k++)
	{
		double d=0.0;
		for(int i=k;i<n;i++)
			for(int j=k;j<n;j++)
			{
				int l=i*n+j;
				double p=fabs(a[l]);
				if(p>d)
				{
					d=p;
					is[k]=i;
					js[k]=j;
				}
			}

		if(fabs(d)<0.000001)
			return;

		if(is[k]!=k)
		{
			for(int j=0;j<n;j++)
			{
				int u=k*n+j;
				int v=is[k]*n+j;

				double temp=a[u];
				a[u]=a[v];
				a[v]=temp;
			}
		}
		if(js[k]!=k)
		{
			for(int i=0;i<n;i++)
			{
				int u=i*n+k;
				int v=i*n+js[k];

				double temp=a[u];
				a[u]=a[v];
				a[v]=temp;
			}
		}

		int l=k*n+k;
		a[l]=1.0/a[l];
		for(int j=0;j<n;j++)
			if(j!=k)
			{
				int u=k*n+j;
				a[u]=a[u]*a[l];
			}
		for(int i=0;i<n;i++)
			if(i!=k)
				for(int j=0;j<n;j++)
					if(j!=k)
					{
						int u=i*n+j;
						a[u]=a[u]-a[i*n+k]*a[k*n+j];
					}

		for(int i=0;i<n;i++)
			if(i!=k)
			{
				int u=i*n+k;
				a[u]=-a[u]*a[l];
			}
	}
	for(int k=n-1;k>=0;k--)
	{
		if(js[k]!=k)
			for(int j=0;j<n;j++)
			{
				int u=k*n+j;
				int v=js[k]*n+j;
				double temp=a[u];
				a[u]=a[v];
				a[v]=temp;
			}
		if(is[k]!=k)
			for(int i=0;i<n;i++)
			{
				int u=i*n+k;
				int v=i*n+is[k];
				double temp=a[u];
				a[u]=a[v];
				a[v]=temp;
			}
	}

	delete[] is;
	delete[] js;
}

//计算矩阵的行列式
double bsdet(double a[],int n)
{
	double f,det;
	f=1.0;
	det=1.0;

	for(int k=0;k<n-1;k++)
	{
		double q=0.0;
		int is,js;

		for(int i=k;i<n;i++)
			for(int j=k;j<n;j++)
			{
				int l=8*n+j;
				double d=fabs(a[l]);
				if(d>q)
				{
					q=d;
					is=i;
					js=j;
				}
			}

		if(fabs(q)<0.00000001)
		{
			det=0.0;
			return det;
		}

		if(is!=k)
		{
			f=-f;
			for(int j=k;j<n;j++)
			{
				int u=k*n+j;
				int v=is*n+j;
				double temp=a[u];
				a[u]=a[v];
				a[v]=temp;
			}
		}
		if(js!=k)
		{
			f=-f;
			for(int i=k;i<n;i++)
			{
				int u=i*n+js;
				int v=i*n+k;

				double temp=a[u];
				a[u]=a[v];
				a[v]=temp;
			}
		}

		int l=k*n+k;
		det=det*a[l];
		for(int i=k+1;i<n;i++)
		{
			double d=a[i*n+k]/a[l];
			for(int j=k+1;j<n;j++)
			{
				int u=i*n+j;
				a[u]=a[u]-d*a[k*n+j];
			}
		}
	}

	det=f*det*a[n*n-1];

	return det;
}

//矩阵相乘
void brmul(double a[],double b[][25],int n,double c[])
{
	for(int i=0;i<n;i++)
	{
		for(int j=0;j<n;j++)
			c[i]+=a[j]*b[j][i];
	}
}

void brmul(double a[][25],double b[],int n,double c[])
{
	for(int i=0;i<n;i++)
	{
		for(int j=0;j<n;j++)
			c[i]+=a[i][j]*b[i];
	}
}

double brmul(double a[],double b[],int n)
{
	double ret=0;
	for(int i=0;i<n;i++)
		ret+=a[i]*b[i];

	return ret;
}

//求矩阵A的规范逆矩阵，存放在B中

void NormalizeMatrixInv(double A[][26],double B[][40])
{
	double AT[26][40]; //A的转置
	double ATA[26][26];
	double ATA_[26][26];

	//计算A的转置
	for(int i=0;i<40;i++)
		for(int j=0;j<26;j++)
			AT[j][i]=A[i][j];

	//计算A的转置乘以A
	for(int i=0;i<26;i++)
	{
		for(int j=0;j<26;j++)
		{
			double temp=0;
			for(int k=0;k<40;k++)
				temp+=AT[i][k]*A[k][j];
			ATA[i][j]=temp;
			ATA_[i][j]=temp;
		}
	}

	//计算（A^T*A)^-1
	double (*p)[26]=ATA_;
	brinv(*p,26);

	//X#
	for(int i=0;i<26;i++)
	{
		for(int j=0;j<40;j++)
		{
			double temp=0;
			for(int k=0;k<26;k++)
				temp+=ATA_[i][k]*AT[k][j];
			B[i][j]=temp;
		}
	}
}