matrix.cpp

卫星单点定位程序

/*******************************************************************************

  文件名称：Matrix.cpp
  作者：武汉大学测绘学院测绘工程2003级2班 陈胜 学号：200331610170
  创建日期：2006-10-6 14:40
  最后修改日期：
  功能：Matrix 类成员函数的实现
  
	该函数依赖于以下文件（不包括库函数）：
	
******************************************************************************/

#include "Matrix.h"


//构造函数
Matrix::Matrix ()
{
	a = NULL;
	m = n = i = j = 0;
}

Matrix::Matrix (int m_, int n_)
{
	m = m_;
	n = n_;
	i = j = 0;
	a = new double [m_ * n_];
	memset (a, 0, m * n * sizeof (double));
}

Matrix::Matrix (double *a_, int m_, int n_)
{
	m = m_;
	n = n_;
	i = j = 0;
	a = new double [m_ * n_]; 
	memcpy (a, a_, m * n * 8);
}

Matrix::Matrix (const Matrix &b)
{
	m = b.m;
	n = b.n;
	i = j = 0;
	a = new double [m * n];
	memcpy (a, b.a, m * n * sizeof (double));
}

//析构函数
Matrix::~Matrix () 
{
	if (a != NULL) {
		delete []a;
		a = NULL;
	}
}

//设置矩阵
void Matrix::set (int m_, int n_) 
{
	m = m_;
	n = n_;
	i = j = 0;
	if (a != NULL) {
		delete []a;
		a = NULL;
	}
	a = new double [m_ * n_];
	memset(a, 0, m * n * sizeof (double));
}

void Matrix::set(double* a_, int m_, int n_) 
{
	m = m_;
	n = n_;
	i = j = 0;
	if (a != NULL) {
		delete []a;
		a = NULL;
	}
	a = new double[m_ * n_];
	memcpy(a, a_, m * n * sizeof (double));
}


//将矩阵初始化为m_*m_的单位矩阵 
void Matrix::Unit ()	
{
	for (i = 0; i < m; i++)
		a[i * m + i] = 1;
}


//矩阵元素置零
void Matrix::Null ()	
{
	memset(a, 0, m * n * sizeof (double));
}



//对 + 进行重载，使之能对矩阵进行运算
Matrix& Matrix::operator + (Matrix &b) 
{
	Matrix *tmp;
	if (m != b.m || n != b.n) printf("两个行数或列数不同的矩阵不能相加!!!\n");
	else{
		tmp = new Matrix (m, n);
		for (i = 0; i < m * n; i++)
			tmp->a[i] = a[i] + b.a[i];
	}
	return *tmp;
}

//对 - 进行重载，使之能对矩阵进行运算
Matrix& Matrix::operator - (Matrix &b)	
{
	Matrix *tmp;
	if (m != b.m || n != b.n) printf("两个行数或列数不同的矩阵不能相减!!!\n");
	else{
		tmp = new Matrix (m, n);
		for (i = 0; i < m * n; i++)
			tmp->a[i] = a[i] - b.a[i]; 
	}
	return *tmp;
}

//对 * 进行重载，使之能对矩阵进行运算
Matrix& Matrix::operator *(Matrix &b)	
{
	int k;			//循环变量
	Matrix *tmp;
	if (n != b.m) {
		printf("这两个矩阵行数和列数不符合乘法规定!!!\n");
		return *this;
	}
	else{
		tmp = new Matrix (m, b.n);
		for (i = 0; i < m; i++)
			for (j = 0; j < b.n; j++)
				for (k = 0; k < n; k++) {
					tmp->a[i * b.n + j] 
						+= a [i * n + k] * b.a [k * b.n + j];
				}
	}
	return *tmp;
}

//对 * 进行重载，使之能进行 矩阵 * 实数 运算
Matrix& Matrix::operator * (double b) 
{
	Matrix *tmp = new Matrix (m, n);
	for (i = 0; i < m * n; i++)
		tmp->a[i] = a[i] * b;
	return *tmp;
}

//对 = 进行重载，使之能对矩阵进行运算
Matrix& Matrix::operator = (Matrix &b)	
{
	this->m = b.m;
	n = b.n;
	i = j = 0;
	if (a != NULL) {
		delete []a;
		a = NULL;
	}
	a = new double[m * n];
	memcpy (a, b.a, m * n * sizeof (double));
	return *this;
}

//对 () 进行重载，以行号和列号取得某个元素 (a[m_*n+n_]) 的值
double& Matrix::operator () (int m_, int n_)	
{
	return a [m_ * n + n_];
}

//求矩阵的转置矩阵
Matrix& Matrix::operator ~ () 
{
	Matrix *tmp = new Matrix (n, m);
	for (i = 0; i < n; i++)
		for (j = 0; j < m; j++)	tmp->a[i * m + j] = a[j * n + i];
		return *tmp;
}

//求矩阵的逆矩阵
Matrix& Matrix::operator ! ()
{
	Matrix *tmp = new Matrix (a, m, n);
/*	int k, m1;			//仅用于正定阵
	double w, g;
	double* p;
	p = new double[m];
	memset (p, 0, m * 8);
	// 逐列处理
	for (k = 0; k <= m - 1; k++) { 
		w = tmp->a[0];
		if (w == 0) {
			printf(" 本程序只能计算正定矩阵!!!!\n第一个元素不能为 0 !\n");
			return *tmp * 0;
		}
		m1 = m - k - 1;
		for (i = 1; i <= m - 1; i++) { 
			g = tmp->a[i * tmp->m + 0]; 
			p[i] = g / w;
			if (i <= m1)		p[i] = -p[i];
			for (j = 1; j <= i; j++)
				tmp->a[(i - 1) * m + j - 1] = tmp->a[i * m + j] + g * p[j];
		}
		tmp->a[(m - 1) * m + m - 1] = 1.0 / w;
		for (i = 1; i <= m - 1; i++) 
			tmp->a[(m - 1) * m + i - 1] = p[i];
	}
	// 行列调整
	for (i = 0; i <= m - 2; i++)
		for (j = i + 1; j <= m - 1; j++)	
			tmp->a[i * m + j] = tmp->a[j * m + i];*/
	dcinv(tmp->a,m);
		return *tmp;
}

//显示矩阵，显示n_位小数，共显示m_位数，(%m.nf)
int Matrix::display(int m_,int n_)  
{
	printf("%d行，%d列\n",m,n);
	printf("%3d\t",0);
	for(i=0;i<n;i++)	{
		printf("%6d   \t",i+1);
	}
	printf("\n");
	for(i=0;i<m;i++)	{
		printf("%3d\t",i+1);
		for(j=0;j<n;j++)	{
			if(m_==0&&n_==0)
				printf("%f\t",a[i*n+j]);
			else 
				printf("%*.*f\t",m_,n_,a[i*n+j]);
		}
		printf("\n");
	}
	return 1;
}

//对 * 进行重载，使之能进行 实数 * 矩阵 运算
Matrix& operator * (double b, Matrix &c)  
{
	Matrix *tmp = new Matrix (c.a, c.m, c.n);
	int i;
	for (i = 0; i < c.m * c.n; i++)
		tmp->a[i] *= b;
	return *tmp;
}

int dcinv (double *a, int n)	 // 求a[n][n]的逆 
{
	int *is, *js, i, j, k, l, u, v;
	double d, p;
	is = (int *) malloc (n * sizeof (int));
	js = (int *) malloc (n * sizeof (int));
	for (k = 0; k <= n - 1; k++)	  {
		d = 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 == 1)	{
			free (is);
			free (js);
			printf ("Err**not inv\n");
			return (0);
		}
		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 / 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;
			}
		}
	}
	free (is);
	free (js);
	return (1);
}