tranf.c

GPS应用程序设计.程序源代码.GPS应用程序设计.程序源代码

tranf.c

double zenith1(xj,yj,zj,xi,yi,zi)
double xj,yj,zj,xi,yi,zi;
/* 利用卫星坐标xj, yj, zj和测站坐标xi, yi, zi 计算卫星j相对于测站i的天顶距离 */
{
      double fi,lambda,h,pji,z,Ae=0.,FLAT=0.;
      pji = sqrt((xj-xi)*(xj-xi) + (yj-yi)*(yj-yi) + (zj-zi)*(zj-zi));
      TRANF(2,&fi,&lambda,&h,&xi,&yi,&zi,Ae,FLAT);
      z = ((xj-xi)*cos(fi)*cos(lambda) + (yj-yi)*cos(fi)*sin(lambda)
                   + (zj-zi)*sin(fi))/pji;
      z = acos(z);
      return(z);
}

void TRANF(ISWITCH,GLAT,ELON,HT,X,Y,Z,AE1,FLAT)
double *GLAT,*ELON,*HT,*X,*Y,*Z,AE1,FLAT;
int ISWITCH;
{
      static double Flat=298.2572221e0,Ae=6378137.e0;
      double TOL=1.e-14;
      int MAX=10,ITER;
      double FLATFN,FUNSQ,SPHI,G1,G2,RSQ,R,E,RHO,GLATR,CPHI,DR,DZ,DHT,DLATR;
      if(AE1<6000000.) {AE1=Ae; FLAT=1.e0/Flat;}
      if(FLAT<=0.0001) {AE1=Ae; FLAT=1.e0/Flat;}
      FLATFN=(2.0e0-FLAT)*FLAT;
      FUNSQ=(1.0e0-FLAT)*(1.0e0-FLAT);
      if(ISWITCH==1) {
      SPHI=sin(*GLAT);
      G1=AE1/sqrt(1.e0-FLATFN*SPHI*SPHI);
      G2=G1*FUNSQ+*HT;
      G1=G1+*HT;
      *X=G1*cos(*GLAT);
      *Y=*X*sin(*ELON);
      *X=*X*cos(*ELON);
      *Z=G2*SPHI;
      } else { 
      RSQ=*X*(*X)+*Y*(*Y);
      R=sqrt(RSQ);
      E=atan2(*Y,*X);
      if(E<0.0)E=E+2.*acos(-1.0);
      *ELON=E;
      RHO=sqrt(*Z*(*Z)+RSQ);
      SPHI=*Z/RHO;
      GLATR=asin(SPHI);
      *HT=RHO-AE1*(1.0e0-FLAT*SPHI*SPHI);
      ITER=0;
      I1100:;
      SPHI=sin(GLATR);
      CPHI=cos(GLATR);
      G1=AE1/sqrt(1.e0-FLATFN*SPHI*SPHI);
      G2=G1*FUNSQ+*HT;
      G1=G1+*HT;
      DR=R-G1*CPHI;
      DZ=*Z-G2*SPHI;
      DHT=DR*CPHI+DZ*SPHI;
      *HT=*HT+DHT;
      DLATR=(DZ*CPHI-DR*SPHI)/(AE1+*HT);
      GLATR=GLATR+DLATR;
      ITER=ITER+1;
      if(ITER>MAX) goto I1200;
      if(fabs(DLATR)>TOL) goto I1100;
      if(fabs(DHT)/(AE1+*HT)>TOL) goto I1100;
      I1200:;
      *GLAT=GLATR;
      }
}

void rotation(fp,fi,lambda,X,Q,icon)
/* 地心坐标和站心坐标之间的转换 */
FILE *fp;
double fi,lambda,X[3],Q[6];
int icon;
{
     int i,j,k,k1;
     double R[3][3],w[6],v[3],cl,sl,cfi,sfi,f1,sum;
/* 矩阵R旋转的初始化 */
     cl = cos(lambda); sl = sin(lambda);
     cfi = cos(fi);    sfi = sin(fi);
     R[0][0] = sfi*cl; R[0][1] = sfi*sl; R[0][2] = -cfi;
     R[1][0] = -sl;    R[1][1] = cl;     R[1][2] = 0.;
     R[2][0] = cfi*cl; R[2][1] = cfi*sl; R[2][2] = sfi;
/* 向量旋转 */
     for (i=0;i<3;i++) { v[i] = 0.;
        for (j=0;j<3;j++) {
           if(icon==1) v[i] += R[i][j]*X[j];       
           else v[i] += R[j][i]*X[j];   
        }
     }
     for (i=0;i<3;i++) X[i] = v[i];
/* 矩阵旋转 */
     for (i=0;i<3;i++) {
     for (j=0;j<=i;j++) {
        sum=0.;
        for (k=0;k<3;k++) {
        for (k1=0;k1<3;k1++) {
           if (k>=k1) { f1 = Q[k1+k*(k+1)/2];}
           else { f1 = Q[k+k1*(k1+1)/2];}  
           if (icon==1) sum += R[i][k]*R[j][k1]*f1;
           else sum += R[k][i]*R[k1][j]*f1;
        }}
        w[i*(i+1)/2+j] = sum;
     }}
     for (i=0;i<6;i++) Q[i] = w[i];
/* 为控制的输出 */
}