matrix.cpp

..三维DLT变换解决外方位元素的求解以及对11个参数的确定

//功能：输出矩阵范数
//参数：无
//返回值：矩阵范数
f8 CMatrix::GetNorm()const
{
	int4 i4ElementNum = m_i4RowNumber * m_i4ColumnNumber;
	f8 f8Norm = 0.0;
	for(int4 i=0; i<i4ElementNum; i++)
	{
		f8Norm += m_pData[i] * m_pData[i];
	}
	
	return f8Norm;
}

//功能：转换地址中的数值,矩阵转置核心函数
/*参数：pm1:原数据首地址
		pm2：转置后数据的首地址
		i4m：原数组行数
		i4n：原数组列数*/
void CMatrix::Transpose(const f8* pm1, f8* pm2, int4 i4m, int4 i4n)const
{
	int4 i,j;
	for(i=0; i<i4m; i++)
		for(j=0; j<i4n; j++)
			pm2[j*i4m+i] = pm1[i*i4n+j];
	return;
}

//功能：两个矩阵相乘核心函数
/*参数：pm1:		第一矩阵数据首地址
		pm2：		第二矩阵数据首地址
		presult：	结果矩阵数据首地址
		i_1：		第一矩阵行数，即结果矩阵行数
		j_12：		第一矩阵列数，即第二矩阵的行数
		j_2：		第二矩阵的列数，即结果矩阵列数*/
void CMatrix::Mult(const f8* pm1, const f8* pm2, f8* pResult, int4 i_1, int4 j_12, int4 j_2)const
{
	int4 i,j,k;
	for(i=0; i<i_1; i++)
		for(j=0;j<j_2;j++)
		{
			pResult[i*j_2+j]= 0.0;
			for(k=0; k<j_12; k++)
				pResult[i*j_2+j] += pm1[i*j_12+k] * pm2[j+k*j_2];
		}
	return;
}

//功能：方矩阵求逆核心函数
/*参数：pm1:	原矩阵数据首地址，也是逆阵的首地址
		i4n:	矩阵行数或列数
//返回值：成功返回1，失败返回0*/
int4 CMatrix::InversMatrix(f8* pm1, int4 i4n)const
{ 
	int4 *pis,*pjs;
	int4 i,j,k,l,u,v;
	f8 temp,max_v;
	pis = new int4[i4n];
	pjs = new int4[i4n];
	if(pis==NULL || pjs==NULL)	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;

}

//功能：将矩阵元素设零
//参数：无
//返回值：1
int4 CMatrix::SetZero()
{
	if(m_pData != NULL)
	{
		memset(m_pData, 0, sizeof(f8) * m_i4RowNumber * m_i4ColumnNumber);
	}
	return 1;
}

//功能：将矩阵元素设负
//参数：无
//返回值：1
int4 CMatrix::SetMinus()
{
	int4 nElementNumber = m_i4RowNumber * m_i4ColumnNumber;
	for(int4 i=0; i<nElementNumber; i++)
	{
		*(m_pData + i) *= -1;
	}
	
	return 1;
}

//功能：将矩阵元素乘以倍数----注意：此函数和“*”操作符重载不同，这个函数视将自身元素改变，而后者则自身不变 
//参数：f8MulNum	倍数
//返回值：1
int4 CMatrix::Multiply(f8 f8MulNum)
{
	int4 i4ElementNumber = m_i4RowNumber * m_i4ColumnNumber;
	for(int4 i=0;i<i4ElementNumber;i++)
	{
		m_pData[i] *= f8MulNum;
	}
	
	return 1;
}

//功能：将矩阵设成单位阵
//参数：i4RowNumber	单位阵的维数
//返回值：1
int4 CMatrix::SetIdentity(int4 i4RowNumber)
{
	if(i4RowNumber == 0) return 0;

	this->InitEmptyMatrix(i4RowNumber, i4RowNumber);//开空间
	memset(m_pData, 0, sizeof(f8) * i4RowNumber * i4RowNumber);//第一步全部置零
	for(int4 j=0; j<i4RowNumber; j++)
	{
		*(m_pData + j*(i4RowNumber+1) ) = 1.0;//第二步将对角线置1
	}

	return 1;
}

//功能：将矩阵设成简单相关阵
//参数：i4RowNumber	相关阵的维数
//返回值：1
int4 CMatrix::SetCorrelativity(int4 i4RowNumber)
{
	if(i4RowNumber==0) return 0;

	if(i4RowNumber * i4RowNumber != m_i4RowNumber * m_i4ColumnNumber)//空间不相同
	{
		this->InitEmptyMatrix(i4RowNumber, i4RowNumber);//重新开空间
	}
	
	for(int4 i=0; i<i4RowNumber*i4RowNumber; i++)
	{
		*(m_pData + i ) = 1.0;//第一步全部置1
	}
	for(int4 j=0; j<i4RowNumber; j++)
	{
		*(m_pData + j*(i4RowNumber+1) ) = 2.0;//第二步将对角线置2
	}

	return 1;
}

///////////////////////////////////////////////////
//公用操作符重载    operator override
///////////////////////////////////////////////////

//功能：赋值操作符重载
/*参数：SecMatrix:		源矩阵
//返回值：自身矩阵的引用，这样可以使用连等形式，如: A = B = C ，此时 A , B 都等于 C 的值;
//实现方法：调用InitMatrix()函数实现功能
使用示例：A=B=C 意为: B=C; A=B; */
//CMatrix& CMatrix::operator= (const CMatrix & SecMatrix)
//{
//	if(&SecMatrix == this) return *this;
//	this->InitMatrix(SecMatrix.GetDataAddress(),SecMatrix.GetRowNumber(),SecMatrix.GetColumnNumber());
//
//	return *this;
//}

int4 CMatrix::operator= (const CMatrix& SecMatrix)
{
	if(&SecMatrix == this) return 1;
	return this->InitMatrix(SecMatrix.GetDataAddress(),SecMatrix.GetRowNumber(),SecMatrix.GetColumnNumber());
}

//功能：矩阵加法操作符重载
/*参数：SecMatrix:		源矩阵
//返回值：相加后的结果矩阵
使用示例：A=B+C 意为: A中的元素为B、C中元素之和 */
CMatrix CMatrix::operator+ (const CMatrix & SecMatrix)const
{
	CMatrix TmpMatrix;
	//行、列不相等的矩阵不能相加
	if((m_i4RowNumber != SecMatrix.GetRowNumber()) || (m_i4ColumnNumber != SecMatrix.GetColumnNumber())) return TmpMatrix;

	int4 i4ElementNumber = m_i4RowNumber * m_i4ColumnNumber;
	f8* pTmpData2 = new f8[i4ElementNumber];
	SecMatrix.GetData(pTmpData2);
	for(int4 i=0; i<i4ElementNumber; i++)
	{
		*(pTmpData2 + i) += *(m_pData + i);
	}
	
	TmpMatrix.InitMatrix(pTmpData2, m_i4RowNumber, m_i4ColumnNumber);
	delete [] pTmpData2;

	return TmpMatrix;
}

//功能：矩阵减法操作符重载
/*参数：SecMatrix:		源矩阵
//返回值：相加后的结果矩阵
使用示例：A=B-C 意为: A中的元素为B、C中元素之差*/
CMatrix CMatrix::operator- (const CMatrix & SecMatrix)const
{
	CMatrix TmpMatrix(SecMatrix);
	TmpMatrix.SetMinus();

	CMatrix SecTmpMatrix(* this + TmpMatrix);
	return SecTmpMatrix;
}

//功能：取负操作符重载
/*参数：无
//返回值：自身矩阵的负矩阵
//实现方法：调用SetMinus()函数实现功能
使用示例：A=-C 意为: A中的元素为B中元素的负值 */
CMatrix CMatrix::operator- ()
{
	CMatrix TmpMatrix(* this);
	TmpMatrix.SetMinus();
	return TmpMatrix;
}

//功能：矩阵相乘
/*参数：SecMatrix:		被乘矩阵
//返回值：矩阵相乘的结果矩阵
使用示例：C=A*B C为结果矩阵 */
CMatrix CMatrix::operator* (const CMatrix& SecMatrix)const
{
	CMatrix maTemp;
	if(m_i4ColumnNumber != SecMatrix.m_i4RowNumber) return maTemp;//A阵列数不等于B阵行数
	int4 i4ElementNumber = m_i4RowNumber * SecMatrix.GetColumnNumber();
	f8* pTmpData = new f8[i4ElementNumber];
	Mult(m_pData,SecMatrix.GetDataAddress(), pTmpData, m_i4RowNumber, m_i4ColumnNumber, SecMatrix.GetColumnNumber());

	CMatrix TmpMatrix(pTmpData, m_i4RowNumber, SecMatrix.GetColumnNumber());
	delete [] pTmpData;

	return TmpMatrix;
}

//功能：矩阵乘以实数
/*参数： f8MultiNum:	被乘数
//返回值：矩阵乘以实数的结果矩阵
使用示例：C= A * dMultiNum C为结果矩阵 */
CMatrix CMatrix::operator *(const f8 f8MultiNum) const
{
	int4 i4ElementNumber = m_i4RowNumber * m_i4ColumnNumber;
	f8* pTmpData = new f8[i4ElementNumber];
	GetData(pTmpData);
	for(int4 i=0; i<i4ElementNumber; i++)
	{
		pTmpData[i] *= f8MultiNum;
	}
	
	CMatrix TmpMatrix(pTmpData, m_i4RowNumber, m_i4ColumnNumber);
	delete [] pTmpData;

	return TmpMatrix;
}

//功能：矩阵除以实数
/*参数： f8MultiNum:	被除数，不能为0
//返回值：矩阵乘以实数的结果矩阵
使用示例：C= A * dMultiNum C为结果矩阵 */
CMatrix CMatrix::operator /(const f8 f8DevideNum) const
{
	CMatrix TmpMatrix;
	if(f8DevideNum == 0) return TmpMatrix;

	f8 f8Temp = 1/f8DevideNum;
	
	TmpMatrix = this->operator * (f8Temp);

	return TmpMatrix;
}

//功能：矩阵正向除
/*参数：SecMatrix:		被除矩阵
//返回值：矩阵正向除的结果矩阵 */
//使用方法：C=A/B C为A乘以B的逆阵的结果阵
CMatrix CMatrix::operator/ (const CMatrix & SecMatrix)const
{
	//先求B的逆阵
	CMatrix TmpMatrix1; SecMatrix.GetInverseMatrix(TmpMatrix1);
	if(!TmpMatrix1.GetRowNumber()) return TmpMatrix1;//如果求逆失败，则返回空阵

	//再矩阵相乘
	CMatrix TmpMatrix2 = (*this) * TmpMatrix1;

	return TmpMatrix2;
}

//功能：矩阵逆向除，A%B:表示A的逆阵乘以B
/*参数：SecMatrix:		被操作矩阵
//返回值：矩阵逆向除的结果矩阵 */
//使用方法：C=A%B C为A的逆阵乘以B的结果阵
CMatrix CMatrix::operator% (const CMatrix & SecMatrix)const
{
	//先求A的逆阵
	CMatrix TmpMatrix1; this->GetInverseMatrix(TmpMatrix1);
	if(!TmpMatrix1.GetRowNumber()) return TmpMatrix1;//如果求逆失败，则返回空阵

	//再矩阵相乘
	CMatrix TmpMatrix2 = TmpMatrix1 * SecMatrix;

	return TmpMatrix2;
}

//功能：矩阵加法操作符重载
/*参数：SecMatrix:		源矩阵
//返回值：相加后的结果矩阵
使用示例：A=A+C 意为: A中的元素为原来A、C中元素之和 */
int4 CMatrix::operator+= (const CMatrix& SecMatrix)
{
	//行、列不相等的矩阵不能相加
	if((m_i4RowNumber != SecMatrix.GetRowNumber()) || (m_i4ColumnNumber != SecMatrix.GetColumnNumber())) return 0;
	
	int4 i4ElementNumber = m_i4RowNumber * m_i4ColumnNumber;
	f8* pSecData = SecMatrix.GetDataAddress();
	for(int4 i=0; i<i4ElementNumber; i++)
	{
		*(m_pData + i) += *(pSecData + i);
	}
	return 1;
}

//功能：矩阵减法操作符重载
/*参数：SecMatrix:		源矩阵
//返回值：相加后的结果矩阵
使用示例：A=A-C 意为: A中的元素为原来A、C中元素之差 */
int4 CMatrix::operator-= (const CMatrix& SecMatrix)
{
	//行、列不相等的矩阵不能相加
	if((m_i4RowNumber != SecMatrix.GetRowNumber()) || (m_i4ColumnNumber != SecMatrix.GetColumnNumber())) return 0;
	
	int4 i4ElementNumber = m_i4RowNumber * m_i4ColumnNumber;
	f8* pSecData = SecMatrix.GetDataAddress();
	for(int4 i=0; i<i4ElementNumber; i++)
	{
		*(m_pData + i) -= *(pSecData + i);
	}
	return 1;
}

//功能：矩阵等于操作符重载
/*参数：SecMatrix:		源矩阵
//返回值：相同 1；不同 0 */
BOOL CMatrix::operator== (const CMatrix& SecMatrix)const
{
	//行、列不相等的矩阵不相等
	if((m_i4RowNumber != SecMatrix.GetRowNumber()) || (m_i4ColumnNumber != SecMatrix.GetColumnNumber())) return FALSE;

	int4 i4ElementNumber = m_i4RowNumber * m_i4ColumnNumber;
	f8* pSecData = SecMatrix.GetDataAddress();
	for(int4 i=0; i<i4ElementNumber; i++)
	{
		if(fabs(*(m_pData + i) - *(pSecData + i)) > EPSILON) return FALSE;
	}
	return TRUE;
}

BOOL CMatrix::operator!= (const CMatrix& SecMatrix)const
{
	if(*this == SecMatrix) return 0;
	else return 1;
}


//功能：四舍五入，计算最靠近的整数
//例如：1.2 -> 1; 1.8 -> 2; -33.3 -> -33; -6.9 -> -7 
//参数：f8 dNum：要处理的实数
//返回：返回的整数
int4 CMatrix::round(f8 dNum)const
{
	int4 nIntNum;
	if(dNum>=0) nIntNum=(int4)(dNum+0.5);
	else nIntNum=(int4)(dNum-0.5);
	return nIntNum;
}
double Det(CMatrix A)
{
	CMatrix B(A.m_i4RowNumber,A.m_i4RowNumber),D;
	int i,j=-1,k;
	double R=0,r=0;
	for(i=0;i<B.m_i4RowNumber*B.m_i4ColumnNumber;i++)
	{
		B.m_pData[i]=A.m_pData[i];
	}
	if((B.GetInverseMatrix(D))==0) 
		return 0;
	else
	{
		D.InitEmptyMatrix(0,0);
		if(B.m_i4ColumnNumber==1) 
			return B.m_pData[0];
		for(i=0;i<B.m_i4ColumnNumber;i++)
		{
			CMatrix C(B.m_i4RowNumber,B.m_i4RowNumber);
			for(k=0;k<C.m_i4RowNumber*C.m_i4ColumnNumber;k++)
			{
				C.m_pData[k]=B.m_pData[k];
			}
			j*=-1;
			r=B.m_pData[i];
			C.DeleteOneColumn(i);
			C.DeleteOneRow(0);
			double t=Det(C);
			R+=j*r*Det(C);
		}
	}
	return R;
}