qj0097.cpp

高斯正算坐标程序计算高斯投影的正算问题克拉索夫斯基椭球

#include "math.h"
#include "string.h"
#include "stdio.h"
#include "stdlib.h"
#define PI 3.141592653589793

double DMS2RAD(double x)
{double deg,min,sec,X;
 deg=int(x);
 min=int((x-deg)*100);
 sec=((x-deg)*100-min)*100;
 X=deg*PI/180+min*PI/180/60+sec*PI/180/3600;
 return X;
}

main()
{
	FILE *fp1,*fp2,*fp3;
	
	int i(0),j(0);
	int k,l;
	double xy[4][3],data[6][5];
	double Xs[2],Ys[2],Zs[2];
	double Phi,Kappa,Omega;
	double x[2],y[2],f;
	double a[3],b[3],c[3];
	double u[2],v[2],w[2];
	double Bu,Bv,Bw;
	double N1,N2;
	double U1,V1,W1,U2,V2,W2;
	double X,Y,Z;

	printf("----前方交会计算程序----\n");
	f=150.00;
	
	if((fp1=fopen("d:\\0097\\DATA\\Ori_element.DAT","r"))==NULL)
	{
		fprintf(stderr,"sorry fail open file!\n");
		exit(1);
	}
	for(k=0;k<4;k++)
	{
	    for(l=0;l<3;l++)
		{
			fscanf(fp1,"%lf",&xy[k][l]);
		}
	}
	
	if((fp2=fopen("d:\\0097\\DATA\\data.dat","r"))==NULL)
	{
		fprintf(stderr,"sorry fail open file!\n");
		exit(1);
	}
	for(k=0;k<6;k++)
	{
	    for(l=0;l<5;l++)
		{
			fscanf(fp2,"%lf",&data[k][l]);
		}
	}
	
	fp3=fopen("D:\\0097\\Result\\result.dat","w");
    fprintf(fp3,"  前方交会计算结果:\n");
	fprintf(fp3,"\n点号      X坐标        Y坐标       Z坐标\n");
    for(k=0;k<6;k++)
	{
        if(i>=2)
		{
			i=0,j=0;
		}
		
	    while (i<2)
		{
		    x[i]=data[k][2*i+1];
		    y[i]=data[k][2*i+2];
	        Phi=xy[j][0];Omega=xy[j][1];Kappa=xy[j][2];
		    Xs[i]=xy[j+1][0];Ys[i]=xy[j+1][1];Zs[i]=xy[j+1][2];

			Phi=DMS2RAD(Phi);
			Omega=DMS2RAD(Omega);
			Kappa=DMS2RAD(Kappa);
			
	    	//计算旋转矩阵
	        a[0]=cos(Phi)*cos(Kappa)-sin(Phi)*sin(Omega)*sin(Kappa);
	        a[1]=-cos(Phi)*sin(Kappa)-sin(Phi)*sin(Omega)*cos(Kappa);
	        a[2]=-sin(Phi)*cos(Omega);
	        b[0]=cos(Omega)*sin(Kappa);
	        b[1]=cos(Omega)*cos(Kappa);
	        b[2]=-sin(Omega);
	        c[0]=sin(Phi)*cos(Kappa)+cos(Phi)*sin(Omega)*sin(Kappa);
	        c[1]=-sin(Phi)*sin(Kappa)+cos(Phi)*sin(Omega)*cos(Kappa);
	        c[2]=cos(Phi)*cos(Omega);
			//摄影基线分量
			Bu=Xs[1]-Xs[0];
    	    Bv=Ys[1]-Ys[0];
	        Bw=Zs[1]-Zs[0];
	
	        //计算u,v,w
		    u[i]=a[0]*x[i]+a[1]*y[i]+a[2]*(-f);
	        v[i]=b[0]*x[i]+b[1]*y[i]+b[2]*(-f);
		    w[i]=c[0]*x[i]+c[1]*y[i]+c[2]*(-f);
			
			

		    if(i==1)
			{
			
                //投影系数
				N1=(Bu*w[1]-Bw*u[1])/(u[0]*w[1]-u[1]*w[0]);
    	        N2=(Bu*w[0]-Bw*u[0])/(u[0]*w[1]-u[1]*w[0]);
			    //计算地面点在向空间辅助坐标系中的坐标
	     		U1=N1*u[0];  U2=N2*u[1];
	            V1=N1*v[0];  V2=N2*v[1];
	            W1=N1*w[0];  W2=N2*w[1];
			    //计算地面点坐标
		        X=Xs[0]+U1;
	            Y=((Ys[0]+V1)+(Ys[1]+V2))/2;
	            Z=Zs[0]+W1;
			
                fprintf(fp3,"%.2lf    %.2lf    %.2lf    %.2lf\n",data[k][0],X,Y,Z);
			}
		
	    	i++;
		    j=j+2;
		}
	}	
	
	fclose(fp3);
	fclose(fp2);
	fclose(fp1);
	return 0;
}