/*****************************************************************
PROGRAM NAME:
hj***---A MAIN PROGRAM FOR AERIAL PHOTOGRAPH SPACE RESECTION
VERS----A REVERSE SUBPROGRAM
FILE NAME:
HJ***---PROGRAM FILE OF THE MAIN PROGRAM AND SUBPROGRAM
DAT1***---DATA FILE OF THE ORIENTATION POINT IMAGE COORDINATE
DAT2***---DATA FILE OF THE TERRAIN COORDINATE
DAT3***---DATA FILE OF THE COMPUTING RESULT
******************************************************************/
#include "math.h"
#include "string.h"
#include "stdio.h"
float hj(float);
void vers(float b[][6],int);
/*----THIS IS FUNCTION SUBPROGRAM----*/
float hj(float t)
{
int e,e1;
float p;
double pi;
pi=3.1415926535;
p=t*180./pi;
e=(int)p;
p=(p-e)*60.;
e1=(int)p;
p=(p-e1)*60.;
t=e+e1/100.+p/10000.;
return t;
}
/*----THIS IS A REVERSE SUBPROGRAM----*/
void vers(float b[][6],int n)
{
int i,j,k,i1;
float dm;
float f[6][6];
for(i=0;i<n;i++)
for(j=0;j<n;j++)
if(i==j) f[i][j]=1.0;
else f[i][j]=0;
k=0;
loop1:dm=b[k][k];
for(i=0;i<n;i++)
{ b[k][i]=b[k][i]/dm;
f[k][i]=f[k][i]/dm;
}
i1=-1;
loop2:i1++;
if(i1<n)
{ dm=b[i1][k];
for(j=0;j<n;j++)
if(i1==k) goto loop2;
else
{ b[i1][j]=b[i1][j]-dm*b[k][j];
f[i1][j]=f[i1][j]-dm*f[k][j];
}
goto loop2;
}
k++;
if(k<n) goto loop1;
for(i=0;i<n;i++)
for(j=0;j<n;j++) b[i][j]=f[i][j];
return;
}
/*----THIS IS MAIN PROGRAM----*/
main()
{
FILE *fp1,*fp2,*fp3;
int i,j,k,n,nn,ip,pn;
float ff,fi,omeka,capa,z;
float f[3],x[3],bl[6],df[6],v[8],p[8],xo[4],yo[4],xy[4][3],r[3][3],aa[6][6],a[8][6];
double xs,ys,zs,XY[4][4];
fp1=fopen("c:\\dat1.c","r");
fp2=fopen("c:\\dat2.c","r");
fp3=fopen("c:\\dat3.c","w");
pn=1;
n=4;
ff=150.00;
xs=103000.00;
ys=140000.00;
zs=4800.00;
for(i=0;i<3;i++) f[i]=0.0;
fprintf (fp3," ---------NOW COMPUTING LEFT PHOTO--------- ");
goto loop;
loop1: fprintf(fp3,"\n\n\n ---------NOW COMPUTING RIGHT PHOTO--------- ");
xs=106000.00;
ys=140000.00;
zs=4800.00;
for(i=0;i<3;i++) f[i]=0.0;
loop: fprintf(fp3,"\n ORIENTION POINT NUMBER n= %d",n);
fprintf(fp3,"\n PRINCIPAL DISTANCE f= %3.2f",ff);
fprintf(fp3,"\n ELEMENTS INITIAL VALUE :");
fprintf(fp3,"\n xso=%6.3lf yso=%6.3lf zso=%6.3lf ao=%1.2f bo=%1.2f co=%1.2f",xs,ys,zs,f[0],f[1],f[2]);
for(i=0;i<4;i++)
for(j=0;j<3;j++) fscanf(fp1,"%f",&xy[i][j]);
for(i=0;i<4;i++)
for(j=0;j<4;j++) fscanf(fp2,"%lf",&XY[i][j]);
ip=0;
nn=2*n;
for(i=0;i<nn;i++) p[i]=1.0;
loop2: for(i=0;i<6;i++)
{
for(j=0;j<6;j++) aa[i][j]=0.0;
bl[i]=0.0;
}
ip=ip+1;
/*---------------------------------------------------------------------
COMPUTING NINE DIRECTION COSINE
-----------------------------------------------------------------------*/
r[0][0]=cos(f[0])*cos(f[2])-sin(f[0])*sin(f[1])*sin(f[2]);
r[0][1]=-cos(f[0])*sin(f[2])-sin(f[0])*sin(f[1])*cos(f[2]);
r[0][2]=-sin(f[0])*cos(f[1]);
r[1][0]=cos(f[1])*sin(f[2]);
r[1][1]=cos(f[1])*cos(f[2]);
r[1][2]=-sin(f[1]);
r[2][0]=sin(f[0])*cos(f[2])+cos(f[0])*sin(f[1])*sin(f[2]);
r[2][1]=-sin(f[0])*sin(f[2])+cos(f[0])*sin(f[1])*cos(f[2]);
r[2][2]=cos(f[0])*cos(f[1]);
if(ip<2)
{
fprintf(fp3,"\n\n DH x y X Y Z");
for(i=0;i<n;i++)
{
fprintf(fp3,"\n");
fprintf(fp3,"%4.1f %8.3f %8.3f ",xy[i][0],xy[i][1],xy[i][2]);
for(j=1;j<4;j++) fprintf(fp3,"%9.2f ",XY[i][j]);
}
}
for(i=0;i<n;i++)
{
for(j=0;j<3;j++) x[j]=r[0][j]*(XY[i][1]-xs)+r[1][j]*(XY[i][2]-ys)+r[2][j]*(XY[i][3]-zs);
/*----------------------------------------------------
COMPUTING CONSTANT TERM OF ERROR EQUATION
------------------------------------------------------*/
xo[i]=-ff*x[0]/x[2];
yo[i]=-ff*x[1]/x[2];
v[i]=xy[i][1]-xo[i];
v[i+n]=xy[i][2]-yo[i];
z=XY[i][3]-zs;
/*------------------------------------------------------
COMPUTING COEFFICIENTS OF ERROR EQUATION
--------------------------------------------------------*/
a[i][0]=-ff-xy[i][1]*xy[i][1]/ff;
a[i][1]=-xy[i][1]*xy[i][2]/ff;
a[i][2]=xy[i][2];
a[i][3]=ff/z;
a[i][4]=0.0;
a[i][5]=xy[i][1]/z;
a[i+n][0]=a[i][1];
a[i+n][1]=-ff-xy[i][2]*xy[i][2]/ff;
a[i+n][2]=-xy[i][1];
a[i+n][3]=0.0;
a[i+n][4]=a[i][3];
a[i+n][5]=xy[i][2]/z;
/*---------------------------------------------------------------------
COMPUTING COEFFICIENTS AND CONSTANT TERM OF NORMAL EQUATION
-----------------------------------------------------------------------*/
for(j=0;j<6;j++)
{
for(k=0;k<6;k++) aa[j][k]=aa[j][k]+a[i][j]*p[i]*a[i][k]+a[i+n][j]*p[i]*a[i+n][k];
bl[j]=bl[j]+a[i][j]*p[i]*v[i]+a[i+n][j]*p[i]*v[i+n];
}
}
/*---------------------------------------------------------------------
TURN SUBPROGRAM VERS COMPUTING REVERSE
-----------------------------------------------------------------------*/
vers(aa,6);
/*---------------------------------------------------------------------
COMPUTING CORRECTION OF ORIENTION ELEMENTS
-----------------------------------------------------------------------*/
for(i=0;i<6;i++)
{ df[i]=0;
for(j=0;j<6;j++) df[i]=df[i]+aa[i][j]*bl[j];
}
f[0]=f[0]+df[0];
f[1]=f[1]+df[1];
f[2]=f[2]+df[2];
xs=xs+df[3];
ys=ys+df[4];
zs=zs+df[5];
fprintf(fp3,"\n\n CORRECTION OF ORIENTION ELEMENTS:");
fprintf(fp3,"\n %f %f %f %f %f %f",df[0],df[1],df[2],df[3],df[4],df[5]);
if(ip>4) goto loop3;
for(i=0;i<3;i++)
if(fabs(df[i])>3e-5) goto loop2;
loop3: fi=hj(f[0]);
omeka=hj(f[1]);
capa=hj(f[2]);
fprintf(fp3,"\n\n CIRCLE NUMBER ip= %d",ip);
fprintf(fp3,"\n\n ORIENTION ELEMENT:");
fprintf(fp3,"\n fi= %f omeka= %f capa= %f",fi,omeka,capa);
fprintf(fp3,"\n xs= %lf ys= %lf zs= %lf",xs,ys,zs);
if(pn==2) goto loop4;
pn=pn+1;
goto loop1;
loop4: fclose(fp3);
fclose(fp2);
fclose(fp1);
return(0);
}