qianfangjiaohuidoc.cpp

前方交会解算像点坐标

// qianfangjiaohuiDoc.cpp : implementation of the CQianfangjiaohuiDoc class
//

#include "stdafx.h"
#include "qianfangjiaohui.h"
#include "qianfangjiaohui1.h"
#include "qianfangjiaohuiDoc.h"
#include "Matrix.h"
#include "math.h"
#include "DIEDAI.h"

//#include "Matrix.h"

#ifdef _DEBUG
#define new DEBUG_NEW
#undef THIS_FILE
static char THIS_FILE[] = __FILE__;
#endif

/////////////////////////////////////////////////////////////////////////////
// CQianfangjiaohuiDoc

IMPLEMENT_DYNCREATE(CQianfangjiaohuiDoc, CDocument)

BEGIN_MESSAGE_MAP(CQianfangjiaohuiDoc, CDocument)
	//{{AFX_MSG_MAP(CQianfangjiaohuiDoc)
	ON_COMMAND(ID_QIAN, OnQian)
	ON_BN_CLICKED(ID_jisuan, Onjisuan)
	ON_COMMAND(ID_DIEDAI, OnDiedai)
	//}}AFX_MSG_MAP
END_MESSAGE_MAP()

/////////////////////////////////////////////////////////////////////////////
// CQianfangjiaohuiDoc construction/destruction

CQianfangjiaohuiDoc::CQianfangjiaohuiDoc()
{
	// TODO: add one-time construction code here

}

CQianfangjiaohuiDoc::~CQianfangjiaohuiDoc()
{
}

BOOL CQianfangjiaohuiDoc::OnNewDocument()
{
	if (!CDocument::OnNewDocument())
		return FALSE;

	// TODO: add reinitialization code here
	// (SDI documents will reuse this document)

	return TRUE;
}



/////////////////////////////////////////////////////////////////////////////
// CQianfangjiaohuiDoc serialization

void CQianfangjiaohuiDoc::Serialize(CArchive& ar)
{
	if (ar.IsStoring())
	{
		// TODO: add storing code here
	}
	else
	{
		// TODO: add loading code here
	}
}

/////////////////////////////////////////////////////////////////////////////
// CQianfangjiaohuiDoc diagnostics

#ifdef _DEBUG
void CQianfangjiaohuiDoc::AssertValid() const
{
	CDocument::AssertValid();
}

void CQianfangjiaohuiDoc::Dump(CDumpContext& dc) const
{
	CDocument::Dump(dc);
}
#endif //_DEBUG

/////////////////////////////////////////////////////////////////////////////
// CQianfangjiaohuiDoc commands

void CQianfangjiaohuiDoc::OnQian() 
{
	// TODO: Add your command handler code here
	
	Cqianfangjiaohui dlgdlg;
	dlgdlg.DoModal();	     	
}


void CQianfangjiaohuiDoc::Onjisuan() 
{
	// TODO: Add your control notification handler code here
/*	Cqianfangjiaohui dlg;	     
	
    angel_to_(dlg.m_Kappa_L,dlg.m_Kappa_L1 );
    angel_to_(dlg.m_Kappa_R,dlg.m_Kappa_R1 );
	angel_to_(dlg.m_Omega_L,dlg.m_Omega_L1 );
	angel_to_(dlg.m_Omega_R,dlg.m_Omega_R1 );
	angel_to_(dlg.m_Phi_L,dlg.m_Phi_L1 );
	angel_to_(dlg.m_Phi_R,dlg.m_Phi_R1 );


	double a11,a12,a13,b11,b12,b13,c11,c12,c13;
	double L11,L12,L13,L1x,L14,L15,L16,L1y;
	
	a11=cos(dlg.m_Phi_L1)*cos(dlg.m_Kappa_L1)-sin(dlg.m_Phi_L1)*sin(dlg.m_Omega_L1)*sin(dlg.m_Kappa_L1);
	a12=-cos(dlg.m_Phi_L1)*sin(dlg.m_Kappa_L1)-sin(dlg.m_Phi_L1)*sin(dlg.m_Omega_L1)*cos(dlg.m_Kappa_L1);
	a13=-sin(dlg.m_Phi_L1)*cos(dlg.m_Omega_L1);
	b11=cos(dlg.m_Omega_L1)*sin(dlg.m_Kappa_L1);
	b12=cos(dlg.m_Omega_L1)*cos(dlg.m_Kappa_L1);
	b13=-sin(dlg.m_Omega_L1);
	c11=sin(dlg.m_Phi_L1)*cos(dlg.m_Kappa_L1)+cos(dlg.m_Phi_L1)*sin(dlg.m_Omega_L1)*sin(dlg.m_Kappa_L1);
	c12=-sin(dlg.m_Phi_L1)*sin(dlg.m_Kappa_L1)+cos(dlg.m_Phi_L1)*sin(dlg.m_Omega_L1)*cos(dlg.m_Kappa_L1);
	c13=cos(dlg.m_Phi_L1)*cos(dlg.m_Omega_L1);

    L11=dlg.m_F_L*a11+(dlg.m_x_L-dlg.m_X0_L)*a13;
	L12=dlg.m_F_L*b11+(dlg.m_x_L-dlg.m_X0_L)*b13;
	L13=dlg.m_F_L*c11+(dlg.m_x_L-dlg.m_X0_L)*c13;
    L1x=dlg.m_F_L*a11*dlg.m_Xs_L+dlg.m_F_L*b11*dlg.m_Ys_L+dlg.m_F_L*c11*dlg.m_Zs_L+(dlg.m_x_L-dlg.m_X0_L)*a13*dlg.m_Xs_L+
        (dlg.m_x_L-dlg.m_X0_L)*b13*dlg.m_Ys_L+(dlg.m_x_L-dlg.m_X0_L)*c13*dlg.m_Zs_L;
    L14=dlg.m_F_L*a12+(dlg.m_y_L-dlg.m_Y0_L)*a13;
	L15=dlg.m_F_L*b12+(dlg.m_y_L-dlg.m_Y0_L)*b13;
	L16=dlg.m_F_L*c12+(dlg.m_y_L-dlg.m_Y0_L)*c13;
    L1y=dlg.m_F_L*a12*dlg.m_Xs_L+dlg.m_F_L*b12*dlg.m_Ys_L+dlg.m_F_L*c12*dlg.m_Zs_L+(dlg.m_y_L-dlg.m_Y0_L)*a13*dlg.m_Xs_L+
        (dlg.m_y_L-dlg.m_Y0_L)*b13*dlg.m_Ys_L+(dlg.m_y_L-dlg.m_Y0_L)*c13*dlg.m_Zs_L;
	
	double a21,a22,a23,b21,b22,b23,c21,c22,c23;
    double L21,L22,L23,L2x,L24,L25,L26,L2y;
    
	a21=cos(dlg.m_Phi_R1)*cos(dlg.m_Kappa_R1)-sin(dlg.m_Phi_R1)*sin(dlg.m_Omega_R1)*sin(dlg.m_Kappa_R1);
	a22=-cos(dlg.m_Phi_R1)*sin(dlg.m_Kappa_R1)-sin(dlg.m_Phi_R1)*sin(dlg.m_Omega_R1)*cos(dlg.m_Kappa_R1);
	a23=-sin(dlg.m_Phi_R1)*cos(dlg.m_Omega_R1);
	b21=cos(dlg.m_Omega_R1)*sin(dlg.m_Kappa_R1);
	b22=cos(dlg.m_Omega_R1)*cos(dlg.m_Kappa_R1);
	b23=-sin(dlg.m_Omega_R1);
	c21=sin(dlg.m_Phi_R1)*cos(dlg.m_Kappa_R1)+cos(dlg.m_Phi_R1)*sin(dlg.m_Omega_R1)*sin(dlg.m_Kappa_R1);
	c22=-sin(dlg.m_Phi_R1)*sin(dlg.m_Kappa_R1)+cos(dlg.m_Phi_R1)*sin(dlg.m_Omega_R1)*cos(dlg.m_Kappa_R1);
	c23=cos(dlg.m_Phi_R1)*cos(dlg.m_Omega_R1);

    L21=dlg.m_F_R*a21+(dlg.m_x_R-dlg.m_X0_R)*a23;
	L22=dlg.m_F_R*b21+(dlg.m_x_R-dlg.m_X0_R)*b23;
	L23=dlg.m_F_R*c21+(dlg.m_x_R-dlg.m_X0_R)*c23;
    L2x=dlg.m_F_R*a21*dlg.m_Xs_R+dlg.m_F_R*b21*dlg.m_Ys_R+dlg.m_F_R*c21*dlg.m_Zs_R+(dlg.m_x_R-dlg.m_X0_R)*a23*dlg.m_Xs_R+
        (dlg.m_x_R-dlg.m_X0_R)*b23*dlg.m_Ys_R+(dlg.m_x_R-dlg.m_X0_R)*c23*dlg.m_Zs_R;
    L24=dlg.m_F_R*a22+(dlg.m_y_R-dlg.m_Y0_R)*a23;
	L25=dlg.m_F_R*b22+(dlg.m_y_R-dlg.m_Y0_R)*b23;
	L26=dlg.m_F_R*c22+(dlg.m_y_R-dlg.m_Y0_R)*c23;
    L2y=dlg.m_F_R*a22*dlg.m_Xs_R+dlg.m_F_R*b22*dlg.m_Ys_R+dlg.m_F_R*c22*dlg.m_Zs_R+(dlg.m_y_R-dlg.m_Y0_R)*a23*dlg.m_Xs_R+
        (dlg.m_y_R-dlg.m_Y0_R)*b23*dlg.m_Ys_R+(dlg.m_y_R-dlg.m_Y0_R)*c23*dlg.m_Zs_R;
    
	double A1[3][3],L1[3];
	double X11[3];
	double X1,Y1,Z1;
        A1[0][0]=L11;
	    A1[0][1]=L12;
		A1[0][2]=L13;
		A1[1][0]=L14;
		A1[1][1]=L15;
		A1[1][2]=L16;
		A1[2][0]=L21;
		A1[2][1]=L22;
		A1[2][2]=L23;
        X11[0]=X1;
        X11[1]=Y1;
		X11[2]=Z1;
        L1[0]=L1x;
		L1[1]=L1y;
		L1[2]=L2x;
        InverMatrix(&A1[0][0], 3);
        MultMatrix(&A1[0][0],L1,X11, 3, 3, 1);



    double A[4][3],X22[3],L[4],V[4];
	double X2,Y2,Z2;
	    A[0][0]=L11;
	    A[0][1]=L12;
		A[0][2]=L13;
		A[1][0]=L14;
		A[1][1]=L15;
		A[1][2]=L16;
		A[2][0]=L21;
		A[2][1]=L22;
		A[2][2]=L23;
		A[3][0]=L24;
		A[3][1]=L25;
		A[3][2]=L26;
		X22[0]=X2;
        X22[1]=Y2;
		X22[2]=Z2;
        L[0]=L1x;
		L[1]=L1y;
		L[2]=L2x;
        L[3]=L2y;
		
		double AT[3][4],B[3][3],C[3][4];

        TranMatrix(&A[0][0], &AT[0][0], 4, 3);
        MultMatrix(&AT[0][0], &A[0][0],&B[3][3] ,3, 4, 3);
        InverMatrix( &B[0][0], 3);
        MultMatrix(&B[0][0], &AT[0][0],&C[3][4], 3, 3, 4);
		MultMatrix(&C[0][0], L,X22 ,3, 4, 1);
        
		double X,Y,Z;
		X = X11[0] + X22[0];
		Y = X11[1] + X22[1];
		Z = X11[2] + X22[2];
		
		dlg.m_X=X;
		dlg.m_Y=Y;
		dlg.m_Z=Z;
		*/
}

void CQianfangjiaohuiDoc::angel_to_(CString A,double B )
{
   /* double pi;
	pi=3.14159265;
	
     CString temp,s;
	 temp = A;
	 s = "";
	 int a,b,c,j;
	 j=0;
	 for(int i=0;i<temp.GetLength();i++)
	 {
		
	   if(temp[i]=='.')
	   {
	     j=i;
		 a=atoi(s);
		 break;
	   }
	   s+=temp[i];
       if(j!=0)
	   {
	    s="";
		s+=temp[j+1];
		s+=temp[j+2];
		b=atoi(s);
		s="";
		s+=temp[j+3];
		s+=temp[j+4];
		c=atoi(s);
	   }
      }
	 B=(a+(b/60)+(c/360))*pi/180;
	 */
}
void CQianfangjiaohuiDoc::OnDiedai() 
{
	// TODO: Add your command handler code here
	CDIEDAI dlg;
	dlg.DoModal();
}