//后方交會程序
#include <stdio.h>
#include "math.h"
#include "iostream.h"

struct WaifangweiYuansu
{
	//double t,w,k,Xs0,Ys0,Zs0,
	double t;
	double w;
	double k;
	double Xs0;
	double Ys0;
	double Zs0;
};


void transpose(double *m1,double *m2,int m,int n) //矩陣轉(zhuǎ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 hj(struct WaifangweiYuansu *p,int N,int M,double S1,double S2,double m,double f,double *x,double *y,double *X,double *Y,double *Z)
{
	 int i;
	 double H[6]={1},a[3],b[3],c[3];
	 double C[36],D[6];
	 double t,w,k,Xs0,Ys0,Zs0;

	 double * Xo = NULL;
	 Xo = new double [N+10];
	 double * Yo = NULL;
	 Yo = new double [N+10];
	 double * Zo = NULL;
	 Zo = new double [N+10];
	 double * A = NULL;
	 A = new double [6*M+10];
	 double * B = NULL;
	 B = new double [6*M];
	 double * l = NULL;
	 l = new double [M];


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;
//-----------------循環(huán) 
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];
}

    p->k = k;
	p->t = t;
	p->w = w;
	p->Xs0 = Xs0;
	p->Ys0 = Ys0;
	p->Zs0 = Zs0;

	 delete [] Xo;
	 delete [] Yo;
	 delete [] Zo;
	 delete [] A;
	 delete [] B;
	 delete [] l;

	return;
}

void main()
{ 
	 int N,M;
	 int i,m;
	 double f;
	 double S1=0.0,S2=0.0;
	 struct WaifangweiYuansu *p;
	 struct WaifangweiYuansu WfwYs = {0,0,0,0,0,0};
	 p = (WaifangweiYuansu*)malloc(sizeof(WaifangweiYuansu));
	 *p = WfwYs;
  	 cout<<"請輸控制點(diǎn)個(gè)數(shù):N=";
	 cin>>N;
	 M = 2*N;
	 double * x = NULL;
	 x = new double [N+10];
	 double * y = NULL;
	 y = new double [N+10];
	 double * X = NULL;
	 X = new double [N+10];
	 double * Y = NULL;
	 Y = new double [N+10];
	 double * Z = NULL;
	 Z = new double [N+10];

	 cout<<"請輸入比例尺分母:m=";
	 cin>>m;
	 cout<<"請輸入焦距(mm):f=";
	 cin>>f;
   for(i=0;i<N;i++)
{
        cout<<"請輸入第"<<(i+1)<<"個(gè)點(diǎn)的影像坐標(biāo) x y(mm):"<<endl;
        cin>>x[i]>>y[i];  
        cout<<"請輸入第"<<(i+1)<<"個(gè)點(diǎn)的地面坐標(biāo) X Y Z(m):"<<endl;
        cin>>X[i]>>Y[i]>>Z[i];
        S1+=X[i];
        S2+=Y[i];
}


   hj(p,N,M,S1,S2,m,f,&x[N],&y[N],&X[N],&Y[N],&Z[N]);
//----------------------------------------
   cout<<"像主點(diǎn)的空間坐標(biāo)為："<<endl;   
   cout<<"Xs="<<WfwYs.Xs0<<endl;
   cout<<"Ys="<<WfwYs.Ys0<<endl;
   cout<<"Zs="<<WfwYs.Zs0<<endl;
   cout<<"t="<<WfwYs.t<<endl;
   cout<<"w="<<WfwYs.w<<endl;
   cout<<"k="<<WfwYs.k<<endl;

	 delete [] x;
	 delete [] y;
	 delete [] X;
	 delete [] Y;
	 delete [] Z;
}