C C****************************************************************************** C C DISP3DB.FOR, Woltring 1983-02-06 (modified 1992-03-05 with polarity switch) 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 DISP3DB ( X, Y, N, P, R, S, IS ) 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 IS - Input polarity switch. If zero, the sign of S on output C is determined from the data, with S > 0 assumed for a C purely planar distribution; if non-zero, the sign of S C is taken from IS. Imposing the sign of S may be useful C in the case of noisy measurements on (nearly) planar point C distributions in X and/or Y [modification, 1992-03-05]. 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 the Journal of Biomechanics. [Ultimately published as C Veldpaus et al., Journal of Biomechanics, January 1988.] C C****************************************************************************** C SUBROUTINE DISP3DB ( X, Y, N, P, R, S, IS) 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 IF (IS.EQ.0) THEN S = SIGN(S,SNGL(DETT)) ELSE S = SIGN(S,FLOAT(IS)) ENDIF D WRITE(*,700) I,DTN D 700 FORMAT(' ***DEBUG DISP3DB: 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