#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;i<m;i++)
     for(j=0;j<n;j++)
m2[j*m+i]=m1[i*n+j];
return;
}

void inv(double *a,int n)/*正定矩陣求逆*/
{ 
int i,j,k;
    for(k=0;k<n;k++)
{
        for(i=0;i<n;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;i<n;i++)
   {
            if(i!=k)
    {
                for(j=0;j<n;j++)
     {
                    if(j!=k)
                    *(a+i*n+j)+=*(a+k*n+j)* *(a+i*n+k);
     }
    }
   }
        for(j=0;j<n;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;i<i_1;i++)
        for(j=0;j<j_2;j++)
   {
            result[i*j_2+j]=0.0;
            for(k=0;k<j_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=";
 cin>>m;
 cout<<"請輸入焦距(mm):f=";
 cin>>f;
  for(i=0;i<N;i++)
{
        cout<<"請輸入第"<<(i+1)<<"個點的影像坐標 x y(mm):"<<endl;
        cin>>x[i]>>y[i];  
        cout<<"請輸入第"<<(i+1)<<"個點的地面坐標 X Y Z(m):"<<endl;
        cin>>X[i]>>Y[i]>>Z[i];
        S1+=X[i];
        S2+=Y[i];
}
//m=50000;f=153.24;


for(i=0;i<N;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);
    a[1]=-cos(t)*sin(k)-sin(t)*sin(w)*cos(k);
    a[2]=-sin(t)*cos(w);
    b[0]=cos(w)*sin(k);
    b[1]=cos(w)*cos(k);
    b[2]=-sin(w);
    c[0]=sin(t)*cos(k)+cos(t)*sin(w)*sin(k);
    c[1]=-sin(t)*sin(k)+cos(t)*sin(w)*cos(k);
    c[2]=cos(t)*cos(w);

    for(i=0;i<N;i++)
{
        Xo[i]=-f*(a[0]*(X[i]-Xs0)+b[0]*(Y[i]-Ys0)+c[0]*(Z[i]-Zs0))/(a[2]*(X[i]-Xs0)+b[2]*(Y[i]-Ys0)+c[2]*(Z[i]-Zs0));
        Yo[i]=-f*(a[1]*(X[i]-Xs0)+b[1]*(Y[i]-Ys0)+c[1]*(Z[i]-Zs0))/(a[2]*(X[i]-Xs0)+b[2]*(Y[i]-Ys0)+c[2]*(Z[i]-Zs0));
        Zo[i]=a[2]*(X[i]-Xs0)+b[2]*(Y[i]-Ys0)+c[2]*(Z[i]-Zs0);

        A[12*i+0]=(a[0]*f+a[2]*x[i])/Zo[i];
        A[12*i+1]=(b[0]*f+b[2]*x[i])/Zo[i];
        A[12*i+2]=(c[0]*f+c[2]*x[i])/Zo[i];
        A[12*i+3]=y[i]*sin(w)-(x[i]*(x[i]*cos(k)-y[i]*sin(k))/f+f*cos(k))*cos(w);
        A[12*i+4]=-f*sin(k)-x[i]*(x[i]*sin(k)+y[i]*cos(k))/f;
        A[12*i+5]=y[i];
        A[12*i+6]=(a[1]*f+a[2]*y[i])/Zo[i];
        A[12*i+7]=(b[1]*f+b[2]*y[i])/Zo[i];
        A[12*i+8]=(c[1]*f+c[2]*y[i])/Zo[i];
        A[12*i+9]=-x[i]*sin(w)-(y[i]*(x[i]*cos(k)-y[i]*sin(k))/f-f*sin(k))*cos(w);
        A[12*i+10]=-f*cos(k)-y[i]*(x[i]*sin(k)+y[i]*cos(k))/f;
        A[12*i+11]=-x[i];

        l[2*i]=x[i]-Xo[i];
        l[2*i+1]=y[i]-Yo[i];
}

    transpose(A,B,8,6);
    mult(B,A,C,6,8,6);
    mult(B,l,D,6,8,1);
    inv(C,6);
    mult(C,D,H,6,6,1);

    Xs0+=H[0];
    Ys0+=H[1];
    Zs0+=H[2];
    t+=H[3];
    w+=H[4];
    k+=H[5];
}


//----------------------------------------
   cout<<"像主點的空間坐標為："<<endl;   
   cout<<"Xs="<<Xs0<<endl;
   cout<<"Ys="<<Ys0<<endl;
   cout<<"Zs="<<Zs0<<endl;
   cout<<"t="<<t<<endl;
   cout<<"w="<<w<<endl;
   cout<<"k="<<k<<endl;

}