#include "iostream.h"
#include "math.h"
#include "stdio.h"
#define pi 3.1415926535897932  //圓周率 

void xyz_xyz(double xyz[],double xyz1[],double canshu[]);
void gstyz(double blh[],double xy[],double para2[]);
void blh_xyz(double blh[],double xyz[],double para1[]);
double ziwuhu(double B,double a,double e2);
double xyz_blh(double blh[],double xyz[],double para2[]);
double huahu(double b);

main()
{     
      double blh[3],xyz[3],xyz1[3],xy[2],para1[2],para2[2],canshu[7];
      int i;
     cout<<"請輸入大地坐標B,L,H，角度輸入方式如下："<<endl;
	 cout<<"若輸入的角度為30度30分30秒，對應的代碼為：30.3030"<<endl;
     cin>>blh[0]>>blh[1]>>blh[2];
     cout<<endl;
     //將輸入的角度形式轉化為弧度 
     blh[0]=huahu(blh[0]);
     blh[1]=huahu(blh[1]);
    
    cout<<"請輸入WGS84橢球的橢球參數，長半軸以及扁率的倒數："<<endl;
    cin>>para1[0]>>para1[1];
    cout<<endl;
    cout<<"請輸入北京54橢球的橢球參數，長半軸以及扁率的倒數："<<endl;
    cin>>para2[0]>>para2[1];
     cout<<endl;
     //調用函數，實現大地坐標向空間直角坐標系中的轉換 
     blh_xyz(blh,xyz,para1);
     
     cout<<"對應的空間直角坐標系中的坐標為："<<endl;
     printf("x=%10.6f,y=%10.6f,z=%10.6f",xyz[0],xyz[1],xyz[2]);
     cout<<endl;
     
      cout<<"請分別輸入兩坐標系之間的縮放參數，旋轉參數和位移參數："<<endl;
       for(i=0;i<7;i++)
      {
        cin>>canshu[i];
      }
      cout<<endl;
      
      //將輸入的角度轉換為弧度的形式 
      canshu[1]=huahu(canshu[1]);
      canshu[2]=huahu(canshu[2]);
      canshu[3]=huahu(canshu[3]);
      
      //調用函數，實現兩個空間直角坐標系之間的轉換 
      xyz_xyz(xyz,xyz1,canshu);
    
     //調用函數，將大地坐標轉換為直角坐標 
     xyz_blh(blh,xyz,para2);
     //調用函數，計算對應的高斯投影面上的坐標 
	 gstyz(blh,xy,para2);
      //輸出結果 
      printf("對應的高斯投影坐標系中的坐標為：\n");
      printf("x=%10.6f,y=%10.6f",xy[0],xy[1]);

      
}


//大地坐標轉換為直角坐標 
void blh_xyz(double blh[],double xyz[],double para1[])
{   double b,e2,N;
    

    
    b=para1[0]*(1-1/para1[1]);
	e2=1-b*b/(para1[0]*para1[0]);
    
	N=para1[0]/sqrt(1-e2*sin(blh[0])*sin(blh[0]));
	
	xyz[0]=(N+blh[2])*cos(blh[0])*cos(blh[1]);
	xyz[1]=(N+blh[2])*cos(blh[0])*sin(blh[1]);
	xyz[2]=(N*(1-e2)+blh[2])*sin(blh[0]);
}
//計算子午弧長函數 
double ziwuhu(double B,double a,double e2)
{     
      double X,m0,m2,m4,m6,m8,q0, q2,q4,q6,q8;
      
      m0=a*(1-e2);
      m2=3*e2*m0/2;
      m4=5*e2*m2/4;
      m6=7*e2*m4/6;
      m8=9*e2*m6/8;
      
      q0=m0+m2/2+3*m4/8+5*m6/16+35*m8/128;
      q2=m2/2+m4/2+15*m6/32+7*m8/16;
      q4=m4/8+3*m6/16+7*m8/32;
      q6=m6/32+m8/16;
      q8=m8/128;
      
      X=q0*B-q2*sin(2*B)/2+q4*sin(4*B)/4-q6*sin(6*B)/6+q8*sin(8*B)/8;
      return X;
}
//坐標轉換的直接解法求解經緯度 
double xyz_bl(double blh[],double xyz[],double para2[])
{   
    double a,b,e2,p1,r,A1,A2,A3,A4; 
   	a=para2[0];   
    b=a*(1-1/para2[1]);
	e2=1-b*b/(a*a);
    p1=atan(xyz[2]/sqrt(xyz[0]*xyz[0]+xyz[1]*xyz[1]));
	r=a*sqrt(1-e2)/((1-e2*sin(p1)*sin(p1))*(1-e2*sin(p1)*sin(p1)));
    r=sqrt(xyz[0]*xyz[0]+xyz[1]*xyz[1]+xyz[2]*xyz[2]);
	A1=para2[0]*tan(p1)/r;
    A2=sin(p1)*sin(p1)+2*(a/r)*cos(p1)*cos(p1);
    A3=3*sin(p1)*sin(p1)*sin(p1)*sin(p1)+16*((a/r)*sin(p1)*sin(p1)*cos(p1)*cos(p1)+4*(a/r)*(a/r)*cos(p1)*cos(p1)*(2-5*sin(p1)*sin(p1)));
	A4=5*sin(p1)*sin(p1)*sin(p1)*sin(p1)*sin(p1)*sin(p1)+48*((a/r)*sin(p1)*sin(p1)*sin(p1)*sin(p1)*cos(p1)*cos(p1)+20*(a/r)*(a/r)*sin(p1)*sin(p1)*cos(p1)*cos(p1)*(4-7*sin(p1)*sin(p1))+16*(a/r)*(a/r)*(a/r)*cos(p1)*cos(p1)*(1-7*sin(p1)*sin(p1)+8*sin(p1)*sin(p1)*sin(p1)*sin(p1)));
	
	blh[0]=atan(tan(p1)+A1*e2*(1+e2/2*(A2+e2*e2/4*(A3+A4/2))));

	blh[1]=atan(xyz[1]/xyz[0]);
	if(blh[1]<0)
    	blh[1]+=pi;
       
}
//使用實用電算公式計算進行高斯投影正算
void gstyz(double blh[],double xy[],double para2[])
{     
      double B,L,l,b,e2,t,g2,N,x1,x2,x3,y1,y2,y3;
      int i;
      
      
      B=blh[0];
      L=blh[1]*180/pi;
      //計算高斯投影帶的帶號 
      for(i=0;fabs(3*i-L)>1.5;i++);
      cout<<"高斯投影帶的帶號為："<<i<<endl;
      //計算經差，并用弧度的形式表示       
      l=(L-3*i)*pi/180;
      
      b=para2[0]*(1-1/para2[1]);
	  e2=1-b*b/(para2[0]*para2[0]);

      t=tan(B);
      g2=e2*cos(B)*cos(B)/(1-e2);
      N=para2[0]/sqrt(1-e2*sin(B)*sin(B));
      
      x1=N*t*cos(B)*cos(B)*l*l/2;
      x2=N*t*cos(B)*cos(B)*cos(B)*cos(B)*l*l*l*l*(5-t*t+9*g2+4*g2*g2)/24;
      x3=N*t*cos(B)*cos(B)*cos(B)*cos(B)*cos(B)*cos(B)*l*l*l*l*l*l*(61-58*t*t+t*t*t*t)/720;

        
      y1=N*cos(B)*l;
      y2=N*cos(B)*cos(B)*cos(B)*l*l*l*(1-t*t+g2)/6;
      y3=N*cos(B)*cos(B)*cos(B)*cos(B)*cos(B)*l*l*l*l*l*(5-18*t*t+t*t*t*t+14*g2-58*g2*t*t)/120;

      xy[0]=ziwuhu(B,para2[0],e2)+x1+x2+x3;
      xy[1]=y1+y2+y3;
     
}
//將輸入的角度化為弧度的函數 
double huahu(double b)
{
	double b1,b2,b3;
	b1=(int)b;
	b2=(int)((b-b1)*100);
	b3=((b-b1)*100-b2)*100;
	b=(b1+b2/60+b3/3600)*pi/180;
	return b;
}
//直角坐標系轉換函數 
void xyz_xyz(double xyz[],double xyz1[],double canshu[])
{    
     double k,ox,oy,oz,oX,oY,oZ,a1,a2,a3,b1,b2,b3,c1,c2,c3;
     k=1+canshu[0];
     ox=canshu[1];
     oy=canshu[2];
     oz=canshu[3];
     oX=canshu[4];
     oY=canshu[5];
     oZ=canshu[6];
     //計算旋轉矩陣 
     a1=cos(oz)*cos(oy);
     a2=cos(ox)*sin(oz)+sin(ox)*sin(oy)*cos(oz);
     a3=sin(ox)*sin(oz)-cos(ox)*sin(oy)*cos(oz);
     b1=-cos(oy)*sin(oz);
     b2=cos(ox)*cos(oz)-sin(ox)*sin(oy)*sin(oz);
     b3=sin(ox)*cos(oz)+cos(ox)*sin(oy)*sin(oz);
     c1=sin(oy);
     c2=-sin(ox)*cos(oy);
     c3=cos(ox)*cos(oy);
     //計算在轉換后直角坐標系中的坐標 
     xyz1[0]=k*(a1*xyz[0]+a2*xyz[1]+a3*xyz[2])+oX;
     xyz1[1]=k*(b1*xyz[0]+b2*xyz[1]+b3*xyz[2])+oY;
     xyz1[2]=k*(c1*xyz[0]+c2*xyz[1]+c3*xyz[2])+oZ;
}