C语言空间后方交会源代码.doc

#includestdio.h #includemath.h #define n 4 //控制点个数 #define PI 3 struct coordinate { double x; //像点坐标 double y; double Xt; //控制点坐标 double Yt; double Zt; }; // void inverse(double c[6][6]) //矩阵求逆 // { // int i,j,h,k; // double p; // double q[6][12]; // for(i=0;i6;i++)//构造高斯矩阵 // for(j=0;j6;j++) // q[i][j]=c[i][j]; // for(i=0;i6;i++) // for(j=6;j12;j++) // { // if(i+6==j) // q[i][j]=1; // else // q[i][j]=0; // } // for(h=k=0;kn-1;k++,h++)//消去对角线以下的数据 // for(i=k+1;i6;i++) // { // if(q[i][h]==0) // continue; // p=q[k][h]/q[i][h]; // // p=q[i][h]/q[k][h]; // for(j=0;j12;j++) // { // q[i][j]*=p; // q[i][j]-=q[k][j]; // } // } // for(h=k=5;k0;k--,h--) // 消去对角线以上的数据 // for(i=k-1;i=0;i--) // { // if(q[i][h]==0) // continue; // p=q[k][h]/q[i][h]; // // p=q[i][h]/q[k][h]; // for(j=11;j0;j--) // { // q[i][j]*=p; // q[i][j]-=q[k][j]; // } // } // for(i=0;i6;i++)//将对角线上数据化为1 // { // p=1.0/q[i][i]; // for(j=0;j12;j++) // q[i][j]*=p; // } // for(i=0;i6;i++) //提取逆矩阵 // for(j=0;jn;j++) // { // c[i][j]=q[i][j+6]; // } // } void ContraryMatrix(double *const pMatrix, double *const _pMatrix, const int dim) { int ii,jj,kk; int flag=0; double *tMatrix = new double[2*dim*dim]; for (int i=0; idim; i++){ for (int j=0; jdim; j++) tMatrix[i*dim*2+j] = pMatrix[i*dim+j]; } for (i=0; idim; i++){ for (int j=dim; jdim*2; j++) tMatrix[i*dim*2+j] = 0.0; tMatrix[i*dim*2+dim+i] = 1.0; } //Initialization over! for (i=0; idim; i++)//Process Cols { double base = tMatrix[i*dim*2+i]; if (fabs(base) 1E-300) { for (ii=i;iidim;ii++) { if(tMatrix[ii*dim*2+i]!=0) { flag=1; for (jj=0;jj2*dim;jj++) { tMatrix[i*dim*2+jj]+=tMatrix[ii*dim*2+jj]; } } }