C DLTC.FOR, Woltring 1988-11-04 (1991-08-16) C C*********************************************************************** C C DLTC C C Purpose: C ******* C C To calibrate a camera in terms of the Direct Linear Transfor- C mation, for accurate or noisy landmark coordinates and noisy C image observations, with optional constraints on the internal C camera parameters. C C Calling convention: C ****************** C C CALL DLTC (XYZ,XYI,NP, VARP,VARI,IC,XY0C, DLT,DU,SDRES, IT ) C C Meaning of parameters: C ********************* C C XYZ(3,NP) [ I ] Object-space coordinates, ordered as C X,Y,Z per point. C XYI (2,NP) [ I ] Image coordinates, ordered as x,y; C image points whose absolute coordinate(s) C are >= ABIG are ignored. C NP [ I ] Number of points (>= 6). C VARP(3) [ I ] Variances of object-space coordinates, C ordered as XYZ. C VARI(2) [ I ] Variances of image coordinates, ordered C as XYI. C IC [ I ] Constraint control parameter: C IC <= 0: no constraints C IC >= 1: set ALPHA == 0 C IC >= 2: fix X0 and Y0 to input values C IC = 3: set Cx == Cy C IC >= 4: fix Cx and Cy to input values C XY0C(2,2) [ I ] Prior values for X0 and Y0 (column 1) C and for Cx and Cy (column 2) C DLT(11) [I/O] DLT-parameters (see model below). C DU(11,11) [ O ] DU-factorized information matrix of DLT C SDRES [ O ] Residual standard deviation, C cov(DLT) = SDRES**2 * (U' * D * U)**-1. C If the input variances VARP and VARI are C correct, the expected value for SDRES is C one; if the VARP elements are zero, and C if the VARI elements are proportional to C the noise variances per image axis, the C square of the expected value of SDRES C provides the proportionality factor, as- C suming suitably chosen values for the C constraints and absence of nonlinear C image distortion. C IT [I/O] On INPUT: initialization switch. If the C XYZ distribution is truely spatial, C perspective projection should allow proper C running of the package at IT = 0, unless C the noise level is too high. C If 1.le.IABS(IT).le.3, IABS(IT) indicates C non-significant variability in the relevant C co-ordinate of the control points, while the C sign of IT indicates the polarity of a C corresponding camera position co-ordinate C w.r.t. the centre of the XYZ distribution C (IABS(IT) = 1 - X, 2 - Y, 3 - Z). C If IABS(IT).gt.3, DLT on input is taken to C contain suitable, initial guesses. C On OUTPUT: error parameter. If > 0, num- C ber of iterations until convergence; if C = 0, convergence failure (max=ITM, see C below); if < 0, underdeterminacy or C negative weights. C C Mathematical model: C ****************** C C fx = d1*X + d2*Y + d3*Z + d4 - x * ( d9*X + d10*Y + d11*Z + 1) ~ 0 C fy = d5*X + d6*Y + d7*Z + d8 - y * ( d9*X + d10*Y + d11*Z + 1) ~ 0 C C Initial estimates: see description for IT above. C C C Weight matrix: optimal in the Gauss-Markov sense, assuming zero- C mean, uncorrelated noise. C C Nota Bene: C ********* C C (1) The projection of the camera station's position vector onto C the camera's optical axis should not vanish, and the control C point distribution should be three-dimensional. In the case of C a "minimal" distribution, no three points should be collinear. C C (2) The scaling factor SCL has been included to accomodate over- C and underflow problems in subroutine GIVLSQ, in particular for C floating point hardware with limited dynamic range. If underflow C is found to occur, SCL should be increased; in case of overflow, C SCL should be reduced. Otherwise, its value should have no effect C on the I/O of DLTC. C C Subprogrammes called: C C MATCLR, MATCOP, MATSCL, MATSUM, C DINPRD, GIVENS, GIVLSQ, SLVGIV C C*********************************************************************** C SUBROUTINE DLTC(XYZ,XYI,NP,VARP,VARI,IC,XY0C, DLT,DU,SDRES,IT) C PARAMETER (ITM=10, ABIG=1E36, SCL=1E10) DIMENSION XYZ(3,NP),XYI(2,NP),VARP(3),VARI(2),XY0C(2,2), 1 DLT(11),DU(11,11),DA(11),ROW(4,3,5),FN(2,3),DW(2,2) EQUIVALENCE (RHO,DW(1,2)) SQ(X) = X * X C C*** Initialize and check degrees of freedom C IF ( IABS(IT).le.3) CALL MATCLR(11,1,DLT,11) IF ((IABS(IT).ne.0).and.(IABS(IT).le.3)) 1 DLT(IABS(IT)+8) = -ISIGN(1,IT) IT = -2 IF ((VARP(1).lt.0.0).or.(VARP(2).lt.0.0).or.(VARP(3).lt.0.0)) 1 GO TO 110 !Bad object-space weight factors NDF = -11 IF ((VARI(1).gt.0.0).and.(VARI(2).gt.0.0)) NDF = NDF + 2*NP DO 5 IP=1,NP IF (AMAX1(ABS(XYI(1,IP)),ABS(XYI(2,IP))).ge.ABIG) 1 NDF = NDF - 2 5 CONTINUE C C*** Constraints preparation C IF (IC.ge.1) THEN C*** ALPHA == 0.0 NDF = NDF + 1 X0Y0 = XY0C(1,1) * XY0C(2,1) TX0Y0M = -2.0 * X0Y0 ENDIF IF (IC.ge.2) THEN C*** Prior X0 and Y0 NDF = NDF + 2 TX0M = -2.0 * XY0C(1,1) TY0M = -2.0 * XY0C(2,1) ENDIF IF (IC.eq.3) THEN C*** Cx == Cy NDF = NDF + 1 X0Y0SQ = SQ(XY0C(1,1)) - SQ(XY0C(2,1)) TXY0SM = -2.0 * X0Y0SQ ENDIF IF (IC.ge.4) THEN C*** Prior Cx and Cy NDF = NDF + 2 X0CXSQ = SQ(XY0C(1,1)) + SQ(XY0C(1,2)) TXCXSM = -2.0 * X0CXSQ Y0CYSQ = SQ(XY0C(2,1)) + SQ(XY0C(2,2)) TYCYSM = -2.0 * Y0CYSQ ENDIF C*** (Over)determinacy? IF (NDF.lt.0) GO TO 120 !Bad image weight factors or C !indeterminacy (IT = -2) C C*** Iterate at most ITM times C DO 90 IT=1,ITM CALL MATCLR(11, 1,DA,11) !DLT correction vector CALL MATCLR(11,11,DU,11) !DU-factorized information matrix SDRES = 0.0 C*** Process all points DO 70 IP=1,NP IF (AMAX1(ABS(XYI(1,IP)),ABS(XYI(2,IP))).ge.ABIG) GO TO 70 T3 = DINPRD(1,3,XYZ(1,IP),1,DLT(9),1,1.0) T3SQ = SQ(T3) C*** Process x and y DO 20 J=1,2 K = 3 - J JJ = 4 * (J - 1) DW(J,J) = VARI(J) * T3SQ !Inverse weight matrix DW(K,J) = 0.0 ! for camera noise only TJ = DINPRD(1,3,XYZ(1,IP),1,DLT(JJ+1),1,DLT(JJ+4)) CALL MATCOP(3,1,XYZ(1,IP),3,ROW(1,J,J),3) ROW(4,J,J) = 1.0 CALL MATCLR(4,1,ROW(1,K,J),4) XY = XYI(J,IP) CALL MATSCL(3,1,XYZ(1,IP),3,-XY,ROW(1,3,J),3) ROW(4,3,J) = XY * T3 - TJ !Right-hand side C*** Calculate landmark noise gain terms DO 10 K=1,3 IF (VARP(K).ne.0.0) FN(J,K) = DLT(JJ+K) - XY * DLT(8+K) 10 CONTINUE 20 CONTINUE C*** Adjoin landmark noise terms to weight matrix DO 30 K=1,3 W = VARP(K) IF (W.ne.0.0) CALL GIVENS(FN(1,K),DW,2,W) 30 CONTINUE C*** Decorrelate equations IF (RHO.NE.0.0) THEN DO 50 J=1,3 DO 40 K=1,4 ROW(K,J,2) = ROW(K,J,2) - RHO * ROW(K,J,1) 40 CONTINUE 50 CONTINUE ENDIF C*** Rotate equations DO 60 J=1,2 W = SCL / DW(J,J) !scale to prevent GIVLSQ underflow SDRES = SDRES + W * SQ(ROW(4,3,J)) CALL GIVLSQ(ROW(1,1,J),DU,DA,1,11,1,W,W,W,.FALSE.) 60 CONTINUE 70 CONTINUE !End of "all points" C*** Assess constraint equations IF (IC.gt.0) CALL MATCLR(12,5,ROW,12) IF (IC.ge.1) THEN C*** ALPHA == 0 T0 = DLT(1)*DLT(5) + DLT(2)*DLT( 6) + DLT(3)*DLT( 7) T1 = DLT(1)*DLT(9) + DLT(2)*DLT(10) + DLT(3)*DLT(11) T2 = DLT(5)*DLT(9) + DLT(6)*DLT(10) + DLT(7)*DLT(11) T3 = SQ(DLT(9)) + SQ(DLT(10)) + SQ(DLT(11)) IF (IC.eq.1) THEN C*** ALPHA == 0.0 for current X0 and Y0 estimates DO 72 J=1,3 ROW(J,1,1) = T3*DLT(J+4) - T2*DLT(J+8) ROW(J,2,1) = T3*DLT(J ) - T1*DLT(J+8) ROW(J,3,1) = 2.0*T0*DLT(J+8) 1 - (T2*DLT(J ) + T1*DLT(J+4)) 72 CONTINUE ROW(4,3,1) = T1*T2 - T0*T3 ELSE C*** ALPHA == 0.0 for prior X0 and Y0 values CALL MATCOP(3,1,DLT(5),3,ROW ,4) CALL MATCOP(3,1,DLT ,3,ROW(1,2,1),4) CALL MATSCL(3,1,DLT(9),3,TX0Y0M,ROW(1,3,1),4) ROW(4,3,1) = X0Y0*T3 - T0 ENDIF ENDIF IF (IC.ge.2) THEN C*** Prior X0 CALL MATCOP(3,1,DLT(9),3,ROW(1,1,2),4) CALL MATSCL(3,1,DLT(9),3,TX0M,ROW(1,3,2),4) CALL MATSUM(3,1,DLT ,3,ROW(1,3,2),4,ROW(1,3,2),4) ROW(4,3,2) = XY0C(1,1) * T3 - T1 C*** Prior Y0 CALL MATCOP(3,1,DLT(9),3,ROW(1,2,3),4) CALL MATSCL(3,1,DLT(9),3,TY0M,ROW(1,3,3),4) CALL MATSUM(3,1,DLT(5),3,ROW(1,3,3),4,ROW(1,3,3),4) ROW(4,3,3) = XY0C(2,1) * T3 - T2 ENDIF IF (IC.eq.3) THEN C*** Cx == Cy CALL MATSCL(3,1,DLT ,4, 2.0,ROW(1,1,4),4) CALL MATSCL(3,1,DLT ,4, -2.0,ROW(1,2,4),4) CALL MATSCL(3,1,DLT(9),3,TXY0SM,ROW(1,3,4),4) ROW(4,3,4) = X0Y0SQ * T3 - 1 ( SQ(DLT(1)) + SQ(DLT(2)) + SQ(DLT(3)) ) + 2 ( SQ(DLT(5)) + SQ(DLT(6)) + SQ(DLT(7)) ) ENDIF IF (IC.ge.4) THEN C*** Prior Cx CALL MATSCL(3,1,DLT ,4, 2.0,ROW(1,1,4),4) CALL MATSCL(3,1,DLT(9),3,TXCXSM,ROW(1,3,4),4) ROW(4,3,4) = X0CXSQ * T3 - 1 ( SQ(DLT(1)) + SQ(DLT(2)) + SQ(DLT(3)) ) C*** Prior Cy CALL MATSCL(3,1,DLT(5),4, 2.0,ROW(1,2,5),4) CALL MATSCL(3,1,DLT(9),3,TYCYSM,ROW(1,3,5),4) ROW(4,3,5) = Y0CYSQ * T3 - 1 ( SQ(DLT(5)) + SQ(DLT(6)) + SQ(DLT(7)) ) ENDIF C*** Adjoin all constraint equations IF (IC.gt.0) THEN WW = 0.0 DO 75 J=1,11 WW = AMAX1(WW,DU(J,J)) 75 CONTINUE DO 80 J=1,5 W = 1E6 * WW !Empirical CALL GIVLSQ(ROW(1,1,J),DU,DA,1,11,1,W,W,W,.FALSE.) 80 CONTINUE ENDIF C*** Solve and adjust equations, unless convergence attained SDRES = SDRES / SCL / MAX0(1,NDF) !residual variance SDNRM = DINPRD(1,11,DA,1,DA,1,0.0) IF (IT.gt.1).and.((SDNRM.le.11E-4*SDRES)) GO TO 100 CALL SLVGIV(DU,DA,DA,11,1) CALL MATSUM(11,1,DLT,11,DA,11,DLT,11) 90 CONTINUE !End of iteration C C*** Ready C IT = 0 !Convergence failure 100 SDRES = SQRT(SDRES) GO TO 130 110 IT = -1 !Bad image weight factors or indeterminacy 120 SDRES = 0.0 C C** Scale DU and SDRES back to normal C 130 IF (SCL.ne.1.0) THEN DO 140 I=1,11 DU(I,I) = DU(I,I) / SCL 140 CONTINUE ENDIF RETURN END C CNVDLT, WOLTRING 1988-10-31 C C*********************************************************************** C C CNVDLT C C PURPOSE: C ******* C C Conversion from conventional photogrammetric prameters to DLT C parameters. C C Calling convention: C ****************** C C CALL CNVDLT ( CNV, DLT ) C C Meaning of parameters: C ********************* C C CNV(11) [ I ] Input array containing the 11 conventional C parameters, ordered as: C CNV( 1) ... CNV( 3): Camera position in object- C space coordinates C CNV( 4) ... CNV( 6): Camera attitude angles in C radians (see subr. PHIROT) C CNV( 7) ... CNV( 8): Principal point (X0,Y0)' C CNV( 9) ... CNV(10): Principal distances (CX,CY)' C CNV(11): Image shearing factor ALPHA C C DLT(11) [ O ] Output array containing the 11 DLT parameters C C Mathematical model: C ****************** C C The DLT, or Direct Linear Transformation equations are: C C Xi = ( D1*Xp + D2 *Yp + D3 *Zp + D4 ) / DEN C Yi = ( D5*Xp + D6 *Yp + D7 *Zp + D8 ) / DEN C DEN = D9*Xp + D10*Yp + D11*Zp + 1.0 C C (See G.T. Marzan & H.M. Karara, Proc. Symp. on Close-Range C Photogrammetric Systems, pp. 420-476: American Society of C Photogrammetry, Falls Church, Virginia/USA 1975) C C The conventional collinearity equations are: C C (Xi - X0)/Cx + R1/R3 = 0 C ALPHA*(Xi - X0)/Cx + (Yi - Y0)/Cy + R2/R3 = 0 C C with C C Ri = ROT(i,1)*(Xp - Xc) + ROT(i,2)*(Yp - Yc) + ROT(i,3)*(Zp - Zc) C [i=1,2,3] C C (Xi,Yi)' image coordinates C (Xp,Yp,Zp)' object space target coordinates C ROT(i,j) camera attitude matrix C C Nota Bene: C ********* C C The denominator normalization in the DLT-equations (i.e., D12 = C = 1.0) follows by normalizing with respect to the term C C ROT(3,1)*Xc + ROT(3,2)*Yc + ROT(3,3)*Zc C C which, therefore, should not vanish. This term is equal to the C projection of the camera position vector (Xc,Yc,Zc)' onto the C camera's optical (or Z-) axis. By choosing the origin of object C space somewhere in the camera's field of view, this singularity C condition can be avoided. C C Subprogrammes called: C ******************** C C PHIROT, PRAB C C*********************************************************************** C SUBROUTINE CNVDLT ( CNV, DLT ) C DIMENSION CNV(11), DLT(11), ROT(3,3), PAR(3,3), B(3,4) C PAR(1,1) = CNV( 9) ! Cx PAR(1,2) = 0.0 PAR(1,3) = -CNV( 7) !-X0 PAR(2,1) = -CNV(11)*CNV(10) !-ALPHA*Cy PAR(2,2) = CNV(10) ! Cy PAR(2,3) = -CNV( 8) !-Y0 PAR(3,1) = 0.0 PAR(3,2) = 0.0 PAR(3,3) = -1.0 CALL PHIROT(CNV(4),ROT,1) CALL PRAB(3,3,3,PAR,3,ROT,3,B,3) CALL PRAB(3,3,1,B,3,CNV,3,B(1,4),3) B(1,4) = -B(1,4) B(2,4) = -B(2,4) SC = -B(3,4) K = 0 DO 20 I=1,3 DO 10 J=1,4 K = K + 1 IF (K.ne.12) DLT(K) = B(I,J) / SC 10 CONTINUE 20 CONTINUE C RETURN END C DLTCNV, WOLTRING 1988-10-29 C C*********************************************************************** C C DLTCNV C C PURPOSE: C ******* C C Conversion from DLT prameters to conventional photogrammetric C parameters. C C Calling convention: C ****************** C C CALL DLTCNV ( DLT, CNV, LREFL ) C C Meaning of parameters: C ********************* C C DLT(11) [ I ] Input array containing the 11 DLT parameters C C CNV(11) [ O ] Output array containing the 11 conventional C parameters, ordered as: C CNV( 1) ... CNV( 3): Camera position in object- C space coordinates C CNV( 4) ... CNV( 6): Camera attitude angles in C radians (see subr. PHIROT) C CNV( 7) ... CNV( 8): Principal point (X0,Y0)' C CNV( 9) ... CNV(10): Principal distances (CX,CY)' C CNV(11): Image shearing factor ALPHA C C LREFL [ I ] Logical flag: in the case of mirrored obser- C vations, LFLAG = .TRUE., and Cx is set < 0; C in the opposite case, Cx is set > 0; Cy is C always > 0. C C Mathematical model: C ****************** C C The DLT, or Direct Linear Transformation equations are: C C Xi = ( D1*Xp + D2 *Yp + D3 *Zp + D4 ) / DEN C Yi = ( D5*Xp + D6 *Yp + D7 *Zp + D8 ) / DEN C DEN = D9*Xp + D10*Yp + D11*Zp + 1.0 C C (See G.T. Marzan & H.M. Karara, Proc. Symp. on Close-Range C Photogrammetric Systems, pp. 420-476: American Society of C Photogrammetry, Falls Church, Virginia/USA 1975) C C The conventional collinearity equations are: C C (Xi - X0)/Cx + R1/R3 = 0 C ALPHA*(Xi - X0)/Cx + (Yi - Y0)/Cy + R2/R3 = 0 C C with C C Ri = ROT(i,1)*(Xp - Xc) + ROT(i,2)*(Yp - Yc) + ROT(i,3)*(Zp - Zc) C [i=1,2,3] C C (Xi,Yi)' image coordinates C (Xp,Yp,Zp)' object space target coordinates C ROT(i,j) camera attitude matrix C C Nota Bene: C ********* C C The denominator normalization in the DLT-equations (i.e., D12 = C = 1.0) follows by normalizing with respect to the term C C ROT(3,1)*Xc + ROT(3,2)*Yc + ROT(3,3)*Zc C C which, therefore, should not vanish. This term is equal to the C projection of the camera position vector (Xc,Yc,Zc)' onto the C camera's optical (or Z-) axis. By choosing the origin of object C space somewhere in the camera's field of view, this singularity C condition can be avoided. C C Subprogrammes called: C ******************** C C ROTPHI, VECPRD C C*********************************************************************** C SUBROUTINE DLTCNV ( DLT, CNV, LREFL ) C IMPLICIT LOGICAL (L) DIMENSION DLT(11), CNV(11), ROT(3,3) C C*** Interior camera parameters C DA = DD(DLT(9),DLT(9)) IF (DA.eq.0.0) STOP 'DLTCNV: singularity' DA = 1.0 / DA CNV( 7) = DD(DLT(1),DLT(9)) * DA !X0 CNV( 8) = DD(DLT(5),DLT(9)) * DA !Y0 CNV( 9) = SQRT(DD(DLT(1),DLT(1)) * DA - CNV(7) * CNV(7) ) !Cx IF (LREFL) CNV(9) = -CNV(9) CNV(11) = ( CNV(7)*CNV(8) - DD(DLT(1),DLT(5))*DA ) / CNV(9) CNV(10) = SQRT( DD(DLT(5),DLT(5)) * DA 1 - CNV(8) * CNV(8) - CNV(11) * CNV(11) ) !Cy CNV(11) = CNV(11) / CNV(10) !alpha C C*** Exterior camera parameters C DA = SQRT(DA) DACX = DA / CNV( 9) DACY = DA / CNV(10) DO 10 I=1,3 J = I + 8 ROT(1,I) = DACX * (CNV(7) * DLT(J) - DLT(I )) ROT(2,I) = DACY * (CNV(8) * DLT(J) - DLT(I+4)) + 1 CNV(11) * ROT(1,I) ROT(3,I) = DA * DLT(J) 10 CONTINUE CALL VECPRD(ROT(1,2),ROT(1,3),CNV(4)) IF (DD(CNV(4),ROT).LE.0.0) THEN DO 30 I=1,3 DO 20 J=1,3 ROT(I,J) = -ROT(I,J) 20 CONTINUE 30 CONTINUE DA = -DA DACX = -DACX DACY = -DACY ENDIF C Position vector CNV(4) = (DLT(4) - CNV(7)) * DACX CNV(5) = (DLT(8) - CNV(8)) * DACY + CNV(11)*CNV(4) CNV(6) = -DA DO 50 I=1,3 CNV(I) = DD(ROT(1,I),CNV(4)) 50 CONTINUE C Attitude angles CALL ROTPHI(ROT,CNV(4)) C C*** Ready C RETURN END C REAL*4 FUNCTION DD(A,B) DIMENSION A(3),B(3) C DD = A(1)*DBLE(B(1)) + A(2)*DBLE(B(2)) + A(3)*DBLE(B(3)) C RETURN END C DLTPOS.FOR, Woltring 1989-03-08 (MS-DOS Fortran 1991-08-08) C C*********************************************************************** C C DLTPOS C C Purpose C ******* C C Iterative, minimum-variance procedure to reconstruct the 3-D C coordinates X, Y, Z of a point for given image coordinates C x, y and DLT-parameters, based on the general DLT-equations C C x = (A1*X + A2*Y + A3*Z + A4) / (A9*X + A10*Y + A11*Z + 1) C y = (A5*X + A6*Y + A7*Z + A8) / (A9*X + A10*Y + A11*Z + 1) C C under the assumption that the DLT-parameters are error free. C C Calling convention C ****************** C C CALL DLTPOS(XY,MXY,W,DLT,MDLT,NC,XYZ,SDRES) C C Meaning of parameters (default parameter types) C *********************************************** C C XY(MXY,NC) (I) - Image coordinates, ordered as X, Y C MXY (I) - First dimension of XY (MXY.ge.2) C W(NC) (I) - Weight factors for data; if W(IC).le.0.0), C the corresponding observation is ignored C DLT(MDLT,NC) (I) - DLT-parameters C MDLT (I) - First dimension of DLT (MDLT.ge.11) C NC (I) - Number of camera's (NC.ge.2) C XYZ(3) (O) - Estimated position upon return C SDRES (O) - Residual standard deviation upon return C C Notes C ***** C C The constant 1 in the denominators of the DLT equations implies C that the projection of the camera's position vector onto the C camera's optical axis should not vanish. Since the origin of C object space is usually selected in the field of view, this C condition is easily avoided. C C On entry, the weight factors should nominally be equal to one, C for proper d.o.f. assessment in residual s.d. evaluation. If C some (x,y) pair is thought to be N times less precise than the C nominal value, the corresponding weight factor should be set C to 1/N^2. C C Upon proper return, SDRES > 0.0; upon erroneous return, SDRES C will be < 0.0, and XYZ is set to zero. See the source code for C the meaning of these error codes (-1.0 ... -6.0). C C Subprograms called: C ****************** C C MATCLR, GIVLSQ, SLVGIV C C*********************************************************************** C SUBROUTINE DLTPOS( XY, MXY, W, DLT, MDLT, NC, XYZ, SDRES ) PARAMETER ( NT=20 ) DIMENSION XY(MXY,NC), W(NC), DLT(MDLT,NC), XYZ(3), 1 DU(3,4), DA(3), ROW(4) EQUIVALENCE ( DA, DU(1,4) ) C C*** Initialize and check parameters C CALL MATCLR(3,1,XYZ,3) SDRES = 0.0 IF ( MXY.lt. 2) SDRES = -1.0 !MXY too small IF (MDLT.lt.11) SDRES = -2.0 !MDLT too small IF ( NC.lt. 2) SDRES = -3.0 !NC too small IF (SDRES.lt.0.0) RETURN C C*** Iterate until convergence or NT times C DO 100 IT=1,NT CALL MATCLR(3,4,DU,3) SDRES = 0.0 WT = -3.0 C*** Iterate over all camera's with valid observations DO 90 IC=1,NC IF (W(IC).gt.0.0) THEN DEN = DINPRD(1,3,DLT(9,IC),1,XYZ,1,1.0) IF (DEN.eq.0.0) THEN SDRES = -4.0 !Singularity condition in DLT GO TO 110 ENDIF WW = W(IC) / (DEN*DEN) WT = WT + 2.0*W(IC) DO 80 I=1,2 !x, y XYI = XY(I,IC) IXY = 4*(I - 1) DO 70 J=1,3 !p.d.'s to X, Y, Z ROW(J) = DLT(J+IXY,IC) - XYI*DLT(J+8,IC) 70 CONTINUE ROW(4) = DEN*XYI - !r.h.s. 1 DINPRD(1,3,DLT(1+IXY,IC),1,XYZ,1,DLT(4+IXY,IC)) WI = WW SDRES = SDRES + WW*ROW(4)*ROW(4) CALL GIVLSQ( ROW, DU, DA, 1, 3, 1, WI, WI, WI, 1 .FALSE. ) 80 CONTINUE ENDIF 90 CONTINUE IF (WT.le.0.0) THEN SDRES = -5.0 !Underdeterminacy GO TO 110 ELSE C*** Test on normal exit and add adjustments SDRES = SQRT(SDRES/WT) RHS = SQRT(DINPRD(1,3,DA,1,DA,1,0.0)/3.0) IF ((IT.gt.1).and.(RHS.le.1E-3*SDRES)) GO TO 110 CALL SLVGIV(DU,DA,DA,3,1) CALL MATSUM(1,3,DA,1,XYZ,1,XYZ,1) ENDIF D TYPE 700, IT, SDRES, SQRT(DINPRD(1,3,DA,1,DA,1,0.0)) D 700 FORMAT(' Debug DLTPOS: IT=',I2,',SDres=',1PE11.4,',ADJrms=', D 1 E11.4) 100 CONTINUE C*** No convergence SDRES = -6.0 CALL MATCLR(3,1,XYZ,3) C 110 RETURN END C C****************************************************************************** C C DISP3D.FOR, Woltring 1983-02-06 (text expanded 1991-07-16) C C PURPOSE: C ******* C C to calculate the translation vector, pure rotation matrix, and optional C scaling factor in the 3-D equiform transformation, based upon observed C coordinates Y in the second coordinate system of at least three non-co- C linear markers with known coordinates X in the first coordinate system. C C MATHEMATHICAL MODEL: C ******************* C C Y(I) + V(I) = S * R * X(I) + P; I = 1,...,N>=3. C C with: X(I) The 3-D coordinate vector of the I-th marker with respect C to the first coordinate system C Y(I) the 3-D coordinate vector of the I-th marker as observed C in the second coordinate system C V(I) error term in the observed Y(I), modeled as additive C S Optional scale factor between the two coordinate systems C R Pure rotation matrix between the two coordinate systems C P Translation vector of the origin of the second system C with respect to the first system C C DESCRIPTION: C *********** C C For the given X(I) and Y(I), those values for P, R, and possibly S C are estimated for which the sum of squared residuals SUM[V(I)'V(I)] C is minimized. If S is nonpositive on input, the optimal value for S C is estimated; if S is positive on input, this value is used. On out- C put, DET(R) = +1, and the sign of S reflects the similarity between C the two coordinate systems. For coplanar distributions in X and/or C Y, S is taken > 0. If S = 0.0 on output, either the X- or Y-coordi- C nates, or both, ar colinear, signifying algorithm failure. C C CALLING CONVENTION: C ****************** C C CALL DISP3D(X,Y,N,P,R,S) C C MEANING OF PARAMETERS: C ********************* C C X(3,N) - Input array containing known coordinates w.r.t. the first C coordinate system C Y(3,N) - Input array containing observed coordinates w.r.t. the C second coordinate system C N - Input variable: number of marker coordinates >= 3 C P(3) - Output array containing the translation vector of the C first coordinate system's origin w.r.t. the second C coordinate system C R(3,3) - Output array containing the rotation matrix relating the C first coordinate system to the second coordinate system C S - Input/output variable. If nonpositive on input, the C estimated scaling factor is returned; if positive on C input, its value is left unmodified and used as an a C priori scaling factor. If a zero value is returned, C either the x- or y-coordinates, or both, are colinear. C C C REFERENCES: C ********** C C (1) Tienstra, M. (1969), A method for the calculation of orthogonal C rotation matrices and its application in photogrammetry and other C disciplines. Ph.D.-thesis, Technical University of Delft, The C Netherlands. Waltman, Delft. C C (2) Woltring, H.J. (1981), On definition and calculus of rigid-body C kinematics using spatial marker coordinates. Submitted for publi- C cation to Journal of Biomechanics. [Ultimately published as Veld- C paus et al., Journal of Biomechanics, January 1988.] C C****************************************************************************** C SUBROUTINE DISP3D(X,Y,N,P,R,S) C IMPLICIT REAL*8 (D), LOGICAL (L) DIMENSION X(3,N),Y(3,N),P(3),R(3,3),AVX(3), 1 DT(3,3),DD(3,3),DTADJ(3,3) ICYCL(I) = MOD(I,3) + 1 DSQ(D) = D * D C C*** Calculate mean values X0, Y0, and DT matrix C IF (N.LT.3) GO TO 200 DO 20 I=1,3 DSX = 0D0 DSY = 0D0 DO 10 K=1,N DSX = DSX + X(I,K) DSY = DSY + Y(I,K) 10 CONTINUE AVX(I) = DSX/N !X0 P(I) = DSY/N !Y0 20 CONTINUE C DSS = TRACE ( [X(I) - X0].[X(I) - X0]' ) DSS = 0.0 DO 40 I=1,3 DSX = AVX(I) DO 30 K=1,N DSS = DSS + DSQ(X(I,K) - DSX) 30 CONTINUE 40 CONTINUE IF (DSS.EQ.0D0) GO TO 200 C DT = [Y(I) - Y0].[X(I) - X0]' / DSS DO 70 I=1,3 DSX = AVX(I) DO 60 J=1,3 DSY = P(J) D1 = 0D0 DO 50 K=1,N D1 = D1 + (Y(J,K) - DSY) * (X(I,K) - DSX) 50 CONTINUE DT(J,I) = D1 / DSS 60 CONTINUE 70 CONTINUE C C*** Calculate DD, DT ADJOINT, and principal minor sums C C DD = DT'DT DO 100 I=1,3 DO 90 J=1,I DSS = 0D0 DO 80 K=1,3 DSS = DSS + DT(K,I)*DT(K,J) 80 CONTINUE DD(I,J) = DSS IF (I.NE.J) DD(J,I) = DSS 90 CONTINUE 100 CONTINUE C DTADJ and principal minor sums D1, D2, D3 D1 = DD(1,1) + DD(2,2) + DD(3,3) D2 = 0D0 DO 120 I1=1,3 J1 = ICYCL(I1) K1 = ICYCL(J1) DO 110 I2=1,3 J2 = ICYCL(I2) K2 = ICYCL(J2) DTADJ(I2,I1) = DT(J1,J2)*DT(K1,K2) - DT(J1,K2)*DT(K1,J2) 110 CONTINUE D2 = D2 + (DD(J1,J1)*DD(K1,K1) - DD(J1,K1)*DD(K1,J1)) 120 CONTINUE D2 = 4 * D2 !times 4 DETT = 0D0 IF (N.GT.3) DETT = 1 DT(1,1)*DTADJ(1,1) + DT(1,2)*DTADJ(2,1) + DT(1,3)*DTADJ(3,1) D3SQ = 8 * DABS(DETT) !8 times square root C C*** Calculate T-value and S if required C DTO = 1D0 IF (S.NE.0.0) DTO = ABS(S) DO 130 I=1,25 DTN = D2 + D3SQ*DTO IF (DTN.LT.0D0) DTN = 0D0 DTN = DSQRT(D1 + DSQRT(DTN)) IF (DABS(DTN-DTO).LE.1D-15*DABS(DTO)) GO TO 140 DTO = DTN 130 CONTINUE GO TO 200 140 IF (DTN.EQ.0D0) GO TO 200 IF (S.LE.0.0) S = DTN S = SIGN(S,SNGL(DETT)) D TYPE 700, I,DTN D 700 FORMAT(' ***DEBUG DISP3D: ITER =',I3,', DTN =',1PD20.13) C C*** Calculate rotation matrix R C DSS = (D1 + DTN*DTN) / 2 DO 150 I=1,3 DD(I,I) = DD(I,I) - DSS 150 CONTINUE DSS = DABS(DETT) + DTN*(D1 - DSS) IF (DSS.EQ.0D0) GO TO 200 DO 170 I=1,3 DO 160 J=1,3 R(I,J) = ( DT(I,1)*DD(1,J)+DT(I,2)*DD(2,J)+DT(I,3)*DD(3,J) 1 - DTN*DTADJ(J,I) ) / DSS 160 CONTINUE 170 CONTINUE C C*** Calculate translation vector P C DO 190 I=1,3 DSS = 0D0 DO 180 J=1,3 DSS = DSS + R(I,J) * DBLE(AVX(J)) 180 CONTINUE P(I) = P(I) - S * DSS 190 CONTINUE C C*** Ready C RETURN 200 S = 0.0 RETURN END C C********************************************************************** C C PHIROT 1978-5-27 C C PURPOSE: C ******** C C PROCEDURE TO ASSESS A ROTATION MATRIX AND ITS PARTIAL DERIVA- C TIVES, FOR ROTATING AXES AND ORDER PHI(1), PHI(2) AND PHI(3). C C CALLING CONVENTION: C ******************* C C CALL PHIROT(PHI,ROT,K) C C MEANING OF PARAMETERS: C ********************** C C PHI(3) - REAL*4 ARRAY CONTAINING THE THREE ROTATION ANGLES C UPON ENTRY (IN RADIANS). C ROT(3,3,K) - REAL*4 ARRAY CONTAINING ONLY THE ROTATION MATRIX C UPON RETURN IF K=1, AND IN ADDITION THE PARTIAL C DERIVATIVES TO PHI(I) IN ROT(*,*,I+1) IF K=4. C K - INTEGER*2 INPUT DIMENSIONING PARAMETER: SEE ABOVE. C C*********************************************************************** C SUBROUTINE PHIROT(PHI,ROT,K) C IMPLICIT REAL*8 (C,D,S) DIMENSION PHI(3),ROT(3,3,K),SINCOS(2,3) EQUIVALENCE (SINCOS(1,1),SINOME),(SINCOS(2,1),COSOME), 1 (SINCOS(1,2),SINPHI),(SINCOS(2,2),COSPHI), 2 (SINCOS(1,3),SINKAP),(SINCOS(2,3),COSKAP) C C SINE AND COSINE TERMS C DO 10 J=1,3 DPHI = PHI(J) SINCOS(1,J) = DSIN(DPHI) SINCOS(2,J) = DCOS(DPHI) 10 CONTINUE SOMSKA = SINOME*SINKAP SOMCKA = SINOME*COSKAP COMSKA = COSOME*SINKAP COMCKA = COSOME*COSKAP C C ROTATION MATRIX R = R(OMEGA) * R(PHI) * R(KAPPA) C ROT(1,1,1) = COSPHI*COSKAP ROT(1,2,1) = -COSPHI*SINKAP ROT(1,3,1) = SINPHI ROT(2,1,1) = COMSKA + SOMCKA*SINPHI ROT(2,2,1) = COMCKA - SOMSKA*SINPHI ROT(2,3,1) = -SINOME*COSPHI ROT(3,1,1) = SOMSKA - COMCKA*SINPHI ROT(3,2,1) = SOMCKA + COMSKA*SINPHI ROT(3,3,1) = COSOME*COSPHI IF (K.NE.4) RETURN C C PARTIAL DERIVATIVES IF K.EQ.4 C DO 20 J=1,3 ROT(1,J,2) = 0.0 ROT(2,J,2) = -ROT(3,J,1) ROT(3,J,2) = ROT(2,J,1) ROT(J,1,4) = ROT(J,2,1) ROT(J,2,4) = -ROT(J,1,1) ROT(J,3,4) = 0.0 20 CONTINUE ROT(1,1,3) = -SINPHI*COSKAP ROT(1,2,3) = SINPHI*SINKAP ROT(1,3,3) = COSPHI ROT(2,1,3) = SOMCKA*COSPHI ROT(2,2,3) = -SOMSKA*COSPHI ROT(2,3,3) = SINOME*SINPHI ROT(3,1,3) = -COMCKA*COSPHI ROT(3,2,3) = COMSKA*COSPHI ROT(3,3,3) = -COSOME*SINPHI C C READY C RETURN END C C*********************************************************************** C C ROTPHI 1978-8-23 C C PURPOSE: C ******* C C TO CALCULATE THE ROTATION ANGLES OMEGA, PHI AND KAPPA FOR A C GIVEN PURE ROTATION MATRIX R . C C CALLING CONVENTION: C ****************** C C CALL ROTPHI(ROT,PHI) C C MEANING OF PARAMETERS (DEFAULT TYPES): C ************************************* C C ROT(3,3) - GIVEN INPUT ROTATION MATRIX C PHI(3) - OUTPUT ANGLE VECTOR, ORDERED AS OMEGA,PHI,KAPPA C (IN RADIANS). C C REMARK: C ****** C C 1) KAPPA IS CHOSEN IN QUADRANTS I/IV. IF A CERTAIN COMBINATION C (OMEGA , PHI,KAPPA )' IS A SOLUTION, SO IS C (OMEGA + PI, PI - PHI,KAPPA + PI)'. C 2) IF ABS(PHI) = PI/2, OMEGA AND KAPPA ARE PERFECTLY CORRELATED C AND ONE OF THEM MUST BE FIXED. HERE, KAPPA IS SET TO ZERO. C C*********************************************************************** C SUBROUTINE ROTPHI(ROT,PHI) DIMENSION ROT(3,3),PHI(3) C SINPHI = ROT(1,3) COSPHI = SIGN(SQRT(AMAX1(0.0,1.0 - SINPHI*SINPHI)),ROT(1,1)) PHI(2) = ATAN2(SINPHI,COSPHI) IF (ABS(COSPHI).GT.8E-3) GO TO 10 C DEGENERATED: ABS(PHI).EQ.PI/2. PHI(1) = ATAN2(ROT(3,2),ROT(2,2)) PHI(3) = 0.0 RETURN C GENERAL SOLUTION 10 PHI(1) = ATAN2(-ROT(2,3)/COSPHI,ROT(3,3)/COSPHI) PHI(3) = ATAN2(-ROT(1,2)/COSPHI,ROT(1,1)/COSPHI) RETURN C END C GAUSS.FOR, Woltring (modified 1991-07-25 for MSDOS-Fortran) C************************************************************* C C GAUSS 1978-10-19 C C Purpose: C ******** C C To generate a pseudo-gaussian deviate with standard C deviation SIGMA and mean 0.0 C C Calling convention: C ******************* C C RANDOM = GAUSS(SIGMA) C C Meaning of variables: C ********************* C C SIGMA - standard deviation (input) C C Functions and subroutines called: C ********************************* C C RANDOM - MS-Fortran uniform noise generator C C N.B.: C **** C C On other systemss, subroutine RANDOM may have to be replaced by C and other routine. C C*********************************************************************** C FUNCTION GAUSS(SIGMA) C GAUSS = -6.0 DO 10 I=1,12 CALL RANDOM(RAN) GAUSS = GAUSS + RAN 10 CONTINUE GAUSS = SIGMA * GAUSS C RETURN END C C*********************************************************************** C C GIVENS 1977-4-28 C C PURPOSE: C ******** C C ORTHOGONAL TRIANGULARIZATION OF A MATRIX C BY MEANS OF GENTLEMAN'S SQUARE-ROOT FREE VERSION C (1) OF GIVENS TRANSFORMATIONS. C C CALLING CONVENTION: C ******************* C C CALL GIVENS(ROW,DU,N,DELTA) C C MEANING OF PARAMETERS: C ********************** C C ROW(N) - NEW ROW TO BE ROTATED WITH MATRIX DU (I/O) C DU(N,N) - TRIANGULAR OUTPUT MATRIX (OUTPUT) C N - DIMENSIONING PARAMETER (INPUT) C DELTA - WEIGHT FACTOR FOR ROW (INPUT/OUTPUT) C C REFERENCE: C ********* C C (1) W.M.GENTLEMAN: LEAST SQUARES COMPUTATIONS BY GIVENS TRANS- C FORMATIONS WITHOUT SQUARE ROOTS. J.INST.MATHS APPLICS C 12(1973) 329-336. C C*********************************************************************** C SUBROUTINE GIVENS(ROW,DU,N,DELTA) C DIMENSION ROW(N),DU(N,N) SQUARE(D) = D*D DO 80 I=1,N IF (DELTA.EQ.0.0) RETURN I1 = I + 1 ROWI = ROW(I) IF (ROWI.EQ.0.0) GO TO 80 ROW(I) = 0.0 D = DU(I,I) SBAR = DELTA*ROWI D1 = D + SBAR*ROWI CBAR = D/D1 SBAR = SBAR/D1 DELTA = CBAR*DELTA DU(I,I) = D1 IF (I.EQ.N) GO TO 80 IF (CBAR.GE.0.1) GO TO 40 DO 10 L=I1,N DUIL = DU(I,L) ROWL = ROW(L) DU(I,L) = SBAR*ROWL + DUIL*CBAR ROW(L) = ROWL - DUIL*ROWI 10 CONTINUE GO TO 80 40 DO 50 L=I1,N DUIL = DU(I,L) ROWL = ROW(L) - ROWI*DUIL DU(I,L) = SBAR*ROWL + DUIL ROW(L) = ROWL 50 CONTINUE 80 CONTINUE C RETURN END C C*********************************************************************** C C GIVLSQ C C PURPOSE: C ******** C C TO ROTATE A ROW OF OBSERVATIONS INTO A D-U FACTORIZED SET OF C EQUATIONS BY MEANS OF GENTLEMAN'S SQUARE-ROOT FREE VERSION C (1) OF GIVENS TRANSFORMATIONS. C C CALLING CONVENTION: C ******************* C C CALL GIVLSQ(ROW,DU,QB,N1,N,K,SSQ,SQ,DELTA,LSSQ) C C MEANING OF PARAMETERS: C ********************** C C ROW(N+K) - REAL*4 INPUT ARRAY; NEW ROW TO BE ROTATED WITH C ROWS N1 THROUGH N OF MATRIX DU; THE RIGHT MEMBERS C ARE CONTAINED IN THE LAST K ELEMENTS. THE CON- C TENTS OF THE NEW ROW ARE DESTROYED UPON RETURN. C DU(N,N) - REAL*4 INPUT/OUTPUT ARRAY CONTAINING THE D-U FAC- C TORIZED CONDITION MATRIX. C QB(N,K) - REAL*4 INPUT/OUTPUT ARRAY CONTAINING THE TRANS- C FORMED RIGHT HAND MEMBERS. C SSQ(K) - REAL*4 INPUT/OUTPUT ARRAY CONTAINING THE RESIDUAL C SUM OF SQUARES FOR EACH RIGHT HAND COLUMN. IT IS C UPDATED IF LSSQ=.TRUE. UPON ENTRY. C SQ(K) - REAL*4 OUTPUT ARRAY CONTAINING THE SQUARED C RESIDUAL UPON RETURN IF LSSQ=.TRUE. UPON ENTRY. C DELTA - REAL*4 INPUT/OUTPUT PARAMETER; SQUARED WEIGHT C FACTOR FOR THE NEW ROW. C LSSQ - LOGICAL INPUT VARIABLE. IF .TRUE. UPON ENTRY, C SQ IS CALCULATED AND SSQ IS UPDATED. C N1 - INTEGER INPUT PARAMETER; INDEX OF THE FIRST C ROW IN DU TO BE ROTATED WITH THE NEW ROW. C N - INTEGER INPUT PARAMETER; NUMBER OF UNKNOWNS. C K - INTEGER INPUT PARAMETER; NUMBER OF RIGHT HAND C SIDES AND DIMENSIONING PARAMETER. C C REFERENCE: C ********** C C (1) W.M.GENTLEMAN: LEAST SQUARES COMPUTATIONS BY GIVENS TRANS- C FORMATIONS WITHOUT SQUARE ROOTS. J.INST.MATHS APPLICS C 12(1973) 329-336. C C*********************************************************************** C SUBROUTINE GIVLSQ(ROW,DU,QB,N1,N,K,SSQ,SQ,DELTA,LSSQ) LOGICAL LSSQ DIMENSION ROW(1),DU(N,N),QB(N,K),SSQ(K),SQ(K) SQUARE(D) = D*D DO 80 I=N1,N IF (DELTA.EQ.0.0) RETURN I1 = I + 1 ROWI = ROW(I) IF (ROWI.EQ.0.0) GO TO 80 ROW(I) = 0.0 D = DU(I,I) SBAR = DELTA*ROWI D1 = D + SBAR*ROWI CBAR = D/D1 SBAR = SBAR/D1 DELTA = CBAR*DELTA DU(I,I) = D1 IF (CBAR.GE.0.1) GO TO 40 IF (I.EQ.N) GO TO 20 DO 10 L=I1,N DUIL = DU(I,L) ROWL = ROW(L) DU(I,L) = SBAR*ROWL + DUIL*CBAR ROW(L) = ROWL - DUIL*ROWI 10 CONTINUE 20 DO 30 L=1,K LN = L + N DUIL = QB(I,L) ROWL = ROW(LN) QB(I,L) = SBAR*ROWL + DUIL*CBAR ROW(LN) = ROWL - DUIL*ROWI 30 CONTINUE GO TO 80 40 IF (I.EQ.N) GO TO 60 DO 50 L=I1,N DUIL = DU(I,L) ROWL = ROW(L) - ROWI*DUIL DU(I,L) = SBAR*ROWL + DUIL ROW(L) = ROWL 50 CONTINUE 60 DO 70 L=1,K LN = L + N DUIL = QB(I,L) ROWL = ROW(LN) - ROWI*DUIL QB(I,L) = SBAR*ROWL + DUIL ROW(LN) = ROWL 70 CONTINUE 80 CONTINUE C IF ((DELTA.EQ.0.0).OR.(.NOT.LSSQ)) RETURN DO 90 L=1,K SQ (L) = DELTA*SQUARE(ROW(L+N)) SSQ(L) = SSQ(L) + SQ(L) 90 CONTINUE RETURN END C C*********************************************************************** C C SLVGIV C C PURPOSE: C ******** C C SOLUTION BY BACKSUBSTITUTION OF A D-U FACTORIZED CONSISTENT C SET OF LINEAR EQUATIONS, E.G. OUTPUT FROM SUBROUTINE GIVLSQ. C SEE THE COMMENTS OF THAT SUBROUTINE FOR FURTHER DETAILS. C C CALLING CONVENTION: C ******************* C C CALL SLVGIV(DU,QB,A,N,K) C C MEANING OF PARAMETERS (DEFAULT PARAMETER TYPES): C ************************************************ C C DU(N,N) - INPUT ARRAY CONTAINING THE D-U FACTORIZATION. C QB(N,K) - INPUT ARRAY CONTAINING THE TRANSFORMED RIGHT C HAND MEMBERS. C A (N,K) - OUTPUT ARRAY CONTAINING THE RESULTANT SOLUTION. C N,K - DIMENSIONING PARAMETERS. C C REMARK: C ******* C C QB AND A MAY SHARE (EXACTLY) THE SAME STORAGE. C C*********************************************************************** C SUBROUTINE SLVGIV(DU,QB,A,N,K) DIMENSION DU(N,N),QB(N,K),A(N,K) C NM1 = N - 1 DO 20 I=1,K A(N,I) = QB(N,I) DO 10 J=1,NM1 NMJ = N - J NMJP1 = NMJ + 1 A(NMJ,I) = -DINPRD(NMJP1,N,DU(NMJ,NMJP1),N,A(NMJP1,I),1, 1 -QB(NMJ,I)) 10 CONTINUE 20 CONTINUE C RETURN END C C*********************************************************************** C C DINPRD C C PURPOSE: C ******** C C REAL*4 FUNCTION TO ASSESS THE IN-PRODUCT OF TWO VECTORS, WITH C THE INTERMEDIATE OPERATIONS PERFORMED IN DOUBLE PRECISION. C C CALLING CONVENTION: C ******************* C C A = DINPRD(N1,N,AA,NA,BBB,NB,AB0) C C MEANING OF PARAMETERS: C ********************** C C DINPRD - REAL*4 OUTPUT ARGUMENT. C N1 - INTEGER*2 INPUT ARGUMENT; START INDEX OF LOOP C COUNT. C N - INTEGER*2 INPUT ARGUMENT; STOP INDEX OF LOOP C COUNT. C AA(1) - REAL*4 INPUT ARRAY; C NA - INTEGER*2 INPUT PARAMETER; INDEX INCREMENT OF C AA. C BB(1) - REAL*4 INPUT ARRAY. C NB - INTEGER*2 INPUT PARAMETER; INDEX INCREMENT OF C BB. C AB0 - REAL*4 INPUT ARGUMENT; SUM INITIALIZATION TERM. C C*********************************************************************** C REAL FUNCTION DINPRD(N1,N,AA,NA,BB,NB,AB0) DOUBLE PRECISION SUM,DBLE DIMENSION AA(1),BB(1) C SUM = AB0 IF (N1.GT.N) GO TO 20 IA = 1 IB = 1 DO 10 I=N1,N SUM = SUM + DBLE(AA(IA))*BB(IB) IA = IA + NA IB = IB + NB 10 CONTINUE 20 DINPRD = SUM C RETURN END C C*********************************************************************** C C MATCLR C C PURPOSE: C ******** C C ROUTINE TO CLEAR THE FIRST M ROWS OF A MATRIX A(NA,N) C C CALLING CONVENTION (DEFAULT PARAMETER TYPES): C ********************************************* C C CALL MATCLR(M,N,A,NA) C C MEANING OF PARAMETERS (DEFAULT PARAMETER TYPES): C ************************************************ C C A(NA,N) - TARGET MATRIX (INPUT/OUTPUT) C NA - COLUMN LENGTH OF MATRIX A (INPUT) C N - ROW LENGTH OF MATRIX A (INPUT) C M - NUMBER OF (FIRST) ROWS OF A TO BE CLEARED (INPUT). C C*********************************************************************** C SUBROUTINE MATCLR(M,N,A,NA) DIMENSION A(NA,N) C DO 20 J=1,N DO 10 I=1,M A(I,J) = 0.0 10 CONTINUE 20 CONTINUE C RETURN END C C*********************************************************************** C C MATCOP C C PURPOSE: C ******** C C TO COPY THE FIRST M ROWS OF A MATRIX A(NA,N) INTO THE FIRST M C ROWS OF A MATRIX B(NB,N). C C CALLING CONVENTION: C ******************* C C CALL MATCOP(M,N,A,NA,B,NB) C C*********************************************************************** C SUBROUTINE MATCOP(M,N,A,NA,B,NB) DIMENSION A(NA,N),B(NB,N) C DO 20 I=1,M DO 10 J=1,N B(I,J) = A(I,J) 10 CONTINUE 20 CONTINUE C C READY C RETURN END C C*********************************************************************** C C MATSCL C C PURPOSE: C ******* C C TO SCALE A MATRIX WITH A GIVEN CONSTANT C C CALLING CONVENTION: C ****************** C C CALL MATSCL(M,N,A,NA,SC,B,NB) C C MEANING OF PARAMETERS (DEFAULT TYPES): C ************************************* C C A(NA,N) - INPUT ARRAY, THE FIRST M ROWS OF WHICH WILL BE C SCALED, AND STORED INTO: C B(NB,N) - OUTPUT ARRAY. C NA,NB - COLUMN LENGTHS OF THE ARRAYS A AND B, RESPECTIVELY. C C REMARK: C ****** C C A AND B MAY SHARE (EXACTLY) THE SAME STORAGE C C*********************************************************************** C SUBROUTINE MATSCL(M,N,A,NA,SC,B,NB) C DIMENSION A(NA,N),B(NB,N) C DO 20 J=1,N DO 10 I=1,M B(I,J) = SC*A(I,J) 10 CONTINUE 20 CONTINUE C RETURN END C C*********************************************************************** C C MATSUM C C PURPOSE: C ******** C C TO ADD THE FIRST M ROWS OF A MATRIX A(NA,N) AND OF A MATRIX C B(NB,N) AND STORE THE RESULT IN THE FIRST M ROWS OF A MATRIX C C(NC,N). C C CALLING CONVENTION (DEFAULT PARAMETER TYPES): C ********************************************* C C CALL MATSUM(M,N,A,NA,B,NB,C,NC) C C*********************************************************************** C SUBROUTINE MATSUM(M,N,A,NA,B,NB,C,NC) DIMENSION A(NA,N),B(NB,N),C(NC,N) C DO 20 I=1,M DO 10 J=1,N C(I,J) = A(I,J) + B(I,J) 10 CONTINUE 20 CONTINUE C C READY C RETURN END C C*********************************************************************** C C MATDIF C C PURPOSE: C ******** C C TO SUBTRACT THE FIRST M ROWS OF A MATRIX B(NB,N) FROM THOSE IN C A MATRIX A(NA,N) AND TO STORE THE RESULT IN THE FIRST M ROWS OF C A MATRIX C(NC,N). C C CALLING CONVENTION (DEFAULT PARAMETER TYPES): C ********************************************* C C CALL MATDIF(M,N,A,NA,B,NB,C,NC) C C*********************************************************************** C SUBROUTINE MATDIF(M,N,A,NA,B,NB,C,NC) DIMENSION A(NA,N),B(NB,N),C(NC,N) C DO 20 I=1,M DO 10 J=1,N C(I,J) = A(I,J) - B(I,J) 10 CONTINUE 20 CONTINUE C C READY C RETURN END C C*********************************************************************** C C PRAB 1978-5-1 C C PURPOSE: C ******** C C TO ASSESS THE PRODUCT OF MATRICES A(MA,M) AND B(M,MB) AND TO C STORE THE RESULTS INTO A MATRIX C(MA,MB). THE COLUMN LENGTHS C OF THE MATRICES ARE NA,NB,NC, RESPECTIVELY. C C CALLING CONVENTION (DEFAULT PARAMETER TYPES): C ********************************************* C C CALL PRAB(MA,M,MB,A,NA,B,NB,C,NC) C C SUBPROGRAMS CALLED: C ******************* C C DINPRD C C*********************************************************************** C SUBROUTINE PRAB(MA,M,MB,A,NA,B,NB,C,NC) DIMENSION A(NA,M),B(NB,MB),C(NC,MB) C DO 20 IR=1,MA DO 10 IC=1,MB C(IR,IC) = DINPRD(1,M,A(IR,1),NA,B(1,IC),1,0.0) 10 CONTINUE 20 CONTINUE C RETURN END C C************************************************************* C C VECPRD 1978-10-19 C C PURPOSE: C ******* C C SUBROUTINE TO ASSESS THE VECTORPRODUCT C = A * B C C CALLING CONVENTION: C ****************** C C CALL VECPRD(A,B,C) C C MEANING OF PARAMETERS (DEFAULT PARAMETER TYPES): C *********************************************** C C A(3) - INPUT VECTOR C B(3) - INPUT VECTOR C C(3) - OUTPUT VECTOR C C************************************************************* C SUBROUTINE VECPRD(A,B,C) C DOUBLE PRECISION DBLE DIMENSION A(3),B(3),C(3) EQUIVALENCE (I,II) C DO 10 II=1,3 J = MOD(I,3) + 1 K = MOD(J,3) + 1 C(I) = A(J)*DBLE(B(K)) - A(K)*DBLE(B(J)) 10 CONTINUE C RETURN END