#include    <iostream.h>
#include    <fstream.h>
#include	<math.h>
#include    <process.h>
#include    <iomanip.h>
void main(){
		fstream iofile("E://攝影測量//21504103//計算結果.txt",ios::in|ios::out|ios::ate);
/////////////////////將計算結果輸出到該路徑，運行時請保證路徑存在的正確性
if(! iofile)
{
      cout<<"不能打開文件："<<"myfile.txt"<<endl;
      exit(1);
}
  double f=0.15272,x0=-0.000004,y0=-0.000008; //內方位元素 單位m
  double Xs1=14928.752,   Xs2=16297.601,
	     Ys1=11534.169,   Ys2=11524.011,
		 Zs1=3232.757,    Zs2=3240.056,
		 fai1=-0.00313379,fai2=0.00095182,
	 	 w1=-0.00264562,  w2=-0.00897463,
		 k1=-0.01154886,  k2=-0.00297374;  //左右像片的外方位元素
  double aa1,aa2,aa3,ba1,ba2,ba3,ca1,ca2,ca3,//組成左像片的旋轉矩陣
	    ab1,ab2,ab3,bb1,bb2,bb3,cb1,cb2,cb3; //組成右像片的旋轉矩陣
  double Bx,By,Bz; //攝影基線的分量
        Bx=Xs2-Xs1;
		By=Ys2-Ys1;
		Bz=Zs2-Zs1;

        aa1=cos(fai1)*cos(k1)-sin(fai1)*sin(w1)*sin(k1);
	    aa2=-cos(fai1)*sin(k1)-sin(fai1)*sin(w1)*cos(k1);
	    aa3=-sin(fai1)*cos(w1);
        ba1=cos(w1)*sin(k1);
    	ba2=cos(w1)*cos(k1);
    	ba3=-sin(w1);
    	ca1=sin(fai1)*cos(k1)+cos(fai1)*sin(w1)*sin(k1);
    	ca2=-sin(fai1)*sin(k1)+cos(fai1)*sin(w1)*cos(k1);
    	ca3=cos(fai1)*cos(w1);

		ab1=cos(fai2)*cos(k2)-sin(fai2)*sin(w2)*sin(k2);
	    ab2=-cos(fai2)*sin(k2)-sin(fai2)*sin(w2)*cos(k2);
	    ab3=-sin(fai2)*cos(w2);
        bb1=cos(w2)*sin(k2);
    	bb2=cos(w2)*cos(k2);
    	bb3=-sin(w2);
    	cb1=sin(fai2)*cos(k2)+cos(fai2)*sin(w2)*sin(k2);
    	cb2=-sin(fai2)*sin(k2)+cos(fai2)*sin(w2)*cos(k2);
    	cb3=cos(fai2)*cos(w2);

double xl[14]={2.417, 88.239, -2.346, 83.775, 0.332, 85.721, 43.633, 
               25.601, 2.967,29.761, 26.803, 8.754, 102.778, 25.745};
double yl[14]={-67.506,-73.695,-15.229,-1.916,59.081,69.664,-13.703,
               70.648,75.76,-84.546,96.212,45.97,-31.574,100.498};
double xr[14]={-82.972,2.713,-97.159,-3.341,-84.531,0.571,-48.313,
               -61.169,-81.625,-55.83,-57.475,-76.425,13.769};
double yr[14]={-65.728,-72.52,-13.79,-1.278,60.381,70.157,-12.637,
                     71.717,77.062,-82.822,97.373,47.167,-30.88};
//左右像片的像點坐標 單位mm
 double N1[50],N2[50]; //投影系數
 double X1[50],Y1[50],Z1[50],X2[50],Y2[50],Z2[50]; //左右像點的像空間輔助坐標
 double Xtp[50],Ytp[50],Ztp[50]; //地面攝影測量坐標
   int i;     
   for(i=0;i<14;i++){
	    X1[i]=aa1*(xl[i]*0.001-x0)+aa2*(yl[i]*0.001-y0)+aa3*-f;
		Y1[i]=ba1*(xl[i]*0.001-x0)+ba2*(yl[i]*0.001-y0)+ba3*-f;
		Z1[i]=ca1*(xl[i]*0.001-x0)+ca2*(yl[i]*0.001-y0)+ca3*-f;

		X2[i]=ab1*(xr[i]*0.001-x0)+ab2*(yr[i]*0.001-y0)+ab3*-f;
		Y2[i]=bb1*(xr[i]*0.001-x0)+bb2*(yr[i]*0.001-y0)+bb3*-f;
		Z2[i]=cb1*(xr[i]*0.001-x0)+cb2*(yr[i]*0.001-y0)+cb3*-f;

		N1[0]=(Bx*Z2[i]-Bz*X2[i])/(X1[i]*Z2[i]-X2[i]*Z1[i]); 
		N2[0]=(Bx*Z1[i]-Bz*X1[i])/(X1[i]*Z2[i]-X2[i]*Z1[i]);

        Xtp[i]=N1[0]*X1[i]+Xs1; 
		Ytp[i]=((N1[0]*Y1[i]+Ys1)+(N2[0]*Y2[i]+Ys2))/2;
		Ztp[i]=N1[0]*Z1[i]+Zs1; 

cout<<"Xtp"<<i+1<<"="<<Xtp[i]<<setw(10)<<"Ytp"<<i+1<<"="<<Ytp[i]<<setw(10)<<"Ztp"<<i+1<<"="<<Ztp[i]<<endl;
iofile<<"Xtp"<<i+1<<"="<<Xtp[i]<<setw(10)<<"Ytp"<<i+1<<"="<<Ytp[i]<<setw(10)<<"Ztp"<<i+1<<"="<<Ztp[i]<<endl;
   }





}