后方交会程序.docVIP

//后方交会程序#include stdio.h#include math.h#include iostream.h#define N 4#define M 2*N void transpose(double *m1,double *m2,int m,int n) //矩阵转置{ int i,j;for(i=0;im;i++)???? for(j=0;jn;j++)m2[j*m+i]=m1[i*n+j];return;} void inv(double *a,int n)/*正定矩阵求逆*/{ int i,j,k;??? for(k=0;kn;k++){??????? for(i=0;in;i++)?? {??????????? if(i!=k)??????????? *(a+i*n+k)=-*(a+i*n+k)/(*(a+k*n+k));?? }??????? *(a+k*n+k)=1/(*(a+k*n+k));??????? for(i=0;in;i++)?? {??????????? if(i!=k)??? {??????????????? for(j=0;jn;j++)???? {??????????????????? if(j!=k)??????????????????? *(a+i*n+j)+=*(a+k*n+j)* *(a+i*n+k);???? }??? }?? }??????? for(j=0;jn;j++)?? {??????????? if(j!=k)??????????? *(a+k*n+j)*=*(a+k*n+k);?? }}}void mult(double *m1,double *m2,double *result,int i_1,int j_12,int j_2)//矩阵相乘 { int i,j,k; for(i=0;ii_1;i++)??????? for(j=0;jj_2;j++)?? {??????????? result[i*j_2+j]=0.0;??????????? for(k=0;kj_12;k++)??????????? result[i*j_2+j]+=m1[i*j_12+k]*m2[j+k*j_2];?? }??? return;} void main(){ int i,m;??? double t,w,k,Xs0,Ys0,Zs0,f,S1=0.0,S2=0.0;??? double x[N]={-86.15,-53.40,-14.78,10.46},y[N]={-68.99,82.21,-76.63,64.43};double X[N]={36589.41,37631.08,39100.97,40426.54},Y[N]={25273.32,31324.51,24934.98,30319.81},Z[N]={2195.17,728.69,2386.50,757.31};??? double H[6]={1},a[3],b[3],c[3],Xo[N],Yo[N],Zo[N],A[6*M],B[6*M],l[M],C[36],D[6];// double x[N],y[N],X[N],Y[N],Z[N];// cout请输入比例尺分母:m=;// cinm;// cout请输入焦距(mm):f=;// cinf;?? /* for(i=0;iN;i++){??????? cout请输入第(i+1)个点的影像坐标 x y(mm):endl;??????? cinx[i]y[i];????????? cout请输入第(i+1)个点的地面坐标 X Y Z(m):endl;??????? cinX[i]Y[i]Z[i];??????? S1+=X[i];??????? S2+=Y[i];}*/m=50000;f=153.24; for(i=0;iN;i++){?? x[i]=x[i]/1000.0;?? y[i]=y[i]/1000.0;} t=w=k=0.0;??? Xs0=S1/N;??? Ys0=S2/N;f=f/1000.0; ??? Zs0=m*f;//-----------------循环 while(fabs(H[0])0.00001||fabs(H[1])0.00001||fabs(H[2])0.00001||fabs(H[3])0.00001||fabs(H[4])0.00001||fabs(H[5])0.00001){a[0]=cos(t)*cos(k)-sin(t)*sin(w)*sin(k);