#include<iostream.h>
#include<fstream.h>
#include<string.h>
#include<math.h>
#define M1 12//控制點數
#define M2 6//加密點數
#define M3 (M1+M2)//總點數(M1+M2)
#define M (M1*2)//觀測值方程個數
#define N (3*M2)//未知數個數
double VV;//平差后單位權中誤差
double X[M3],Y[M3],Z[M3];//控制點和加密點的物方空間坐標
double x(0),y(0),z(0);//控制點的中心坐標
double U[M3],V[M3];//左像片量測儀坐標
double R[M3],T[M3];//右像片量測儀坐標
double L[22];//左,右像片的變換參數
double E[22];//變換參數的存儲
double C[M][12];//觀測值方程系數的增廣陣
double Q[M2][4];//加密點物方坐標平差后單位權中誤差及坐標中誤差
double A[M];//Ai
double D[M];//方程式解算中前后兩迭代值的差值
double S,u,LA,RA,DL,DR,SS;//A值迭代差中最大絕對值
double M0(0);
double W(0);
double AL,AR,XL,YL,XR,YR;
int i,j,k;
	
 void main()
 {  void sub220();
	void sub200();
    void sub240();
	void sub280();
	 ofstream outw;
 outw.open("Wut.txt",ios::out);
	cout<<"_______________________________________________________________________________"<<endl;
	 cout<<"         DIRECT LINEAR TRANSFORMATOIN FOR cLOSE-RANGE PHOTOGRAMMETRY"<<endl;
     cout<<"_______________________________________________________________________________"<<endl;

     outw<<"_______________________________________________________________________________"<<endl;
	 outw<<"         DIRECT LINEAR TRANSFORMATOIN FOR cLOSE-RANGE PHOTOGRAMMETRY"<<endl;
     outw<<"_______________________________________________________________________________"<<endl;
 //following to input data
/*    for(int i=0;i<M1;i++)
	{outw<<"input X("<<i+1<<")=";
	cin>>X[i];outw<<endl;x+=X[i];
	
	outw<<"input Y("<<i+1<<")=";
	cin>>Y[i];outw<<endl;y+=Y[i];
	
	outw<<"input Z("<<i+1<<")=";
	cin>>Z[i];outw<<endl;z+=Z[i];
		
	}*/
 X[0]=12.6999;Y[0]=23.5522;Z[0]=10.1393;
  X[1]=12.6996;Y[1]=23.5176;Z[1]=8.8415;
  X[2]=16.1862;Y[2]=23.5390;Z[2]=10.7109;
  X[3]=16.1850;Y[3]=23.5353;Z[3]=9.8167;
  X[4]=19.6903;Y[4]=23.5155;Z[4]=10.6328;
  X[5]=19.6804;Y[5]=23.5667;Z[5]=8.7975;
  X[6]=12.7182;Y[6]=28.5716;Z[6]=10.7504;
  X[7]=12.7242;Y[7]=28.5425;Z[7]=9.1554;
  X[8]=16.2201;Y[8]=28.5804;Z[8]=10.7509;
  X[9]=16.2217;Y[9]=28.5656;Z[9]=9.0678;
  X[10]=19.7399;Y[10]=28.5745;Z[10]=9.9164;
  X[11]=19.7469;Y[11]=28.5885;Z[11]=8.8185;
	char c,e;int t;
// following to change input data
y:outw<<"              X           Y           Z"<<endl;   

  for(i=0;i<M1;i++)
  { x+=X[i];
    y+=Y[i];
	z+=Z[i];
	outw<<"    "<<i+1<<"         "<<X[i]<<"     "<<Y[i]<<"      "<<Z[i]<<endl; 
	cout<<"    "<<i+1<<"         "<<X[i]<<"     "<<Y[i]<<"      "<<Z[i]<<endl; 
  }

  
  cout<<"   Are there errors in input data? (y/n)"<<endl;
 cin>>e; 
 
 if(e=='n') goto n;
   else if(e=='y')
  {
a:          cout<<"input the error in X/Y/Z and the number of point"<<endl; 
          cin>>c>>t;
		  if(c=='x'){cout<<" X("<<t<<")=";cin>>X[t-1];cout<<endl;}
          if(c=='y'){cout<<" Y("<<t<<")=";cin>>Y[t-1];cout<<endl;}
          if(c=='z'){cout<<" Z("<<t<<")=";cin>>Z[t-1];cout<<endl;}
  outw<<"Is there another error?y/n"<<endl;
  cin>>c;
  if(c=='y') goto a;
		  else if(c=='n') goto y;
		  else cout<<"error inputting"<<endl;
  }  
   else goto y;
n:   ;
//following to get average controlling coordernates
	 x=x/M1;
	 y=y/M1;
	 z=z/M1;
	 outw<<endl<<x<<"   "<<y<<"    "<<z<<endl;
	 cout<<endl<<x<<"   "<<y<<"    "<<z<<endl;
cout<<"input the photo-coordernates "<<endl;
/*	 for(i=0;i<M3;i++)
	{
		outw<<"xl("<<i+1<<")=";cin>>U[i];
        outw<<"yl("<<i+1<<")=";cin>>V[i];
        outw<<"xr("<<i+1<<")=";cin>>R[i];
		outw<<"yr("<<i+1<<")=";cin>>T[i];outw<<endl;
	 }*/
U[0]=1.69040;V[0]=0.68066;R[0]=1.28816;T[0]=0.67424;
U[1]=1.69310;V[1]=0.62480;R[1]=1.28966;T[1]=0.62408;
U[2]=1.82660;V[2]=0.70728;R[2]=1.41680;T[2]=0.70380;
U[3]=1.82822;V[3]=0.67218;R[3]=1.41788;T[3]=0.66642;
U[4]=1.94160;V[4]=0.70584;R[4]=1.56572;T[4]=0.70772;
U[5]=1.94260;V[5]=0.63998;R[5]=1.56750;T[5]=0.62654;
U[6]=1.65890;V[6]=0.69534;R[6]=1.36090;T[6]=0.69176;
U[7]=1.66140;V[7]=0.64436;R[7]=1.36246;T[7]=0.64480;
U[8]=1.76516;V[8]=0.69762;R[8]=1.46200;T[8]=0.69660;
U[9]=1.76724;V[9]=0.64740;R[9]=1.46354;T[9]=0.64482;
U[10]=1.85968;V[10]=0.67624;R[10]=1.57382;T[10]=0.67482;
U[11]=1.86080;V[11]=0.64560;R[11]=1.57500;T[11]=0.63948;
U[12]=1.68508;V[12]=0.67814;R[12]=1.31012;T[12]=0.67308;
U[13]=1.70760;V[13]=0.65926;R[13]=1.37300;T[13]=0.65710;
U[14]=1.75214;V[14]=0.63514;R[14]=1.42914;T[14]=0.63258;
U[15]=1.78474;V[15]=0.68182;R[15]=1.40162;T[15]=0.67724;
U[16]=1.79968;V[16]=0.63510;R[16]=1.44578;T[16]=0.62984;
U[17]=1.84504;V[17]=0.66670;R[17]=1.50800;T[17]=0.66258;
y11:outw<<"               xl         yl            xr         yr "<<endl;   
cout<<"               xl        yl            xr         yr "<<endl;   

  for(i=0;i<M3;i++)
  { 
	outw<<"    "<<i+1<<"         "<<U[i]<<"     "<<V[i]<<"         "<<R[i]<<"         "<<T[i]<<endl; 
	cout<<"    "<<i+1<<"         "<<U[i]<<"     "<<V[i]<<"         "<<R[i]<<"         "<<T[i]<<endl; 
  }
  cout<<"   Are there errors in input data? (y/n)"<<endl;
 cin>>e; 
 
 if(e=='n') goto n11;
   else if(e=='y')
  {
a11:      cout<<"input the error in XL/YL/XR/YR and the number of point"<<endl; 
          cin>>c>>t;
		  if(c=='u'){cout<<" XL("<<t<<")=";cin>>U[t-1];cout<<endl;}
          if(c=='v'){cout<<" YL("<<t<<")=";cin>>V[t-1];cout<<endl;}
          if(c=='r'){cout<<" XR("<<t<<")=";cin>>R[t-1];cout<<endl;}
          if(c=='t'){cout<<" YR("<<t<<")=";cin>>T[t-1];cout<<endl;}
                 
  cout<<"Is there another error?y/n"<<endl;
  cin>>c;
  if(c=='y') goto a11;
		  else if(c=='n') goto y11;
		  else cout<<"error inputting"<<endl;
  }  
   else goto y11;
 //
   //--------以上為數據輸入及檢查--------------------//
n11:
      for(i=0;i<M1;i++){X[i]-=x;Y[i]-=y;Z[i]-=z;cout<<X[i]<<"   "<<Y[i]<<"   "<<Z[i]<<endl;}
W=0;
#define	N 11
 
  
	  for(i=0;i<M1;i++)
		 A[i]=1;
a1:      sub200();
	     sub240();
	  outw<<"變換參數計算結果"<<endl;
	  if(W==0) outw<<"左片:";
	  else outw<<"右片:"<<endl;
rr:     for(i=0;i<M1;i++){
	  A[M1+i]=L[19]*X[i]+L[20]*Y[i]+L[21]*Z[i]+1;
		cout<<"A"<<i<<"="<<A[i]<<endl;}for(i=0;i<M1;i++)
	   {D[i]=A[M1+i]-A[i];A[i]=A[M1+i];}S=0;
	     for(i=0;i<M1;i++) if(fabs(D[i])>S) S=fabs(D[i]);
	
		if(S<0.00001) goto l5;
        sub200();
		cout<<"second sub200 is ended"<<endl;
		sub240();
        goto rr;//above is checked

l5:      outw<<"  (2)A=";
		 for(i=0;i<M1;i++) outw<<A[i]<<endl;
		 outw<<"  (3)L=";
		  for(i=0;i<11;i++) outw<<L[11+i]<<endl;
		  outw<<"M.S.E of UNIT-WEIGHT="<<VV<<endl;
		  if(W==1) {
			  goto Q;}
           for(i=0;i<11;i++) L[i]=L[11+i];
		    for(i=0;i<M1;i++)
			{U[i]=R[i];V[i]=T[i];}
			W=1;goto a1;
Q:;
#define N 3
#define M 4
			for(i=0;i<22;i++) E[i]=L[i];
			outw<<"OBject-Space coordernates of Determined Points:"<<endl;
			outw<<"              X             Y            Z"<<endl;
       for(i=0;i<M2;i++){
			AL=1;AR=1;XL=U[M1+i];YL=V[M1+i];XR=R[M1+i];YR=T[M1+i];
A6:     	sub220();
			sub240();
			LA=E[8]*L[11]+E[9]*L[12]+E[10]*L[13]+1;
			RA=E[19]*L[11]+E[20]*L[12]+E[21]*L[13]+1;
			DL=LA+1/AL;DR=RA+1/AR;
			if(fabs(DL)>fabs(DR)) SS=fabs(DL);
			else SS=fabs(DR);
			if(SS<0.00001) goto A10;
			AL=LA;AR=RA;goto A6;
A10:		X[M1+i]=L[11]+x;
			Y[M1+i]=L[12]+y;
			Z[M1+i]=L[13]+z;
			outw<<X[M1+i]<<Y[M1+i]<<Z[M1+i]<<endl;
			sub280();
			Q[i][0]=VV;Q[i][1]=sqrt(C[0][0]);
			Q[i][2]=sqrt(C[1][1]);Q[i][3]=sqrt(C[2][2]);
			M0=M0+VV*VV;
			}
			outw<<"Precision of determined points:"<<endl;
			M0=sqrt(M0/M2);
            for(i=0;i<M2;i++)
			{
				Q[i][1]*=M0;Q[i][2]*=M0;Q[i][3]*=M0;
			}
			outw<<"          M0          Mx         My        Mz"<<endl;
			 for(i=0;i<M2;i++)
				 outw<<"          "<<Q[i][0]<<"        "<<Q[i][1]<<"        "<<Q[i][2]<<"         "<<Q[i][3]<<endl;
			 outw<<"Mean M.S.E of UNIT-WEIGHT="<<M0<<endl;
			 outw<<"----------------------END------------------------"<<endl;
             outw.close();   
}

void sub200()
	 {int B;double K;
		 
	 for(i=1;i<=M-1;i+=2)
	 {
		 B=(i+1)/2-1;K=-1/A[B];
		 C[i][0]=K*X[B];C[i][1]=K*Y[B];
		 C[i][2]=K*Z[B];C[i][3]=K;
		 for(int j=4;j<8;j++)
		 C[i][j]=0;
	     C[i][8]=K*X[B]*U[B];C[i][9]=K*Y[B]*U[B];
		 C[i][10]=K*Z[B]*U[B];C[i][11]=K*U[B];
	 }
      for(i=1;i<M;i+=2)
	  {
		  B=(i+1)/2;K=-1/A[B];
		  for(j=0;j<4;j++) C[i][j]=0;
		  C[i][4]=K*X[B];C[i][5]=K*Y[B];
		  C[i][6]=K*Z[B];C[i][7]=K;
		  C[i][8]=K*X[B]*V[B];C[i][9]=K*Y[B]*V[B];
		  C[i][10]=K*Z[B]*V[B];C[i][11]=K*V[B];
	  }
	 }


void sub240()
	  {double B;
		  for(i=0;i<N+1;i++)
		  {S=0;
		  for(k=i;k<=M;k++)
			  S=S+C[k][i]*C[k][i];
		  if(C[i][i]<0) S=sqrt(S);
		  else S=-sqrt(S);
          u=C[i][i]-S;C[i][i]=S;
		  if(i==N) goto zz;
          B=S*u;
		  for(j=i+1;j<=N+1;j++)
		  {
			  double SI(0);
			  for(k=i+1;k<=M;k++)
				  SI+=C[k][i]*C[k][j];
			  SI=(u*C[i][j]+SI)/B;
			  C[i][j]+=SI*u;
              for(k=i+1;k<=M;k++)
				  C[k][j]+=SI*C[k][i];  
		  }
		  }
zz:VV=C[N+1][N+1]*C[N+1][N+1]/(M-N);VV=sqrt(VV);
        for(i=N-1;i>=0;i--)
		{L[11+i]=-C[i][N+1]/C[i][i];
		if(i==N-1) goto Z66;
		for(j=i+1;j<N;j++) L[i+11]-=L[j+11]*C[i][j]/C[i][i];
Z66: ;
		}
}

void sub220()
{
	AL=-1/AL;AR=-1/AR;
	for(j=0;j<3;j++)
	{C[0][j]=AL*(E[j]+E[j+8]*XL);
	C[1][j]=AL*(E[j+4]+E[j+8]*YL);
	}
	C[0][3]=AL*(E[3]+XL);C[1][3]=AL*(E[7]+YL);
    for(j=0;j<3;j++)
	C[2][j]=AR*(E[j+11]+E[j+19]*XR);
	for(j=0;j<3;j++)
	C[3][j]=AR*(E[j+15]+E[j+19]*YR);
	C[2][3]=AR*(E[14]+XR);C[3][3]=AR*(E[18]+YR);
	
}


void sub280()
{
 
	for(i=0;i<N;i++)
	{C[0][i]=1/C[i][i];
	for(j=i+1;j<N;j++)
	{if(i==N-1) goto ZB;
	S=0;
	for(k=i;k<=j-1;k++)
		S-=C[i][k]*C[k][j];
	C[i][j]=S/C[j][j];
	}
ZB: ; 
	}
	for(i=0;i<N;i++)
			for(j=i;j<N;j++)
			{S=0;
			for(k=j;k<N;k++)
				S=S+C[i][k]*C[j][k];
			C[i][j]=S;
			}

}