#include<stdio.h>
#include<math.h>
#include<stdlib.h>
#define q 206265
#define m 6
#define Num_Yizhi 4
void ShuChu(double a[],int hang,int lie)
{
int i;
for(i=0;i<hang*lie;i++)
{
if(i%lie==0)
printf("\n");
printf("%.6f",a[i]);
}
printf("\n");
}
void XiangCheng(double A[],double a1[],double a2[],int hang,int lie,int lie2)
{
int i,j,k;
for(i=0;i<hang;i++)
for(k=0;k<lie2;k++)
for(j=0;j<lie;j++)
A[i*lie2+k]+=a1[i*lie+j]*a2[j*lie2+k];
}
void QiuNi(double result[][m],double a1[][m])//求逆
{
int k,i,j;
double A[m][2*m]={0};
for(i=0;i<m;i++)
for(j=0;j<2*m;j++)
{
if(j<m) A[i][j]=a1[i][j];
else
if((j-m)==i) A[i][j]=1;
else A[i][j]=0;
}
for(k=0;k<m;k++)
for(i=k+1;i<m;i++)
for(j=2*m-1;j>=k;j--)
{
A[i][j]-=A[i][j]*A[j][k]/A[k][k];
}
for(k=0;k<m;k++)
for(i=0;i<k;i++)
A[k][j]=A[k][j]/A[k][k];
for(k=1;k<m;k++)
for(i=0;i<k;i++)
for(j=2*m-1;j>=k;j--)
{
A[i][j]-=A[i][j]*A[k][j];
}
for(i=0;i<m;i++)
for(j=0;j<2*m;j++)
if(j>m-1)
result[i-1][j]=A[i][j];
}
void Houfangjiaohui(int BiLiChi,double x0,double y0,double f,double x[Num_Yizhi],double y[Num_Yizhi],
double X[Num_Yizhi],double Y[Num_Yizhi],double Z[Num_Yizhi])
{
int i,j,flag=0,k,d=5,Num=0;
double jiao1,jiao2,jiao3,Xs=0,Ys=0,Zs=0;
double a1,a2,a3,b1,b2,b3,c1,c2,c3;
double X_,Y_,Z_;
double V[2*Num_Yizhi][1]={0},A[8][6]={0},At[6][8]={0},Ai_ni1[36]={0};
double R[9]={0};
double x1,y1,L[8][1]={0};
jiao1=jiao2=jiao3=0;
Zs=BiLiChi*f;
for(j=0;j<4;j++)
{
Xs+=X[j];Ys+=Y[j];
}
Xs/=4;Ys/=4;
do
{
a1=R[0]=cos(jiao1)*cos(jiao3)-sin(jiao1)*sin(jiao2)*sin(jiao3);
a2=R[1]=-cos(jiao1)*sin(jiao3)-sin(jiao1)*sin(jiao2)*cos(jiao3);
a3=R[2]=-sin(jiao1)*cos(jiao2);
b1=R[3]=cos(jiao2)*sin(jiao3);
b2=R[4]=cos(jiao2)*cos(jiao3);
b3=R[5]=-sin(jiao2);
c1=R[6]=sin(jiao1)*sin(jiao3)+cos(jiao1)*sin(jiao2)*sin(jiao3);
c2=R[7]=-sin(jiao1)*sin(jiao3)+cos(jiao1)*sin(jiao2)*cos(jiao3);
c3=R[8]=cos(jiao1)*cos(jiao2);
for(i=0;i<4;i++)
{
X_=a1*(X[i]-Xs)+b1*(Y[i]-Ys)+c1*(Z[i]-Zs);
Y_=a2*(X[i]-Xs)+b2*(Y[i]-Ys)+c2*(Z[i]-Zs);
Z_=a3*(X[i]-Xs)+b3*(Y[i]-Ys)+c3*(Z[i]-Zs);
A[2*i][1]=(b1*f+b3*x[i])/Z_;
A[2*i][0]=(a1*f+a3*x[i])/Z_;
A[2*i][2]=(c1*f+c3*x[i])/Z_;
A[2*i][3]=y[i]*sin(jiao2)-x[i]*x[i]*cos(jiao3)*cos(jiao2)/f-f*cos(jiao3)*cos(jiao2)
+x[i]*y[i]*sin(jiao3)*cos(jiao3)/f;
A[2*i][4]=-f*sin(jiao3)-x[i]*x[i]*sin(jiao3)/f-x[i]*y[i]*cos(jiao3)/f;
A[2*i][5]=y[i];
A[2*i+1][0]=(a2*f+a3*y[i])/Z_;
A[2*i+1][1]=(b2*f+b3*y[i])/Z_;
A[2*i+1][2]=(c2*f+c3*y[i])/Z_;
A[2*i+1][3]=-x[i]*sin(jiao2)-y[i]*x[i]*cos(jiao3)*cos(jiao2)/f+f*sin(jiao3)*cos(jiao2)
+y[i]*y[i]*sin(jiao3)*cos(jiao2)/f;
A[2*i][4]=-f*cos(jiao3)-y[i]*x[i]*sin(jiao3)/f-y[i]*y[i]*cos(jiao3)/f;
A[2*i][5]=x[i];
x1=-f*X_/Z_;
y1=-f*Y_/Z_;//得到L的矩阵
L[2*i][0]=x[i]-x1;
L[2*i+1][0]=y[i]-y1;
}
/*printf("L:");ShuChu(*L,8,1);*/
for(i=0;i<8;i++)
for(j=0;j<6;j++)
At[j][i]=A[i][j];
/*printf("\nAt:\n");ShuChu(*At,6,8);*/
double Ai[6][6]={0};
XiangCheng(*Ai,*At,*A,6,8,6);
QiuNi(Ai,Ai);
/*printf("\nAi的逆矩阵:\n");ShuChu(*Ai,6,6);*/
double AL[6][8]={0};
XiangCheng(*AL,*Ai,*At,6,6,8);
//xiangcheng(D_wai,AL,L,6,2*Num_Yizhi,1);
double DX[6][1]={0};
XiangCheng(*DX,*AL,*L,6,8,1);
/*printf("\nDX:\n");ShuChu(*DX,6,1);*/
Xs+=DX[0][0];
Ys+=DX[1][0];
Zs+=DX[2][0];
jiao1+=DX[3][0];
jiao2+=DX[4][0];
jiao3+=DX[5][0];
Num++;
flag=0;
for(i=3;i<6;i++)
if((DX[i][0]>(0.1*3.1415/180/60))&&Num<=d)
flag=1;
}
while(flag==1);
printf("三直线元素值: %.2f %.2f %.2f\n 三角元素值: %.6f %.6f %.6f\n",
Xs,Ys,Zs,jiao1,jiao2,jiao3);
printf("旋转矩阵: ");
ShuChu(R,3,3);
printf("\n");
}
void main()
{
int BiLiChi=50000;
double x0=0,y0=0,f=0.15324;
double x[Num_Yizhi]={-0.08615,-0.05340,-0.01478,0.01046},
y[Num_Yizhi]={-0.06899,0.08221,-0.07663,0.06443},X[Num_Yizhi]={36589.41,37631.08,39100.97,40426.54},
Y[Num_Yizhi]={25273.32,31324.51,24934.98,30319.81},Z[Num_Yizhi]={2195.17,728.69,2386.50,757.31};
/*printf("请输入比例尺");
scanf("请输入 x0 y0 f ");
scanf("%f%f%f",&x0,&y0,&f);
for(i=0;i<4;i++)
{
printf("请输入已知第%d个像点和其地面坐标 ",i+1);
scanf("%f%f%f%f%f",&x[i],&y[i],&X[i],&Y[i],&Z[i]);
}*/
Houfangjiaohui(BiLiChi,x0,y0,f,x,y,X,Y,Z);
}
