back.cpp

Moravec算子实现程序 Moravec算子实现程序

// BACK.cpp : implementation file
//

#include "stdafx.h"
#include "Dan_backcom.h"
#include "BACK.h"
#include "Matrixs.h"
#include "math.h"
#include "ControlPoint.h"

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

/////////////////////////////////////////////////////////////////////////////
// BACK dialog


BACK::BACK(CWnd* pParent /*=NULL*/)
	: CDialog(BACK::IDD, pParent)
{
	m_num = 0;
	m_File = false;
	//{{AFX_DATA_INIT(Kongzhi)
	m_num = 0;
	m_mm = 0;
	m_mf = 0;
	//}}AFX_DATA_INIT
}


void BACK::DoDataExchange(CDataExchange* pDX)
{
	CDialog::DoDataExchange(pDX);
	//{{AFX_DATA_MAP(BACK)
	DDX_Text(pDX, IDC_NUM, m_num);
	DDX_Text(pDX, IDC_M_MM, m_mm);
	DDX_Text(pDX, IDC_m_mf, m_mf);
	//}}AFX_DATA_MAP
}


BEGIN_MESSAGE_MAP(BACK, CDialog)
	//{{AFX_MSG_MAP(BACK)
	ON_BN_CLICKED(IDC_OPEN, OnOpen)
	ON_BN_CLICKED(IDC_COUNT, OnCount)
	//}}AFX_MSG_MAP
END_MESSAGE_MAP()

/////////////////////////////////////////////////////////////////////////////
// BACK message handlers

void BACK::OnOpen() 
{
	// TODO: Add your control notification handler code here
	CFileDialog back(TRUE,"txt",".txt");
	if(back.DoModal()==IDOK)
	{
		m_FileName=back.GetPathName();
		ShellExecute(hwnd,NULL,m_FileName,NULL,NULL,SW_SHOWNORMAL);
	}
	m_File=TRUE;
}

void BACK::OnCount() 
{
	// TODO: Add your control notification handler code here
	UpdateData(true);
	int i;
	if(m_num<=0||m_num>30)
	{
		AfxMessageBox("请先输入控制点的个数！");
		return;
	}
	if(m_File==FALSE)
	{
		AfxMessageBox("请先新建一个数据文件或打开一个数据文件!");
		return;
	}

	m_x = new double [m_num];
	m_y = new double [m_num];
	m_xtp = new double [m_num];
	m_ytp = new double [m_num];
	m_ztp = new double [m_num];

	FILE *fp;
    fp=fopen(m_FileName,"r");
	for(i=0;i<m_num;i++)
		fscanf(fp,"%lf %lf %lf %lf %lf",&m_x[i],&m_y[i],&m_xtp[i],&m_ytp[i],&m_ztp[i]);
	fclose(fp);

	m_m1 = 0.00001;
	m_m2 =  0.01;

	
	CDan_backcomApp *app=(CDan_backcomApp*)AfxGetApp();
	
	app->m_f = m_mf/1000;
	app->m_m = m_mm;
	app->m_Xs = 0;
	app->m_Ys = 0;
	app->m_Zs = app->m_m * app->m_f;
	for(i=0;i<m_num;i++)
	{
		app->m_Xs += m_xtp[i]/m_num;
		app->m_Ys += m_ytp[i]/m_num;
		app->m_Zs += m_ztp[i]/m_num;
	}

	app->m_jiaox = 0;
	app->m_jiaoy = 0;
	app->m_jiaoz = 0;
	app->m_a1 = 1;
	app->m_a2 = 0;
	app->m_a3 = 0;
	app->m_b1 = 0;
	app->m_b2 = 1;
	app->m_b3 = 0;
	app->m_c1 = 0;
	app->m_c2 = 0;
	app->m_c3 = 1;
	Matrixs X(6,1);	 //外方位元素改矩阵m_Xs,m_Ys,m_Zs,m_jiaox,m_jiaoy,m_jiaoz
	Matrixs dX(6,1);	 //外方位元素改正数矩阵m_dXs,m_dYs,m_dZs,m_djiaox,m_djiaoy,m_djiaoz
	Matrixs T1(6,6),T2(6,1);
	Matrixs t(6,6);
	X.elems[0] = app->m_Xs;
	X.elems[1] = app->m_Ys;
	X.elems[2] = app->m_Zs;
	X.elems[3] = app->m_jiaox;
	X.elems[4] = app->m_jiaoy;
	X.elems[5] = app->m_jiaoz;

	int m;
	for(m=0;m<36;m++)
		T1.elems[m] = 0;
	for(m=0;m<6;m++)
		T2.elems[m] = 0;
	do 
	{
		
		for(i=0;i<4;i++)
		{
			app->m_a1 =  cos(X.elems[3])*cos(X.elems[5]) - sin(X.elems[3])*sin(X.elems[4])*sin(X.elems[5]);
			app->m_a2 = -cos(X.elems[3])*sin(X.elems[5]) - sin(X.elems[3])*sin(X.elems[4])*cos(X.elems[5]);
			app->m_a3 = -sin(X.elems[3])*cos(X.elems[4]);
			app->m_b1 =  cos(X.elems[4])*sin(X.elems[5]);
			app->m_b2 =  cos(X.elems[4])*cos(X.elems[5]);
			app->m_b3 =  sin(X.elems[4]);
			app->m_c1 =  sin(X.elems[3])*cos(X.elems[5]) + cos(X.elems[3])*sin(X.elems[4])*sin(X.elems[5]);
			app->m_c2 = -sin(X.elems[3])*sin(X.elems[5]) + cos(X.elems[3])*sin(X.elems[4])*cos(X.elems[5]);
			app->m_c3 =  cos(X.elems[3])*cos(X.elems[4]);
			CControlPoint P(m_x[i]/1000,m_y[i]/1000,m_xtp[i],m_ytp[i],m_ztp[i]);

			Matrixs A(2,6);
			Matrixs l(2,1);
			A = P.A();
			l = P.l();
			T1 = A.T() * A +T1;
			T2 = A.T() *l + T2;
		}
		T1.invers_matrix(6);

		dX = T1 * T2;

		app->m_Xs = X.elems[0];
	    app->m_Ys = X.elems[1];
	    app->m_Zs = X.elems[2];
        app->m_jiaox = X.elems[3];
	    app->m_jiaoy = X.elems[4]; 
	    app->m_jiaoz = X.elems[5];
		X = X + dX;
	}
	while(fabs(dX.elems[0])>m_m1||fabs(dX.elems[1])>m_m1||fabs(dX.elems[2])>m_m1||fabs(dX.elems[3])>m_m2||fabs(dX.elems[4])>m_m2||fabs(dX.elems[5])>m_m2);
//	while(dX.elems[0]>m_m1||dX.elems[1]>m_m1||dX.elems[2]>m_m1||dX.elems[3]>m_m2||dX.elems[4]>m_m2||dX.elems[5]>m_m2);

	CString str="后方交会计算外方位元素结果.txt";
		FILE *fpt;
		fpt=fopen(str,"w+");
		fprintf(fpt,"%10s\n","后方交会计算外方位元素结果表");    	
		fprintf(fpt,"%s\n","***************************************************************");
		fprintf(fpt," %5s=%lf\n %5s=%lf\n %5s=%lf\n %5s=%lf\n %5s=%lf\n %5s=%lf\n","Xs",app->m_Xs,"Ys",app->m_Ys,"Zs",app->m_Zs,"phi",app->m_jiaox,"omega",app->m_jiaoy,"kappa",app->m_jiaoz);

		fprintf(fpt,"%s\n ","*****************************************************************");
		fclose(fpt);
		ShellExecute(hwnd,NULL,str,NULL,NULL,SW_SHOWNORMAL);
}