Ƭռ󷽽ᣨӰ
(2008-10-26 10:18:10) 


#include "stdafx.h"

#include "iostream.h"
#include "stdio.h"
#include "stdlib.h"
#include "math.h"
#define N 4
#define PI 3.141592653589793

////////////////////////////////////////////////  AAinv
double *inv(double *A,double *Ainv, int n) 
{
int *is,*js,i,j,k,l,u,v;
double d,p;
for(i=0;i<n*n;i++)
*(Ainv+i)=*(A+i);
is=(int*)malloc(n*sizeof(int));
js=(int*)malloc(n*sizeof(int));
for(k=0;k<=n-1;k++)
{
d=0.0;
for(i=k;i<=n-1;i++)
for(j=k;j<=n-1;j++)
{
l=i*n+j; p=(double)fabs(Ainv[l]);
if(p>d)
{d=p;is[k]=i;js[k]=j;}
}
if(d+1.0==1.0)
{
free(is);free(js);cout<<""<<endl;
return NULL;
}
if(is[k]!=k)
for(j=0;j<=n-1;j++)
{
u=k*n+j;v=is[k]*n+j;
p=Ainv[u];Ainv[u]=Ainv[v];Ainv[v]=p;
}
if(js[k]!=k)
for(i=0;i<=n-1;i++)
{
u=i*n+k;v=js[k]+i*n;
p=Ainv[u];Ainv[u]=Ainv[v];Ainv[v]=p;

}
l=k*n+k;
Ainv[l]=(double)1.0/Ainv[l];

for(j=0;j<=n-1;j++)
if(j!=k)
{
u=k*n+j;Ainv[u]=Ainv[u]*Ainv[l];
}
for(i=0;i<=n-1;i++)
if(i!=k)
for(j=0;j<=n-1;j++)
if(j!=k)
{
u=i*n+j;
Ainv[u]=Ainv[u]-Ainv[i*n+k]*Ainv[k*n+j];
}
for(i=0;i<=n-1;i++)
if(i!=k)
{u=i*n+k;Ainv[u]=-Ainv[u]*Ainv[l];}
}
for(k=n-1;k>=0;k--)
{
//ָ
if(js[k]!=k)
for(j=0;j<=n-1;j++)
{
u=k*n+j;v=js[k]*n+j;
p=Ainv[u];Ainv[u]=Ainv[v];Ainv[v]=p;
}
//ָ
if(is[k]!=k)
for(i=0;i<=n-1;i++)
{
u=i*n+k;v=i*n+is[k];
p=Ainv[u];Ainv[u]=Ainv[v];Ainv[v]=p;
}
}
free(is);
free(js);
return Ainv;
}

/////////////////////////////////////////////ˣABΪCΪm,nΪк
void mulAB(double *A,double *B,double *C,int am,int an,int bm,int bn)
{
int i,j,l,u;
if(an!=bm)
{
printf("err**!Can\'t do the metrix multiplication.\n");
return;
}
for(i=0;i<am;i++)
for(j=0;j<bn;j++)
{
u=i*bn+j;
C[u]=0.0;
for(l=0;l<an;l++)
C[u]=C[u]+A[i*an+l]*B[l*bn+j];
}
return;
}

///////////////////////////////////////////////////ת
void Turn(double *A,double A2[],int m,int n) 
{

for(int i=0;i<m;i++)
{
for(int j=0;j<n;j++)
{
A2[j*m+i]=A[i*n+j];
}
}
}
/////////////////////////////////////////////// 
void PrintMatrix(double M[],int m,int n)
{
int i,j;
// printf("The Matrix is:\n");
for(i=0;i<m;i++)
{
for(j=0;j<n;j++)
printf("%f\t",M[i*n+j]);
printf("\n");
}
printf("\n\n");
}


void main()
{

int m,i;
double f,t,w,k,S1,S2,Xs0,Ys0,Zs0,G[N],a[3],b[3],c[3],xo[N],yo[N],l[2*N];
double A[2*N*6],AT[6*2*N],ATA[6*6],ATA_[6*6],ATl[6],V[6]={1,1,1,1,1,1};
m=30000, f=150.0; 
f=150.0/1000.0; ///λm
S1=0.0;
S2=0.0;



// double x[N]={ -98.104,-99.644,86.952,88.595 },y[N]={-94.902, 65.907, -93.238, 60.477};
// double X[N]={103135.00, 103012.00, 109009.00, 108998.00},
// Y[N]={137018.00, 142008.00, 136988.00, 141989.00 },
// Z[N]={76.00, 30.00 ,7.00 ,3.00 };

double x[N]={ -91.007 , -93.708 , 94.194 ,92.390 },y[N]={ -77.759 , 66.986 , -91.534 , 77.730 };
double X[N]={ 100021.00 , 100009.00 , 105972.00 , 105865.00 }, 
Y[N]={137408.00 , 142047.00, 136984.00 , 142336.00 }, 
Z[N]={16.00, 34.00 ,42.00 ,55.00 };



for(i=0;i<N;i++)
{
x[i]/=1000.0; ////////λ
y[i]/=1000.0; ////////λ
S1+=X[i];
S2+=Y[i];
} 
/////////////////////////ⷽλԪصĳʼֵ
Xs0=S1/N;
Ys0=S2/N;
Zs0=m*f;
t=w=k=0.0;
while(fabs(V[3])>0.1/180*PI||fabs(V[4])>0.1/180*PI||fabs(V[5])>0.1/180*PI)
{
/////////////////////////תR
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);
/////////////////////////ռĽֵ(x),(y)
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));
G[i]=a[2]*(X[i]-Xs0)+b[2]*(Y[i]-Ys0)+c[2]*(Z[i]-Zs0);
}
//////////////////////////////A;l
for(i=0;i<N;i++)
{
A[i*12+0]=(a[0]*f+a[2]*x[i])/G[i];
A[i*12+1]=(b[0]*f+b[2]*x[i])/G[i];
A[i*12+2]=(c[0]*f+c[2]*x[i])/G[i];
A[i*12+3]=y[i]*sin(w)-(x[i]*(x[i]*cos(k)-y[i]*sin(k))/f+f*cos(k))*cos(w);
A[i*12+4]=-f*sin(k)-x[i]*(x[i]*sin(k)+y[i]*cos(k))/f;
A[i*12+5]=y[i];
A[i*12+6]=(a[1]*f+a[2]*y[i])/G[i];
A[i*12+7]=(b[1]*f+b[2]*y[i])/G[i];
A[i*12+8]=(c[1]*f+c[2]*y[i])/G[i];
A[i*12+9]=-x[i]*sin(w)-(y[i]*(x[i]*cos(k)-y[i]*sin(k))/f-f*sin(k))*cos(w);
A[i*12+10]=-f*cos(k)-y[i]*(x[i]*sin(k)+y[i]*cos(k))/f;
A[i*12+11]=-x[i];

l[i*2+0]=x[i]-xo[i];
l[i*2+1]=y[i]-yo[i];
}
cout<<"A"<<endl;
PrintMatrix(A,2*N,6);

cout<<"l"<<endl;
PrintMatrix(l,2*N,1);

Turn( A, AT,2*N,6);
cout<<"AT"<<endl;
PrintMatrix(AT,6,2*N);

mulAB( AT, A, ATA,6,2*N,2*N,6);
cout<<"ATA"<<endl;
PrintMatrix(ATA ,6,6);

inv( ATA, ATA_, 6) ;
cout<<"ATA_"<<endl;
PrintMatrix(ATA_,6,6);

mulAB(AT,l,ATl,6,2*N,2*N,1);
cout<<"ATl"<<endl;
PrintMatrix(ATl,6,1);

mulAB(ATA_,ATl,V,6,6,6,1);
cout<<"V"<<endl;
PrintMatrix(V,6,1);
Xs0+=V[0];
Ys0+=V[1];
Zs0+=V[2];
t+=V[3];
w+=V[4];
k+=V[5];
} 





cout<<"V"<<endl;
PrintMatrix(V,6,1);


cout<<"ΪλԪ"<<endl;
cout<<"t="<<t*180/PI<<endl;
cout<<"w="<<w*180/PI<<endl;
cout<<"k="<<k*180/PI<<endl;

printf("Xs0=%f\n",Xs0);
printf("Ys0=%f\n",Ys0);
printf("Zs0=%f\n",Zs0);

//fclose(fp);
}

 

 

 

 

 

 

 

#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;
  
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;

}
