matrix.cpp

基于VC环境下的组合导航卡尔曼滤波仿真器设计

#include "stdafx.h"
#include <stdio.h>
#include <math.h>
#include <string.h>
#include <fstream.h>
// 本程序实现matrix类
// 对matrix类的定义
#include "matrix.h"

matrix::matrix(buffer * b): // 缺省构造函数，产生0行0列空矩阵
	rownum(0),colnum(0),istrans(0),isneg(0),issym(0)
{
		if(b){ // 如用户给出b，则使用b作为数据缓存
			buf=b;
			buf->alloc(0);
			}
		else // 否则，产生一个新的缓存，类别是当前缺省的缓存类
			buf = getnewbuffer(0);
}

matrix::matrix(size_t n, buffer * b): // 产生n阶单位方阵
		rownum(n),colnum(1),istrans(0),isneg(0),issym(0)
{
		if(b){ // 如用户给出b，则使用b作为数据缓存
			buf=b;
			buf->alloc(n);
			}
		else // 否则，产生一个新的缓存，类别是当前缺省的缓存类
			buf = getnewbuffer(n);
}

matrix unit(size_t n)	// 产生n阶单位矩阵
{
	if(n==0) throw TMESSAGE("n must larger than 0\n");
	matrix m(n,n);
	for(size_t i=0; i<n; i++)
	for(size_t j=0; j<n; j++)
		if(i==j) m.set(i,j,1.0);
		else m.set(i,j,0.0);
	m.issym = 1;
	return m;
}


matrix::matrix(size_t r, size_t c, buffer * b):
		rownum(r),colnum(c),istrans(0),isneg(0),issym(0)
{
		if(b)// 如用户给出b，则使用b作为数据缓存
		{ 
			buf=b;
			buf->alloc(r*c);
		}
		else // 否则，产生一个新的缓存，类别是当前缺省的缓存类
			buf=getnewbuffer(r*c);
}

matrix::matrix(matrix& m) // 拷贝构造函数
{
	rownum = m.rownum;	// 拷贝行数与列数
	colnum = m.colnum;
	istrans = m.istrans;	// 拷贝转置标记
	isneg = m.isneg;		// 拷贝负值标记
	issym = m.issym;		// 拷贝对称标记
	buf = m.buf;		// 设置指向相同缓存的指针
	buf->refnum++;	// 缓存的引用数增1
}

matrix::matrix(const char * filename, buffer * b): // 从数据文件构造矩阵
	istrans(0), isneg(0), issym(0)
{
	char label[10];
	ifstream in(filename); // 打开文件流
	in >> label; // 文件开始必须有matrix关键词
	if(strcmp(label, "matrix")!=0) throw TMESSAGE("format error!");
	in >> rownum >> colnum; // 读取行数和列数
	if(!in.good()) throw TMESSAGE("read file error!");
	// 申请或产生缓存
	if(b) { buf=b;
		buf->alloc(rownum*colnum);
	}
	else buf = getnewbuffer(rownum*colnum);
	size_t line;
	for(size_t i=0; i<rownum; i++) { // 依次按行读取
		in >> line; // 读行号
		if(line != i+1) throw TMESSAGE("format error!");
		in.width(sizeof(label));
		in >> label; // 行号后跟冒号
		if(label[0] != ':') throw TMESSAGE("format error!");
		DOUBLE a;
		for(size_t j=0; j<colnum; j++) { // 读取一行数据
			in >> a;
			set(i,j,a);
		}
		if(!in.good()) throw TMESSAGE("read file error!");
	}
	checksym(); // 检查是否为对称阵
}

matrix::matrix(void * data, size_t r, size_t c, buffer * b):
		rownum(r),colnum(c),istrans(0),isneg(0),issym(0) // 数据构造函数
{
	if(b){
		buf=b;
		buf->alloc(r*c);
		}
	else
		buf = getnewbuffer(r*c);
	DOUBLE * d = (DOUBLE *)data;
	for(size_t i=0; i<r*c; i++)  // 这里进行数据拷贝，因此原数据的内存是可以释放的
		buf->set(i,d[i]);
	checksym();	// 检查是否为对称阵
}

DOUBLE matrix::operator()(size_t r, size_t c)
{
	if(r>= rownum || c>= colnum)
		throw TMESSAGE("Out range!");
	return value(r,c);
}

matrix& matrix::operator=(matrix& m)  // 赋值重载
{
	rownum = m.rownum; //  行数和列数的拷贝
	colnum = m.colnum;
	istrans = m.istrans; // 转置标志的拷贝
	isneg = m.isneg;	// 取负标志的拷贝
	issym = m.issym;	// 对称标志的拷贝
	if(buf == m.buf)  // 如果原缓存与m的缓存一样，则返回
		return (*this);
	buf->refnum--; // 原缓存不同，则原缓存的引用数减1
	if(!buf->refnum)delete buf; // 减1后的原缓存如果引用数为0，则删除原缓存
	buf = m.buf; // 将原缓存指针指向m的缓存
	buf->refnum++; // 新缓存的引用数增1
	checksym();	// 检查是否为对称阵
	return (*this); // 返回自己的引用
}

matrix& matrix::operator=(DOUBLE a) // 通过赋值运算符将矩阵所有元素设为a
{
	if(rownum == 0 || colnum == 0) return (*this);
	for(size_t i=0; i<rownum; i++)
	for(size_t j=0; j<colnum; j++)
		set(i,j,a);
	if(rownum == colnum)issym = 1;
	return (*this);
}

matrix matrix::operator-() // 矩阵求负，产生负矩阵
{
	matrix mm(*this);
	mm.neg();
	return mm;
}

ostream& operator<<(ostream& o, matrix& m) // 流输出运算符
{
	// 先输出关键字matrix,然后是行号，制表符，列号，换行
	o << "matrix " << m.rownum << '\t' << m.colnum << endl;
	for(size_t i=0; i<m.rownum; i++) { // 依次输出各行
		o<< (i+1) <<':'; // 行号后跟冒号。注意输出的行号是从1开始
				// 是内部编程的行号加1
		size_t k=8; // 后面输入一行的内容，每次一行数字超过八个时
			// 就换一行
		for(size_t j=0; j<m.colnum; j++) {
			o<<'\t'<<m.value(i,j);
			if(--k==0) {
				k=8;
				o<<endl;
			}
		}
		o<<endl; // 每行结束时也换行
	}
	return o;
}

istream& operator>>(istream& in, matrix& m) // 流输入运算符
{
	char label[10];
	in.width(sizeof(label));
	in >> label; // 输入matrix关键字
	if(strcmp(label, "matrix")!=0)
		throw TMESSAGE("format error!\n");
	in >> m.rownum >> m.colnum; // 输入行号和列号
	if(!in.good()) throw TMESSAGE("read file error!");
	m.buf->refnum--; // 原缓存引用数减1
	if(!m.buf->refnum) delete m.buf; // 如原缓存引用数为0，则删去原缓存
	m.isneg = m.istrans = 0; // 转置和取负标志清零
	m.buf = getnewbuffer(m.rownum*m.colnum); // 按缺省子类产生新缓存
	size_t line; // 按矩阵行输入
	for(size_t i=0; i<m.rownum; i++) {
		in >> line; // 先输入行号
		if(line != i+1) throw TMESSAGE("format error!\n");
		in.width(sizeof(label)); // 行号后应跟冒号
		in >> label;
		if(label[0] != ':') throw TMESSAGE("format error!\n");
		DOUBLE a; // 随后是本行的各个数值
		for(size_t j=0; j<m.colnum; j++) {
			in >> a;
			m.set(i,j,a);
		}
		if(!in.good()) throw TMESSAGE("read file error!");
	}
	m.checksym();	// 检查是否为对称阵
	return in;
}

matrix& matrix::operator*=(DOUBLE a) // 矩阵数乘常数a，结果放在原矩阵
{
	for(size_t i=0; i<rownum; i++)
	for(size_t j=0; j<colnum; j++)
		set(i,j,a*value(i,j));
	return (*this);
}

matrix matrix::operator*(DOUBLE a) // 矩阵数乘常数a，原矩阵内容不变，返回一新矩阵
{
	matrix m(rownum, colnum);
	for(size_t i=0; i<rownum; i++)
	for(size_t j=0; j<colnum; j++)
		m.set(i,j,a*value(i,j));
	return m;
}

matrix matrix::operator+(matrix& m) // 矩阵相加，产生一新的矩阵并返回它
{
	if(rownum != m.rownum || colnum != m.colnum) // 对应行列必须相同
		throw TMESSAGE("can not do add of matrix\n");
	matrix mm(rownum, colnum); // 产生一同自己同形的矩阵
	DOUBLE a;
	for(size_t i=0; i<rownum; i++) // 求和
	for(size_t j=0; j<colnum; j++)
	{
		a = value(i,j)+m.value(i,j);
		mm.set(i,j,a);
	}
	mm.checksym();	// 检查是否为对称阵
	return mm;
}

matrix& matrix::operator+=(matrix &m) // 矩阵求和，自己内容改变为和
{
	DOUBLE a;
	for(size_t i=0; i<rownum; i++)
	for(size_t j=0; j<colnum; j++)
	{
		a = value(i,j)+m.value(i,j);
		set(i,j,a);
	}
	checksym();	// 检查是否为对称阵
	return (*this);
}

matrix matrix::operator+(DOUBLE a)	// 矩阵加常数，指每一元素加一固定的常数，产生
											// 新矩阵，原矩阵不变
{
	matrix m(rownum, colnum);
	for(size_t i=0; i<rownum; i++)
	for(size_t j=0; j<colnum; j++)
		m.set(i,j,a+value(i,j));
	return m;
}

matrix& matrix::operator+=(DOUBLE a)	// 矩阵自身加常数，自身内容改变
{
	for(size_t i=0; i<rownum; i++)
	for(size_t j=0; j<colnum; j++)
		set(i,j,a+value(i,j));
	return (*this);
}

matrix operator-(DOUBLE a, matrix& m) { // 常数减矩阵，产生新的矩阵
	return (-m)+a;
};


matrix matrix::operator-(matrix& m) // 矩阵相减，产生新的矩阵
{
	matrix mm(*this); // 产生一同自己同形的矩阵
	mm += (-m);	// 加上相应的负矩阵
	return mm;
}

matrix& matrix::operator-=(matrix& m) // 矩阵相减，结果修改原矩阵
{
	(*this) += (-m);
	return (*this);
}

matrix matrix::operator*(matrix& m) // 矩阵相乘，原矩阵内容不变，产生一新矩阵
{
	if(colnum != m.rownum) // 必须满足相乘条件
		throw TMESSAGE("can not multiply!");
	matrix mm(rownum,m.colnum); // 计算并产生一合要求的矩阵放乘积
	DOUBLE a;
	for(size_t i=0; i<rownum; i++) // 计算乘积
	for(size_t j=0; j<m.colnum; j++){
		a = 0.0;
		for(size_t k=0; k<colnum; k++)
			a += value(i,k)*m.value(k,j);
		mm.set(i,j,a);
	}
	mm.checksym();	// 检查是否为对称阵
	return mm; // 返回乘积
}

matrix& matrix::operator*=(matrix& m) // 矩阵相乘，自己修改成积矩阵
{
	(*this) = (*this)*m;
	return (*this);
}

matrix matrix::t()  // 矩阵转置，产生新的矩阵
{
	matrix mm(*this);
	mm.trans();
	return mm;
}

int matrix::isnear(matrix& m, double e) // 检查二矩阵是否近似相等
{
	if(rownum != m.rownum || colnum != m.colnum) return 0;
	for(size_t i=0; i< rownum; i++)
	for(size_t j=0; j< colnum; j++)
		if(fabs(value(i,j)-m.value(i,j)) > e) return 0;
	return 1;
}

int matrix::isnearunit(double e) // 检查矩阵是否近似为单位矩阵
{
	if(rownum != colnum) return 0;
	return isnear(unit(rownum), e);
}

matrix matrix::row(size_t r) // 提取第r行行向量
{
	matrix mm(1, colnum);
	for(int i=0; i< colnum; i++)
		mm.set(0, i, value(r,i));
	return mm;
}

matrix matrix::col(size_t c) // 提取第c列列向量
{
	matrix mm(rownum, 1);
	for(int i=0; i< rownum; i++)
		mm.set(i, value(i, c));
	return mm;
}

void matrix::swapr(size_t r1, size_t r2, size_t k) // 交换矩阵r1和r2两行
{
	DOUBLE a;
	for(size_t i=k; i<colnum; i++) {
		a = value(r1, i);
		set(r1, i, value(r2, i));
		set(r2, i, a);
	}
}

void matrix::swapc(size_t c1, size_t c2, size_t k) // 交换c1和c2两列
{
	DOUBLE a;
	for(size_t i=k; i<colnum; i++) {
		a = value(i, c1);
		set(i, c1, value(i, c2));
		set(i, c2, a);
	}
}

DOUBLE matrix::maxabs(size_t &r, size_t &c, size_t k) // 求第k行和第k列后的主元及位置
{
	DOUBLE a=0.0;
	for(size_t i=k;i<rownum;i++)
	for(size_t j=k;j<colnum;j++)
		if(a < fabs(value(i,j))) {
			r=i;c=j;a=fabs(value(i,j));
		}
	return a;
}

size_t matrix::zgsxy(matrix & m, int fn) // 进行主高斯消元运算，fn为参数，缺省为0