//后方交會(huì)程序
#include "stdio.h"
#include "math.h"
#include "iostream.h"
#include <stdlib.h>
#include <malloc.h>
struct WaifangweiYuansu
{
	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;
}




//后方交會(huì)程序
//在已知比例尺和主距，并輸入4個(gè)或4個(gè)以上控制點(diǎn)的像點(diǎn)和地面點(diǎn)坐標(biāo)后求出航片的外方位元素
//參數(shù)說(shuō)明：
//輸出參數(shù)：
//struct WaifangweiYuansu *p,			外方位元素的結(jié)構(gòu)體
//輸入?yún)?shù)：
//double *x,double *y,					影像像點(diǎn)x,y坐標(biāo)
//double *X,double *Y,double *Z,		像點(diǎn)對(duì)應(yīng)的地面點(diǎn)坐標(biāo)
//int N,int M,							N為輸入的控制點(diǎn)個(gè)數(shù)，M＝2×N
//double m								m為比例尺分母
//double f								f為主距

void hj(struct WaifangweiYuansu *p,double *x,double *y,double *X,double *Y,double *Z,int N,int M,double m,double f)
{
	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 S1=0.0,S2=0.0;


	 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++)
{
        S1+=X[i];
        S2+=Y[i];
}


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;

	struct WaifangweiYuansu *p;
	struct WaifangweiYuansu WfwYs = {0,0,0,0,0,0};
	p = (WaifangweiYuansu*)malloc(sizeof(WaifangweiYuansu));
	*p = WfwYs;
  	cout<<"請(qǐng)輸控制點(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<<"請(qǐng)輸入比例尺分母:m=";
	cin>>m;
	cout<<"請(qǐng)輸入焦距(mm):f=";
	cin>>f;

    for(i=0;i<N;i++)
	{
		cout<<"請(qǐng)輸入第"<<(i+1)<<"個(gè)點(diǎn)的影像坐標(biāo) x y(mm):"<<endl;
        cin>>x[i]>>y[i];  
        cout<<"請(qǐng)輸入第"<<(i+1)<<"個(gè)點(diǎn)的地面坐標(biāo) X Y Z(m):"<<endl;
        cin>>X[i]>>Y[i]>>Z[i];
	}


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

    if(x != NULL)
	  delete [] x;
    if(y != NULL)
	  delete [] y;
    if(X != NULL)
	  delete [] X;
    if(Y != NULL)
	  delete [] Y;
    if(Z != NULL)
	  delete [] Z;
    if(p != NULL)
	  delete p;
}