matrix.cpp

摄影测量中进行后方交会的程序

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

#include "stdafx.h"
#include "Matrix.h"
#include "iostream.h"
#include "math.h"
#include "string.h"
//////////////////////////////////////////////////////////////////////
// Construction/Destruction                                         //
//////////////////////////////////////////////////////////////////////
#define MinError 0.00000000000000001


CMatrix::CMatrix()
{
   m_Line=0;
   m_Row=0;
   m_Data=NULL;
}

CMatrix::~CMatrix()
{
   if(NULL != m_Data)
   {
	   delete []m_Data;
	   m_Data = NULL;
   }
}

CMatrix::CMatrix(int r,int l,bool f,double *a)
{
	m_Row=r;
	m_Line=l;
	m_Data= new double [m_Row*m_Line];

    if(!f)//f为假时生成零矩阵
	{
	    for(int i=0;i<m_Row;i++)	
			for(int j=0;j<m_Line;j++)
			    m_Data[i*m_Line+j] = 0;			   
	}
	else//f为真时自定义矩阵
	{	
		if(a)
		{
		   for(int i=0;i<m_Row;i++)	
		      for(int j=0;j<m_Line;j++)
		         m_Data[i*m_Line+j] = a[i*l+j];
		}
		else
		{
			for(int i=0;i<m_Row;i++) 
				for(int j=0;j<m_Line;j++)
					cin>>m_Data[i*m_Line+j];
	
		}
	}
}



bool CMatrix::ChangeRow(int m, int n)
{
/*   if(m<1||m>m_Row||n<1||n>m_Row)
	   return false;
   double *temp;
   temp=m_Data[m-1];
   m_Data[m-1]=m_Data[n-1];
   m_Data[n-1]=temp;*/
   return true;
}

bool CMatrix::ChangeLine(int m, int n)
{
/*   if(m<1||m>m_Line||n<1||n>m_Line)
	  return false;

   transpose();
   double *temp;
   temp=m_Data[m-1];
   m_Data[m-1]=m_Data[n-1];
   m_Data[n-1]=temp; 
   transpose();*/
   return true;
}


void CMatrix::DisplayMatrix()
{
   for(int i=0;i<m_Row;i++)
	{
		for(int j=0;j<m_Line;j++)
			cout<<m_Data[i*m_Line+j]<<"  ";
        cout<<endl;
	}
   cout<<endl;
}


void CMatrix::transpose()
{
    if(m_Data == NULL)
		return;
//    if(m_Line==m_Row)//行列相同的转置
//	{
	double* pTemp = new double[m_Line*m_Row];
	for(int i=0; i<m_Row; i++)
		for(int j=0; j<m_Line; j++)
			pTemp[j*m_Row+i] = m_Data[i*m_Line+j];
	int temp1 = 0;
	temp1=m_Row;	m_Row=m_Line;	m_Line=temp1;
	delete []m_Data;
	m_Data = pTemp;
/*	}
	else//行列不同的转置
	{		
		double *dtemp;
	    dtemp = new double[m_Row*m_Line];//分配内存空间

		int temp1;
		temp1=m_Row;	m_Row=m_Line;	m_Line=temp1;
		if(NULL != m_Data)
			delete []m_Data;
        m_Data = new double[m_Line*m_Row];
		
		for(i=0;i<m_Row;i++)
			for(int j=0;j<m_Line;j++)
				m_Data[i][j]=dtemp[j][i];
        for(i=0;i<m_Line;i++)  delete [m_Row]dtemp[i];
	    delete [m_Line]dtemp;
	}*/
}

bool CMatrix::InsertRow(int *m,double *a,int n,int t)//n为a[][]的行数，t为a[][]的列数
{
/*	if(t!=m_Line)
		return false;
	int j;
	double **dtemp;
	dtemp= new double*[m_Row];//分配内存空间
	
    for(int i=0;i<m_Row;i++)   dtemp[i]= m_Data[i];
	m_Row = m_Row+n;
	m_Data= new double*[m_Row];//分配内存空间

	for(i=0;i<n;i++)
	{
		m_Data[m[i]]=new double[m_Line];
		for(j=0;j<m_Line;j++)
			m_Data[m[i]][j]=a[i*t+j];
	}
	bool flag=false;
	for(j=0,i=0;i<m_Row;i++)
	{
		for(int k=0;k<n;k++) 
			if((m[k])==i) {flag=true;break;};
		if(!flag) {m_Data[i]=dtemp[j];j++;}
		flag=false;
	}*/
	return true;
}

bool CMatrix::InsertLine(int *m,double *a,int n,int t)//m[]必须按升序排列，在矩阵第m[i]列下加一列a[][i],n为a[][]的行数，t为a[][]的列数	
{
/*	if(n!=m_Row)
		return false;
	int j;
    transpose();
	double **dtemp;
	dtemp= new double*[m_Row];//分配内存空间
	
    for(int i=0;i<m_Row;i++)   dtemp[i]= m_Data[i];
	m_Row = m_Row+t;
	m_Data= new double*[m_Row];//分配内存空间

	for(i=0;i<t;i++)
	{
		m_Data[m[i]-1]=new double[m_Line];
		for(j=0;j<m_Line;j++)
			m_Data[m[i]-1][j]=a[j*t+i];
	}
	bool flag=false;
	for(j=0,i=0;i<m_Row;i++)
	{
		for(int k=0;k<t;k++) 
			if((m[k]-1)==i) {flag=true;break;};
		if(!flag) {m_Data[i]=dtemp[j];j++;}
		flag=false;
	}
	transpose();*/
	return true;
}

CMatrix CMatrix::operator*(const CMatrix &multiplier)
{
	CMatrix maRes;
	if(this->m_Line!=multiplier.m_Row)
		return maRes;
	double t=0;
	maRes.Initate(this->m_Row, multiplier.m_Line);
	int imuLine = multiplier.m_Line;
	for(int i=0;i<this->m_Row;i++)
	   for(int j=0;j<multiplier.m_Line;j++)
	   {
		   for(int k=0;k<this->m_Line;k++)
			   t+=this->m_Data[i*m_Line+k]*multiplier.m_Data[k*imuLine+j];
		   maRes.m_Data[i*imuLine+j]=t;
		   t=0;
	   }
	return maRes;
	
}

int CMatrix::GetRow()
{
	return m_Row;
}

int CMatrix::GetLine()
{
	return m_Line;
}

double* CMatrix::GetData()
{
	return m_Data;
}

double CMatrix::GetMatrixValue()
{
 /*   if(m_Line!=m_Row)
	{
		cout<<"This Matrix doesn't have value!"<<endl;
	    return 0.0;
	}
    int y=1,i,j,k,t=0;
    double *ptemp,f,x=1,**m_DataReplace;
//////////////////////////////////////////////////////////////////////////	
	m_DataReplace= new double*[m_Row];
	for(i=0;i<m_Row;i++) m_DataReplace[i]= new double[m_Line];
    for(i=0;i<m_Row;i++)
		for(j=0;j<m_Line;j++)  m_DataReplace[i][j]=m_Data[i][j];
//////////////////////////////////////////////////////////////////////////
    for(k=0;k<m_Row;k++)
	{
	    if(m_DataReplace[k][k]>-0.0000000000000001&&m_DataReplace[k][k]<0.0000000000000001)
		{
		   for(t=k+1;t<m_Row;t++)  if(m_DataReplace[t][k]<-0.0000000000000001||m_DataReplace[t][k]>0.0000000000000001) break;//寻找不是零的一行
		   if(t==m_Row) break;//主对角元素非零没找到
		   else
		   {
			  y*=(-1);
			  ptemp=m_DataReplace[t];
			  m_DataReplace[t]=m_DataReplace[k];
			  m_DataReplace[k]=ptemp;
		   }
		}
//////////////////////////////////////////////////////////////////////////
	    if(t==m_Row) break;
	    for(j=k+1;j<m_Row;j++)
		{
		   f=m_DataReplace[j][k]/m_DataReplace[k][k]; 
		   if(f>-0.0000000000000001&&f<0.0000000000000001) break;
		   for(i=k+1;i<m_Row;i++)
		   m_DataReplace[j][i]=m_DataReplace[j][i]-f*m_DataReplace[k][i];
		}
	}
	if(t==m_Row)  x=0;
	else
	{
       for(i=0;i<m_Row;i++) x*=m_DataReplace[i][i];
	}
	for(i=0;i<m_Row;i++)
      delete [m_Line]m_DataReplace[i];
	delete [m_Row]m_DataReplace;
    return x*y;
*/
	return 1;
}


CMatrix CMatrix::GetConvertMatrix()
{
	CMatrix ConMa;
    if(m_Line != m_Row)
	    return  ConMa;

    int y=1,i,j,k,t=0;
    double RLtemp = NULL,f,x=1,*m_DataReplace = NULL;
//////////////////////////////////////////////////////////////////////////
	ConMa.Initate(m_Row,m_Line);//生成对角矩阵
    for(i=0; i<m_Row; i++) 
		for(j=0; j<m_Line; j++)
			ConMa.m_Data[i*m_Line+j] = 0;
	for(i=0; i<m_Row; i++)
		ConMa.m_Data[i*m_Line+i] = 1;

	m_DataReplace= new double [m_Line*m_Row];
	memcpy(m_DataReplace, m_Data, sizeof(double)*m_Line*m_Row);

//////////////////////////////////////////////////////////////////////////
	int* piLineChange = new int[m_Row];
	int* piRowChange = new int[m_Line];
	bool flag = true;
    for(k=0;k<m_Row;k++)
	{
		//////////////////////////////////////////////////////////////////////////
		//寻找行列中的最大
		int imaxRow = k;
		int imaxLine = k;
		double dmaxNum = fabs(m_DataReplace[k*m_Line+k]);
		for(t=k; t<m_Row; t++)
			for(int t1=k; t1<m_Line; t1++)
				if(fabs(m_DataReplace[t*m_Line+t1]) > dmaxNum)
				{
					imaxRow = t;
					imaxLine = t1;
					dmaxNum = fabs(m_DataReplace[t*m_Line+t1]);
				}
		piLineChange[k] = imaxLine;
		piRowChange[k] = imaxRow;
		if(dmaxNum < MinError) 
			return ConMa;

		if(imaxRow != k)
			for(int i1=0; i1<m_Row; i1++)
			{			  
				RLtemp = m_DataReplace[imaxRow*m_Line+i1];
				m_DataReplace[imaxRow*m_Line+i1] = m_DataReplace[k*m_Line+i1];
				m_DataReplace[k*m_Line+i1] = RLtemp;	
				RLtemp = ConMa.m_Data[imaxRow*m_Line+i1];
				ConMa.m_Data[imaxRow*m_Line+i1] = ConMa.m_Data[k*m_Line+i1];
				ConMa.m_Data[k*m_Line+i1] = RLtemp;
			 }
		if(imaxLine != k)
			for(int i1=0; i1<m_Row; i1++)
			{			  
				RLtemp = m_DataReplace[i1*m_Line+imaxLine];
				m_DataReplace[i1*m_Line+imaxLine] = m_DataReplace[i1*m_Line+k];
				m_DataReplace[i1*m_Line+k] = RLtemp;	
				RLtemp = ConMa.m_Data[i1*m_Line+imaxLine];
				ConMa.m_Data[i1*m_Line+imaxLine] = ConMa.m_Data[i1*m_Line+k];
				ConMa.m_Data[i1*m_Line+k] = RLtemp;
			 }


		double f1 = m_DataReplace[k*m_Line+k];
		m_DataReplace[k*m_Line+k] = 1.0/m_DataReplace[k*m_Line+k];
		for(int i2=0; i2<m_Row; i2++)
		{
			if(i2 != k)
			{
			
				m_DataReplace[k*m_Line+i2] = m_DataReplace[k*m_Line+i2]/f1;
				ConMa.m_Data[k*m_Line+i2] = ConMa.m_Data[k*m_Line+i2]/f1;
			}
		}

	    for(j=0;j<m_Row;j++)
		{
		   if(j != k)
		   {			   
			   f = m_DataReplace[j*m_Line+k];
			   if(f>-MinError && f<MinError) break;
			   for(i=0; i<m_Row; i++) 
			   {
				   if(i != k)
				   {				   
						m_DataReplace[j*m_Line+i] = m_DataReplace[j*m_Line+i] -
											        f*m_DataReplace[k*m_Line+i]; 
						ConMa.m_Data[j*m_Line+i] = ConMa.m_Data[j*m_Line+i] - 
											       f*ConMa.m_Data[k*m_Line+i];
					}
			   }
		   }
		}
		for(int j1=0; j1<m_Line; j1++)
		{
			if(j1 != k)
				m_DataReplace[j1*m_Line+k] *= -m_DataReplace[k*m_Line+k];
		}
	}

	int imaxRow= 0;

	for(i=m_Line-1; i>=0; i--)
	{		

		if(piLineChange[i] != i)
		{
			imaxRow = piLineChange[i];
			for(int i1=0; i1<m_Row; i1++)
			{			  
				RLtemp = m_DataReplace[imaxRow*m_Line+i1];
				m_DataReplace[imaxRow*m_Line+i1] = m_DataReplace[i*m_Line+i1];
				m_DataReplace[i*m_Line+i1] = RLtemp;
			 }
		} 		
		if(piRowChange[i] != i)
		{
			imaxRow = piRowChange[i];
			for(int i1=0; i1<m_Row; i1++)
			{			  
				RLtemp = m_DataReplace[i1*m_Line+imaxRow];
				m_DataReplace[i1*m_Line+imaxRow] = m_DataReplace[i1*m_Line+i];
				m_DataReplace[i1*m_Line+i] = RLtemp;
			 }
		}
  	}
	delete []piLineChange;
	delete []piRowChange;
	
	delete []ConMa.m_Data;
	//m_Data = NULL;
	ConMa.m_Data = m_DataReplace;
    return ConMa;

}
//一个函数里是不是不能重复进行分配空间和释放空间
void CMatrix::CreateData(double *a,int m,int n)
{
	if(NULL != m_Data)
	{
		delete []m_Data;
		m_Data = NULL;		
	}

	m_Row = m;
	m_Line = n;
	m_Data = new double[m_Row*m_Line];
//	if(NULL == m_Data)
//		m_Data = new double[m_Row*m_Line];
    for(int i=0;i<m_Row;i++) 
		for(int j=0;j<m_Line;j++) 
			m_Data[i*m_Line+j]=a[n*i+j];
}

CMatrix::CMatrix (CMatrix &s)
{
    m_Row=s.m_Row;
	m_Line=s.m_Line;
    m_Data = new double[m_Line*m_Row];
//	while(NULL == m_Data)
// 	   m_Data = new double[m_Line*m_Row];
	for(int i=0;i<m_Row;i++) 
		for(int j=0;j<m_Line;j++) 
			m_Data[i*m_Line+j]=s.m_Data[i*m_Line+j];
}

//CMatrix& CMatrix::operator = (const CMatrix &s)
CMatrix CMatrix::operator= (const CMatrix &s)
{
	if(m_Data)//将原先的资源先释放
	{
		delete []m_Data;
		m_Data = NULL;
	}
    m_Row=s.m_Row;
	m_Line=s.m_Line;
	m_Data= new double [m_Row*m_Line];

	for(int i=0;i<m_Row;i++) 
		for(int j=0;j<m_Line;j++) 
			m_Data[i*m_Line+j]=s.m_Data[i*m_Line+j];
    return *this;

/*	if(this==&s)  return 1;
    m_Row=s.m_Row;
	m_Line=s.m_Line;
	m_Data= new double*[m_Row];
	for(int i=0;i<m_Row;i++) m_Data[i]= new double[m_Line];
	for(i=0;i<m_Row;i++) for(int j=0;j<m_Line;j++) m_Data[i][j]=s.m_Data[i][j];
	return 1;*/
}

CMatrix CMatrix::operator-(const CMatrix &m1)
{
	CMatrix maMinus;
    if(m1.m_Line!=this->m_Line||m1.m_Row!=this->m_Row)
		return maMinus;
	int m=m1.m_Row,n=m1.m_Line;
	maMinus.Initate(m,n);
	for(int i=0;i<m;i++)
		for(int j=0;j<n;j++)
		   maMinus.m_Data[i*m_Line+j]=this->m_Data[i*m_Line+j]-m1.m_Data[i*m_Line+j];
	return maMinus;
}

CMatrix CMatrix::operator+(const CMatrix &m1)
{
	CMatrix maAdd;
    if(m1.m_Line!=this->m_Line||m1.m_Row!=this->m_Row)
		return maAdd;
	int m=m1.m_Row,n=m1.m_Line;
	maAdd.Initate(m,n);
	for(int i=0;i<m;i++)
		for(int j=0;j<n;j++)
		   maAdd.m_Data[i*m_Line+j] = this->m_Data[i*m_Line+j] + m1.m_Data[i*m_Line+j];
	return maAdd;
}

double CMatrix::GetElement(int iRow, int iLine)
{
	return m_Data[iRow*m_Line+iLine];
}

	/*
	功能：初始化函数
	参数：iRowNum:行数； iLineNum：列数
	返回值：成功返回1 失败返回 -1
	说明：
	*/
int CMatrix::Initate(int iRowNum, int iLineNum)
{
	if(iRowNum<0 || iLineNum<0) return -1;
	
	if(m_Data != NULL)
	{
		delete []m_Data;
		m_Data = NULL;
	}
	m_Row=iRowNum;
	m_Line=iLineNum;
	m_Data= new double[m_Row*m_Line];
	//memset(m_Data, 0, sizeof(double)*m_Line*m_Row);
	for(int i=0; i<m_Row; i++)
		for(int j=0; j<m_Line; j++)
			m_Data[i*m_Line+j] = 0;
	return 1;
}


int CMatrix::InversMatrix(double* pm1, int i4n)const
{ 
	int *pis,*pjs;
	int i,j,k,l,u,v;
	double temp,max_v;
	pis = new int[i4n];
	pjs = new int[i4n];
	if(NULL==pis || NULL==pjs)	return(0);

	for(k=0; k<i4n; k++)
	{
		max_v = 0.0;
		for(i=k; i<i4n; i++)
			for(j=k; j<i4n; j++)
			{
				temp = fabs(pm1[i*i4n+j]);
				if( temp>max_v )
				{
			        max_v = temp; 
					pis[k] = i; 
					pjs[k] = j;
				}
			}
		if(max_v==0.0)
		{
			delete []pis; 
			delete []pjs;
			return(0);
		}
		if(pis[k]!=k)
			for(j=0; j<i4n; j++)
			{
			   u = k*i4n+j;
			   v = pis[k]*i4n+j;
			   temp = pm1[u]; 
			   pm1[u] = pm1[v];
			   pm1[v] = temp;
			}
		if(pjs[k]!=k)
			for(i=0; i<i4n; i++)
			{
				u = i*i4n+k; v = i*i4n+pjs[k];
				temp=pm1[u]; pm1[u]=pm1[v]; pm1[v]=temp;
			}
		l=k*i4n+k;
		pm1[l]=1.0/pm1[l];
		for(j=0; j<i4n; j++)
			if(j!=k)
			{
				u = k*i4n+j;
				pm1[u] *= pm1[l];
			}
		for(i=0; i<i4n; i++)
			if(i!=k)
				for(j=0; j<i4n; j++)
					if(j!=k)
					{
					  u = i*i4n+j;
					  pm1[u] -= pm1[i*i4n+k] * pm1[k*i4n+j];
					}
		for(i=0; i<i4n; i++)
			if(i != k)
			{
				u = i*i4n+k;
				pm1[u] *= -pm1[l];
			}
	}
	for(k=i4n-1; k>=0; k--)
	{
		if(pjs[k]!=k)
			for(j=0; j<i4n; j++)
			{
				u = k*i4n+j; v = pjs[k]*i4n+j;
				temp=pm1[u]; pm1[u]=pm1[v]; pm1[v]=temp;
			}
		if(pis[k] != k)
			for(i=0; i<i4n; i++)
			{
				u=i*i4n+k; v=i*i4n+pis[k];
				temp=pm1[u]; pm1[u]=pm1[v]; pm1[v]=temp;
			}
	}
	delete []pis; delete []pjs;

  return 1;

}

CMatrix CMatrix::GetConvertMatrix(int iBool)
{
	CMatrix maConvert = *this;
//	maConvert.Initate(m_Row,m_Line);
	InversMatrix(maConvert.GetData(),m_Row);
	return maConvert;
}