serfaty,theme,of,the,earth,algorithm.cpp

白塞尔大地主题解算法：提供白塞尔大地主题解算法正反算功能。

#include<iostream.h>
#include<math.h>
#include<stdio.h>
#define PI 3.1415926535897932
#define e2 0.006693421622966
#define c 6399698.9017827110
#define p11 206264.806247096355
void main()
{
int K;
double L1,B1,A12,S,L2,B2,A21;
cout<<"K=1(正算),K=2(反算),K=其他(程序中止)"<<endl<<"K=";
cin>>K;
if(K==1)
{double a1,b1,c1,t,m,n,h,v,j,k,z,w,b;
double W1,Q0,A,B,C,x,y;
 cout<<"度、分、秒制的B1=";
 cin>>a1>>b1>>c1;
 if(a1>=0) B1=PI*(a1+b1/60+c1/3600)/180;
 if(a1<0)  B1=-PI*(-a1+b1/60+c1/3600)/180;
 cout<<"弧度制的B1="<<B1<<endl;
 cout<<"度、分、秒制的L1=";
 cin>>a1>>b1>>c1;
 if(a1>=0) L1=PI*(a1+b1/60+c1/3600)/180;
 if(a1<0)  L1=-PI*(-a1+b1/60+c1/3600)/180;
 cout<<"弧度制的L1="<<L1<<endl;
 cout<<"度、分、秒制的A12=";
 cin>>a1>>b1>>c1;
 if(a1>=0) A12=PI*(a1+b1/60+c1/3600)/180;
 if(a1<0)  A12=-PI*(-a1+b1/60+c1/3600)/180;
 cout<<"弧度制的A12="<<A12<<endl;
 cout<<"单位m制S=";
 cin>>S;
 //计算起点的归化纬度
 W1=sqrt(1-e2*sin(B1)*sin(B1));                             
 printf("W1=%10.9f\n",W1);
 t=sin(B1)*sqrt(1-e2)/W1;                                   
 printf("sin(u1)=%10.9f\n",t);
 m=cos(B1)/W1;
 //计算辅助函数值
  n=m*sin(A12);                                             
  printf("sinA0=%10.9f\n",n);
  h=1-n*n;
  w=t/(m*cos(A12));                                         
  printf("cotQ1=%10.9f\n",1/w);
  v=2*(1/w)/(1/(w*w)+1);                                    
  printf("sin(2Q1)==%10.9f\n",v);
  j=(1/(w*w)-1)/(1/(w*w)+1);                                
  printf("cos(2Q1)=%10.9f\n",j);
 //计算系数A,B,C,x,y
                                                           
A=6356863.020+(10708.949-13.474*h)*h;                       
printf("A=%10.9f\n",A); 
B=(5354.469-8.978*h)*h;                                     
printf("B=%10.9f\n",B); 
C=(2.238*h)*h+0.006;                                        
printf("C=%10.9f\n",C); 
x=(691.46768-(0.58143-0.00144*h)*h);                      
printf("x=%10.9f\n",x);  
y=(0.2907-0.0010*h)*h;                                      
printf("y=%10.9f\n",y); 
//计算球面长度
double Q,lanm,q,q1;
Q0=(S-(B+C*j)*v)/A;
k=v*cos(2*Q0)+j*sin(2*Q0);                                  
printf("sin2(Q1+r0)=%10.9f\n",k); 
z=j*cos(2*Q0)-v*sin(2*Q0);                                  
printf("cos2(Q1+r0)=%10.9f\n",z); 
Q=Q0+(B+5*C*z)*k/A;                                         
printf("Q=%10.9f\n",Q);  
//计算经纬度改正数
q=(x*Q+y*(k-v))*n; q1=q/p11;                                         
printf("q=%10.9f\n",q);  
//计算终点大地坐标及大地方位角
b=t*cos(Q)+m*cos(A12)*sin(Q);
B2=atan(b/(sqrt(1-e2)*sqrt(1-b*b)));
lanm=atan(sin(A12)*sin(Q)/(m*cos(Q)-t*sin(Q)*cos(A12)));
if(sin(A12)>=0&&tan(lanm)>=0)
lanm=fabs(lanm);
else if(sin(A12)>=0&&tan(lanm)<0)
lanm=PI-fabs(lanm);
else if(sin(A12)<0&&tan(lanm)<0)
lanm=-fabs(lanm);
else if(sin(A12)<0&&tan(lanm)>=0)
lanm=fabs(lanm)-PI;                                         
printf("lanm=%10.9f\n",lanm);
L2=L1+lanm-q1;
A21=atan(m*sin(A12)/(m*cos(Q)*cos(A12)-t*sin(Q)));
if(sin(A12)<0&&tan(A21)>=0)
A21=fabs(A21);
else if(sin(A12)<0&&tan(A21)<0)
A21=PI-fabs(A21);
else if(sin(A12)>=0&&tan(A21)>=0)
A21=PI+fabs(A21);
else if(sin(A12)>=0&&tan(A21)<0)
A21=2*PI-fabs(A21);
if(lanm>=0) {a1=floor(p11*lanm/3600);b1=floor((p11*lanm/3600-a1)*60);c1=((p11*lanm/3600-a1)*60-b1)*60;}
if(lanm<0)  {a1=ceil(p11*lanm/3600);b1=floor((a1-p11*lanm/3600)*60);c1=((a1-p11*lanm/3600)*60-b1)*60;}

cout<<"lanm="<<a1<<"度"<<b1<<"分"<<c1<<"秒"<<endl;
if(B2>=0) {a1=floor(p11*B2/3600);b1=floor((p11*B2/3600-a1)*60);c1=((p11*B2/3600-a1)*60-b1)*60;}
if(B2<0)  {a1=ceil(p11*B2/3600);b1=floor((a1-p11*B2/3600)*60);c1=((a1-p11*B2/3600)*60-b1)*60;}
cout<<"B2="<<a1<<"度"<<b1<<"分"<<c1<<"秒"<<endl;
if(L2>=0) {a1=floor(p11*L2/3600);b1=floor((p11*L2/3600-a1)*60);c1=((p11*L2/3600-a1)*60-b1)*60;}
if(L2<0)  {a1=ceil(p11*L2/3600);b1=floor((a1-p11*L2/3600)*60);c1=((a1-p11*L2/3600)*60-b1)*60;}
cout<<"L2="<<a1<<"度"<<b1<<"分"<<c1<<"秒"<<endl;
if(A21>=0) {a1=floor(p11*A21/3600);b1=floor((p11*A21/3600-a1)*60);c1=((p11*A21/3600-a1)*60-b1)*60;}
if(A21<0)  {a1=ceil(p11*A21/3600);b1=floor((a1-p11*A21/3600)*60);c1=((a1-p11*A21/3600)*60-b1)*60;}
cout<<"A21="<<a1<<"度"<<b1<<"分"<<c1<<"秒"<<endl;
}






if(K==2)
{double   a1,b1,c1,l;

 cout<<"度、分、秒制的B1=";
 cin>>a1>>b1>>c1;
 if(a1>=0) B1=PI*(a1+b1/60+c1/3600)/180;
 if(a1<0)  B1=-PI*(-a1+b1/60+c1/3600)/180;
 cout<<"弧度制的B1="<<B1<<endl;

 /*cout<<"度、分、秒制的L1=";
 cin>>a1>>b1>>c1;
 if(a1>=0) L1=PI*(a1+b1/60+c1/3600)/180;
 if(a1<0)  L1=-PI*(-a1+b1/60+c1/3600)/180;
 cout<<"弧度制的L1="<<L1<<endl;*/

 cout<<"度、分、秒制的B2=";
 cin>>a1>>b1>>c1;
 if(a1>=0) B2=PI*(a1+b1/60+c1/3600)/180;
 if(a1<0)  B2=-PI*(-a1+b1/60+c1/3600)/180;
 cout<<"弧度制的B2="<<B2<<endl;

 cout<<"度、分、秒制的l=";
 cin>>a1>>b1>>c1;
 if(a1>=0) l=PI*(a1+b1/60+c1/3600)/180;
 if(a1<0)  l=-PI*(-a1+b1/60+c1/3600)/180;
 cout<<"弧度制的l="<<l<<endl;

 /*cout<<"度、分、秒制的L2=";
 cin>>a1>>b1>>c1;
 if(a1>=0)  L2=PI*(a1+b1/60+c1/3600)/180;
 if(a1<0)   L2=-PI*(-a1+b1/60+c1/3600)/180;
 cout<<"弧度制的L2="<<L2<<endl;*/
 //(1)辅助计算
double W1,W2,su1,su2,cu1,cu2,a2,b2,p,q,q0,q1,lanm,sq1,cq1,sA0,x0,y0,x,lanm1,q00;
W1=sqrt(1-e2*sin(B1)*sin(B1));                       
printf("W1=%10.9f\n",W1);
W2=sqrt(1-e2*sin(B2)*sin(B2));                       
printf("W2=%10.9f\n",W2);
su1=sin(B1)*sqrt(1-e2)/W1;                           
printf("sin(u1)=%10.9f\n",su1);
su2=sin(B2)*sqrt(1-e2)/W2;                           
printf("sin(u2)=%10.9f\n",su2);
cu1=cos(B1)/W1;                                      
printf("cos(u1)=%10.9f\n",cu1);
cu2=cos(B2)/W2;                                      
printf("cos(u2)=%10.9f\n",cu2);
a1=su1*su2;a2=cu1*cu2;b1=cu1*su2;b2=su1*cu2;         
printf("a1=%10.9f\n",a1); printf("a2=%10.9f\n",a2);  
printf("b1=%10.9f\n",b1); printf("b2=%10.9f\n",b2);
//(2)用逐次趋近法
q0=0;lanm=l+q0;
for(int i=0;;i++)
{p=cu2*sin(lanm);q=b1-b2*cos(lanm);A12=atan(p/q);    
printf("p=%10.9f\n",p);printf("q=%10.9f\n",q);
if(p>=0&&q>=0)     A12=fabs(A12);
else if(p>=0&&q<0) A12=PI-fabs(A12);
else if(p<0&&q<0)  A12=PI+fabs(A12);
else if(p<0&&q>=0) A12=2*PI-fabs(A12);               
printf("A12=%10.9f\n",A12);
sq1=p*sin(A12)+q*cos(A12);cq1=a1+a2*cos(lanm);q1=atan(sq1/cq1);
if(cq1>=0)     q1=fabs(q1);
else if(cq1<0) q1=PI-fabs(q1);                       
printf("q1=%10.9f\n",q1);
sA0=cu1*sin(A12);x=2*a1-(1-sA0*sA0)*cq1;             
printf("x=%10.9f\n",x);
x0=(33523299-(28189-70*(1-sA0*sA0))*sA0*sA0)*(1.0E-10);
printf("x0=%10.9f\n",x0);
y0=(28189-94*(1-sA0*sA0))*(1.0E-10);                 
printf("y0=%10.9f\n",y0);
q00=(x0*q1-y0*x*sq1)*sA0;                            
printf("q0=%10.9f\n\n",q0);
lanm1=l+q00;
if(fabs(q00*p11-q0*p11)<0.000000001) break;
q0=q00;lanm=lanm1;                                   
}
//（3）计算大地线长度
double A,B11,C11,y;
y=((1-sA0*sA0)*(1-sA0*sA0)-2*x*x)*cq1;
A=6356863.020+(10708.949-13.474*(1-sA0*sA0))*(1-sA0*sA0);
B11=10708.938-17.956*(1-sA0*sA0);
C11=4.487;
S=A*q1+(B11*x+C11*y)*sq1;                                       cout<<"S="<<S<<"米"<<endl;
//(4)计算反方位角
A21=atan(cu1*sin(lanm1)/(b1*cos(lanm1)-b2));
if(p>=0&&q>=0)     A21=fabs(A21);
else if(p>=0&&q<0) A21=PI-fabs(A21);
else if(p<0&&q<0)  A21=PI+fabs(A21);
else if(p<0&&q>=0) A21=2*PI-fabs(A21);
if(A21>=0) {a1=floor(p11*A21/3600);
b1=floor((p11*A21/3600-a1)*60);
c1=((p11*A21/3600-a1)*60-b1)*60;}
if(A21<0)  {a1=ceil(p11*A21/3600);
b1=floor((a1-p11*A21/3600)*60);
c1=((a1-p11*A21/3600)*60-b1)*60;}
cout<<"A21="<<a1<<"度"<<b1<<"分"<<c1<<"秒"<<endl;
}
}