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