matrix.cpp

立体影像对的光束法严密解求外方位元素：已知控制点的左右影像像点坐标和物方空间坐标

//////////////////////////////////////////////////////////////////////////
////Matrix.ccp
////
////源文件中各函数的作用主要是进行矩阵的基本运算
////
////	MultMatrix()		矩阵相乘
////	TranMatrix()		矩阵转置
////	InverMatrix()		矩阵求逆
////	SumMatrix()			矩阵相减
////
//////////////////////////////////////////////////////////////////////////




#include "stdafx.h"
#include "Matrix.h"
#include <math.h>

#define PI 3.1415925

//////////////////////////////////////////////////////////////////////////
//		矩阵相乘
//
//		*A			矩阵相乘的前一个矩阵(m*p)
//		*B			矩阵相乘的后一个矩阵(p*n)
//		m,p,n		矩阵的维数
//
//////////////////////////////////////////////////////////////////////////
void MultMatrix(double *A, double *B, double *Result, int m, int p, int n)
{
	int i;
	int j;
	int k;

	Result = new double[m*n];

	for(i=0; i<m; i++)
		for(j=0; j<n; j++)
		{
			Result[i*n+j] = 0.0;
			for(k=0; k<p; k++)
				Result[i*n+j] += A[i*p+k]*B[k*n+j];
		}
		
	return ;
}

//////////////////////////////////////////////////////////////////////////
//		矩阵转置
//
//		*AT			转置后的矩阵
//		*A			待转置的矩阵
//		m, n		矩阵的维数
//
//////////////////////////////////////////////////////////////////////////

void TranMatrix(double *A, double *AT, int m, int n)
{
	int i;
	int j;

	for(i=0; i<m; i++)
		for(j=0; j<n; j++)
		{
			AT[j*m+i] = A[i*n+j];
		}
}

//////////////////////////////////////////////////////////////////////////
//		矩阵求逆
//
//		*A			待求逆的方阵
//		n			方阵的维数
//
//////////////////////////////////////////////////////////////////////////

void InverMatrix(double *A, int n)
{
    int *is=new int[n];
    int *js=new int[n];

    int i,j,k,l,u,v;
    double temp,max_v;

    if(is==NULL||js==NULL)
    {
        printf("out of memory!\n");
        return ;
    }

    for(k=0;k<n;k++)
    {
		max_v=0.0;
		
		for(i=k;i<n;i++)
			for(j=k;j<n;j++)
			{
				temp=fabs(A[i*n+j]);
			
				if(temp>max_v)
				{
				    max_v=temp; is[k]=i; js[k]=j;
				}
			}
	
		if(max_v==0.0)
		{
			delete []is;
			delete []js;
			
			printf("inverts is not available!\n");
			return ;
		}
	
		if(is[k]!=k)
			for(j=0;j<n;j++)
			{
				u=k*n+j; v=is[k]*n+j;
				temp=A[u]; A[u]=A[v]; A[v]=temp;
			}
	
		if(js[k]!=k)
			for(i=0;i<n;i++)
			{
				u=i*n+k; v=i*n+js[k];
				temp=A[u]; A[u]=A[v]; A[v]=temp;
			}
		
		l=k*n+k;
		A[l]=1.0/A[l];
	
		for(j=0;j<n;j++)
			if(j!=k)
			{
				u=k*n+j;
				A[u]*=A[l];
			}
	
		for(i=0;i<n;i++)
			if(i!=k)
				for(j=0;j<n;j++)
					if(j!=k)
					{
						u=i*n+j;
						A[u]-=A[i*n+k]*A[k*n+j];
					}
	
		for(i=0;i<n;i++)
			if(i!=k)
			{
				u=i*n+k;
				A[u]*=-A[l];
			}

    }
    
	for(k=n-1;k>=0;k--)
    {
		if(js[k]!=k)
			for(j=0;j<n;j++)
			{
				u=k*n+j; v=js[k]*n+j;
				temp=A[u]; A[u]=A[v]; A[v]=temp;
			}
	
		if(is[k]!=k)
			for(i=0;i<n;i++)
			{
				u=i*n+k; v=i*n+is[k];
				temp=A[u]; A[u]=A[v]; A[v]=temp;
			}
    }
    
	delete []is;
    delete []js;
    return  ;
}

//////////////////////////////////////////////////////////////////////////
//		矩阵相减
//
//		*A			被减的矩阵
//		*B			待减的矩阵
//		*Sub		矩阵相减的结果
//		m, n		矩阵维数
//
//////////////////////////////////////////////////////////////////////////

void SubMatrix(double *A, double *B, double *Sub, int m, int n)
{
	int i;
	int j;

	for(i=0; i<m; i++)
		for(j=0; j<n; j++)
			Sub[i*n+j] = A[i*n+j] - B[i*n+j];
}

void AddMatrix(double *A, double *B, double *Add, int m, int n)
{
	int i;
	int j;

	for(i=0; i<m; i++)
		for(j=0; j<n; j++)
			Add[i*n+j] = A[i*n+j] + B[i*n+j];
}



double Trans(double rad)
{
    double degree, minute, second;
	double result;
    if(0<rad)
	{
		degree=rad*180/PI;
		minute=(degree-(int)degree)*60;
		degree=(int)degree;
		second=(minute-(int)minute)*60;
		result=degree*10000+((int)minute)*100+int(second+0.5);
	}
	else
	{
		rad=fabs(rad);
		degree=rad*180/PI;
		minute=(degree-(int)degree)*60;
		degree=(int)degree;
		second=(minute-(int)minute)*60;
		result=-degree*10000-((int)minute)*100-int(second+0.5);

	}
	result=result/10000;
	return result;

}