calculate.cpp

像片外方位元素计算程序

// Calculate.cpp : implementation file
//

#include "stdafx.h"
#include "Ygpc.h"
#include "MainFrm.h"
#include "Calculate.h"
#include "ReadWrite.h"
#include "YgpcDoc.h"
#ifdef _DEBUG
#define new DEBUG_NEW
#undef THIS_FILE
static char THIS_FILE[] = __FILE__;
#endif

/////////////////////////////////////////////////////////////////////////////
// CCalculate dialog

#define PI atan(1.0)*4;
CCalculate::CCalculate(CWnd* pParent /*=NULL*/)
	: CDialog(CCalculate::IDD, pParent)
{   m_e=FALSE;
m_e1=FALSE;
//{{AFX_DATA_INIT(CCalculate)

	//}}AFX_DATA_INIT
q=w=k=0;

 f=x=y=0.0;
Xp=Yp=Zp=Xs=Ys=Zs=0.0;
pXs=pYs=pZs=pq=pw=pk=0.0;  

}


void CCalculate::DoDataExchange(CDataExchange* pDX)
{
	CDialog::DoDataExchange(pDX);
	//{{AFX_DATA_MAP(CCalculate)

	//}}AFX_DATA_MAP
}


BEGIN_MESSAGE_MAP(CCalculate, CDialog)
	//{{AFX_MSG_MAP(CCalculate)
   ON_BN_CLICKED(ID_DIALOG_CALCULATE, OnDialogCalculate)
	ON_BN_CLICKED(ID_OUTPUTDATA, OnOutputdata)
	ON_BN_CLICKED(IDC_SHOWWINDOW, OnShowwindow)
	ON_BN_CLICKED(IDC_DIALOG_CANCEL, OnDialogCancel)
	ON_BN_CLICKED(IDC_BUTTON2, OnButton2)
	ON_BN_CLICKED(ID_DIALOG_SHOWXY, OnDialogShowxy)
	ON_BN_CLICKED(ID_DIALOG_SHOWOUTDATA, OnDialogShowoutdata)
	ON_BN_CLICKED(ID_DIALOG_SHOWINDATA, OnDialogShowindata)
	ON_BN_CLICKED(ID_DIALOG_CANCEL, OnDialogCancel)
	
	//}}AFX_MSG_MAP
END_MESSAGE_MAP()

/////////////////////////////////////////////////////////////////////////////
// CCalculate message handlers




void CCalculate::JZXC(double **A,int m,int n)
{     
	for(int ii=0;ii<m;ii++){
	for(int jj=0;jj<m;jj++){
		Naa[ii][jj]=0.0;
	}
	}
 
	for(int k=0;k<m;k++){
	for(int l=0;l<m;l++){

		for(int j=0;j<n;j++)
		{
	
		Naa[k][l]+=A[k][j]*A[l][j];

		}
	}
	}
}
//gauss直接消去法]
void CCalculate::QJFCZ(double **Naa, double *K, double *W, int n) //n为控制点数
{ double NN=0,MM;
for(int k=0;k<2*n-1;k++){

	for(int i=k+1;i<2*n;i++){

	 MM=Naa[i][k]/Naa[k][k];
        W[i]-=W[k]*MM;
for(int j=k;j<2*n;j++){

		Naa[i][j]=Naa[i][j]-Naa[k][j]*MM;
            
			
			}
		}
	}
	for(int i=2*n-1;i>=0;i--){


	NN=0;
		for(int j=i+1;j<2*n;j++){
    
			NN=NN+Naa[i][j]*K[j];
	
	}
	K[i]=(W[i]-NN)/Naa[i][i];
	
	}

}


BOOL CCalculate::OnDialogCalculate() 
{
	// TODO: Add your control notification handler code here
CReadWrite m_p;

if (!m_p.Read())
return false;
	q=m_p.m_pInDataHead.q;//*Pai/180;
	w=m_p.m_pInDataHead.w;//*Pai/180;
	k=m_p.m_pInDataHead.k;//*Pai/180;
//==================================

    GZS[0][0]=cos(q)*cos(k)-sin(q)*sin(w)*sin(k);
	GZS[0][1]=-cos(q)*sin(k)-sin(q)*sin(w)*cos(k);
	GZS[0][2]=-sin(q)*cos(w);
	GZS[1][0]=cos(w)*sin(k);
	GZS[1][1]=cos(w)*cos(k);
	GZS[1][2]=-sin(w);
	GZS[2][0]=sin(q)*cos(k)+cos(q)*sin(w)*sin(k);
	GZS[2][1]=-sin(q)*sin(k)+cos(q)*sin(w)*cos(k);
	GZS[2][2]=cos(q)*cos(w);
 
//======================================

	
//==================================
//==============分配内存================

A=(double **)malloc(sizeof(double)*m_p.m_pInDataHead.ControlNum*2);
Naa=(double **)malloc(sizeof(double)*m_p.m_pInDataHead.ControlNum*2);

for(int jj=0;jj<m_p.m_pInDataHead.ControlNum*2;jj++){
A[jj]=(double*)malloc(sizeof(double)*6);
Naa[jj]=(double *)malloc(sizeof(double)*m_p.m_pInDataHead.ControlNum*2);
W=(double*)malloc(sizeof(double)*m_p.m_pInDataHead.ControlNum*2);
}
K=(double *)malloc(sizeof(double)*m_p.m_pInDataHead.ControlNum*2);
V=(double*)malloc(sizeof(double)*6);
for(int ii=0;ii<6;ii++)
V[ii]=0.0;
for(ii=0;ii<m_p.m_pInDataHead.ControlNum*2;ii++)
K[ii]=0.0;
//===========================
//================为A和W数组赋值========

for(int i=0;i<2*m_p.m_pInDataHead.ControlNum;i=i+2){
int j=i+1;
//==============================
Xp=m_p.m_pInData[i/2].Xp;
Yp=m_p.m_pInData[i/2].Yp;
Zp=m_p.m_pInData[i/2].Zp;
Xs=m_p.m_pInDataHead.Xs;
Ys=m_p.m_pInDataHead.Ys;
Zs=m_p.m_pInDataHead.Zs;	
f=m_p.m_pInDataHead.f;
x=m_p.m_pInData[i/2].x;
y=m_p.m_pInData[i/2].y;
//==============================
    //A[i][0]=-GZS[0][2]*m_p.m_pInData[i/2].x-GZS[0][0]*m_p.m_pInDataHead.f;
	//A[i][1]=-GZS[1][2]*m_p.m_pInData[i/2].x-GZS[1][0]*m_p.m_pInDataHead.f;
	//A[i][2]=-(GZS[2][2]*m_p.m_pInData[i/2].x+GZS[2][0]*m_p.m_pInDataHead.f);
   
   A[i][0]=-GZS[0][2]*x-GZS[0][0]*f;
  A[i][1]=-GZS[1][2]*x-GZS[1][0]*f;
  A[i][2]=-(GZS[2][2]*x+GZS[2][0]*f);
 
  A[i][3]=(-GZS[2][2]*(Xp-Xs)*x+GZS[0][2]*x*(Zp-Zs)-GZS[2][0]*f*(Xp-Xs)+GZS[0][0]*f*(Zp-Zs))/m_pp;
  A[i][4]=(sin(q)*sin(w)*(Xp-Xs)*x-cos(w)*x*(Yp-Ys)-cos(q)*sin(w)*x*(Zp-Zs)-sin(q)*cos(w)*sin(k)*f*(Xp-Xs)-sin(w)*sin(k)*f*(Yp-Ys)+cos(q)*cos(w)*sin(k)*f*(Zp-Zs))/m_pp;
  A[i][5]=(GZS[0][1]*f*(Xp-Xs)+GZS[1][1]*f*(Yp-Ys)+GZS[2][1]*f*(Zp-Zs))/m_pp;

  //A[i][3]=A[i][4]=A[i][5]=0.0;
	A[j][0]=-GZS[0][2]*y-GZS[0][1]*f;
	A[j][1]=-(GZS[1][2]*y+GZS[1][1]*f);
	A[j][2]=-GZS[2][2]*y-GZS[2][1]*f;
   //A[j][3]=A[j][4]=A[j][5]=0.0;
//A[j][0]=A[j][1]=A[j][2]=0.0;
A[j][3]=(-GZS[2][2]*y*(Xp-Xs)+GZS[0][2]*y*(Zp-Zs)-GZS[2][1]*f*(Xp-Xs)+GZS[0][1]*f*(Zp-Zs))/m_pp;
A[j][4]=(sin(q)*sin(w)*(Xp-Xs)*y-cos(w)*y*(Yp-Ys)-cos(q)*sin(w)*y*(Zp-Zs)-sin(q)*cos(w)*cos(k)*f*(Xp-Xs)-sin(w)*cos(k)*f*(Yp-Ys)+cos(q)*cos(w)*cos(k)*f*(Zp-Zs))/m_pp;
A[j][5]=(-GZS[0][0]*f*(Xp-Xs)-GZS[1][0]*f*(Yp-Ys)-GZS[2][0]*f*(Zp-Zs))/m_pp;

W[i]=-(GZS[0][2]*(Xp-Xs)*x+GZS[1][2]*x*(Yp-Ys)+GZS[2][2]*x*(Zp-Zs)+GZS[0][0]*f*(Xp-Xs)+GZS[1][0]*f*(Yp-Ys)+GZS[2][0]*f*(Zp-Zs));
W[j]=-(GZS[0][2]*(Xp-Xs)*y+GZS[1][2]*y*(Yp-Ys)+GZS[2][2]*y*(Zp-Zs)+GZS[0][1]*f*(Xp-Xs)+GZS[1][1]*f*(Yp-Ys)+GZS[2][1]*f*(Zp-Zs));
}
//=======================================
JZXC(A,m_p.m_pInDataHead.ControlNum*2,6);
  //QJFCZ(Naa,K,W,m_p.m_pInDataHead.ControlNum);
  double **Naa1;

  Naa1=(double **)malloc(sizeof(double)*m_p.m_pInDataHead.ControlNum*2);

  for(jj=0;jj<m_p.m_pInDataHead.ControlNum*2;jj++){

	 Naa1[jj]=(double *)malloc(sizeof(double)*m_p.m_pInDataHead.ControlNum*2);
  }

for(i=0;i<m_p.m_pInDataHead.ControlNum*2;i++)
  for(int j=0;j<m_p.m_pInDataHead.ControlNum*2;j++)
	  Naa1[i][j]=Naa[i][j];

invMatrixGJ(Naa1,m_p.m_pInDataHead.ControlNum*2);

if(gauss(Naa,W,m_p.m_pInDataHead.ControlNum*2)!=0)
{ 
/*===========================================

  现在W[]极为方程的界
  	===========================================
   */
  //把W[]赋给K[];
   for (jj=0;jj<m_p.m_pInDataHead.ControlNum*2;jj++)
	   K[jj]=W[jj];
   //===========
  for(int j=0;j<6;j++){
  for(int i=0;i<m_p.m_pInDataHead.ControlNum*2;i++)
  {
	 
V[j]+=A[i][j]*K[i];
}
	  
	  }
}
   	CButton *m_pB1=(CButton *)GetDlgItem(IDC_SHOWWINDOW);
    CButton *m_pB2=(CButton *)GetDlgItem(ID_OUTPUTDATA);
    m_pB1->EnableWindow(true);
    m_pB2->EnableWindow(true);
    return TRUE;
}

void CCalculate::OnOutputdata() 
{
	// TODO: Add your control notification handler code here

	CReadWrite m_p;
	CStdioFile file;
	 CYgpcDoc *pDoc=(CYgpcDoc*)((CMainFrame*)AfxGetApp()->m_pMainWnd)->GetActiveDocument();
    CFileDialog m_Save(false,NULL,"outdata.txt",OFN_HIDEREADONLY | OFN_OVERWRITEPROMPT,
		"(*.txt)|*.txt",NULL);
	m_Save.DoModal();
	CString m_path=m_Save.GetPathName();
    CString a;
    file.Open(m_path,CFile::modeWrite|CFile::modeCreate|CFile::typeText);
  //============输出该证书
	//===========与要改正!!!!!!!!!!!!!!!!!!!!!!!!!!
a.Format("%s\n","六个改正数:");
 file.WriteString(a);
 for(int i=0;i<6;i++){
  a.Format("V[%d]=%8.12f\n",i+1,V[i]);
  file.WriteString(a);
}
 a.Format("%s\n","======================================================");
 file.WriteString(a);

 //==================================
 //======================输出(x,y)====================
m_p.Read1();

//dX=(double*)malloc(sizeof(double)*pDoc->dwdNumb);
//dY=(double*)malloc(sizeof(double)*pDoc->dwdNumb);
CMainFrame *pMain=(CMainFrame *)AfxGetApp()->m_pMainWnd;
/*=========================
Xs=m_p.m_pInDataHead.Xs;
Ys=m_p.m_pInDataHead.Ys;
Zs=m_p.m_pInDataHead.Zs;	
f=m_p.m_pInDataHead.f;
q=m_p.m_pInDataHead.q;
w=m_p.m_pInDataHead.w;
k=m_p.m_pInDataHead.k;
======================================*/
//==============平差后的六个外方为元素
pXs=m_p.m_pInDataHead.Xs+V[0];
pYs=m_p.m_pInDataHead.Ys+V[1];
pZs=m_p.m_pInDataHead.Zs+V[2];

/*	
pq=(m_p.m_pInDataHead.q+V[3])*Pai/180;
pw=(m_p.m_pInDataHead.w+V[4])*Pai/180;
pk=(m_p.m_pInDataHead.k+V[5])*Pai/180;
*/
pq=(m_p.m_pInDataHead.q+V[3]);//*Pai/180;
pw=(m_p.m_pInDataHead.w+V[4]);//*Pai/180;
pk=(m_p.m_pInDataHead.k+V[5]);//*Pai/180;

//========================================
   //=============改正之后的矩阵

    pGZS[0][0]=cos(pq)*cos(pk)-sin(pq)*sin(pw)*sin(pk);
	pGZS[0][1]=-cos(pq)*sin(pk)-sin(pq)*sin(pw)*cos(pk);
	pGZS[0][2]=-sin(pq)*cos(pw);
	pGZS[1][0]=cos(pw)*sin(pk);
	pGZS[1][1]=cos(pw)*cos(pk);
	pGZS[1][2]=-sin(pw);
	pGZS[2][0]=sin(pq)*cos(pk)+cos(pq)*sin(pw)*sin(pk);
	pGZS[2][1]=-sin(pq)*sin(pk)+cos(pq)*sin(pw)*cos(pk);
	pGZS[2][2]=cos(pq)*cos(pw);
//===============================================
//a.Format("%s\n","由外方位元素dXs,dYs,dZs,dq,dw,dk造成的像点位移:");
 a.Format("%s\n","由平差之后的外方位元素计算出的地物点图像坐标:");
	file.WriteString(a);
for(i=0;i<pDoc->dwdNumb;i++)
{
Xp=m_p.m_pDmzb[i].Xp;
Yp=m_p.m_pDmzb[i].Yp;
Zp=m_p.m_pDmzb[i].Zp;
//H=Zs-Zp;
//double x1,y1;
//x1=f*(pGZS[0][0]*(Xp-pXs)+pGZS[1][0]*(Yp-pYs)+pGZS[2][0]*(Zp-pZs))+(pGZS[0][2]*(Xp-pXs)+pGZS[1][2]*(Yp-pYs)+pGZS[2][2]*(Zp-pZs))*x;
//y1=f*(pGZS[0][1]*(Xp-pXs)+pGZS[1][1]*(Yp-pYs)+pGZS[2][1]*(Zp-pZs))+(pGZS[0][2]*(Xp-pXs)+pGZS[1][2]*(Yp-pYs)+pGZS[2][2]*(Zp-pZs))*y;
x=-f*(pGZS[0][0]*(Xp-pXs)+pGZS[1][0]*(Yp-pYs)+pGZS[2][0]*(Zp-pZs))/(pGZS[0][2]*(Xp-pXs)+pGZS[1][2]*(Yp-pYs)+pGZS[2][2]*(Zp-pZs));
y=-f*(pGZS[0][1]*(Xp-pXs)+pGZS[1][1]*(Yp-pYs)+pGZS[2][1]*(Zp-pZs))/(pGZS[0][2]*(Xp-pXs)+pGZS[1][2]*(Yp-pYs)+pGZS[2][2]*(Zp-pZs));
a.Format("x[%d]=%5.8lf,      y[%d]=%5.8lf\n",i+1,x,i+1,y);
file.WriteString(a); 
/*

dX[i]=-(-(pGZS[0][2]*x+pGZS[0][0]*f)*V[0]-(pGZS[2][2]*x+pGZS[2][0]*f)*V[1]-(pGZS[2][2]*x+pGZS[2][0]*f)*V[2]+(-pGZS[2][2]*(Xp-pXs)*x+pGZS[0][2]*x*(Zp-pZs)-pGZS[2][0]*f*(Xp-pXs)+pGZS[0][0]*f*(Zp-pZs))*V[3]
+(sin(q)*sin(w)*(Xp-pXs)*x-cos(w)*x*(Yp-pYs)-cos(q)*sin(w)*x*(Zp-pZs)-sin(q)*cos(w)*sin(k)*f*(Xp-pXs)-sin(w)*sin(k)*f*(Yp-pYs)+cos(q)*cos(w)*sin(k)*f*(Zp-pZs))*V[4]+	(pGZS[0][1]*f*(Xp-pXs)+pGZS[1][1]*f*(Yp-pYs)+pGZS[2][1]*f*(Zp-pZs))*V[5])/(-sin(q)*cos(w)*(Xp-pXs)-sin(w)*(Yp-pYs)+cos(q)*cos(w)*(Zp-pZs));
dY[i]=-((-pGZS[0][2]*y-pGZS[0][1]*f)*V[0]-(pGZS[1][2]*y+pGZS[1][1]*f)*V[1]+(-pGZS[2][2]*y-pGZS[2][1]*f)*V[2]+(-pGZS[2][2]*y*(Xp-pXs)+pGZS[0][2]*y*(Zp-pZs)+pGZS[2][0]*f*(Xp-pXs)+pGZS[0][1]*f*(Zp-pZs))*V[3]+(sin(q)*sin(w)*(Xp-pXs)*y-cos(w)*y*(Yp-pYs)-cos(q)*sin(w)*y*(Zp-pZs)-sin(q)*cos(w)*cos(k)*f*(Xp-pXs)-sin(w)*cos(k)*f*(Yp-pYs)+cos(q)*cos(w)*cos(k)*f*(Zp-pZs))*V[4]+
(-pGZS[0][0]*f*(Xp-pXs)-pGZS[1][0]*f*(Yp-pYs)-pGZS[2][0]*f*(Zp-pZs))*V[5])/(-sin(q)*cos(w)*(Xp-pXs)-sin(w)*(Yp-pYs)+cos(q)*cos(w)*(Zp-pZs));	
a.Format("dX[%d]=%lf,      dY[%d]=%lf\n",i+1,dX[i],i+1,dY[i]);
file.WriteString(a);  
*/       




} 







/* ============================================ 预留区域
a.Format("%s\n","纠正之前的地物点图像坐标:");
file.WriteString(a);
a.Format("x[%d]=%lf      y[%d]=%lf\n",i+1,x,i+1,y);
file.WriteString(a);
a.Format("%s\n","像点位移:");