fangwei.cpp

这个程序还不完整

// FangWei.cpp: implementation of the CFangWei class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "fagnwei1.h"
#include "FangWei.h"
#include "math.h"
#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////
//参数分别代表 内方位元素x0，y0，f；方位角初值，p，o，k；pc，p1，p2，p3的像平面坐标；最后解求的角元素；
CFangWei::CFangWei(double m_x0,double m_y0,double m_f,double m_phi0,double m_omiga0,double m_kapa0,
		double m_xc,double m_yc,double m_x1,double m_y1,double m_x2,double m_y2,double m_x3,double m_y3,
		double *m_phi,double *m_omiga,double *m_kapa)
{

	double A,B,C;
	double a[3],b[3],c[3];
	double d_phi,d_omiga,d_kapa;
	double j,w,k;
	double *MA,*MAT,*MATMA,*L,*L0,*L1;
	int ncount=0;
	double f,xc,yc,x1,y1,x2,y2,x3,y3;
	f=m_f;
	xc=m_xc;
	yc=m_yc;
	x1=m_x1;
	y1=m_y1;
	x2=m_x2;
	y2=m_y2;
	x3=m_x3;
	y3=m_y3;

	//分别代表A,A的转置，A的转置和A的乘积
	MA = (double*)malloc(3*3*sizeof(double));
	MAT = (double*)malloc(3*3*sizeof(double));
	MATMA = (double*)malloc(3*3*sizeof(double));
	L = (double*)malloc(3*1*sizeof(double));
	L0= (double*)malloc(3*1*sizeof(double));
	L1= (double*)malloc(3*3*sizeof(double));
	//角元素的初值
	j=m_phi0;
	w=m_omiga0;
	k=m_kapa0;
	//A,B,C的初值
	A=(m_x0-xc)*(y1-yc)-(m_y0-yc)*(x1-xc);
	B=(m_x0-xc)*(y2-yc)-(m_y0-yc)*(x2-xc);
	C=(m_x0-xc)*(y3-yc)-(m_y0-yc)*(x3-xc);
	do
	{
		a[0]=A*(-cos(j)*cos(w))- f*(y1-yc)*(-sin(j)*cos(k)-cos(j)*sin(w)*sin(k))+f*(x1-xc)*(sin(j)*sin(k)-cos(j)*sin(w)*cos(k));
        a[1]=0;
        a[2]= -C*sin(j)*cos(w)-f*(y3-yc)*(cos(j)*cos(k)-sin(j)*sin(w)*sin(k))+f*(x3-xc)*(-cos(j)*sin(k)-sin(j)*sin(w)*cos(k));

        b[0]= A*(sin(j)*sin(w))+ f*(y1-yc)*(sin(j)*cos(w)*sin(k))+f*(x1-xc)*(-sin(j)*cos(w)*cos(k));
        b[1]= B*(-cos(w))+f*(y2-yc)*sin(w)*sin(k)-f*(x2-xc)*sin(w)*cos(k);
        b[2]= -C*cos(j)*sin(w)-f*(y3-yc)*cos(j)*cos(w)*sin(k)+f*(x3-xc)*cos(j)*cos(w)*cos(k);

        c[0]= f*(y1-yc)*(cos(j)*sin(k)+sin(j)*sin(w)*cos(k))+f*(x1-xc)*(-cos(j)*cos(k)+sin(j)*sin(w)*sin(k));
        c[1]= -f*(y2-yc)*cos(w)*cos(k)-f*(x2-xc)*cos(w)*sin(k);
        c[2]= f*(y3-yc)*(-sin(j)*sin(k)+cos(j)*sin(w)*cos(k))+f*(x3-xc)*(-sin(j)*cos(k)-cos(j)*sin(w)*sin(k));
        //定义矩阵A

		MA[0]=a[0];
		MA[1]=b[0];
		MA[2]=c[0];
		MA[3]=a[1];
		MA[4]=b[1];
		MA[5]=c[1];
		MA[6]=a[2];
		MA[7]=b[2];
		MA[8]=c[2];
		//定义矩阵L0
		L0[0]=(A*(-sin(j)*cos(w))- f*(y1-yc)*(cos(j)*cos(k)-sin(j)*sin(w)*sin(k))+f*(x1-xc)*(-cos(j)*sin(k)-sin(j)*sin(w)*cos(k)));
		L0[1]=(B*(-sin(w))-f*(y2-yc)*cos(w)*sin(k)+f*(x2-xc)*cos(w)*cos(k));
        L0[2]=(C*cos(j)*cos(w)-f*(y3-yc)*(sin(j)*cos(k)+cos(j)*sin(w)*sin(k))+f*(x3-xc)*(-sin(j)*sin(k)+cos(j)*sin(w)*cos(k)));

		//MA的转置MAT
       MAT=MatrixInver(MA,3,3);
	

    	//MAT和MA的乘积结果放在MATMA中
        MultiplyMatrix(MAT,MA,MATMA,3,3,3);
		//MATMA的逆MATMA
        InverseMatrix(MATMA,3);
		
		//MATMA的逆和MAT的乘积结果放在L1中
		MultiplyMatrix(MATMA,MAT,L1,3,3,3);
        //L1和L0的乘积结果放在L中
		MultiplyMatrix(L1,L0,L,3,3,1);

		d_phi=L[0];
		d_omiga=L[1];
		d_kapa=L[2];
		
		j=j+d_phi;
		w=w+d_omiga;
		k=k+d_kapa;
		ncount++;
	}
	while(ncount<30);
   /* while (j > 3.1416) j = j - 2*3.1416;
	while (j < -3.1416) j = j + 2*3.1416;
	while (w > 3.1416) w = w - 2*3.1416;
	while (w < -3.1416) w = w + 2*3.1416;	
	while (k > 3.1416) k = k - 2*3.1416;
	while (k < -3.1416) k = k + 2*3.1416;*/

    *m_phi=j;
	*m_omiga=w;
	*m_kapa=k;

}

double*  CFangWei::MatrixInver(double A[],int m,int n) 
	{
     int i,j; 
     double *B=NULL; 
     B=(double *)malloc(m*n*sizeof(double)); 
     
     for(i=0;i<n;i++) 
     for(j=0;j<m;j++) 
     B[i*m+j]=A[j*n+i]; 
     
     return B; 
	}
   
 int CFangWei::InverseMatrix(double *a, 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)	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(a[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);
			return(0);
		}
		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;
			}
		}
	}
	free(is); free(js);
	return(1);

}
 void CFangWei::MultiplyMatrix(double *a, double *b, double *c, int m, int n, int k)
{
	int i,j,ii;
	for(i=0;i<m;i++)
	{
		for(j=0;j<k;j++)
		{
			*(c+i*k+j)=0;
			for(ii=0;ii<n;ii++)
				*(c+i*k+j)=*(c+i*k+j)+*(a+i*n+ii)**(b+ii*k+j);
		}
	}
}

CFangWei::~CFangWei()
{

}