C ****************************************************************** C ****************************************************************** C ***** ***** C ***** PROGRAM DLT WITH DATA SNOOPING ***** C ***** ------------------------------ ***** C ***** ***** C ***** PROGRAM DLT WITH DATA SNOOPING WAS WRITTEN BY ***** C ***** LIANG-CHIEN CHEN AT THE UNIVERSITY OF ILLINOIS IN ***** C ***** 1985 AND HAS BEEN ADAPTED AT THE UNIVERSITY OF NEW ***** C ***** BRUNSWICK BY S.J. JODOIN. NUMEROUS MINOR ADDITIONS ***** C ***** AND ALTERATIONS TO THE ORIGINAL PROGRAM ARE INCLUDED ***** C ***** IN THIS VERSION, HOWEVER THESE MOSTLY CONSIST OF ***** C ***** FORMAT CHANGES AND DIMENSION ALTERATIONS. THE USER ***** C ***** IS REFERRED TO THE PROGRAM AUTHOR'S PH.D DISSERT- ***** C ***** ATION FOR A COMPLETE LISTING OF THE ORIGINAL SOURCE ***** C ***** CODE. THIS VERSION OF THE PROGRAM IS ALTERED TO ***** C ***** ACCOMMODATE 4 PHOTOGRAPHS AND 350 POINTS IN OBJECT ***** C ***** SPACE. ***** C ***** ***** C ***** S.J. JODOIN ***** C ***** JUNE, 1987 ***** C ***** ***** C ***** REFERENCE ***** C ***** --------- ***** C ***** ***** C ***** CHEN, LIANG-CHIEN (1985). "A SELECTION SCHEME FOR ***** C ***** NON-METRIC CLOSE-RANGE PHOTOGRAMMETRIC SYSTEMS". ***** C ***** PH.D DISSERTATION, DEPARTMENT OF CIVIL ENGINEER- ***** C ***** ING, UNIVERSITY OF ILLINOIS AT URBANA-CHAMPAIGN ***** C ***** ***** C ***** ***** C ****************************************************************** C ****************************************************************** C C PROGRAM MODULAR IS A PART OF THE 3-PHASE ADJUSTMENT FOR ANALYTICAL C CLOSE-RANGE PHOTOGRAMMETRY. MODULAR INCLUDES TWO PARTS: C THE DLT AND GERISO. THE DIRECT LINEAR TRANSFORMATION IS THE METHOD C OF SOLVING THE COLLINEARITY CONDITION OF PHOTOGRAMMETRY IN ONE C STEP. IT CAN HANDLE REPEATED OBSERVATIONS OF COMPARATOR C COORDINATES FOR EITHER MONO OR STEREO OBSERVATIONS. AS PRESENTLY C WRITTEN, IT CAN HANDLE UP TO FOUR PHOTOGRAPHS BUT CAN BE MADE TO C TO HANDLE AS MANY PHOTOGRAPHS AS ARE USED IN THE SOLUTION BY C SIMPLY CHANGING THE DIMENSIONS OF THE VARIABLES AFFECTED BY THE C NUMBER OF PHOTOGRAPHS. C C DEPENDING ON THE EXTENT OF LENS DISTORTION CORRECTION, IT IS SET C UP TO SOLVE FOR ANY OF FOUR GROUPS OF UNKNOWNS: C C 11 - INVOLVING THE ELEVEN DLT PARAMETERS, IN WHICH LINEAR C FILM DEFORMATION AND LENS DISTORTION ARE IMPLICIT; C C 12 - INVOLVING THE DLT PARAMETERS PLUS THE COEFFICIENT C OF THE 3RD ORDER TERM OF SYMMETRICAL LENS DISTORTION; C C 14 - INVOLVING THE DLT PARAMETERS PLUS THE COEFFICIENTS C OF THE 3RD, 5TH AND 7TH ORDER TERMS OF SYMMETRICAL C LENS DISTORTION; C C 16 - INVOLVING THE 14 UNKNOWNS ABOVE PLUS THE COEFFICIENTS C OF THE FIRST TWO TERMS OF ASYMMETRICAL LENS DISTORTION. C C IF THE RESULT OF DLT IS NOT ACCURATE ENOUGH, THE SECOND PHASE C SOLUTION, GERISO, WILL BE CALLED. C C GERISO IS THE GENERAL RESECTION/INTERSECTION SOLUTION, WHICH C INCORPORATES POLYNOMIALS WITH COEFFICIENTS K1, K2, K3, P1 AND C P2 FOR SYSTEMATIC ERROR CORRECTION MODEL FOR COORDINATES OF C IMAGE POINTS. C C C C C ****************************************************************** C ****************************************************************** C ***** ***** C ***** INPUT ***** C ***** ***** C ****************************************************************** C ****************************************************************** C C THE INPUT PROCEDURES ARE STATED AS FOLLOWS: C C 1. NPROB, IDISK, IPUNCH (FORMAT: 3I5) C 2. NPHOT, NREP, NTYPE, NPAIR (FORMAT: 5I5) C 3. SX, SY, SZ (FORMAT: 8F10.3) C 4. NUM(I), (E(I,J), J=1,3) (FORMAT: I10, 5F10.3) C 5. NO, XC, YC (FORMAT: I10, 5F10.3) FOR MONO OBSERVATIONS, OR C NO, XC, YC, PX, PY FOR STEREO OBSERVATIONS C 6. IP (FORMAT: I5) C C THE PRECEEDING PROCEDURES ARE THE SAME AS THE ORIGINAL DLT, THE C MEANING OF THE VARIABLES ARE EXPLAINED IN THE PROGRAM. IF GERISO C IS CALLED, THEN THE INPUT DATA WILL INCLUDE: C C 7. EPSILON (FORMAT: F15.7), WHERE EPSILON IS THE ACCURACY C TOLERANCE FOR SPATIAL POSITION OF OBJECT POINTS C 8. (SIGDOT(I), I=4,8) (FORMAT: 5E10.2) SIGDOT(4) - SIGDOT(8) C ARE THE A-PRIORI VARIANCE FOR 5 AP'S IN GERISO. C C************************************************************************ C******* THIS IS THE STARTING POINT OF THE DLT WITH DATA SNOOPING ******* C************************************************************************ C IMPLICIT REAL*8(A-H,O-Z) REAL*8 Q(20),SD(20),A(4),R(3,3),OM(4),CR(20,20) REAL*8 AA(100,20),QQQ(100),QQ(100,20),DASN(100),V(100) REAL*8 AAA(100),AAAA(100) DIMENSION KA(4) COMMON/DATA/E(350,3),X(350,4),Y(350,4),NUM(350),NNC,NPHOT,NPTS COMMON/TRANS/D(4,20),XP(4),YP(4),W(20,20),CX(4),CY(4),DL(4), *WW(4,20,20) COMMON/NUMBER/NC,NPC(4),NP(350,4) COMMON/XTYT/XT,YT,INXT,IFILE,IDISK,IPUNCH COMMON/PP/PCX,PCY,C(4) COMMON/PHASE/PHI(4),KAPA(4),OMEGA(4),SXYZ(4,3),IDO,IMO,SO,IDP, *IMP,SPP,IDK,IMK,SK COMMON/PHASE2/SIGPHI(4),SIGKAP(4),SIGOM(4),SIGXYZ(4,3), *SIGXP(4),SIGYP(4),SIGC(4) COMMON/PHASE3/DUOO(16),DN(16,16),SP,SX,SY,SZ COMMON/XYZ/XYZUN(350,3) C REAL KAPA REAL*8 KAPA C TOL=1.E-6 INXT=1 IFILE=10 C C TOL = TEST FOR SINGULARITY OF NORMAL EQUATION MATRIX C NPROB = NUMBER OF PROBLEMS TO SOLVE C READ(5,998) NPROB,IDISK,IPUNCH 998 FORMAT(3I5) C DO 500 IJK=1,NPROB READ(5,9) NPHOT,NREP,NTYPE,NPAIR 9 FORMAT(5I5) C C NPHOT = NUMBER OF PHOTOGRAPHS USED IN THE SOLUTION C NREP = NUMBER OF REPETITIONS IN COMPARATOR COORDINATE OBSERVATIONS C NTYPE = TYPE OF COMPARATOR OBSERVATIONS, 0 = MONO, 1 = STEREO C NPAIR = NUMBER OF STEREOPAIRS OBSERVED C READ(5,10) SX,SY,SZ 10 FORMAT(3F15.10) C C SX,SY,SZ = STANDARD ERRORS OF OBJECT SPACE COORDINATES C C READ OBJECT SPACE COORDINATES OF POINTS C NC=0 DO 5 I=1,350 READ(5,15) NUM(I), (E(I,J),J=1,3) 15 FORMAT(4X,I3,F12.8,2F13.8) IF(NUM(I).EQ.0) GOTO 16 WRITE(6,969) NUM(I),E(I,1),E(I,2),E(I,3) 969 FORMAT(' ',5X,I4,5X,3F15.10) NC=NC+1 C PRINT, NC 5 CONTINUE C C NUM(I) = NUMBER OF POINT I C E(I,J) = OBJECT SPACE COORDINATES OF POINT I, C J=1,X, J=2,Y, J=3,Z. C 16 CONTINUE SVV=0.0 ICOUNT=0 DF=NREP*(NREP-1) IF(NREP.EQ.1) DF=1.0E0 IF(NTYPE.EQ.1) GOTO 19 C C READ OBSERVED COMPARATOR COORDINATES OF POINTS IN ALL C PHOTOS FOR MONO-OBSERVATIONS, COMPUTE MEAN COORDINATES C DO 20 K=1,NPHOT NPC(K)=0 C PRINT, 'INPUTING COORDS FOR PHOTO',K DO 22 I=1,400 X(I,K)=0.0 Y(I,K)=0.0 VV=0.0 DO 25 J=1,NREP READ(5,115) NO,XC,YC 115 FORMAT(19X,I4,2F8.3) IF(NO.EQ.0) GOTO 20 ICOUNT=ICOUNT+1 X(I,K)=X(I,K)+XC Y(I,K)=Y(I,K)+YC VV=VV+XC*XC+YC*YC 25 CONTINUE C NPC(K)=NPC(K)+1 C PRINT, 'HAVE READ IN PHOTO COORDS FOR PT:',NO,'TOTAL',NPC(K) NP(I,K)=NO VV=VV-(X(I,K)**2+Y(I,K)**2)/NREP SVV=SVV+ABS(VV) X(I,K)=X(I,K)/NREP Y(I,K)=Y(I,K)/NREP 22 CONTINUE 20 CONTINUE WRITE(6,1996) NPC(1) WRITE(6,1997) NPC(2) WRITE(6,1998) NPC(3) WRITE(6,1999) NPC(4) 1996 FORMAT(' ',5X,'NUMBER OF PTS IN PHOTO 1:',3X,I5) 1997 FORMAT(' ',5X,'NUMBER OF PTS IN PHOTO 2:',3X,I5) 1998 FORMAT(' ',5X,'NUMBER OF PTS IN PHOTO 3:',3X,I5) 1999 FORMAT(' ',5X,'NUMBER OF PTS IN PHOTO 4:',3X,I5) NPTS=NPC(1) GOTO 28 C C NPC(K) = NUMBER OF POINTS OBSERVED IN PHOTO K C NP(I,K) = POINT NUMBER OF POINT I IN PHOTO K C X(I,K), Y(I,K) = COMPARATOR COORDINATES OF POINT I IN PHOTO K C C READ OBSERVED COMPARATOR COORDINATES OF POINTS IN ALL C PHOTOS FOR STEREO-OBSERVATIONS, AND COMPUTE MEAN COORDINATES C 19 CONTINUE DO 21 K=1,NPAIR M=2*K L=M-1 NPC(L)=0 NPC(M)=0 DO 23 I=1,350 X(I,L)=0.0 Y(I,L)=0.0 X(I,M)=0.0 Y(I,M)=0.0 VV=0.0 DO 24 J=1,NREP READ(5,999) NO,XC,YC,PX,PY 999 FORMAT(I10,4F10.3) IF(NO.EQ.0) GOTO 21 ICOUNT=ICOUNT+2 XD=XC-PX YD=YC-PY XC=XC-1000.0E0 YC=YC-1000.0E0 X(I,L)=X(I,L)+XC Y(I,L)=Y(I,L)+YC X(I,M)=X(I,M)+XD Y(I,M)=Y(I,M)+YD VV=VV+XC*XC+YC*YC+XD*XD+YD*YD 24 CONTINUE C NPC(L)=NPC(L)+1 NPC(M)=NPC(M)+1 NP(I,L)=NO NP(I,M)=NO SUML=X(I,L)**2+Y(I,L)**2 SUMM=X(I,M)**2+Y(I,M)**2 VV=VV-(SUML+SUMM)/NREP SVV=SVV+ABS(VV) X(I,L)=X(I,L)/NREP Y(I,L)=-Y(I,L)/NREP X(I,M)=X(I,M)/NREP Y(I,M)=-Y(I,M)/NREP 23 CONTINUE 21 CONTINUE NPTS=I-1 28 CONTINUE C C COMPUTE STANDARD ERROR OF COMPARATOR COORDINATES C SP=SVV/(2.0E0*DF*ICOUNT) SP=DSQRT(SP) C C SP = STANDARD ERRORS OF COMPARATOR COORDINATES C C DETERMINE THE POINTS WHICH HAVE GIVEN OBJECT SPACE C COORDINATES AND WHICH WERE OBSERVED IN ALL THE PHOTOS, C AND ARRANGE, ACCORDING TO POINT NUMBERS, OBJECT SPACE C COORDINATES AND COMPARATOR COORDINATES IN EACH PHOTO C OF THESE POINTS IN THE SAME EXACT SEQUENCE. C CALL SORT C DO 26 K=1,NPHOT WRITE(6,98) K 98 FORMAT('1',5X,'COMPARATOR COORDINATES, PHOTO',I3,//) DO 27 I=1,NPTS WRITE(6,215) NP(I,K),X(I,K),Y(I,K) 215 FORMAT(' ',5X,I5,10X,F15.10,10X,F15.10) 27 CONTINUE 26 CONTINUE C READ(5,9) IP C C IP = NUMBER OF UNKNOWNS TO BE CARRIED IN SOLUTION C WRITE(6,101) IP 101 FORMAT('1',10X,'NUMBER OF UNKNOWNS =',I5) WRITE(6,102) SX,SY,SZ,SP 102 FORMAT(' ',///,11X,'STANDARD ERRORS OF OBJECT SPACE COORDINATES', *//,15X,'SX = ',F8.5,3X,'SY = ',F8.5,3X,'SZ = ',F8.5,//,11X, *'STANDARD ERROR OF COMPARATOR COORDINATES = ',F10.5,//) IPP1=IP+1 IPM1=IP-1 IF(IP.LT.13) GOTO 29 IPM2=IP-2 IF(IP.EQ.14) GOTO 29 IPM3=IP-3 IPM4=IP-4 29 CONTINUE C C COMPUTE DEGREES OF FREEDOM C DF=2.0E0*NNC-IP IDF=DF WRITE(6,104) IDF 104 FORMAT(' ',10X,'DEGREES OF FREEDOM = ',I5,///) C DO 30 L=1,NPHOT XP(L)=0.0 YP(L)=0.0 C C XP(L),YP(L) = COMPARATOR COORDINATES OF PRINCIPAL POINT OF PHOTO L C II=0 DO 31 LL=1,5 VV=0.0 C C VV = SUM OF RESIDUALS SQUARED C C INITIALIZE NORMAL EQUATIONS C DO 35 I=1,IP DO 35 J=1,IPP1 W(I,J)=0.0 35 CONTINUE C DO 40 K=1,NNC IF(IP.EQ.11) GOTO 49 XR=X(K,L)-XP(L) YR=Y(K,L)-YP(L) R1=XR**2 R3=YR**2 R2=R1+R3 IF(IP.EQ.12) GOTO 49 R4=R2*R2 R6=R2*R4 IF(IP.EQ.14) GOTO 49 R7=2.0E0*R1+R2 R8=2.0E0*R3+R2 R9=2.0E0*XR*YR 49 CONTINUE DO 50 III=1,2 C C INITIALIZE CONDITION EQUATION C DO 60 I=1,15 Q(I)=0.0 60 CONTINUE C AB=1.0E0 A(L)=1.0E0 IF(LL.EQ.1) GOTO 73 DO 74 I=1,3 A(L)=A(L)+D(L,I+8)*E(K,I) 74 CONTINUE C C D(L,I) = UNKNOWN NUMBER I IN PHOTO L C IF I=1,11, DLT PARAMETERS C IF I=12,16, COEFFICIENTS OF LENS DISTORTION C GOTO (61,62),III 61 B1=(D(L,9)*X(K,L)-D(L,1)) B2=(D(L,10)*X(K,L)-D(L,2)) B3=(D(L,11)*X(K,L)-D(L,3)) GOTO 63 62 B1=(D(L,9)*Y(K,L)-D(L,5)) B2=(D(L,10)*Y(K,L)-D(L,6)) B3=(D(L,11)*Y(K,L)-D(L,7)) 63 CONTINUE AB=(A(L)*SP)**2+(B1*SX)**2+(B2*SY)**2+(B3*SZ)**2 IF(LL.NE.5) GOTO 335 II=II+1 AAAA(II)=AB II=II-1 335 CONTINUE C C AB = VARIANCE ASSOCIATED WITH CONDITION EQUATION C AB=DSQRT(AB) 73 CONTINUE GOTO (71,72),III C C FORM X-CONDITION EQUATION C 71 DO 70 I=1,3 Q(I)=E(K,I) Q(I+8)=-1.0E0*E(K,I)*X(K,L) 70 CONTINUE Q(4)=1.0E0 Q(IPP1)=X(K,L) IF(IP.EQ.11) GOTO 275 IF(IP.EQ.12) GOTO 77 IF(IP.EQ.14) GOTO 81 Q(IPM4)=XR*R2*A(L) Q(IPM3)=XR*R4*A(L) Q(IPM2)=XR*R6*A(L) Q(IPM1)=R7*A(L) Q(IP)=R9*A(L) GOTO 275 81 CONTINUE Q(IPM2)=XR*R2*A(L) Q(IPM1)=XR*R4*A(L) Q(IP)=XR*R6*A(L) GOTO 275 77 Q(IP)=XR*R2*A(L) 275 IF(LL.NE.5) GOTO 75 II=II+1 AAA(II)=A(L) DO 325 I=1,IPP1 325 AA(II,I)=Q(I) GOTO 75 C C FORM Y-CONDITION EQUATION C 72 DO 80 I=1,3 Q(I+4)=E(K,I) Q(I+8)=-1.0E0*E(K,I)*Y(K,L) 80 CONTINUE Q(8)=1.0E0 Q(IPP1)=Y(K,L) IF(IP.EQ.11) GOTO 276 IF(IP.EQ.12) GOTO 78 IF(IP.EQ.14) GOTO 82 Q(IPM4)=YR*R2*A(L) Q(IPM3)=YR*R4*A(L) Q(IPM2)=YR*R6*A(L) Q(IPM1)=R9*A(L) Q(IP)=R8*A(L) GOTO 276 82 CONTINUE Q(IPM2)=YR*R2*A(L) Q(IPM1)=YR*R4*A(L) Q(IP)=YR*R6*A(L) GOTO 276 78 Q(IP)=YR*R2*A(L) 276 IF(LL.NE.5) GOTO 75 II=II+1 AAA(II)=A(L) DO 326 I=1,IPP1 326 AA(II,I)=Q(I) 75 CONTINUE C C APPLY WEIGHT TO CONDITION EQUATION C DO 76 I=1,IPP1 Q(I)=Q(I)/AB 76 CONTINUE C C COMPUTE CONTRIBUTIONS OF CONDITION EQUATION TO SUM OF C RESIDUALS SQUARED, AND OBTAIN CUMMULATIVE SUM C VV=VV+Q(IPP1)*Q(IPP1) C C FORM NORMAL EQUATIONS C DO 90 I=1,IP DO 90 J=1,IPP1 W(I,J)=W(I,J)+Q(I)*Q(J) 90 CONTINUE C DO 89 I=1,IPM1 KL=I+1 DO 89 J=KL,IP W(J,I)=W(I,J) 89 CONTINUE 50 CONTINUE 40 CONTINUE C DO 91 I=1,IP Q(I)=W(I,IPP1) 91 CONTINUE C C SOLVE NORMAL EQUATIONS C CALL SWPMAT(W,1,IP,IPP1,KERR,TOL) C C OBTAIN VALUES OF THE UNKNOWNS, COMPUTE CONTRIBUTIONS OF NORMAL C EQUATIONS TO SUM OF RESIDUALS SQUARED, AND OBTAIN TOTAL SUM. C DO 105 I=1,IP D(L,I)=W(I,IPP1) VV=VV-Q(I)*W(I,IPP1) 105 CONTINUE C C COMPUTE COORDINATES OF PRINCIPAL POINT C AND PRINCIPAL DISTANCE FOR PHOTO L C CALL XPYPC(L) C IF(LL.EQ.1) GOTO 31 C C COMPUTE AND PRINT STANDARD ERROR OF UNIT WEIGHT C VV=ABS(VV/DF) STD=DSQRT(VV) 31 CONTINUE WRITE(6,100) L,STD 100 FORMAT(//,5X,'PHOTO :',I3,//,10X,'STANDARD ERROR OF UNIT WEIGHT :' *,F10.4,/) WRITE(6,301) XP(L),YP(L),C(L),IDO,IMO,SO,IDP,IMP,SPP,IDK,IMK,SK, *(SXYZ(L,I), I=1,3) 301 FORMAT(10X,'INTERIOR ORIENTATION :',/,10X,'XP :',F11.4,5X,'YP :', *F11.4,5X,'C :',F11.4,//,10X,'ROTATION ANGLES :',/,16X,'OMEGA', *14X,'PHI',14X,'KAPPA',/,10X,3(2I5,F8.1),/,10X,'COORDINATES OF', *1X,'PHOTO STATION :',/,18X,'X',10X,'Y',10X,'Z',/,12X,3F11.4,/) DO 312 LLL=1,II DO 312 JJJ=1,IP QQ(LLL,JJJ)=0.0 DO 312 KKK=1,IP 312 QQ(LLL,JJJ)=QQ(LLL,JJJ)+AA(LLL,KKK)*W(KKK,JJJ) DO 313 I=1,II QQQ(I)=0.0 DO 313 J=1,IP 313 QQQ(I)=QQQ(I)+QQ(I,J)*AA(I,J) DO 314 I=1,II 314 QQQ(I)=STD*DSQRT(ABS(AAAA(I)-QQQ(I))) DO 316 I=1,II V(I)=0.0 DO 316 J=1,IP 316 V(I)=V(I)+AA(I,J)*D(L,J) DO 318 I=1,II 318 V(I)=V(I)-AA(I,IPP1) DO 319 I=1,II DASN(I)=ABS(V(I)/QQQ(I)) 319 CONTINUE IIII=0 III=II/2 WRITE(6,350) 350 FORMAT(//,10X,'DATA SNOOPING :',//,21X,'VX',8X,'VY',7X,'SIGVX',5X, *'SIGVY',6X,'WX',8X,'WY',/) DO 351 I=1,III IIII=IIII+1 IF(DASN(IIII).GT.3.29.OR.DASN(IIII+1).GT.3.29) WRITE(6,352) * NP(I,L),V(IIII),V(IIII+1),QQQ(IIII),QQQ(IIII+1),DASN(IIII), * DASN(IIII+1) IF(DASN(IIII).LE.3.29.OR.DASN(IIII+1).LE.3.29) WRITE(6,353) * NP(I,L),V(IIII),V(IIII+1),QQQ(IIII),QQQ(IIII+1),DASN(IIII), * DASN(IIII+1) IIII=IIII+1 352 FORMAT(10X,I5,6F10.4,3X,'----------REJECT') 353 FORMAT(10X,I5,6F10.4,3X,'----------O.K.') 351 CONTINUE C 109 FORMAT(//,'0',5X, *'COMPUTED VALUES OF UNKNOWNS AND STANDARD ERRORS :',//, *16X,'DLT PARAMETERS',5X,'STANDARD ERRORS',/) WRITE(6,3) DO 1 I=1,IP DO 2 J=1,I C CR(I,J)=W(I,J)/(DSQRT(W(I,I)*W(J,J))) 2 CONTINUE 1 CONTINUE DO 6 I=1,IP 6 WRITE(6,4) I,(CR(I,J), J=1,I) 3 FORMAT(//,5X,'THE CORRELATION COEFFICIENTS BETWEEN THE PARAMETERS *ARE :',//) 4 FORMAT(5X,I5,16(F7.3)) WRITE(6,109) C C COMPUTE VARIANCE-COVARIANCE MATRIX OF C COMPUTED VALUES OF THE UNKNOWNS C DO 110 I=1,IP DO 111 J=1,IP W(I,J)=W(I,J)*VV WW(L,I,J)=W(I,J) 111 CONTINUE SD(I)=DSQRT(ABS(W(I,I))) 110 CONTINUE C C PRINT VALUES OF THE UNKNOWNS AND THEIR C RESPECTIVE STANDARD ERRORS. C DO 120 I=1,11 WRITE(6,300) D(L,I),SD(I) 300 FORMAT(10X,2E20.8) 120 CONTINUE IF(IP.EQ.11) GOTO 30 C WRITE(6,119) 119 FORMAT('0',5X,'LENS DISTORTION COEFFICIENTS',2X, *'STANDARD ERRORS',///) DO 130 I=12,IP WRITE(6,300) D(L,I),SD(I) 130 CONTINUE C C COMPUTE EXTERIOR ORIENTATION AND OBJECT COORDINATES OF PHOTO C STATION FOR PHOTO L. CALL ELEM(L) C 30 CONTINUE C C CALL OBJECT(IPM4,IPM3,IPM2,IPM1,IP,SP) C 500 CONTINUE STOP END SUBROUTINE SORT C C ****************************************************************** C ****************************************************************** C ***** ***** C ***** SUBROUTINE SORT ***** C ***** ***** C ***** SUBROUTINE SORT WILL ARRANGE, ACCORDING TO POINT ***** C ***** NUMBERS, THE OBJECT SPACE COORDINATES, AND THE ***** C ***** COMPARATOR COORDINATES FOR EACH PHOTO, IN THE SAME ***** C ***** EXACT SEQUENCE. IT WILL DETERMINE THE POINTS WHOSE ***** C ***** OBJECT SPACE COORDINATES ARE GIVEN AND WHICH WERE ***** C ***** OBSERVED IN ALL THE PHOTOS. NNC WILL GIVE THE ***** C ***** NUMBER OF SUCH POINTS. ALL OTHER POINTS WILL BE ***** C ***** DISCARDED. ***** C ***** ***** C ***** ***** C ***** S.J. JODOIN ***** C ***** JUNE, 1987 ***** C ***** ***** C ****************************************************************** C ****************************************************************** C IMPLICIT REAL*8(A-H,O-Z) DIMENSION L(4) COMMON/DATA/E(350,3),X(350,4),Y(350,4),NUM(350),NNC,NPHOT,NPTS COMMON/NUMBER/NC,NPC(4),NP(350,4) C NNC=0 JJ=NC+1 C WRITE(6,9998) C9998 FORMAT(' ','SUBROUTINE SORT HAS BEEN CALLED') C DO 10 I=1,NC 11 CONTINUE ICOUNT=0 DO 20 K=1,NPHOT C PRINT, NPC(K) M=NPC(K) DO 30 J=1,M IF(NUM(I).NE.NP(J,K)) GOTO 30 L(K)=J ICOUNT=ICOUNT+1 GOTO 20 30 CONTINUE 20 CONTINUE C IF(ICOUNT.EQ.NPHOT) GOTO 40 JJ=JJ-1 IF(JJ.EQ.NNC) GOTO 60 NTEMP=NUM(JJ) XTEMP=E(JJ,1) YTEMP=E(JJ,2) ZTEMP=E(JJ,3) NUM(JJ)=NUM(I) E(JJ,1)=E(I,1) E(JJ,2)=E(I,2) E(JJ,3)=E(I,3) NUM(I)=NTEMP E(I,1)=XTEMP E(I,2)=YTEMP E(I,3)=ZTEMP GOTO 11 C 40 CONTINUE NNC=NNC+1 DO 50 K=1,NPHOT J=L(K) IF(J.EQ.I) GOTO 50 NTEMP=NP(J,K) XTEMP=X(J,K) YTEMP=Y(J,K) NP(J,K)=NP(I,K) X(J,K)=X(I,K) Y(J,K)=Y(I,K) NP(I,K)=NTEMP X(I,K)=XTEMP Y(I,K)=YTEMP 50 CONTINUE 10 CONTINUE 60 CONTINUE C WRITE(6,9999) C9999 FORMAT(' ','SUBROUTINE SORT HAS BEEN COMPLETED') C RETURN END SUBROUTINE SWPMAT(A,IN,N,M,KERR,TOL) C C ****************************************************************** C ****************************************************************** C ***** ***** C ***** SUBROUTINE SWPMAT ***** C ***** ***** C ***** SUBROUTINE SWPMAT IS A MATRIX INVERSION ROUTINE FOR ***** C ***** USE WITH LEAST SQUARES PROBLEMS. SWEEPS CLEAR ***** C ***** ACROSS MATRIX TO COLUMN M. BY SWEEPING ENTIRE ***** C ***** MATRIX SWPMAT DEVELOPS REGRESSION COEFFICIENTS ***** C ***** WITHOUT MATRIX MULTIPLICATION. IN IS THE SUBSCRIPT ***** C ***** OF THE FIRST PIVOTAL ELEMENT TO BE USED BY THE ***** C ***** SUBROUTINE. FOR EXAMPLE IN = 2 WILL IGNORE THE ***** C ***** FIRST ROW AND COLUMN OF THE MATRIX A. IN = 1 ***** C ***** USUALLY. BY USING M = N IN CALLING SEQUENCE, ***** C ***** SWPMAT MAY BE USED FOR INVERSION ONLY, SWEEPING ***** C ***** N COLUMNS AND ROWS. THE SWEPT PORTION OF THE ***** C ***** MATRIX REPLACES THE ORIGINAL CONTENTS OF THE A ***** C ***** MATRIX, WHICH ARE DESTROYED. TOL IS THE TEST FOR ***** C ***** SINGULARITY - SUGGEST TOL = 1.0D-06 IN CALLING ***** C ***** PROGRAM ***** C ***** ***** C ***** N = NUMBER OF INDEPENDENT VARIABLES ***** C ***** M = N + NUMBER OF DEPENDENT VARIABLES ***** C ***** KERR = ERROR SWITCH, ZERO IF MATRIX NOT ***** C ***** SINGULAR, HAS ROW OR COLUMN NUMBER ***** C ***** CAUSING SINGULARITY IF SINGULAR. ***** C ***** ***** C ***** THE SUBROUTINE IS APPLICABLE TO NON-SYMMETRIC ***** C ***** CASES ALSO. THE PROGRAM WAS DEVELOPED BY THE ***** C ***** DEPARTMENT OF AGRONOMY AT THE UNIVERSITY OF ***** C ***** ILLINOIS. ***** C ***** ***** C ***** S.J. JODOIN ***** C ***** JUNE, 1987 ***** C ***** ***** C ****************************************************************** C ****************************************************************** C IMPLICIT REAL*8(A-H,O-Z) REAL*8 A(20,20) C C WRITE(6,9998) C9998 FORMAT(' ','SUBROUTINE SWPMAT HAS BEEN CALLED') KERR=0 DO 40 K=IN,N IF(ABS(A(K,K))-TOL) 85,85,86 86 X=1.0E0/A(K,K) DO 41 J=IN,M 41 A(K,J)=A(K,J)*X A(K,K)=X DO 42 I=IN,N IF(I-K) 50,42,50 50 Y=A(I,K) A(I,K)=0.0 DO 43 J=IN,M 43 A(I,J)=A(I,J)-Y*A(K,J) 42 CONTINUE 40 CONTINUE 99 CONTINUE C WRITE(6,9999) C9999 FORMAT(' ','SUBROUTINE SWPMAT HAS BEEN COMPLETED') RETURN 85 KERR=K GOTO 99 END SUBROUTINE XPYPC(L) C C ****************************************************************** C ****************************************************************** C ***** ***** C ***** SUBROUTINE XPYPC ***** C ***** ***** C ***** SUBROUTINE XPYPC COMPUTES THE COORDINATES OF THE ***** C ***** PRINCIPAL POINT, XP AND YP, AND THE PRINCIPAL ***** C ***** DISTANCE, C, OF A PHOTOGRAPH FROM THE DIRECT LINEAR ***** C ***** TRANSFORMATION PARAMETERS OBTAINED FOR THE ***** C ***** PHOTOGRAPH. ***** C ***** ***** C ***** S.J. JODOIN ***** C ***** JUNE, 1987 ***** C ***** ***** C ****************************************************************** C ****************************************************************** C IMPLICIT REAL*8(A-H,O-Z) REAL*8 Q(5),A(20,20) COMMON/TRANS/D(4,20),XP(4),YP(4),W(20,20),CX(4),CY(4),DL(4) *,WW(4,20,20) COMMON/PP/PCX,PCY,C(4) COMMON/PHASE/PHI(4),KAPA(4),OMEGA(4),SXYZ(4,3),IDO,IMO,SO,IDP,IMP, *SPP,IDK,IMK,SK C REAL KAPA REAL*8 KAPA C C WRITE(6,9998) C9998 FORMAT(' ','SUBROUTINE XPYPC HAS BEEN CALLED') DO 10 I=1,5 Q(I)=0.0 10 CONTINUE C DO 30 I=1,3 J=I+8 K=I+4 Q(1)=Q(1)+D(L,I)*D(L,J) Q(2)=Q(2)+D(L,K)*D(L,J) Q(3)=Q(3)+D(L,I)*D(L,I) Q(4)=Q(4)+D(L,K)*D(L,K) Q(5)=Q(5)+D(L,J)*D(L,J) 30 CONTINUE C XP(L)=Q(1)/Q(5) YP(L)=Q(2)/Q(5) DL(L)=Q(5) PCX=Q(3)/Q(5)-XP(L)*XP(L) PCY=Q(4)/Q(5)-YP(L)*YP(L) PCX=DSQRT(PCX) PCY=DSQRT(PCY) C(L)=(PCX+PCY)*0.5E0 C PI=3.141592653 XO=XP(L) YO=YP(L) T=D(L,9)*D(L,9)+D(L,10)*D(L,10)+D(L,11)*D(L,11) ST=DSQRT(1./T) ICNTR=0 40 R31=D(L,9)*ST ICNTR=ICNTR+1 R32=D(L,10)*ST R33=D(L,11)*ST R21=(YO*R31-D(L,5)*ST)/PCY R22=(YO*R32-D(L,6)*ST)/PCY R23=(YO*R33-D(L,7)*ST)/PCY R11=(XO*R31-D(L,1)*ST)/PCX R00=R22*R33-R23*R32 R00=ABS(R11-R00) C WRITE(6,9000) R00 C9000 FORMAT(' ',5X,'THE VALUE OF R00 IS:',5X,F20.10) IF(ICNTR.EQ.100) GOTO 50 IF(R00-.001) 50,50,60 60 ST=-ST GOTO 40 50 R12=(XO*R32-D(L,2)*ST)/PCX R13=(XO*R33-D(L,3)*ST)/PCX OMEGA(L)=ATAN(-R32/R33) IF(R33.LT.0.) OMEGA(L)=PI+OMEGA(L) CALL RDMS(OMEGA(L),IDO,IMO,SO) KAPA(L)=ATAN(-R21/R11) IF(R11.LT.0.) KAPA(L)=PI+KAPA(L) CALL RDMS(KAPA(L),IDK,IMK,SK) PHI(L)=DARSIN(R31) CALL RDMS(PHI(L),IDP,IMP,SPP) A(1,1)=D(L,9) A(1,2)=D(L,10) A(1,3)=D(L,11) A(2,1)=XO*D(L,9)-D(L,1) A(2,2)=XO*D(L,10)-D(L,2) A(2,3)=XO*D(L,11)-D(L,3) A(3,1)=YO*D(L,9)-D(L,5) A(3,2)=YO*D(L,10)-D(L,6) A(3,3)=YO*D(L,11)-D(L,7) A(1,4)=-1. A(2,4)=D(L,4)-XO A(3,4)=D(L,8)-YO C WRITE(6,9997) C9997 FORMAT(' ','CALLING SWPMAT FROM XPYPC') CALL SWPMAT(A,1,3,4,KERR,1.0E-06) DO 129 II=1,3 SXYZ(L,II)=A(II,4) 129 CONTINUE C WRITE(6,9999) C9999 FORMAT(' ','SUBROUTINE XPYPC HAS BEEN COMPLETED') C C RETURN END SUBROUTINE OBJECT(IPM4,IPM3,IPM2,IPM1,IP,SP) C C ****************************************************************** C ****************************************************************** C ***** ***** C ***** SUBROUTINE OBJECT ***** C ***** ***** C ***** SUBROUTINE OBJECT APPLIES LENS DISTORTION ***** C ***** CORRECTIONS TO THE OBSERVED COMPARATOR COORDINATES ***** C ***** OF POINTS, AND COMPUTES THE OBJECT SPACE COORDINATES ***** C ***** OF THE POINTS USING AS MANY PHOTOGRAPHS AS WERE ***** C ***** USED IN THE SOLUTION. ***** C ***** ***** C ***** S.J. JODOIN ***** C ***** JUNE, 1987 ***** C ***** ***** C ****************************************************************** C ****************************************************************** C IMPLICIT REAL*8(A-H,O-Z) REAL*8 Q(10),T(20,20),XX(4),YY(4),R(20),F(20),AX(4),AY(4),A(4) *,XCC(3),SE(4),SSE(4),RES(350,3),SUMR(3),AXX(4),AYY(4) REAL*8 COOR(500,3),NCOR(500),AA(24,4),AAAA(24),AAA(24),QQ(24,4) REAL*8 QQQ(24),DASN(24),V(24) COMMON/DATA/E(350,3),X(350,4),Y(350,4),NUM(350),NNC,NPHOT,NPTS COMMON/NUMBER/NC,NPC(4),NP(350,4) COMMON/TRANS/D(4,20),XP(4),YP(4),W(20,20),CX(4),CY(4),DL(4) *,WW(4,20,20) COMMON/XTYT/XT,YT,INXT,IFILE,IDISK,IPUNCH COMMON/XYZ/XYZUN(350,3) C TOL=1.E-6 C DO 1 I=1,3 SUMR(I)=0.0 1 CONTINUE SPOSR=0.0 C C PRINT HEADING FOR TABULATION OF COMPUTED DATA C WRITE(6,126) NPHOT 126 FORMAT('1',1X,'COMPUTATIONS FOR',I4,2X,'-PHOTO DLT',//,1X, *'POINT',12X,'GIVEN',17X,'COMPUTED',29X,'RESIDUAL', *26X,'RMS',//,2X,3(8X, *'X',8X,'Y',8X,'Z'),8X,'POS',6X,'X',8X,'Y',8X,'Z',8X,'POS',//) C C INITIALIZE AVERAGE STANDARD ERRORS, C AND COMPUTE DEGREES OF FREEDOM C DO 5 I=1,4 SSE(I)=0.0 5 CONTINUE DF=2*NPHOT-3 NO=0 C C APPLY LENS DISTORTION CORRECTIONS TO OBSERVED COMPARATOR C COORDINATES C DO 10 K=1,NPTS KK=0 DO 15 L=1,NPHOT AX(L)=1.0E0 AY(L)=1.0E0 IF(IP.EQ.11) GOTO 12 XXX=X(K,L)-XP(L) YYY=Y(K,L)-YP(L) R1=XXX**2 R3=YYY**2 R2=R1+R3 IF(IP.EQ.12) GOTO 14 R4=R2*R2 R6=R2*R4 IF(IP.EQ.14) GOTO 17 DK=D(L,IPM4)*R2+D(L,IPM3)*R4+D(L,IPM2)*R6 R7=2.0E0*R1+R2 R8=2.0E0*R3+R2 R9=2.0E0*XXX*YYY XX(L)=X(K,L)-(XXX*DK+D(L,IPM1)*R7+D(L,IP)*R9) YY(L)=Y(K,L)-(YYY*DK+D(L,IPM1)*R9+D(L,IP)*R8) C C XX(L),YY(L) = PLATE COORDINATES CORRECTED FOR LENS DISTORTION C GOTO 15 17 CONTINUE DK=D(L,IPM2)*R2+D(L,IPM1)*R4+D(L,IP)*R6 GOTO 18 14 CONTINUE DK=D(L,IP)*R2 18 CONTINUE XX(L)=X(K,L)-XXX*DK YY(L)=Y(K,L)-YYY*DK GOTO 15 12 CONTINUE XX(L)=X(K,L) YY(L)=Y(K,L) 15 CONTINUE C C COMPUTE WEIGHTS ASSOCIATED WITH CONDITION EQUATIONS C DO 20 M=1,5 IF(M.EQ.1) GOTO 80 DO 25 L=1,NPHOT XXX=X(K,L)-XP(L) YYY=Y(K,L)-YP(L) A(L)=1.0E0 DO 30 I=1,3 A(L)=A(L)+D(L,I+8)*XCC(I) 30 CONTINUE IF(M.NE.5.OR.K.LE.NNC) GOTO 420 DO 421 I=1,NPHOT 421 AAA(I)=A(I) 420 CONTINUE C DO 35 II=1,2 GG=0.0 DO 40 I=1,11 R(I)=0.0 F(I)=0.0 40 CONTINUE C IF(II.EQ.2) GOTO 50 DO 45 I=1,3 J=I+8 R(I)=XCC(I) R(J)=XCC(I)*X(K,L) 45 CONTINUE R(4)=1.0E0 GOTO 60 C 50 CONTINUE DO 55 I=5,7 J=I+4 JJ=I-4 R(I)=XCC(JJ) R(J)=XCC(JJ)*Y(K,L) 55 CONTINUE R(8)=1.0E0 60 CONTINUE AB=(A(L)*SP)**2 C DO 65 I=1,11 DO 70 J=1,11 F(I)=F(I)+R(J)*WW(L,I,J) 70 CONTINUE GG=GG+F(I)*R(I) 65 CONTINUE IF(II.EQ.2) GOTO 75 AX(L)=GG+AB AXX(L)=AX(L) IF(M.NE.5.OR.K.LE.NNC) GOTO 401 IJKL=2*NPHOT DO 422 I=1,IJKL,2 JJJJ=I/2+1 422 AAAA(I)=AXX(JJJJ) 401 CONTINUE C C AX(L) = VARIANCE ASSOCIATED WITH X-CONDITION EQUATION C AX(L)=DSQRT(AX(L)) GOTO 35 75 AY(L)=GG+AB AYY(L)=AY(L) IF(M.NE.5.OR.K.LE.NNC) GOTO 402 DO 423 I=2,IJKL,2 IIII=I/2 423 AAAA(I)=AYY(IIII) 402 CONTINUE C C AY(L) = VARIANCE ASSOCIATED WITH Y-CONDITION EQUATION C AY(L)=DSQRT(AY(L)) 35 CONTINUE 25 CONTINUE 80 CONTINUE C C INITIALIZE NORMAL EQUATIONS, AND SUM OF RESIDUALS SQUARED C DO 85 I=1,3 DO 85 J=1,4 T(I,J)=0.0 85 CONTINUE VV=0.0 C DO 90 L=1,NPHOT DO 95 II=1,2 GOTO (91,92), II C C FORM X-CONDITION EQUATION C 91 DO 96 I=1,3 J=I+8 Q(I)=(D(L,J)*XX(L)-D(L,I))/AX(L) 96 CONTINUE Q(4)=(D(L,4)*1.0E0-XX(L))/AX(L) IF(M.NE.5.OR.K.LE.NNC) GOTO 403 KK=KK+1 DO 404 I=1,4 404 AA(KK,I)=Q(I)*AX(L) 403 CONTINUE GOTO 100 C C FORM Y-CONDITION EQUATION C 92 DO 97 I=1,3 J=I+8 JJ=I+4 Q(I)=(D(L,J)*YY(L)-D(L,JJ))/AY(L) 97 CONTINUE Q(4)=(D(L,8)*1.0E0-YY(L))/AY(L) IF(M.NE.5.OR.K.LE.NNC) GOTO 405 KK=KK+1 DO 406 I=1,4 406 AA(KK,I)=Q(I)*AY(L) 405 CONTINUE 100 CONTINUE C C COMPUTE CONTRIBUTIONS OF CONDITION EQUATION TO SUM OF RESIDUALS C SQUARED, AND OBTAIN CUMMULATIVE SUM C VV=VV+Q(4)**2 C C FORM NORMAL EQUATIONS C DO 105 I=1,3 DO 105 J=1,4 T(I,J)=T(I,J)+Q(I)*Q(J) 105 CONTINUE 95 CONTINUE 90 CONTINUE C DO 110 I=1,3 Q(I)=T(I,4) 110 CONTINUE C C SOLVE NORMAL EQUATIONS C CALL SWPMAT(T,1,3,4,KERR,TOL) C C OBTAIN COMPUTED OBJECT SPACE COORDINATES OF A POINT, XCC(I), I=1,3 C I=1,X, I=2,Y, I=3,Z C COMPUTE CONTRIBUTIONS OF NORMAL EQUATIONS TO SUM OF RESIDUALS C SQUARED, AND OBTAIN TOTAL SUM C DO 115 I=1,3 XCC(I)=T(I,4) VV=VV-Q(I)*T(I,4) 115 CONTINUE IF(M.EQ.1) GOTO 20 VV=ABS(VV/DF) C C COMPUTE STANDARD ERROR OF UNIT WEIGHT C SEUW=DSQRT(VV) IF(M.NE.5.OR.K.LE.NNC) GOTO 408 DO 407 LLL=1,KK DO 407 JJJ=1,3 QQ(LLL,JJJ)=0.0 DO 407 KKK=1,3 407 QQ(LLL,JJJ)=QQ(LLL,JJJ)+AA(LLL,KKK)*T(KKK,JJJ) DO 409 I=1,KK QQQ(I)=0.0 DO 409 J=1,3 409 QQQ(I)=QQQ(I)+QQ(I,J)*AA(I,J) DO 410 I=1,KK C C NOTE THAT THE ABSOLUTE VALUE IS USED HERE STEVE C 410 QQQ(I)=SEUW*DSQRT(ABS(AAAA(I)-QQQ(I))) DO 411 I=1,KK V(I)=0.0 DO 411 J=1,3 411 V(I)=V(I)+AA(I,J)*XCC(J) DO 412 I=1,KK 412 V(I)=V(I)-AA(I,4) DO 413 I=1,KK DASN(I)=ABS(V(I)/QQQ(I)) 413 CONTINUE 408 SE(4)=0.0 C C COMPUTE VARIANCE-COVARIANCE MATRIX OF COMPUTED COORDINATES C DO 120 I=1,3 DO 125 J=1,3 T(I,J)=VV*(T(I,J)) 125 CONTINUE SE(I)=T(I,I) SE(4)=SE(4)+SE(I) 120 CONTINUE 20 CONTINUE C C COMPUTE SUMS OF VARIANCES, AND STANDARD ERRORS OF COORDINATES C DO 130 I=1,4 SSE(I)=SSE(I)+SE(I) SE(I)=DSQRT(SE(I)) 130 CONTINUE C C PRINT POINT NUMBER, GIVEN AND COMPUTED COORDINATES, C AND STANDARD ERRORS, FOR EACH POINT C DO 358 I=1,3 358 XYZUN(K,I)=XCC(I) C IF(K.GT.NNC) GOTO 300 POS=0.0 DO 210 I=1,3 RES(K,I)=XCC(I)-E(K,I) SSR=RES(K,I)**2 SUMR(I)=SUMR(I)+SSR POS=POS+SSR 210 CONTINUE POSR=DSQRT(POS) SPOSR=SPOSR+POS WRITE(6,200) NUM(K),(E(K,I), I=1,3),(XCC(I), I=1,3),(RES(K,I), *I=1,3),POSR,(SE(I),I=1,4) 200 FORMAT(1X,I4,14F9.3) IF(K.NE.NNC) GOTO 10 C C COMPUTE AND PRINT AVERAGE STANDARD ERRORS FOR ALL THE POINTS C DKI=K-1 DO 209 I=1,3 SUMR(I)=DSQRT(SUMR(I)/DKI) 209 CONTINUE SPOSR=DSQRT(SPOSR/DKI) WRITE(6,400) NNC,(SUMR(I),I=1,3),SPOSR 400 FORMAT('0',14X,'AVERAGE MEAN SQUARE ERRORS FOR',I3,2X, *'POINTS ARE',4F9.4) WRITE(6,430) 430 FORMAT(' ',////,5X,'TRANSFORMED POINTS :',//) GOTO 10 300 CONTINUE WRITE(6,350) NP(K,1) 350 FORMAT(5X,'POINT :',I5,/) WRITE(6,351) 351 FORMAT(22X,'VX',8X,'VY',7X,'SIGVX',5X,'SIGVY',5X,'WX',8X,'WY',/) KKKK=0 KKK=KK/2 DO 352 I=1,KKK KKKK=KKKK+1 IF(DASN(KKKK).GT.3.29.OR.DASN(KKKK+1).GT.3.29) WRITE(6,353) I, *V(KKKK),V(KKKK+1),QQQ(KKKK),QQQ(KKKK+1),DASN(KKKK),DASN(KKKK+1) IF(DASN(KKKK).LE.3.29.AND.DASN(KKKK+1).LE.3.29) WRITE(6,354) I, *V(KKKK),V(KKKK+1),QQQ(KKKK),QQQ(KKKK+1),DASN(KKKK),DASN(KKKK+1) 353 FORMAT(7X,'PHOTO :',I2,6F10.4,3X,'----------REJECT') 354 FORMAT(7X,'PHOTO :',I2,6F10.4,3X,'----------O.K.') KKKK=KKKK+1 352 CONTINUE WRITE(6,355) 355 FORMAT(/,18X,'X',10X,'Y',10X,'Z',10X,'SIGX',7X,'SIGY',7X,'SIGZ', *6X,'SIGPOS') WRITE(6,356) (XCC(I),I=1,3),(SE(I),I=1,4) 356 FORMAT(12X,7F11.4,////) IF(IPUNCH.EQ.0) GOTO 440 WRITE(6,326) NP(K,1),(XCC(I),I=1,3) 326 FORMAT(' ',I10,3F10.3) C 440 CONTINUE IF(IDISK.NE.1) GOTO 10 NO=NO+1 NCOR(NO)=NP(K,1) DO 320 I=1,3 COOR(NO,I)=XCC(I) 320 CONTINUE C IF(K.EQ.NPTS) WRITE(IFILE,INXT) (NCOR(I),(COOR(I,JJ),JJ=1,3), C *I=1,NO) 10 CONTINUE C READ(5,2) EPSILN 2 FORMAT(F15.7) IF(SPOSR.GT.EPSILN) CALL GERISO RETURN END SUBROUTINE RDMS(D,ID,IM,S) C C ****************************************************************** C ****************************************************************** C ***** ***** C ***** SUBROUTINE RDMS ***** C ***** ***** C ***** SUBROUTINE RDMS CONVERTS ANGULAR VALUES FROM ***** C ***** RADIANS TO DEGREES, MINUTES, SECONDS ***** C ***** ***** C ***** S.J. JODOIN ***** C ***** JUNE, 1987 ***** C ***** ***** C ****************************************************************** C ****************************************************************** C IMPLICIT REAL*8(A-H,O-Z) C WRITE(6,9998) C9998 FORMAT(' ','SUBROUTINE RDMS HAS BEEN CALLED') DTEMP=D/0.484813681E-05 MINS=DTEMP/60 ETC=MINS S=DTEMP-60.0*ETC ID=MINS/60 IM=MINS-60*ID C WRITE(6,9999) C9999 FORMAT(' ','SUBROUTINE XPYPC HAS BEEN COMPLETED') RETURN END SUBROUTINE ELEM(L) C C ****************************************************************** C ****************************************************************** C ***** ***** C ***** SUBROUTINE ELEM ***** C ***** ***** C ***** SUBROUTINE ELEM COMPUTES THE EXTERIOR ORIENTATION ***** C ***** AND OBJECT SPACE COORDINATES OF THE PHOTO STATION ***** C ***** FOR PHOTO L. ***** C ***** ***** C ***** S.J. JODOIN ***** C ***** JUNE, 1987 ***** C ***** ***** C ****************************************************************** C ****************************************************************** C IMPLICIT REAL*8(A-H,O-Z) REAL*8 DD(11),DEL(11) COMMON/TRANS/D(4,20),XP(4),YP(4),W(20,20),CX(4),CY(4),DL(4), *WW(4,20,20) COMMON/PP/PCX,PCY,C(4) COMMON/PHASE/PHI(4),KAPA(4),OMEGA(4),XYZ(4,3),IDO,IMO,SO,IDP,IMP, *SPP,IDK,IMK,SK COMMON/PHASE2/SIGPHI(4),SIGKAP(4),SIGOM(4),SIGXYZ(4,3), *SIGXP(4),SIGYP(4),SIGC(4) C REAL KAPA REAL*8 KAPA CALL XPYPC(L) OKAPA=KAPA(L) OPHI=PHI(L) OOMEGA=OMEGA(L) OXP=XP(L) OYP=YP(L) OC=C(L) OX=XYZ(L,1) OY=XYZ(L,2) OZ=XYZ(L,3) SIGKAP(L)=0.0 SIGPHI(L)=0.0 SIGOM(L)=0.0 SIGXP(L)=0.0 SIGYP(L)=0.0 SIGC(L)=0.0 DO 30 I=1,3 30 SIGXYZ(L,I)=0.0 DO 20 I=1,11 DD(I)=D(L,I) DEL(I)=D(L,I)*0.00001 D(L,I)=D(L,I)+DEL(I) CALL XPYPC(L) SIGKAP(L)=SIGKAP(L)+WW(L,I,I)*((KAPA(L)-OKAPA)/DEL(I))**2 SIGPHI(L)=SIGPHI(L)+WW(L,I,I)*((PHI(L)-OPHI)/DEL(I))**2 SIGOM(L)=SIGOM(L)+WW(L,I,I)*((OMEGA(L)-OOMEGA)/DEL(I))**2 SIGXP(L)=SIGXP(L)+WW(L,I,I)*((XP(L)-OXP)/DEL(I))**2 SIGYP(L)=SIGYP(L)+WW(L,I,I)*((YP(L)-OYP)/DEL(I))**2 SIGC(L)=SIGC(L)+WW(L,I,I)*((C(L)-OC)/DEL(I))**2 SIGXYZ(L,1)=SIGXYZ(L,1)+WW(L,I,I)*((XYZ(L,1)-OX)/DEL(I))**2 SIGXYZ(L,2)=SIGXYZ(L,2)+WW(L,I,I)*((XYZ(L,2)-OY)/DEL(I))**2 SIGXYZ(L,3)=SIGXYZ(L,3)+WW(L,I,I)*((XYZ(L,3)-OZ)/DEL(I))**2 20 D(L,I)=DD(I) KAPA(L)=OKAPA PHI(L)=OPHI OMEGA(L)=OOMEGA XP(L)=OXP YP(L)=OYP C(L)=OC XYZ(L,1)=OX XYZ(L,2)=OY XYZ(L,3)=OZ WRITE(6,111) L,XP(L),YP(L),C(L),KAPA(L),PHI(L),OMEGA(L) WRITE(6,112) SIGXP(L),SIGYP(L),SIGC(L),SIGKAP(L),SIGPHI(L), *SIGOM(L) WRITE(6,113) (XYZ(L,I),I=1,3), (SIGXYZ(L,I),I=1,3) 111 FORMAT(10X,I3,3F11.4,3E15.8) 112 FORMAT(13X,3F11.4,3E15.8) 113 FORMAT(10X,3F11.4,/,10X,3F11.4) RETURN END SUBROUTINE GERISO C C ****************************************************************** C ****************************************************************** C ***** ***** C ***** SUBROUTINE GERISO ***** C ***** ***** C ***** SUBROUTINE GERISO IS THE GENERAL RESECTION, INTER- ***** C ***** SECTION SOLUTION, WHICH INCORPORATES POLYNOMIALS ***** C ***** WITH K1, K2, K3, P1 AND P2 FOR SYSTEMATIC ERROR ***** C ***** CORRECTION MODEL FOR COORDINATES OF IMAGE POINTS. ***** C ***** ***** C ***** ***** C ***** S.J. JODOIN ***** C ***** JUNE, 1987 ***** C ***** ***** C ****************************************************************** C ****************************************************************** C IMPLICIT REAL*8(A-H,O-Z) REAL*8 AF(3,3),DBJ(2,16),DDBJ(2,3),EJ(2),AX(4),AY(4),Q(4), *FF(16),T(20,20),XCC(3),SE(4),SSE(4),RES(350,3),SUMR(3),DO(16), *DD(16),AFF(4,3,3) COMMON/DATA/E(350,3),X(350,4),Y(350,4),NUM(350),NNC,NPHOT,NPTS COMMON/TRANS/D(4,20),XP(4),YP(4),W(20,20),CX(4),CY(4),DL(4), *WW(4,20,20) COMMON/NUMBER/NC,NPC(4),NP(350,4) COMMON/PP/PCX,PCY,C(4) COMMON/PHASE/PHI(4),KAPA(4),OMEGA(4),SXYZ(4,3),IDO,IMO,SO,IDP, *IMP,SPP,IDK,IMK,SK COMMON/PHASE2/SIGPHI(4),SIGKAP(4),SIGOM(4),SIGXYZ(4,3),SIGXP(4), *SIGYP(4),SIGC(4) COMMON/PHASE3/DUOO(16),DN(16,16),SP,SX,SY,SZ COMMON/XYZ/XYZUN(350,3) REAL*8 KAPA C REAL KAPA C DO 1 L=1,NPHOT CALL CALIB(L) DO 4 I=1,14 4 D(L,I)=DUOO(I) 1 CONTINUE TOL=1.0E-6 DO 2 I=1,3 2 SUMR(I)=0.0 SPOSR=0.0 C C PRINT HEADING FOR TABULATION OF COMPUTED DATA C WRITE(6,126) NPHOT 126 FORMAT(2X,'COMPUTATIONS FOR ',I4,2X,'---PHOTO ',//,1X,'POINT', *12X,'GIVEN',17X,'COMPUTED',29X,'RESIDUAL',26X,'RMS',//,2X,3(8X, *'X',8X,'Y',8X,'Z'),8X,'POS',6X,'X',8X,'Y',8X,'Z',8X,'POS',//) DO 5 I=1,4 5 SSE(I)=0.0 DF=2*NPHOT-3 DO 10 K=1,NPTS DO 358 I=1,3 358 XYZUN(K,I)=XYZUN(K,I)+10. SIGO=100. DO 1000 M=1,5 WRITE(6,357) (XYZUN(K,I),I=1,3) 357 FORMAT(12X,7F16.4) VTWV=0. DO 90 I=1,3 DO 90 J=1,4 90 T(I,J)=0. DO 110 L=1,NPHOT CALL OMAT1(D(L,12),D(L,13),D(L,14),AF,3,1) DO 362 I=1,3 DO 362 J=1,3 362 AFF(L,I,J)=AF(I,J) F=AF(3,1)*(XYZUN(K,1)-D(L,9))+AF(3,2)*(XYZUN(K,2)-D(L,10)) *+AF(3,3)*(XYZUN(K,3)-D(L,11)) G=AF(1,1)*(XYZUN(K,1)-D(L,9))+AF(1,2)*(XYZUN(K,2)-D(L,10)) *+AF(1,3)*(XYZUN(K,1)-D(L,11)) H=AF(2,1)*(XYZUN(K,1)-D(L,9))+AF(2,2)*(XYZUN(K,2)-D(L,10)) *+AF(2,3)*(XYZUN(K,3)-D(L,11)) DO(1)=0.00001 DO(2)=0.00001 DO(3)=0.00001 DO 140 I=4,8 140 DO(I)=0.1E-16 DO 145 I=9,11 145 DO(I)=0.1E-5 DO 150 I=12,14 150 DO(I)=0.48481E-10 DO 255 I=1,14 255 DD(I)=D(L,I) CALL FXFY(AF,XYZUN(K,1),XYZUN(K,2),XYZUN(K,3),X(K,L),Y(K,L),DD,3, *14,FX,FY,0) CALL PART(X(K,L),Y(K,L),XYZUN(K,1),XYZUN(K,2),XYZUN(K,3),AF,DD, *DO,DBJ,DDBJ,EJ,0,3,2,14,0) DO 110 II=1,2 GOTO (91,92),II C C FORM X-CONDITION EQUATION C 91 Q(1)=D(L,3)*(AF(1,1)*F-AF(3,1)*G)/(F*F) Q(2)=D(L,3)*(AF(1,2)*F-AF(3,2)*G)/(F*F) Q(3)=D(L,3)*(AF(1,3)*F-AF(3,3)*G)/(F*F) Q(4)=FX DO 60 I=1,14 60 FF(I)=0.0 GG=0.0 DO 65 I=1,14 DO 70 J=1,14 70 FF(I)=FF(I)+DBJ(1,J)*WW(L,I,J) 65 GG=GG+FF(I)*DBJ(1,I) AX(L)=GG+SP*SP AX(L)=DSQRT(AX(L)) DO 61 I=1,4 61 Q(I)=Q(I)/SP GOTO 100 C C FORM Y-CONDITION EQUATION C 92 Q(1)=D(L,3)*(AF(2,1)*F-AF(3,1)*H)/(F*F) Q(2)=D(L,3)*(AF(2,2)*F-AF(3,2)*H)/(F*F) Q(3)=D(L,3)*(AF(2,3)*F-AF(3,3)*H)/(F*F) Q(4)=FY DO 75 I=1,14 75 FF(I)=0.0 GG=0.0 DO 80 I=1,14 DO 85 J=1,14 85 FF(I)=FF(I)+DBJ(2,J)*WW(L,I,J) 80 GG=GG+FF(I)+DBJ(2,I) AY(L)=GG+SP*SP AY(L)=DSQRT(AY(L)) DO 81 I=1,4 81 Q(I)=Q(I)/SP 100 CONTINUE C C FORM NORMAL EQUATIONS C DO 95 I=1,3 DO 95 J=1,4 95 T(I,J)=T(I,J)+Q(I)*Q(J) 110 CONTINUE DO 115 I=1,3 115 Q(I)=T(I,4) CALL SWPMAT(T,1,3,4,KERR,TOL) DO 120 I=1,3 120 XCC(I)=T(I,4)+XYZUN(K,I) DO 195 L=1,NPHOT DO 260 I=1,14 260 DD(I)=D(L,I) DO 363 I=1,3 DO 363 J=1,3 363 AF(I,J)=AFF(L,I,J) CALL FXFY(AF,XCC(1),XCC(2),XCC(3),X(K,L),Y(K,L),DD,3,14,FX,FY,0) VTWV=VTWV+(FX/AX(L))*(FX/AX(L))+(FY/AY(L))*(FY/AY(L)) 195 WRITE(6,357) FX,AX(L),FY,AY(L) VTWV=VTWV/DF RATIO=SIGO/VTWV SE(4)=0.0 DO 125 I=1,3 DO 130 J=1,3 130 T(I,J)=VTWV*T(I,J) SE(I)=T(I,I) 125 SE(4)=SE(4)+SE(I) IF(ABS(RATIO-1.).LE.0.01) GOTO 1001 SIGO=VTWV DO 93 I=1,3 93 XYZUN(K,I)=XCC(I) 1000 CONTINUE 1001 CONTINUE DO 135 I=1,4 SSE(I)=SSE(I)+SE(I) 135 SE(I)=DSQRT(SE(I)) IF(K.GT.NNC) GOTO 300 POS=0.0 DO 210 I=1,3 RES(K,I)=XCC(I)-E(K,I) SSR=RES(K,I)*RES(K,I) SUMR(I)=SUMR(I)+SSR 210 POS=POS+SSR POSR=DSQRT(POS) SPOSR=SPOSR+POS WRITE(6,200) NUM(K),(E(K,I),I=1,3),(XCC(I),I=1,3),(RES(K,I),I=1,3) *,POSR,(SE(I),I=1,4) 200 FORMAT(1X,I4,14F9.3) IF(K.NE.NNC) GOTO 10 C C COMPUTE AND PRINT AVERAGE STANDARD ERRORS FOR ALL THE POINTS C DKI=K-1 DO 209 I=1,3 209 SUMR(I)=DSQRT(SUMR(I)/DKI) SPOSR=DSQRT(SPOSR/DKI) WRITE(6,400) NNC,(SUMR(I),I=1,3),SPOSR 400 FORMAT(14X,'AVERAGE MEAN SQUARE ERRORS FOR',I3,2X,'POINTS ARE', *4F9.4) WRITE(6,430) 430 FORMAT(////,5X,'TRANSFORMED POINTS :',//) GOTO 10 300 CONTINUE WRITE(6,350) NP(K,1) 350 FORMAT(5X,'POINT :',I5,/) WRITE(6,355) 355 FORMAT(/,18X,'X',10X,'Y',10X,'Z',10X,'SIGX',7X,'SIGY',7X,'SIGZ', *6X,'SIGPOS') WRITE(6,356) (XCC(I), I=1,3),(SE(I),I=1,4) 356 FORMAT(12X,7F11.4,////) 10 CONTINUE RETURN END SUBROUTINE CALIB(L) C C ****************************************************************** C ****************************************************************** C ***** ***** C ***** SUBROUTINE CALIB ***** C ***** ***** C ***** SUBROUTINE CALIB PERFORMS THE RESECTION SOLUTION ***** C ***** NEEDED BY GERISO. IT IS A MODIFIED VERSION OF IFCAL ***** C ***** DEVELOPED BY GAMBLE (1972) TO PERFORM IN FLIGHT ***** C ***** CALIBRATION OF CAMERAS. ***** C ***** ***** C ***** ***** C ***** S.J. JODOIN ***** C ***** JUNE, 1987 ***** C ***** ***** C ****************************************************************** C ****************************************************************** C IMPLICIT REAL*8(A-H,O-Z) REAL*8 AF(3,3),DDUOO(150),EPS(50),CR(16,16), *DUO(16),WWW(2,2),DW(16,16),DDW(3,3),SIGDOT(16),EJ(2),DZW(50,3,3), *EPSX(50),EPSY(50),DE(16),DDE(150),DDEL(16),DDDEL(150), *SJ(16,16),DC(16),RJ(16),AOO(3,3),DBJ(2,16),DNJ(16,16),DDBJ(2,3), *TMAT2(3),A1(2,16),A2(16,16),B1(16),B2(3,3),C1(16),C2(16,16), *DDNJ(3,3),DCJ(16),DDCJ(3),BARNJ(16,3),DDNIN(3,3),TMAT1(3) DIMENSION DO(16) COMMON/DATA/E(350,3),X(350,4),Y(350,4),NUM(350),NNC,NPHOT,NPTS COMMON/TRANS/D(4,20),XP(4),YP(4),W(20,20),CX(4),CY(4),DL(4), *WW(4,20,20) COMMON/NUMBER/NC,NPC(4),NP(350,4) COMMON/PP/PCX,PCY,C(4) COMMON/PHASE/PHI(4),KAPA(4),OMEGA(4),XYZ(4,3),IDO,IMO,SO,IDP,IMP, *SPP,IDK,IMK,SK COMMON/PHASE2/SIGPHI(4),SIGKAP(4),SIGOM(4),SIGXYZ(4,3),SIGXP(4), *SIGYP(4),SIGC(4) COMMON/PHASE3/DUOO(16),DN(16,16),SP,SX,SY,SZ C REAL KAPA REAL*8 KAPA C C CRIT=0.25 RPS=0.4848136811E-05 SPR=206264.8063 REJ=0.0 SIGO=1000. MAXIT=3 NOUNK=7 NPON=NNC NEQ=2*NPON+14 NPPP=3*NPON NDF=NEQ-14 DO 50 J=1,NPON DZW(J,1,1)=SX*0.001 DZW(J,2,2)=SY*0.001 50 DZW(J,3,3)=SZ*0.001 DUO(1)=XP(L) SIGDOT(1)=SIGXP(L)*1.E-10 DUO(2)=YP(L) SIGDOT(2)=SIGYP(L)*1.E-10 DUO(3)=-C(L) SIGDOT(3)=SIGC(L)*1.E-10 DO 10 J=4,8 10 DUO(J)=0.0 READ(5,*) (SIGDOT(I), I=4,8) 1 FORMAT(5E10.2) DO 20 J=9,11 I=J-8 SIGDOT(J)=SIGXYZ(L,I)*1.E+20 20 DUO(J)=XYZ(L,I) DUO(12)=OMEGA(L) SIGDOT(12)=SIGOM(L)*1.E-5 DUO(13)=PHI(L) SIGDOT(13)=SIGPHI(L)*1.E-5 DUO(14)=KAPA(L) SIGDOT(14)=SIGKAP(L)*1.E-5 WWW(1,1)=1.0 WWW(1,2)=0.0 WWW(2,1)=0.0 WWW(2,2)=1.0 DO 70 I=1,14 DO 70 J=1,14 70 DW(I,J)=0.0 DO 80 I=1,14 80 DW(I,I)=SP*SP/SIGDOT(I) DO 85 J=1,NPON DO 85 I=1,3 85 DZW(J,I,I)=SP*SP/(DZW(J,I,I)*DZW(J,I,I)) C C INITIALIZE FOR SOLUTION C ITER=0 DO 110 I=1,3 DO 110 J=1,3 110 DDW(I,J)=0.0 DO 120 I=1,14 DUOO(I)=DUO(I) DDEL(I)=0.0 120 DE(I)=0.0E+00 C C INITIALIZE INCREMENTS FOR PARTIALS C DO(1)=0.00001 DO(2)=0.00001 DO(3)=0.00001 DO 140 I=4,8 140 DO(I)=0.1E-16 DO(9)=0.1E-5 DO(10)=0.1E-5 DO(11)=0.1E-5 DO(12)=0.48481E-10 DO(13)=0.48481E-10 DO(14)=0.48481E-10 C C************************************************************************ C************************************************************************ C C SOLUTION PHASE C C KTEST -----> DECISION POINTER GOVERNING PROGRAM FLOW C = 1 -> SOLUTION FOR DELTA DOT C = 2 -> SOLUTION FOR DELTA DOUBLE DOT C = 3 -> COMPUTING N DOT FOR ERROR PROPAGATION C C************************************************************************ C************************************************************************ C 160 KTEST=1 170 CALL OMAT1(DUOO(12),DUOO(13),DUOO(14),AOO,3,1) DO 180 I=1,14 DO 180 J=1,14 DN(I,J)=0.0 DC(I)=0.0 180 CONTINUE C C BEGIN BUILDING NORMAL EQUATIONS POINT BY POINT C 200 DO 300 II=1,NPON DDW(1,1)=DZW(II,1,1) DDW(2,2)=DZW(II,2,2) DDW(3,3)=DZW(II,3,3) TX=X(II,L) TY=Y(II,L) IF((ITER.GT.0).OR.(KTEST.GT.1)) GOTO 210 DDUOO(3*II-2)=E(II,1) DDUOO(3*II-1)=E(II,2) DDUOO(3*II)=E(II,3) DDDEL(3*II-2)=0.0E+0 DDE(3*II-2)=0.0E+0 DDE(3*II-1)=0.0E+0 DDE(3*II)=0.0E+0 DDDEL(3*II-1)=0.0E+0 DDDEL(3*II)=0.0E+0 210 XO=DDUOO(3*II-2) YO=DDUOO(3*II-1) ZO=DDUOO(3*II) CALL PART(TX,TY,XO,YO,ZO,AOO,DUOO,DO,DBJ,DDBJ,EJ,0,3,2,14,0) GOTO (220,230,220), KTEST 220 EPSX(II)=EJ(1) EPSY(II)=EJ(2) DO 3000 LL=1,14 DO 3000 K=1,14 DNJ(K,LL)=0.0 DO 3010 M=1,2 DO 3010 I=1,2 3010 DNJ(K,LL)=DNJ(K,LL)+DBJ(M,K)*WWW(M,I)*DBJ(I,LL) 3000 CONTINUE DO 3020 K=1,14 DCJ(K)=0.0 DO 3030 M=1,2 DO 3030 I=1,2 3030 DCJ(K)=DCJ(K)+DBJ(M,K)*WWW(M,I)*EJ(I) 3020 CONTINUE 230 DO 3040 LL=1,3 DO 3040 K=1,3 DDNJ(K,LL)=0.0 DO 3050 M=1,2 DO 3050 I=1,2 3050 DDNJ(K,LL)=DDNJ(K,LL)+DDBJ(M,K)*WWW(M,I)*DDBJ(I,LL) 3040 CONTINUE DO 3060 K=1,3 DDCJ(K)=0.0 DO 3070 M=1,2 DO 3070 I=1,2 3070 DDCJ(K)=DDCJ(K)+DDBJ(M,K)*WWW(M,I)*EJ(I) 3060 CONTINUE DO 3080 LL=1,3 DO 3080 K=1,14 BARNJ(K,LL)=0.0 DO 3090 M=1,2 DO 3090 I=1,2 3090 BARNJ(K,LL)=BARNJ(K,LL)+DBJ(M,K)*WWW(M,I)*DDBJ(I,LL) 3080 CONTINUE DEC=1.0E+00 DO 3100 I=1,3 DO 3100 M=1,3 3100 DDNIN(I,M)=DDNJ(I,M)+DEC*DDW(I,M) CALL DINV(DDNIN,3,JFLAG,3) TMAT2(1)=DDE(3*II-2) TMAT2(2)=DDE(3*II-1) TMAT2(3)=DDE(3*II) DO 3110 M=1,3 TMAT1(M)=0.0 DO 3120 I=1,3 3120 TMAT1(M)=TMAT1(M)+DDW(M,I)*TMAT2(I) 3110 CONTINUE IF(KTEST.EQ.2) GOTO 240 DO 3130 LL=1,14 DO 3130 K=1,14 SJ(K,LL)=0.0 DO 3140 M=1,3 DO 3140 I=1,3 3140 SJ(K,LL)=SJ(K,LL)+BARNJ(K,M)*DDNIN(M,I)*BARNJ(LL,I) 3130 CONTINUE 240 DO 3150 K=1,3 3150 TMAT1(K)=DDCJ(K)-TMAT1(K) IF(KTEST.EQ.2) GOTO 270 DO 3160 K=1,14 RJ(K)=0.0 DO 3170 M=1,3 DO 3170 I=1,3 3170 RJ(K)=RJ(K)+BARNJ(K,M)*DDNIN(M,I)*TMAT1(I) 3160 CONTINUE DO 3180 LL=1,14 DO 3180 K=1,14 3180 DN(LL,K)=DN(LL,K)+DNJ(LL,K)-SJ(LL,K) IF(KTEST.EQ.3) GOTO 300 DO 3190 LL=1,14 3190 DC(LL)=DC(LL)+DCJ(LL)-RJ(LL) GOTO 300 C C THIS SECTION IS FOR COMPUTING DEL DOUBLE DOT C 270 DO 3200 LL=1,3 3200 TMAT2(LL)=0.0 DO 3210 LL=1,3 DO 3210 K=1,14 3210 TMAT2(LL)=TMAT2(LL)+BARNJ(K,LL)*DDEL(K) DO 3220 LL=1,3 3220 TMAT1(LL)=TMAT1(LL)-TMAT2(LL) DO 4000 M=1,3 TMAT2(M)=0.0 DO 4010 I=1,3 4010 TMAT2(M)=DDNIN(M,I)*TMAT1(I) 4000 CONTINUE DDDEL(3*II-2)=TMAT2(1) DDDEL(3*II-1)=TMAT2(2) DDDEL(3*II)=TMAT2(3) 300 CONTINUE IF(KTEST.EQ.2) GOTO 340 C C ALL POINTS PROCESSED, COMPLETE NORMAL EQUATIONS C DO 3230 M=1,14 RJ(M)=0.0 DO 3240 I=1,14 3240 RJ(M)=RJ(M)+DW(M,I)*DE(I) 3230 CONTINUE DO 3250 K=1,14 DO 3250 M=1,14 3250 DN(M,K)=DN(M,K)+DW(M,K) IF(KTEST.EQ.3) GOTO 410 DO 3260 K=1,14 3260 DC(K)=DC(K)-RJ(K) C C PRINT OUT DISCREPANCY VECTORS FOR THIS RELINEARIZATION C WRITE(6,6100) ITER 6100 FORMAT('1',T30,'RELINEARIZATION',I4,//,' ',T9,'POINT',T25,'RESX', *T45,'RESY',/,' ',T23,'(MICRONS)',T43,'(MICRONS)',//) DO 330 II=1,NPON 330 WRITE(6,6110) NUM(II),EPSX(II),EPSY(II) 6110 FORMAT(5X,I5,2F20.6) C C PRINT OUT DISCREPANCY VECTORS AND LATEST APPROXIMATIONS C WRITE(6,6200) ITER 6200 FORMAT('1',T20,'CAMERA DATA FOR RELINEARIZATION',I4,//,' ',T10, *'LATEST APPROX.',T33,'RESIDUALS',T50,'LATEST CORRECTIONS',//) DO 371 I=12,14 DUOO(I)=DUOO(I)/RPS DE(I)=DE(I)/RPS 371 DDEL(I)=DDEL(I)/RPS DO 370 I=1,14 370 WRITE(6,6210) DUOO(I),DE(I),DDEL(I) 6210 FORMAT('0',3F20.8) DO 372 I=12,14 DUOO(I)=DUOO(I)*RPS DE(I)=DE(I)*RPS 372 DDEL(I)=DDEL(I)*RPS WRITE(6,6230) ITER 6230 FORMAT('1',T20,'CONTROL DATA FOR RELINEARIZATION',I4,//,' ',T5, *'POINT',T18,'LATEST APPROXIMATIONS',T58,'RESIDUALS',T84, *'LATEST CORRECTIONS',/,' ',T24,' (MM.) ',T58,' (MM.) ',T88, *' (MM.) ',//) DO 380 I=1,NPON 380 WRITE(6,6240) NUM(I),DDUOO(3*I-2),DDUOO(3*I-1),DDUOO(3*I), *DDE(3*I-2),DDE(3*I-1),DDE(3*I),DDDEL(3*I-2),DDDEL(3*I-1), *DDDEL(3*I) 6240 FORMAT(5X,I5,9F12.3) CALL SIGMA(NPON,X,Y,DUOO,DDUOO,AOO,DDEL,DDDEL,WWW,DW,DDW,DE, *DDE,NPPP,3,2,NPON,16,B1,C1,RJ,DBJ,DDBJ,EJ,VTWV,DO,1,NDF,DZW,L) C C COMPUTE DEL DOT C CALL SIMEQ(DN,DC,DDEL,16,JFLAG) IF(JFLAG.EQ.1) GOTO 540 KTEST=2 C C GO BACK AND BEGIN PROCESSING DEL DOUBLE DOT POINT BY POINT C GOTO 200 C C UPDATE THE PARAMETERS C 340 DO 350 I=1,14 DE(I)=DE(I)+DDEL(I) 350 DUOO(I)=DUOO(I)+DDEL(I) DO 360 I=1,NPON DDUOO(3*I-2)=DDUOO(3*I-2)+DDDEL(3*I-2) DDUOO(3*I-1)=DDUOO(3*I-1)+DDDEL(3*I-1) DDUOO(3*I)=DDUOO(3*I)+DDDEL(3*I) DDE(3*I-2)=DDE(3*I-2)+DDDEL(3*I-2) DDE(3*I-1)=DDE(3*I-1)+DDDEL(3*I-1) 360 DDE(3*I)=DDE(3*I)+DDDEL(3*I) C C CHECK FOR CONVERGENCE OF SOLUTION C RATIO=SIGO/VTWV IF(ABS(RATIO-1).LE.CRIT) GOTO 400 WRITE(6,7010) RATIO,CRIT 7010 FORMAT(5X,'**** CONVERGENT RATIO ****',//,5X,F20.10,/,5X, *'CONVERGENCY CRITICAL VALUE',F20.10) SIGO=VTWV ITER=ITER+1 IF(ITER.GT.MAXIT) GOTO 500 GOTO 160 C C COMPUTE FINAL RESIDUALS AND VARIANCES C 400 KTEST=3 GOTO 170 C C THIS LOOP IS PART OF A CHECK OF THE INVERSION ROUTINE AND C CAN BE OMITTED C 410 DO 381 I=1,14 DO 381 J=1,14 381 SJ(I,J)=DN(I,J) C C COMPUTE THE COVARIANCE MATRIX C CALL DINV(DN,14,JFLAG,16) IF(JFLAG.EQ.1) GOTO 530 C C THIS LOOP IS PART OF A CHECK OF THE INVERSION ROUTINE AND C CAN BE OMITTED C DO 382 I=1,14 DO 382 J=1,14 C2(I,J)=0.0 DO 382 K=1,14 382 C2(I,J)=C2(I,J)*DN(I,K)*SJ(K,J) C C COMPUTE THE CORRELATION COEFFICIENTS BETWEEN THE PARAMETERS C DO 411 I=1,14 DO 411 J=1,14 411 CR(I,J)=DN(I,J)/(DSQRT(DN(I,I)*DN(J,J))) WRITE(6,412) 412 FORMAT(//,10X,'THE CORRELATION COEFFICIENTS BETWEEN THE *PARAMETERS ARE :') DO 413 I=1,14 413 WRITE(6,414) I,(CR(I,J),J=1,I) 414 FORMAT(5X,I5,14(F7.3)) SIGO=VTWV DO 497 I=1,14 DO 497 J=1,14 497 DN(I,J)=DN(I,J)*SIGO DO 498 I=1,14 DO 498 J=1,14 498 WW(L,I,J)=DN(I,J) DO 420 I=1,14 IF(DN(I,I).LT.0.0) GOTO 420 DN(I,I)=DSQRT(DN(I,I)) 420 CONTINUE C C PRINT OUT THE FINAL CAMERA PARAMETERS C WRITE(6,6400) ITER 6400 FORMAT('1',T20,'FINAL CAMERA PARAMETERS AFTER',I3, *' RELINEARIZATIONS',//,'0',T9,'PARAMETERS',T27,'LAST CORRECTION', *T48,'RESIDUALS',T68,'STD. DEV.',/) DUOO(12)=DUOO(12)*SPR DUOO(13)=DUOO(13)*SPR DUOO(14)=DUOO(14)*SPR DN(12,12)=DN(12,12)*SPR DN(13,13)=DN(13,13)*SPR DN(14,14)=DN(14,14)*SPR DO 430 I=1,14 430 WRITE(6,6410) DUOO(I),DDEL(I),DE(I),DN(I,I) 6410 FORMAT('0',4E20.10) DUOO(12)=DUOO(12)*RPS DUOO(13)=DUOO(13)*RPS DUOO(14)=DUOO(14)*RPS DN(12,12)=DN(12,12)*RPS DN(13,13)=DN(13,13)*RPS DN(14,14)=DN(14,14)*RPS C C PRINT OUT FINAL IMAGE COORDINATE RESIDUALS C WRITE(6,6420) ITER 6420 FORMAT('1',T20,'FINAL RESIDUALS AFTER',I3,' RELINEARIZATIONS',//, *T8,'POINT',T18,'X',T30,'Y',T42,'RESX',T54,'RESY',T66,'RES',/,' ', *T16,'(MM.)',T28,'(MM.)',T40,' (MM.)',T52,' (MM.) ',T64, *' (MM.) ',//) EPSXT=0.0 EPSYT=0.0 SUMEX=0.0 SUMEY=0.0 INDX=1 INDY=1 DO 450 I=1,NPON EPS(I)=DSQRT(EPSX(I)**2+EPSY(I)**2) IF(EPSXT.GE.ABS(EPSX(I))) GOTO 447 EPSXT=ABS(EPSX(I)) INDX=I 447 IF(EPSYT.GE.ABS(EPSY(I))) GOTO 448 EPSYT=ABS(EPSY(I)) INDY=I 448 SUMEX=SUMEX+EPSX(I) SUMEY=SUMEY+EPSY(I) 450 WRITE(6,6430) NUM(I),X(I,L),Y(I,L),EPSX(I),EPSY(I),EPS(I) 6430 FORMAT(5X,I5,5F12.4) WRITE(6,6431) SUMEX,SUMEY,EPSX(INDX),EPSY(INDY) 6431 FORMAT('0',5X,'SUM OF RESX = ',F8.4,' MM.',5X,'SUM OF RESY = ', *F8.4,' MM.',//,5X,'MAXIMUM RESX = ',F8.4,' MM. ',5X, *'MAXIMUM RESY = ',F8.4,' MM.',/) C C PRINT OUT FINAL GROUND CONTROL DATA C WRITE(6,6440) 6440 FORMAT('1',T20,'FINAL ADJUSTED GR COORDS AND RESIDUALS',//,'0',T8, *'POINT',T17,'XO',T27,'RESXO',T41,'YO',T49,'RESYO',T66,'ZO',T77, *'RESZO',/,'0',T16,'(MM.)',5(7X,'(MM.)'),//) DO 460 I=1,NPON WRITE(6,6450) NUM(I),DDUOO(3*I-2),DDE(3*I-2),DDUOO(3*I-1),DDE(3* *I-1),DDUOO(3*I),DDE(3*I) 6450 FORMAT(5X,I5,6F12.5) 460 CONTINUE 1001 CONTINUE GOTO 1000 C C THIS SECTION CONTAINS THE ERROR MESSAGES C 500 WRITE(6,6500) 6500 FORMAT('0',T20,'ITERATION LIMIT EXCEEDED - SOLUTION TERMINATING') GOTO 1000 520 WRITE(6,6520) 6520 FORMAT('0',T20,'SINGULAR MATRIX FOR POINT',I5,'SOLUTION',1X, *'TERMINATING') GOTO 1000 530 WRITE(6,6530) 6530 FORMAT('0',T10,'SINGULAR MATRIX-NDOT-ERROR PROPAGATION PHASE-', *'SOLUTION TERMINATING') GOTO 1000 540 WRITE(6,6540) ITER 6540 FORMAT('0',T10,'SINGULAR MATRIX-NDOT-SOLUTION PHASE-ITERATION', *I4,'SOLUTION TERMINATING') GOTO 1000 550 WRITE(6,6550) 6550 FORMAT('0',T10,'TOO MANY POINTS REJECTED TO GIVE SOLUTION') 1000 RETURN END SUBROUTINE SIMEQ(A,B,C,MMM) C C ****************************************************************** C ****************************************************************** C ***** ***** C ***** SUBROUTINE SIMEQ ***** C ***** ***** C ***** THIS SUBROUTINE SOLVES A LINEAR SYSTEM OF EQUATIONS ***** C ***** BY A MODIFIED GAUSS JORDAN ELIMINATION TECHNIQUE. ***** C ***** THE ORIGINAL MATRIX IS DESTROYED IN THE PROCESS. ***** C ***** ***** C ***** S.J. JODOIN ***** C ***** JUNE, 1987 ***** C ***** ***** C ****************************************************************** C ****************************************************************** C IMPLICIT REAL*8(A-H,O-Z) REAL*8 A(MMM,MMM),B(MMM),C(MMM) M=MMM-2 40 DO 7 L=1,M C C PIVOT PICKER FOLLOWS C IF(L-M) 11,12,12 11 LPLUS=L+1 DO 3 K=LPLUS,M IF(ABS(A(K,1))-ABS(A(L,1))) 3,3,1 1 TEMP=B(K) B(K)=B(L) B(L)=TEMP DO 2 J=1,M TEMP=A(K,J) A(K,J)=A(L,J) A(L,J)=TEMP 2 CONTINUE 3 CONTINUE C C ROW L IS THE PIVOTAL ROW C 12 FMN=1.0/A(L,1) DIV=A(L,1) DO 4 J=2,M 4 A(L,J-1)=A(L,J)/DIV A(L,M)=FMN C C PIVOTAL ROW HAS BEEN MODIFIED. NOW MODIFY OTHER ROWS. C DO 7 J=1,M IF(J-L) 5,7,5 5 FMULT=A(J,1) DO 6 K=2,M 6 A(J,K-1)=(A(J,K)-((FMULT)*(A(L,K-1)))) A(J,M)=-FMN*FMULT 7 CONTINUE C C CLEAR THE MATRIX C 70 DO 80 L=1,M 80 C(L)=0.0 C C MULTIPLY THE INVERSE TIMES THE CONSTANT COLUMN. C DO 90 L=1,M DO 90 K=1,M C(L)=C(L)+A(L,K)*B(K) 90 CONTINUE 120 CONTINUE RETURN END SUBROUTINE DINV(A2,M,JFLAG,MM) C C ****************************************************************** C ****************************************************************** C ***** ***** C ***** SUBROUTINE DINV ***** C ***** ***** C ***** THIS SUBROUTINE INVERTS A MATRIX BY A MODIFIED ***** C ***** GAUSS JORDAN TECHNIQUE. THE ORIGINAL MATRIX IS ***** C ***** DESTROYED IN THE PROCESS. ***** C ***** ***** C ***** S.J. JODOIN ***** C ***** JUNE, 1987 ***** C ***** ***** C ****************************************************************** C ****************************************************************** C IMPLICIT REAL*8(A-H,O-Z) REAL*8 A2(MM,MM) JFLAG=0 DO 7 L=1,M IF(ABS(A2(L,1)).LT.1.0E-14) GOTO 8 FMN=1.0E+0/A2(L,1) DIV=A2(L,1) DO 4 J=2,M A2(L,J-1)=A2(L,J)/DIV 4 CONTINUE A2(L,M)=FMN DO 7 J=1,M IF(J-L) 5,7,5 5 FMULT=A2(J,1) DO 6 K=2,M A2(J,K-1)=(A2(J,K)-((FMULT)*(A2(L,K-1)))) 6 CONTINUE A2(J,M)=-FMN*FMULT 7 CONTINUE RETURN 8 JFLAG=1 RETURN END SUBROUTINE SIGMA(NA,X,Y,DUOO,DDUOO,AOO,DDEL,DDDEL,WWW,DW,DDW, *DE,DDE,NE,NRA,NC,NR,NU,B1,C1,RJ,DBJ,DDBJ,EJ,VTWV,DO,IPRINT,NDF, *DZW,LL) C C ****************************************************************** C ****************************************************************** C ***** ***** C ***** SUBROUTINE SIGMA ***** C ***** ***** C ***** THIS SUBROUTINE COMPUTES THE WEIGHTED SUM OF THE ***** C ***** RESIDUALS SQUARED (SIGO**2) AFTER EACH ITERATION ***** C ***** ***** C ***** S.J. JODOIN ***** C ***** JUNE, 1987 ***** C ***** ***** C ****************************************************************** C ****************************************************************** C IMPLICIT REAL*8(A-H,O-Z) REAL*8 DUOO(NU),DDUOO(NE),AOO(NRA,NRA), *DDEL(NU),DDDEL(NE),DW(NU,NU),DDW(NRA,NRA),DE(NU),DDE(NE), *B1(NC),C1(NRA),V(3),RJ(NU),DBJ(NC,NU),DDBJ(NC,NRA),EJ(NC), *DO(NU),DZW(50,NRA,NRA),WWW(NC,NC),X(350,4),Y(350,4) VTWV=0.0E+0 C C THIS LOOP FORMS THE LINEARIZED COLLINEARITY EQUATIONS ASSOCIATED C WITH EACH POINT, COMPUTES THE RESIDUALS AND ADDS THE CONTRIBUTION C TO VTWV. C DO 300 I=1,NA DDW(1,1)=DZW(I,1,1) DDW(2,2)=DZW(I,2,2) DDW(3,3)=DZW(I,3,3) TX=X(I,LL) TY=Y(I,LL) XO=DDUOO(3*I-2) YO=DDUOO(3*I-1) ZO=DDUOO(3*I) C1(1)=DDDEL(3*I-2) C1(2)=DDDEL(3*I-1) C1(3)=DDDEL(3*I) CALL PART(TX,TY,XO,YO,ZO,AOO,DUOO,DO,DBJ,DDBJ,EJ,0,3,2,14,0) DO 100 JJ=1,2 B1(JJ)=0.0 DO 110 II=1,14 110 B1(JJ)=B1(JJ)+DBJ(JJ,II)*DDEL(II) 100 CONTINUE V(1)=-B1(1) V(2)=-B1(2) DO 120 JJ=1,2 B1(JJ)=0.0 DO 130 II=1,3 130 B1(JJ)=B1(JJ)+DDBJ(JJ,II)*C1(II) 120 CONTINUE V(1)=V(1)-B1(1)+EJ(1) V(2)=V(2)-B1(2)+EJ(2) DO 10 L=1,2 DO 10 K=1,2 C C V TRANSPOSE W V C 10 VTWV=VTWV+V(L)*WWW(L,K)*V(K) V(1)=DDE(3*I-2)+C1(1) V(2)=DDE(3*I-1)+C1(2) V(3)=DDE(3*I)+C1(3) 300 CONTINUE C C NOW ADD THE CONTRIBUTIONS OF V DOT C DO 30 I=1,14 30 RJ(I)=DE(I)+DDEL(I) DO 40 J=1,14 DO 40 I=1,14 C C V DOT TRANSPOSE W DOT V DOT C 40 VTWV=VTWV+RJ(J)*DW(J,I)*RJ(I) C C DIVIDE BY THE DEGREES OF FREEDOM C VTWV=VTWV/NDF SIGO=DSQRT(VTWV) IF(IPRINT.EQ.0) GOTO 50 WRITE(6,6000) SIGO 6000 FORMAT('0','**** OUTPUT FROM SIGMA ****',//,' ',2E20.10) 50 RETURN END SUBROUTINE FXFY(AF,A,B,C,TX,TY,DUOO,N,NU,FX,FY,IPRINT) C C ****************************************************************** C ****************************************************************** C ***** ***** C ***** SUBROUTINE FXFY ***** C ***** ***** C ***** THIS SUBROUTINE COMPUTES THE CONSTANT TERM ***** C ***** ASSOCIATED WITH THE COLLINEARITY EQUATION FOR ***** C ***** EACH POINT ***** C ***** ***** C ***** S.J. JODOIN ***** C ***** JUNE, 1987 ***** C ***** ***** C ****************************************************************** C ****************************************************************** C IMPLICIT REAL*8(A-H,O-Z) REAL*8 AF(N,N),S(3),TF(3),TP(2),TA(2),DUOO(NU) TP(1)=TX-DUOO(1) TP(2)=TX-DUOO(2) TEMPR=TP(1)**2+TP(2)**2 TEMPL=TEMPR*TEMPR TEMP=TEMPL*TEMPR TTT=DUOO(4)*TEMPR+DUOO(5)*TEMPL+DUOO(6)*TEMP TTXY=TP(1)*TP(2) TT1=TP(1)*TTT+DUOO(7)*(TEMPR+2.*TP(1)*TP(1))+DUOO(8)*2.*TTXY TT2=TP(2)*TTT+DUOO(7)*TTXY*2.+DUOO(8)*(TEMPR+2.*TP(2)*TP(2)) FX=TP(1)+TT1 FY=TP(2)+TT2 S(1)=A-DUOO(9) S(2)=B-DUOO(10) S(3)=C-DUOO(11) DO 10 I=1,3 TF(I)=0.0E+0 DO 10 J=1,3 10 TF(I)=TF(I)+AF(I,J)*S(J) C C THE CONSTANT TERMS C FX=FX-DUOO(3)*(TF(1)/TF(3)) FY=FY-DUOO(3)*(TF(2)/TF(3)) C C THIS SEGMENT USED FOR PROGRAM DEBUGGING AND MAY BE REMOVED C IF(IPRINT.EQ.0) GOTO 1000 6000 FORMAT('0','**** OUTPUT FROM FXFY ****') WRITE(6,6001) S,TF,FX,FY WRITE(6,6000) 6001 FORMAT(' ',6E20.10) 1000 RETURN END C SUBROUTINE PART(TX,TY,XO,YO,ZO,AOO,DUOO,DO,DBJ,DDBJ,EJ,IPRT,NN, *NR,NC,IPRINT) C C ****************************************************************** C ****************************************************************** C ***** ***** C ***** SUBROUTINE PART ***** C ***** ***** C ***** THIS SUBROUTINE COMPUTES THE PARTIAL DERIVATIVES ***** C ***** ASSOCIATED WITH THE COLLINEARITY EQUATIONS FOR ***** C ***** EACH POINT ***** C ***** ***** C ***** S.J. JODOIN ***** C ***** JUNE, 1987 ***** C ***** ***** C ****************************************************************** C ****************************************************************** C IMPLICIT REAL*8(A-H,O-Z) REAL*8 AA(3,3),AOO(NN,NN),FP(3),AV(2,2),DO(NC),DUOO(NC), *DDBJ(NR,NN),EJ(NR),DBJ(NR,NC) DA=.1E00 C C COMPUTE THE EPSILON VECTOR C CALL FXFY(AOO,XO,YO,ZO,TX,TY,DUOO,3,14,FXO,FYO,0) EJ(1)=-FXO EJ(2)=-FYO FP(1)=TX FP(2)=TY C C BUILD THE B DOT MATRIX FOR POINT J C DO 20 I=1,11 DUOO(I)=DUOO(I)+DO(I) CALL FXFY(AOO,XO,YO,ZO,TX,TY,DUOO,3,14,FX,FY,0) DBJ(1,I)=(FX-FXO)/DO(I) DBJ(2,I)=(FY-FYO)/DO(I) DUOO(I)=DUOO(I)-DO(I) 20 CONTINUE DO 30 I=12,14 DUOO(I)=DUOO(I)+DO(I) CALL OMAT1(DUOO(12),DUOO(13),DUOO(14),AA,3,0) CALL FXFY(AA,XO,YO,ZO,TX,TY,DUOO,3,14,FX,FY,0) DBJ(1,I)=(FX-FXO)/DO(I) DBJ(2,I)=(FY-FYO)/DO(I) DUOO(I)=DUOO(I)-DO(I) 30 CONTINUE FP(1)=XO FP(2)=YO FP(3)=ZO C C BUILD THE B DOUBLE DOT MATRIX FOR POINT J C DO 40 I=1,3 FP(I)=FP(I)+DA CALL FXFY(AOO,FP(1),FP(2),FP(3),TX,TY,DUOO,3,14,FX,FY,0) DDBJ(1,I)=(FX-FXO)/DA DDBJ(2,I)=(FY-FYO)/DA FP(I)=FP(I)-DA 40 CONTINUE C C THIS SEGMENT USED FOR PROGRAM DEBUGGING. THESE CARDS C MAY BE REMOVED (UP TO THE RETURN STATEMENT) C IF(IPRINT.EQ.0) GOTO 50 WRITE(6,6000) 6000 FORMAT('0','**** OUTPUT FROM PART ****') WRITE(6,6950) AV WRITE(6,6950) (DBJ(1,K),K=1,13) WRITE(6,6950) (DBJ(2,K),K=1,13) WRITE(6,6950) DDBJ WRITE(6,6950) EJ 6950 FORMAT(' ',6E20.10) 50 RETURN END C SUBROUTINE OMAT1(O,F,C,AF,N,IPRINT) C C ****************************************************************** C ****************************************************************** C ***** ***** C ***** SUBROUTINE OMAT1 ***** C ***** ***** C ***** THIS SUBROUTINE BUILDS THE THREE DIMENSIONAL ***** C ***** ROTATION MATRIX ***** C ***** ***** C ***** S.J. JODOIN ***** C ***** JUNE, 1987 ***** C ***** ***** C ****************************************************************** C ****************************************************************** C IMPLICIT REAL*8(A-H,O-Z) REAL*8 AF(N,N) SO=SIN(O) SF=SIN(F) SC=SIN(C) CO=COS(O) CF=COS(F) CC=COS(C) T1=SO*SF T2=CO*SF AF(1,1)=CF*CC AF(2,1)=-CF*SC AF(3,1)=SF AF(1,2)=CO*SC+T1*CC AF(2,2)=CO*CC-T1*SC AF(3,2)=-CF*SO AF(1,3)=SO*SC-T2*CC AF(2,3)=SO*CC+T2*SC AF(3,3)=CF*CO IF(IPRINT.EQ.0) GOTO 1000 1000 RETURN END