matrix.cpp

高精度遥感处理算法(RPC正解)

// Matrix.cpp: implementation of the CMatrix class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
//#include "giscn.h"
#include "Matrix.h"
#include "math.h"
#include <bitset>
#include "Tokenizer.h"

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

CMatrix::CMatrix()
{
	
	m_nNumColumns=1;
	m_nNumRows=1;
	m_pData=NULL;
	Init(m_nNumRows,m_nNumColumns);
}
CMatrix::CMatrix(int nRows,int nCols)
{
	m_nNumRows=nRows;
	m_nNumColumns=nCols;
	m_pData=NULL;
	Init(m_nNumRows,m_nNumColumns);
}
//指定值构造函数 
CMatrix::CMatrix(int nRows,int nCols,double value[])
{
	m_nNumRows=nRows;
	m_nNumColumns=nCols;
	m_pData=NULL;
	Init(m_nNumRows,m_nNumColumns);
	SetData(value);
}
///方阵构造函数 
CMatrix::CMatrix(int nSize)
{
	m_nNumRows=nSize;
	m_nNumColumns=nSize;
	m_pData=NULL;
	Init(nSize,nSize);
}
///方阵构造函数（有值） 
CMatrix::CMatrix(int nSize,double value[])
{
	m_nNumRows=nSize;
	m_nNumColumns=nSize;
	m_pData=NULL;
	Init(nSize,nSize);
	SetData(value);
}
//拷贝构造函数 
CMatrix::CMatrix(const CMatrix& other)
{
	m_nNumColumns=other.GetNumColumns();
	m_nNumRows=other.GetNumRows();
	m_pData=NULL;
	Init(m_nNumRows,m_nNumColumns);

	memcpy(m_pData,other.m_pData,sizeof(double)*m_nNumColumns*m_nNumRows);
}
//初始化函数
bool CMatrix::Init(int nRows,int nCols)
{
	if(m_pData)
	{
		delete[] m_pData;
		m_pData=NULL;
	}
	m_nNumRows=nRows;
	m_nNumColumns=nCols;
	int nSize=nCols*nRows;
	if(nSize<0)
		return FALSE;
	m_pData=new double[nSize];
	if(m_pData==NULL)
		return FALSE;
	if(IsBadReadPtr(m_pData,sizeof(double)*nSize))
		return FALSE;
	memset(m_pData,0,sizeof(double)*nSize);
	return TRUE;
}
//将方阵初始化为单位矩阵
bool CMatrix::MakeUnitMatrix(int nSize)
{
	if(!Init(nSize,nSize))
		return FALSE;
	for(int i=0;i<nSize;++i)
		for(int j=0;j<nSize;++j)
			if(i==j)
				SetElement(i,j,1);
	return TRUE;
}

bool CMatrix::FromString(CString s,const CString&sDelim/*=""*/,bool bLineBreak/*=TRUE*/)
{
	if(s.IsEmpty())
	return FALSE;
	if(bLineBreak)
	{
	   CTokenizer tk(s,"\r\n");
		CStringList ListRow;
		CString sRow;
		while(tk.Next(sRow))
		{
			sRow.TrimLeft();
			sRow.TrimRight();
			if(sRow.IsEmpty())
				break;
			ListRow.AddTail(sRow);
		}
		m_nNumRows=ListRow.GetCount();
		sRow=ListRow.GetHead();
		CTokenizer tkRow(sRow,sDelim);
		CString sElement;
		m_nNumColumns=0;
		while(tk.Next(sElement))
		{
			m_nNumColumns++;
		}
		if(!Init(m_nNumRows,m_nNumColumns))
			return FALSE;
		POSITION pos=ListRow.GetHeadPosition();
		for(int i=0;i<m_nNumRows;i++)
		{
			sRow=ListRow.GetNext(pos);
			int j=0;
			CTokenizer tkRow(sRow,sDelim);
			while(tk.Next(sElement))
			{
				sElement.TrimLeft();
				sElement.TrimRight();
				double v=atof(sElement);
				SetElement(i,j++,v);
			}
		}
		return TRUE;
}
	//
	CTokenizer tk(s,sDelim);
	CString sElement;
	//列数
	tk.Next(sElement);
	sElement.TrimLeft();
	sElement.TrimRight();
	m_nNumRows=atoi(sElement);
	//行数
	tk.Next(sElement);
	sElement.TrimLeft();
	sElement.TrimRight();
	m_nNumColumns=atoi(sElement);
	//初始化矩阵
	if(!Init(m_nNumRows,m_nNumColumns))
		return FALSE;
	int i=0,j=0;
	while(tk.Next(sElement))
	{
		sElement.TrimLeft();
		sElement.TrimRight();
		double v=atof(sElement);
		SetElement(i,j++,v);
		if(j==m_nNumColumns)
		{
			j=0;
			i++;
			if(i==m_nNumRows)
				break;
		}
	}
	return TRUE;
}		

CString CMatrix::ToString(const CString& sDelim/*=""*/,bool bLineBreak/*=TRUE*/)const
{
	CString s="";
	for(int i=0;i<m_nNumRows;++i)
	{	
		for(int j=0;j<m_nNumColumns;++j)
		{
			CString ss;
			ss.Format("%f",GetElement(i,j));
			s+=ss;
			if(bLineBreak)
			{
				if(j!=m_nNumColumns-1)
					s+=sDelim;
			}
			else
			{
				if(i!=m_nNumRows-1 || j!=m_nNumColumns-1)
					s+=sDelim;
			}
			
		}
		if(bLineBreak)
			if(i!=m_nNumRows-1)
				s+="\r\n";
	}
	return s;
}
CMatrix::getMatrix(int nRow,int nCol, double head[])
{
	Init(nRow,nCol);
	SetData(head);
}

CString CMatrix::RowToString(int nRow,const CString&sDelim/*=""*/)const
{
	CString s="";
	if(nRow>=m_nNumColumns)
		return s;
	for(int j=0;j<m_nNumColumns;++j)
	{
		CString ss;
		ss.Format("%f",GetElement(nRow,j));
		s+=ss;
		if(j!=m_nNumColumns-1)
			s+=sDelim;
	}
	return s;
}

CString CMatrix::ColToString(int nCol,const CString& sDelim/*=""*/)const
{
	CString s="";
	if(nCol>=m_nNumColumns)
		return s;
	for(int i=0;i<m_nNumRows;++i)
	{
		CString ss;
		ss.Format("%f",GetElement(i,nCol));
		s+=ss;
		if(i!=m_nNumRows-1)
			s+=sDelim;
	}
	return s;
}	

void CMatrix::SetData(double value[])
{
	memset(m_pData,0,sizeof(double) *m_nNumColumns *m_nNumRows);
	memcpy(m_pData,value,sizeof(double)*m_nNumColumns*m_nNumRows);
}
bool CMatrix::SetElement(int nRow,int nCol,double value)
{
	if(nCol<0||nCol>=m_nNumColumns||nRow<0||nRow>m_nNumRows)
		return FALSE;
	if(m_pData==NULL)
		return FALSE;
	m_pData[nCol+nRow*m_nNumColumns]=value;
	return TRUE;
}

double CMatrix::GetElement(int nRow,int nCol)const
{
	ASSERT(nCol>=0 && nCol<m_nNumColumns && nRow>=0 && nRow<m_nNumRows);
	ASSERT(m_pData);
	return m_pData[nCol+nRow*m_nNumColumns];
}

int CMatrix::GetNumColumns()const
{
	return m_nNumColumns;
}

int CMatrix::GetNumRows()const
{
	return m_nNumRows;
}
double* CMatrix::GetData()const
{
	return m_pData;

}

int CMatrix::GetRowVector(int nRow,double*pVector)const
{
	if(pVector==NULL)
		delete pVector;
	pVector=new double[m_nNumColumns];
	ASSERT(pVector!=NULL);
	for(int j=0;j<m_nNumColumns;++j)
		pVector[j]=GetElement(nRow,j);
	return m_nNumColumns;

}

int CMatrix::GetColVector(int nCol,double* pVector)const
{
	if(pVector==NULL)
		delete pVector;
	pVector=new double[m_nNumRows];
	ASSERT(pVector!=NULL);
	for(int i=0;i<m_nNumRows;++i)
		pVector[i]=GetElement(i,nCol);
	return m_nNumRows;
}

CMatrix& CMatrix::operator =(const CMatrix& other)
{
	if(&other!=this)
	{
		m_nNumColumns=other.GetNumColumns();
		m_nNumRows=other.GetNumRows();
		Init(m_nNumRows,m_nNumColumns);
		memcpy(m_pData,other.m_pData,sizeof(double)*m_nNumColumns*m_nNumRows);

	}
	return *this;
}

bool CMatrix::operator ==(const CMatrix& other)const
{
	if(m_nNumColumns!=other.GetNumColumns()||m_nNumRows!=other.GetNumRows())
		return FALSE;
	for(int i=0;i<m_nNumRows;++i)
	{
		for(int j=0;j<m_nNumColumns;++j)
		{
			if(GetElement(i,j)!=other.GetElement(i,j))
				return FALSE;
		}
	}
	return TRUE;
}

bool CMatrix::operator !=(const CMatrix& other)const
{
	return!(*this==other);
}
CMatrix CMatrix::operator +(const CMatrix& other)const
{
	ASSERT(m_nNumColumns==other.GetNumColumns()&&m_nNumRows==other.GetNumRows());
	CMatrix result(*this);
		for(int i=0;i<m_nNumRows;++i)
		{
			for(int j=0;j<m_nNumColumns;++j)
				result.SetElement(i,j,result.GetElement(i,j)+other.GetElement(i,j));
		}
		return result;
}
CMatrix CMatrix::operator -(const CMatrix& other)const
{		
	ASSERT (m_nNumColumns==other.GetNumColumns()&&m_nNumRows==other.GetNumRows());
	CMatrix result(*this);
	for(int i=0;i<m_nNumRows;++i)
	{
		for(int j=0;j<m_nNumColumns;++j)
			result.SetElement(i,j,result.GetElement(i,j)-other.GetElement(i,j));

	}
	return result;

}
CMatrix CMatrix::operator *(double value)const
{
	CMatrix result(*this);
	for(int i=0;i<m_nNumRows;++i)
	{
		for(int j=0;j<m_nNumColumns;++j)
			result.SetElement(i,j,result.GetElement(i,j)*value);
	}
	return result;
}
CMatrix CMatrix::operator*(const CMatrix& other)const
{
	ASSERT(m_nNumColumns==other.GetNumRows()); 
	/////
	m_nNumColumns==other.GetNumRows();
	CMatrix result(m_nNumRows,other.GetNumColumns());
	double value;
	for(int i=0;i<result.GetNumRows();++i)
	{
		for(int j=0;j<other.GetNumColumns();++j)
		{	
			value=0.0;
			for(int k=0;k<m_nNumColumns;++k)
			{
				value+=GetElement(i,k)*other.GetElement(k,j);
			}
			result.SetElement(i,j,value);
		}
	}

	/*CMatrix result(m_nNumRows,other.GetNumColumns());
	double value;
	for(int i=0;i<m_nNumRows;i++)
	{
		for(int j=0;j<other.GetNumColumns();j++)
		{	
			value=0.0;
			for(int k=0;k<other.GetNumRows();k++)
			{
				value+=GetElement(i,k)*other.GetElement(k,j);
			}
			result.SetElement(i,j,value);
		}
	}	*/	
	return result;
}

CMatrix CMatrix::Transpose()const
{
	CMatrix Trans(m_nNumColumns,m_nNumRows);
	for(int i=0;i<m_nNumRows;++i)
	{
		for(int j=0;j<m_nNumColumns;++j)
			Trans.SetElement(j,i,GetElement(i,j));
	}
	return Trans;
}
bool CMatrix::InvertGaussJordan()
{
	int *pnRow,*pnCol,i,j,k,l,u,v;
	double d=0,p=0;
	//分配内存
	pnRow=new int[m_nNumColumns];
	pnCol=new int[m_nNumColumns];
	if(pnRow==NULL||pnCol==NULL)
	{	
		AfxMessageBox("空");
		return FALSE;
	}
	//消元
	for(k=0;k<=m_nNumColumns-1;k++)
	{
		d=0.0;
		for(i=k;i<=m_nNumColumns-1;i++)
		{
			for(j=k;j<=m_nNumColumns-1;j++)
			{
				l=i*m_nNumColumns+j;p=fabs(m_pData[1]);
				if(p>d)
				{
					d=p;
					pnRow[k]=i;
					pnCol[k]=j;
				}
			}
		}
//
		if(d==0.0)
		{
			delete[] pnRow;
			delete[] pnCol;
		//	AfxMessageBox("消元失败");
			return FALSE;
		}
		if(pnRow[k]!=k)
		{
			for(j=0;j<=m_nNumColumns-1;j++)
			{
				u=k*m_nNumColumns+j;
				v=pnRow[k]*m_nNumColumns+j;
				p=m_pData[u];
				m_pData[u]=m_pData[v];
				m_pData[v]=p;
			}
		}
		if(pnCol[k]!=k)
		{
			for(i=0;i<=m_nNumColumns-1;i++)
			{
				u=i*m_nNumColumns+k;
				v=i*m_nNumColumns+pnCol[k];
				p=m_pData[u];
				m_pData[u]=m_pData[v];
				m_pData[v]=p;
			}
		}
		l=k*m_nNumColumns+k;
		m_pData[l]=1.0/m_pData[l];
		for(j=0;j<=m_nNumColumns-1;j++)
		{
			if(j!=k)
			{
				u=k*m_nNumColumns+j;
				m_pData[u]=m_pData[u]*m_pData[l];
			}
		}
		for(i=0;i<=m_nNumColumns-1;i++)
		{
			if(i!=k)
			{
				for(j=0;j<=m_nNumColumns-1;j++)
				{
					if(j!=k)
					{
						u=i*m_nNumColumns+j;
						m_pData[u]=m_pData[u]-m_pData[i*m_nNumColumns+k]*m_pData[k*m_nNumColumns+j];
					}
				}
				
			}
		}
		for(i=0;i<=m_nNumColumns-1;i++)
		{
			if(i!=k)
			{
				u=i*m_nNumColumns+k;
				m_pData[u]=-m_pData[u]*m_pData[l];
			}
		}
	}
for(k=m_nNumColumns-1;k>=0;k--)
{
	if(pnCol[k]!=k)
	{	
		for(j=0;j<=m_nNumColumns-1;j++)
		{
			u=k*m_nNumColumns+j;
			v=pnCol[k]*m_nNumColumns+j;
			p=m_pData[u];
			m_pData[u]=m_pData[v];
			m_pData[v]=p;

		}

	}
	if(pnRow[k]!=k)
	{
		for(i=0;i<=m_nNumColumns-1;i++)
		{
			u=i*m_nNumColumns+k;
			v=i*m_nNumColumns+pnRow[k];
			p=m_pData[u];
			m_pData[u]=m_pData[v];
			m_pData[v]=p;
		}
	}
}
delete[] pnRow;
delete[] pnCol;
return TRUE;

}
//析构函数
CMatrix::~CMatrix()
{
	if(m_pData)
	{
		delete[] m_pData;
		m_pData=NULL;
	}
}