mathex.cpp

摄影测量相对定向程序

//MathEx.cpp
//



//mult                 用于计算两个可乘的矩阵的乘积
//transpose            用于将一个矩阵进行转置
//invers_matrix        用于计算一个矩阵的逆阵

#include "StdAfx.h"
#include "math.h"
#include <iostream>
#include "MathEx.h"
using namespace std;
void Mtx_Mult(double *m1,double *m2,double *result,int i_1,int j_12,int j_2)
{ 	
	int i,j,k;
	for(i=0;i<i_1;i++)
		for(j=0;j<j_2;j++){
			result[i*j_2+j]=0.0;
			for(k=0;k<j_12;k++)
				result[i*j_2+j]+=m1[i*j_12+k]*m2[j+k*j_2];
		}
		return;
}



//矩阵转置
void Mtx_Transpose(double *m1,double *m2,int m,int n)            
{
	int i,j;                                        
	for(i=0;i<m;i++)                                
		for(j=0;j<n;j++)                        
			m2[j*m+i]=m1[i*n+j];            
		return;                                 		
}



//矩阵求逆
int Mtx_Invers(double *m1,int n)
{
	int *is,*js;
	int i,j,k,l,u,v;
	double temp,max_v;

	is=(int *)malloc(n*sizeof(int));
	js=(int *)malloc(n*sizeof(int));

	if(is==NULL||js==NULL)
	{
		printf("out of memory!\n");
		return(0);
	}
	for(k=0;k<n;k++)
	{
		max_v=0.0;
		for(i=k;i<n;i++)
			for(j=k;j<n;j++)
			{
				temp=fabs(m1[i*n+j]);
				if(temp>max_v)
				{
					max_v=temp; is[k]=i; js[k]=j;
				}
			}
			if(max_v==0.0)
			{
				free(is); free(js);
				printf("invers is not availble!\n");
//				AfxMessageBox("invers is not availble!\n");
				return(0);
			}
			if(is[k]!=k)
				for(j=0;j<n;j++)
				{
					u=k*n+j; 
					v=is[k]*n+j;
					temp=m1[u]; 
					m1[u]=m1[v]; 
					m1[v]=temp;
				}
				if(js[k]!=k)
					for(i=0;i<n;i++)
					{
						u=i*n+k; 
						v=i*n+js[k];
						temp=m1[u]; 
						m1[u]=m1[v]; 
						m1[v]=temp;
					}
					l=k*n+k;
					m1[l]=1.0/m1[l];
					for(j=0;j<n;j++)
						if(j!=k)
						{
							u=k*n+j;
							m1[u]*=m1[l];
						}
						for(i=0;i<n;i++)
							if(i!=k)
								for(j=0;j<n;j++)
									if(j!=k)
									{
										u=i*n+j;
										m1[u]-=m1[i*n+k]*m1[k*n+j];
									}
									for(i=0;i<n;i++)
										if(i!=k)
										{
											u=i*n+k;
											m1[u]*=-m1[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=m1[u]; 
				m1[u]=m1[v]; 
				m1[v]=temp;
			}
			if(is[k]!=k)
				for(i=0;i<n;i++)
				{
					u=i*n+k; 
					v=i*n+is[k];
					temp=m1[u]; 
					m1[u]=m1[v]; 
					m1[v]=temp;
				}
	}
	free(is); 
	free(js);

	return 1;
}

//求矩阵的迹
double Mtx_Trace(double *data, int size)
{
	int i;
	double sum=0;
	for(i=0;i<size;i++)
		sum=sum+data[i+i*size];
	return sum;
}

//求矩阵行列式的值
int Mtx_ValOfDet(double *data, int size, double *result)
{

	int i,j,m,n;
	double sum=1,tmp1,tmp2,tmp3;
	double *ndata=new double [size*size];
	if(ndata==NULL)
		return -1;
	for(i=0;i<size*size;i++)
		*(ndata+i)=*(data+i);
	for(m=1;m<size;m++)
	{
		
		for(i=m;i<size;i++)
		{
			tmp2=*(ndata+i*size+m-1);
			tmp3=*(ndata+(m-1)*size+m-1);
			for(j=0;j<size;j++)
			{
				tmp1=*(ndata+(m-1)*size+j);
				*(ndata+i*size+j)=*(ndata+i*size+j)-tmp1*tmp2/tmp3;
			}
		}
	}
	for(n=0;n<size;n++)
		sum=sum*(*(ndata+size*n+n));
	*result=sum;
	delete [] ndata;
	return 1;
}

int Mtx_Add(double * Num1,double * Num2,double * Res,int num)
{
	int i;
	for(i=0;i<num;i++)
	{
		*(Res+i)=*(Num1+i)+*(Num2+i);
	}
	return 1;
}
int Mtx_SubTract(double * Num1,double * Num2,double * Res,int num)
{
	int i;
	for(i=0;i<num;i++)
	{
		*(Res+i)=*(Num1+i)-*(Num2+i);
	}
	return 1;
}
int Mtx_Assignment(double * data1,double *data2,int num)
{
	int i;
	for(i=0;i<num;i++)
		data1[i]=data2[i];
	return 1;
}

int Mtx_Assignment(int * data1,int *data2,int num)
{
	int i;
	for(i=0;i<num;i++)
		data1[i]=data2[i];
	return 1;
}

int Mtx_Assignment(float * data1,float *data2,int num)
{
	int i;
	for(i=0;i<num;i++)
		data1[i]=data2[i];
	return 1;
}

double Sta_Max(double *data,int num)
{
	int i;
	double tmp=data[0];
	for(i=0;i<num;i++)
	{
		if(tmp<data[i])
			tmp=data[i];
	}
	return tmp;
}