qj.c

基于光束法的严密结算方法

#include "math.h"
#include "string.h"
#include "stdio.h"

#define PI 3.141592653589793
#define N 6 /*待求加密点的像点个数*/

    
double DMS2RAD(double dmsAngle)
{
	int degAngle,minAngle;
	double radAngle,secAngle;
	degAngle=(int)dmsAngle;
	minAngle=(int)((dmsAngle-degAngle)*100);
	secAngle=(dmsAngle-degAngle-minAngle/100.0)*10000.0;
    radAngle=(degAngle+minAngle/60.0+secAngle/3600.0)*PI/180.0;
    return radAngle;
}

void main()
{
	FILE *fp1,*fp2,*fp3,*fp4,*fp5;
	/*fp1存放左右像片外方位线元素，fp2存放外方位角元素，
	fp3存放左像片像点坐标，fp4存放右像片像点坐标，fp5存放计算结果*/
	
	int i,j;
	double Xs1,Ys1,Zs1;
	double Xs2,Ys2,Zs2;
	float fi1,omeka1,capa1;
	float fi2,omeka2,capa2;
	float f1[3],f2[3],Lxy[N][4],Rxy[N][4];
    double u1[N],v1[N],w1[N];
	double u2[N],v2[N],w2[N];
	double Bu,Bv,Bw;
	double N1[N],N2[N];
	double U1[N],V1[N],W1[N];
	double U2[N],V2[N],W2[N];
	double X[N],Y[N],Z[N];
	double XYZ[2][3];
	float angle[2][3];
	double r1[3][3],r2[3][3];

	fp1=fopen("xyzdat.txt","r");
	fp2=fopen("angledat.txt","r");
	fp3=fopen("LXd_xy.txt","r");
	fp4=fopen("RXd_xy.txt","r");
	fp5=fopen("result.txt","w");

	for(i=0;i<2;i++)
		for(j=0;j<3;j++)
			fscanf(fp1,"%lf",&XYZ[i][j]);
	Xs1=XYZ[0][0];
	Ys1=XYZ[0][1];
    Zs1=XYZ[0][2];
	Xs2=XYZ[1][0];
	Ys2=XYZ[1][1];
	Zs2=XYZ[1][2];

	Bu=Xs2-Xs1;
	Bv=Ys2-Ys1;
	Bw=Zs2-Zs1;

    for(i=0;i<2;i++)
		for(j=0;j<3;j++)
			fscanf(fp2,"%f",&angle[i][j]);

	fi1=angle[0][0];
	omeka1=angle[0][1];
	capa1=angle[0][2];
	fi2=angle[1][0];
	omeka2=angle[1][1];
	capa2=angle[1][2];

	f1[0]=(float)DMS2RAD(fi1);
	f1[1]=(float)DMS2RAD(omeka1);
	f1[2]=(float)DMS2RAD(capa1);
	f2[0]=(float)DMS2RAD(fi2);
	f2[1]=(float)DMS2RAD(omeka2);
	f2[2]=(float)DMS2RAD(capa2);
     
    r1[0][0]=(cos(f1[0])*cos(f1[2])-sin(f1[0])*sin(f1[1])*sin(f1[2]));
	r1[0][1]=(-cos(f1[0])*sin(f1[2])-sin(f1[0])*sin(f1[1])*cos(f1[2]));
	r1[0][2]=(-sin(f1[0])*cos(f1[1]));
	r1[1][0]=(cos(f1[1])*sin(f1[2]));
	r1[1][1]=(cos(f1[1])*cos(f1[2]));
	r1[1][2]=(-sin(f1[1]));
	r1[2][0]=(sin(f1[0])*cos(f1[2])+cos(f1[0])*sin(f1[1])*sin(f1[2]));
	r1[2][1]=(-sin(f1[0])*sin(f1[2])+cos(f1[0])*sin(f1[1])*cos(f1[2]));
	r1[2][2]=(cos(f1[0])*cos(f1[1]));

    r2[0][0]=(cos(f2[0])*cos(f2[2])-sin(f2[0])*sin(f2[1])*sin(f2[2]));
	r2[0][1]=(-cos(f2[0])*sin(f2[2])-sin(f2[0])*sin(f2[1])*cos(f2[2]));
	r2[0][2]=(-sin(f2[0])*cos(f2[1]));
	r2[1][0]=(cos(f2[1])*sin(f2[2]));
	r2[1][1]=(cos(f2[1])*cos(f2[2]));
	r2[1][2]=(-sin(f2[1]));
	r2[2][0]=(sin(f2[0])*cos(f2[2])+cos(f2[0])*sin(f2[1])*sin(f2[2]));
	r2[2][1]=(-sin(f2[0])*sin(f2[2])+cos(f2[0])*sin(f2[1])*cos(f2[2]));
	r2[2][2]=(cos(f2[0])*cos(f2[1]));
    
    for(i=0;i<N;i++)
	  for(j=0;j<4;j++)   fscanf(fp3,"%f",&Lxy[i][j]);

    for(i=0;i<N;i++)
	  for(j=0;j<4;j++)   fscanf(fp4,"%f",&Rxy[i][j]);
    
    fprintf(fp5,"待求加密点的左片像点坐标如下表:");
    fprintf(fp5,"\n\n DH         Lx            Ly            Lz");
    for(i=0;i<N;i++)
	     {
	      fprintf(fp5,"\n");
	      fprintf(fp5,"%4.1f    %8.3f       %8.3f       %8.1f  ",Lxy[i][0],Lxy[i][1],Lxy[i][2],Lxy[i][3]); 
	     }

    fprintf(fp5,"\n\n待求加密点的右片像点坐标如下表:");
    fprintf(fp5,"\n\n DH          Rx            Ry            Rz");
    for(i=0;i<N;i++)
	     {
	      fprintf(fp5,"\n");
	      fprintf(fp5,"%4.1f    %8.3lf        %8.3lf      %8.1lf  ",Rxy[i][0],Rxy[i][1],Rxy[i][2],Rxy[i][3]); 
	     }


    for(i=0;i<N;i++)
	{
		u1[i]=r1[0][0]*Lxy[i][1]+r1[0][1]*Lxy[i][2]+r1[0][2]*Lxy[i][3];
        u2[i]=r2[0][0]*Rxy[i][1]+r2[0][1]*Rxy[i][2]+r2[0][2]*Rxy[i][3];
		v1[i]=r1[1][0]*Lxy[i][1]+r1[1][1]*Lxy[i][2]+r1[1][2]*Lxy[i][3];
		w1[i]=r1[2][0]*Lxy[i][1]+r1[2][1]*Lxy[i][2]+r1[2][2]*Lxy[i][3];
		v2[i]=r2[1][0]*Rxy[i][1]+r2[1][1]*Rxy[i][2]+r2[1][2]*Rxy[i][3];
		w2[i]=r2[2][0]*Rxy[i][1]+r2[2][1]*Rxy[i][2]+r2[2][2]*Rxy[i][3];
	}

    for(i=0;i<N;i++)
	{
		N1[i]=(Bu*w2[i]-Bw*u2[i])/(u1[i]*w2[i]-u2[i]*w1[i]);
		N2[i]=(Bu*w1[i]-Bw*u1[i])/(u1[i]*w2[i]-u2[i]*w1[i]);
	}

	for(i=0;i<N;i++)
	{
		U1[i]=N1[i]*u1[i];
		V1[i]=N1[i]*v1[i];
		W1[i]=N1[i]*w1[i];
		U2[i]=N2[i]*u2[i];
		V2[i]=N2[i]*v2[i];
		W2[i]=N2[i]*w2[i];
	}
	

	for(i=0;i<N;i++)
	{
		X[i]=Xs1+U1[i];
		Y[i]=((Ys1+V1[i])+(Ys2+V2[i]))/2;
		Z[i]=Zs1+W1[i];
	}
  
	fprintf(fp5,"\n\n空间前方交会计算地面坐标如下所示:");
	fprintf(fp5,"\n\n DH           X               Y               Z");
	   for(i=0;i<N;i++)
	     {
	      fprintf(fp5,"\n");/*格式化写函数*/
	      fprintf(fp5,"%4.1f    %8.3lf        %8.3lf      %8.3lf  ",Rxy[i][0],X[i],Y[i],Z[i]); 
	      
	     }

}