houfangjiaohui.rar_后方交会_摄影测量

#include <iostream.h> #include <stdio.h> #include <math.h> #include <stdlib.h> #define M 50000 #define pi 3.1415926 #define XC (0.1*3.1415926)/(60*180) /************************************************************************/ /* 求共线条件方程系数矩阵 */ /************************************************************************/ void XSJZ(double f,double w,double k,double* pArray) { //double a1,a2,a3,b1,b2,b3,c1,c2,c3; double a[9]={0}; a[0]=cos(f)*cos(k)-sin(f)*sin(w)*sin(k); a[1]=(-1)*cos(f)*sin(k)-sin(f)*sin(w)*cos(k); a[2]=(-1)*sin(f)*cos(w); a[3]=cos(w)*sin(k); a[4]=cos(w)*cos(k); a[5]=-sin(w); a[6]=sin(f)*cos(k)+cos(f)*sin(w)*sin(k); a[7]=(-1)*sin(f)*sin(k)+cos(f)*sin(w)*cos(k); a[8]=cos(f)*cos(w); for (int m=0;m<9;m++) { pArray[m]=a[m]; } //pArray[]={a1,a2,a3,b1,b2,b3,c1,c2,c3}; } /************************************************************************/ /* 求法方程的系数矩阵 */ /************************************************************************/ void FFCJZ(double x,double y,double f,double h,double* A) { double a[12]={0}; f=f*(-1); a[0]=f/h; a[1]=0; a[2]=x*(-1)/h; a[3]=f*(1+x*x/(f*f)); a[4]=x*y/f; a[5]=y; a[6]=0; a[7]=f/h; a[8]=y*(-1)/h; a[9]=x*y/f; a[10]=f*(1+y*y/(f*f)); a[11]=x*(-1); for (int m=0;m<12;m++) { A[m]=a[m]; } } /************************************************************************/ /* 求矩阵的转置 */ /************************************************************************/ double* ZZJZ(double*pArray,int m,int n) { //double* pArray1=new double[n*m]; double* pArray1=new double[m*n]; for (int k=0;k<m;k++) for(int l=0;l<n;l++) { pArray1[l*m+k]=pArray[k*n+l]; } return pArray1; } /************************************************************************/ /* 矩阵相乘 */ /************************************************************************/ double* JZXC(double *pArray1,double *pArray2,int m,int p,int n) { double *pArray3=new double[m*n]; for (int x=0;x<m;x++) for(int y=0;y<n;y++) { pArray3[x*n+y]=0; } for (int k=0;k<m;k++) for(int l=0;l<n;l++) { for(int h=0;h<p;h++) { pArray3[k*n+l]+=pArray1[k*p+h]*pArray2[h*n+l]; } } return pArray3; } /************************************************************************/ /* 求 方 阵 的 行 列 式 */ /************************************************************************/ double HLS(int rnum,double *pArrayS) { double sum=1.0; double total=0.0; double *pArrayD=0; pArrayD=new double [(rnum*2-1)*rnum]; /************************************************************************/ /* 复 制 矩 阵 */ /************************************************************************/ for (int k=0;k<rnum;k++) for (int l=0;l<rnum;l++) { pArrayD[k*(rnum*2-1)+l]=pArrayS[k*rnum+l]; } /************************************************************************/ /* 在 复 制 的 矩 阵 后 添 加 前 (rnum-1) 列 */ /************************************************************************/ for (int x=0;x<rnum;x++) for(int y=0;y<rnum-1;y++) { pArrayD[x*(rnum*2-1)+rnum+y]=pArrayD[x*(rnum*2-1)+y]; } /************************************************************************/ /* 求 矩 阵 行 列 式 */ /************************************************************************/ for (int l=0;l<=(rnum*2-1);l++) { /************************************************************************/ /* 　　　　正　方　向　斜　相　乘 */ /************************************************************************/ if (l<rnum) { sum=1; for (int i=0,j=l;i<rnum,j<(l+rnum);i++,j++) { sum*=pArrayD[i*(rnum*2-1)+j]; } total+=sum; } /************************************************************************/ /* 反 方 向 斜 相 乘 */ /************************************************************************/ else { sum=1; for (int i=0,j=(l-1);i<rnum,j>=(l-rnum);i++,j--) { sum*=pArrayD[i*(rnum*2-1)+j]; } total+=(sum*(-1)); } } //delete[] pArrayD; return total; } /************************************************************************/ /* 求 矩 阵 的 代 数 余 子 式 */ /************************************************************************/ double* DSYZS(int rnum,double *pArrayS) { double *pArray1=0; pArray1=new double[rnum*rnum]; for (int m=0;m<rnum;m++) for(int n=0;n<rnum;n++) { double Result=0; double *pArray2; pArray2=new double[(rnum-1)*(rnum-1)]; for(int i=0,k=0;i<rnum;i++) for(int j=0;j<rnum;j++) if((i!=m)&&(j!=n)) { pArray2[k++]=pArrayS[i*rnum+j]; } Result=HLS((rnum-1),pArray2); delete[] pArray2; if ((m+n)%2!=0)//计算代数余子式的符号 { Result*=(-1); } pArray1[m+n*rnum]=Result; } return pArray1; } /************************************************************************/ /* 求矩阵的逆矩阵 */ /************************************************************************/ double* NJZ(int m,double* pArray) { double total=0; double* pArray1=0; double* pArray2=0; pArray1=new double[m*m]; pArray2=new double[m*m]; total=HLS(m,pArray); for(int k=0;k<m;k++) for(int l=0;l<m;l++) { pArray1[k*m+l]=0; pArray2[k*m+l]=0; } pArray1=DSYZS(m,pArray); for ( k=0;k<m;k++) for(int l=0;l<m;l++) { pArray2[k*m+l]=pArray1[k*m+l]/total; } return pArray2; } //功 能： 矩阵求逆 //输 入： 矩阵a，阶数n。 //输 出： 返回0（成功），1（失败），经求逆后，a矩阵为逆矩阵 int InverseMatrix(double *a, int n) { int *is,*js; int i,j,k,l,u,v; double temp,max_v; is=(int *)malloc(n*sizeof(int)); js=(int *)malloc(n*sizeof(int)); if(is==NULL||js==NULL) return(0); for(k=0;k<n;k++) { max_v=0.0; for(i=k;i<n;i++) { for(j=k;j<n;j++) { temp=fabs(a[i*n+j]); if(temp>max_v) { max_v=temp; is[k]=i; js[k]=j; } } } if(max_v==0.0) { free(is); free(js); return(0); } if(is[k]!=k) { for(j=0;j<n;j++) { u=k*n+j; v=is[k]*n+j; temp=a[u]; a[u]=a[v]; a[v]=temp; } } if(js[k]!=k) { for(i=0;i<n;i++) { u=i*n+k; v=i*n+js[k]; temp=a[u]; a[u]=a[v]; a[v]=temp; } } l=k*n+k; a[l]=1.0/a[l]; for(j=0;j<n;j++) { if(j!=k) { u=k*n+j; a[u]*=a[l]; } } for(i=0;i<n;i++) { if(i!=k) { for(j=0;j<n;j++) { if(j!=k) { u=i*n+j; a[u]-=a[i*n+k]*a[k*n+j]; } } } } for(i=0;i<n;i++) { if(i!=k) { u=i*n+k; a[u]*=-a[l]; } } } for(k=n-1;k>=0;k--) { if(js[k]!=k) { for(j=0;j<n;j++) { u=k*n+j; v=js[k]*n+j; temp=a[u]; a[u]=a[v]; a[v]=temp; } } if(is[k]!=k) { for(i=0;i<n;i++) { u=i*n+k; v=i*n+is[k]; temp=a[u]; a[u]=a[v]; a[v]=temp; } } } free(is); free(js); return(1); } int main() { double Xo=0.00,Yo=0.00,Zo=0.00,Xs=0.00,Ys=0.00,Zs=0.00,f=0.00; int xnum=0,ynum=0,bFlag=0; double F=0,W=0,K=0; double df=0,dw=0,dk=0; double dX=0,dY=0,dZ=0; FILE