Date:	Thursday, March 25, 1993
To:	Biomch-l requestors
From:	Joe Sommer
Subj:	FORTRAN subroutines for finite helical axis (FHA) and instant
		screw axis (ISA)

Because of the large number of requests, I have assembled this file
containing:
1)  a sample data set of landmark positions, velocities, and accelerations;
2)  sample FHA and ISA output for this data set;
3)  an example main program; and
4)  the requested subroutines.

All of the routines are documented internally, and attempt to follow the
nomenclature in cited references.

The FHA and ISA results will not match EXACTLY in that the ISA represents
instantaneous motion and the FHA represents a finite displacement.  This
concept is subtle, but important.


H.J. Sommer III, Professor of Mechanical Engineering, 207 Reber Building
The Pennsylvania State University, University Park, PA  16802
(814)865-2519, FAX (814)863-4848, Bitnet HJS@PSUECL, Internet HJS@ECL.PSU.EDU

SABBATICAL ADDRESS (8/15/92-6/15/93)
H.J. Sommer III, Guest Researcher, Biomechanics Lab, Rehabilitation Medicine
National Institutes of Health, Bldg 10, Room 6s-207a, Bethesda, MD  20892
(301)496-9890x15, FAX (301)402-0663, Internet SOMMER%BMLVAX.DNET@DXI.NIH.GOV


c********1*********2*********3*********4*********5*********6*********7**

SAMPLE DATA SET

global locations (cm) at time=0
    pt  A     pt B     pt C     pt D     pt E     pt F     pt G     pt H
   11.000    9.000    9.000   11.000   11.000    9.000    9.000   11.000  x
    1.000    1.000   -1.000   -1.000    1.000    1.000   -1.000   -1.000  y
   10.000   10.000   10.000   10.000    8.000    8.000    8.000    8.000  z
global locations (cm) at time=0.02 sec
    pt  A     pt B     pt C     pt D     pt E     pt F     pt G     pt H
  10.2782   8.2853   8.3831  10.3760  10.4153   8.4224   8.5202  10.5131  x
   1.5042   1.4067   -.5909   -.4934   1.5109   1.4134   -.5842   -.4867  y
  10.1345   9.9972   9.9972  10.1345   8.1392   8.0020   8.0020   8.1392  z
global velocities (cm/sec) at time=0
    pt  A     pt B     pt C     pt D     pt E     pt F     pt G     pt H
  -35.750  -35.750  -30.750  -30.750  -28.750  -28.750  -23.750  -23.750  x
   27.500   22.500   22.500   27.500   27.500   22.500   22.500   27.500  y
    8.000    1.000    1.000    8.000    8.000    1.000    1.000    8.000  z
global accelerations (cm/sec/sec) at time=0
    pt  A     pt B     pt C     pt D     pt E     pt F     pt G     pt H
  -45.250   -8.250  -19.250  -56.250  -58.250  -21.250  -32.250  -69.250  x
 -233.000 -222.000 -209.500 -220.500 -198.000 -187.000 -174.500 -185.500  y
 -130.375 -117.375 -117.375 -130.375 -105.875  -92.875  -92.875 -105.875  z


c********1*********2*********3*********4*********5*********6*********7**

SAMPLE OUTPUT FOR ABOVE DATA SET

 Displacement transformation matrix (delta=0.02 sec)
         1.00000        0.00000        0.00000        0.00000
         0.05163        0.99645       -0.04888       -0.06855
         0.00251        0.04877        0.99880       -0.00335
        -0.59685        0.06863        0.00000        0.99764
 Location on FHA (cm)
 (at root of perpendicular through origin)
         5.77483        0.35559        0.30132
 Unit direction along FHA
         0.01990       -0.81451        0.57981
 Translation displacement along FHA (cm)
        -0.34708
 Rotation displacement about FHA (rad)
         0.08431
  
 Location on ISA (cm)
 (at root of perpendicular through centroid of landmarks)
         5.77027       -4.02027        3.37162
 Unit direction along ISA
         0.00000       -0.81373        0.58124
 Translation velocity along ISA (cm/sec)
       -17.72776
 Rotation velocity about ISA (rad/sec)
         4.30116
 Angular acceleration vector (rad/sec/sec)
         8.75000        6.50000       -5.50000
 Location of acceleration pivot (cm)
         2.64398      -11.54136        3.79095


c********1*********2*********3*********4*********5*********6*********7**

EXAMPLE MAIN PROGRAM FOLLOWED BY SUBROUTINES

c	test Spoor/Veldpaus/Woltring/Dortmans finite screw axis routines
c	and Sommer instantaneous screw axis routine
c
c	dimensions for finite helical axis
	REAL*4 XX1(8),YY1(8),ZZ1(8)
	REAL*4 XX2(8),YY2(8),ZZ2(8)
	REAL*4 TT(4,4),PP(3),UU(3)
c
c	dimensions for instant screw axis and angular acceleration axis
	REAL*4 XI(3,8),VI(3,8),AI(3,8),FI(8),SS(3,3)
	REAL*4 WV(3),VP(3),P(3),AV(3),AQ(3),Q(3)
c
c	dummy label in data file
	CHARACTER*40 HEADER
c
c	number of data points ( N >= 3 )
	N=8
c
c	open and read sample data file
	OPEN(UNIT=10,FILE='TEST.DATA',STATUS='OLD')
20000	FORMAT(A40)
	READ(10,20000)HEADER
	READ(10,20000)HEADER
	READ(10,*)(XX1(J),J=1,N)
	READ(10,*)(YY1(J),J=1,N)
	READ(10,*)(ZZ1(J),J=1,N)
	READ(10,20000)HEADER
	READ(10,20000)HEADER
	READ(10,*)(XX2(J),J=1,N)
	READ(10,*)(YY2(J),J=1,N)
	READ(10,*)(ZZ2(J),J=1,N)
	READ(10,20000)HEADER
	READ(10,20000)HEADER
	READ(10,*)(VI(1,J),J=1,N)
	READ(10,*)(VI(2,J),J=1,N)
	READ(10,*)(VI(3,J),J=1,N)
	READ(10,20000)HEADER
	READ(10,20000)HEADER
	READ(10,*)(AI(1,J),J=1,N)
	READ(10,*)(AI(2,J),J=1,N)
	READ(10,*)(AI(3,J),J=1,N)
	CLOSE(UNIT=10)
c
c	determine and print motion
	CALL DATDTM(N,XX1,YY1,ZZ1,XX2,YY2,ZZ2,TT)
	WRITE(6,*)'Displacement transformation matrix (delta=0.02 sec)'
	DO 10000 I=1,4
	WRITE(6,20001)(TT(I,J),J=1,4)
20001	FORMAT(1X,4F15.5)
10000	CONTINUE
c
c	determine and print helical axis parameters
	CALL KINFHA(TT,PP,UU,S,TH)
	WRITE(6,*)'Location on FHA (cm)'
	WRITE(6,*)'(at root of perpendicular through origin)'
	WRITE(6,20001)PP
	WRITE(6,*)'Unit direction along FHA'
	WRITE(6,20001)UU
	WRITE(6,*)'Translation displacement along FHA (cm)'
	WRITE(6,20001)S
	WRITE(6,*)'Rotation displacement about FHA (rad)'
	WRITE(6,20001)TH
c
c	determine and print ISA/AAA paramters at time=0
	DO 10001 J=1,N
	XI(1,J)=XX1(J)
	XI(2,J)=YY1(J)
	XI(3,J)=ZZ1(J)
	FI(J)=1.0
10001	CONTINUE
	DO 10002 I=1,3
	SS(I,1)=0.0
	SS(I,2)=0.0
	SS(I,3)=0.0
	SS(I,I)=1.0
10002	CONTINUE
	CALL DATISA(N,XI,VI,AI,FI,SS,WV,VP,P,AV,AQ,Q)
	WRITE(6,*)' '
	WRITE(6,*)'Location on ISA (cm)'
	WRITE(6,*)'(at root of perpendicular through centroid of landmarks)'
	WRITE(6,20001)P
	W=SQRT(WV(1)*WV(1)+WV(2)*WV(2)+WV(3)*WV(3))
	UU(1)=WV(1)/W
	UU(2)=WV(2)/W
	UU(3)=WV(3)/W
	WRITE(6,*)'Unit direction along ISA'
	WRITE(6,20001)UU
	SD=VP(1)*UU(1)+VP(2)*UU(2)+VP(3)*UU(3)
	WRITE(6,*)'Translation velocity along ISA (cm/sec)'
	WRITE(6,20001)SD
	WRITE(6,*)'Rotation velocity about ISA (rad/sec)'
	WRITE(6,20001)W
	WRITE(6,*)'Angular acceleration vector (rad/sec/sec)'
	WRITE(6,20001)AV
	WRITE(6,*)'Location of acceleration pivot (cm)'
	WRITE(6,20001)Q
c
c	done
	STOP
	END
c
c********1*********2*********3*********4*********5*********6*********7**
c
c	DATDTM(N,XX1,YY1,ZZ1,XX2,YY2,ZZ2,TT)
c
c	calculation of displacement transformation matrix for
c	rigid body motion from position one to position
c	two as described by the global locations of N landmarks
c
c	INPUTS
c	N = number of landmarks ( N >= 3 )
c	XX1 = landmark x location vector of length N at position one
c	YY1 = landmark y location vector of length N at position one
c	ZZ1 = landmark z location vector of length N at position one
c	XX2 = landmark x location vector of length N at position two
c	YY2 = landmark y location vector of length N at position two
c	ZZ2 = landmark z location vector of length N at position two
c
c	OUTPUTS
c	TT - 4x4 transformation matrix
c		| 1 |     |      |   | 1 |
c		| x |  =  |  TT  | * | x |
c		| y |     |      |   | y |
c		| z |     |      |   | z |
c		pos 2                pos 1
c
c	PRECISION:	single
c	COMMONS:	none
c	CALLS:		MATPP3
c	FUNCTIONS:	FLOAT, SQRT
c	REFERENCE:	Spoor, C.W. and F.E. Veldpaus
c			Rigid Body Motion Calculated from Spatial
c			Co-ordinates of Markers
c			J. Biomechanics, 13(4):391-393 (1980).
c			and
c			Veldpaus, F.E., H.J. Woltring, and L.J.M.G. Dortmans
c			A Least-Squares Algorithm for the Equiform
c			Transformation from Spatial Marker Coordinates
c			J. Biomechanics, 21(1):45-54 (1988).
c	DATE:		12/3/92 - HJSIII
c
c
	SUBROUTINE DATDTM(N,XX1,YY1,ZZ1,XX2,YY2,ZZ2,TT)
c
c	dimensions
	REAL*4 XX1(N),YY1(N),ZZ1(N),XX2(N),YY2(N),ZZ2(N)
	REAL*4 TT(4,4)
	REAL*4 M(3,3),R(3,3),S(3,3),D(3,3),V(3,3),MV(3,3)
c
c	initialize
	FN=FLOAT(N)
	XM1=0.0
	YM1=0.0
	ZM1=0.0
	XM2=0.0
	YM2=0.0
	ZM2=0.0
	DO 10000 I=1,3
	M(I,1)=0.0
	M(I,2)=0.0
	M(I,3)=0.0
10000	CONTINUE
	TT(1,1)=1.0
	TT(1,2)=0.0
	TT(1,3)=0.0
	TT(1,4)=0.0
c
c	sum over all points
	DO 10001 I=1,N
	XM1=XM1+XX1(I)
	YM1=YM1+YY1(I)
	ZM1=ZM1+ZZ1(I)
	XM2=XM2+XX2(I)
	YM2=YM2+YY2(I)
	ZM2=ZM2+ZZ2(I)
	M(1,1)=M(1,1)+XX2(I)*XX1(I)
	M(2,1)=M(2,1)+YY2(I)*XX1(I)
	M(3,1)=M(3,1)+ZZ2(I)*XX1(I)
	M(1,2)=M(1,2)+XX2(I)*YY1(I)
	M(2,2)=M(2,2)+YY2(I)*YY1(I)
	M(3,2)=M(3,2)+ZZ2(I)*YY1(I)
	M(1,3)=M(1,3)+XX2(I)*ZZ1(I)
	M(2,3)=M(2,3)+YY2(I)*ZZ1(I)
	M(3,3)=M(3,3)+ZZ2(I)*ZZ1(I)
10001	CONTINUE
c
c	means
	XM1=XM1/FN
	YM1=YM1/FN
	ZM1=ZM1/FN
	XM2=XM2/FN
	YM2=YM2/FN
	ZM2=ZM2/FN
	M(1,1)=M(1,1)/FN-XM2*XM1
	M(2,1)=M(2,1)/FN-YM2*XM1
	M(3,1)=M(3,1)/FN-ZM2*XM1
	M(1,2)=M(1,2)/FN-XM2*YM1
	M(2,2)=M(2,2)/FN-YM2*YM1
	M(3,2)=M(3,2)/FN-ZM2*YM1
	M(1,3)=M(1,3)/FN-XM2*ZM1
	M(2,3)=M(2,3)/FN-YM2*ZM1
	M(3,3)=M(3,3)/FN-ZM2*ZM1
c
c	positive polar decomposition of M
	CALL MATPP3(M,R,S)
	TT(2,2)=R(1,1)
	TT(2,3)=R(1,2)
	TT(2,4)=R(1,3)
	TT(3,2)=R(2,1)
	TT(3,3)=R(2,2)
	TT(3,4)=R(2,3)
	TT(4,2)=R(3,1)
	TT(4,3)=R(3,2)
	TT(4,4)=R(3,3)
c
c	displacement vector
	TT(2,1)=XM2-R(1,1)*XM1-R(1,2)*YM1-R(1,3)*ZM1
	TT(3,1)=YM2-R(2,1)*XM1-R(2,2)*YM1-R(2,3)*ZM1
	TT(4,1)=ZM2-R(3,1)*XM1-R(3,2)*YM1-R(3,3)*ZM1
c
c	done
	RETURN
	END
c
c********1*********2*********3*********4*********5*********6*********7**
c
c	MATPP3(G,R,S)
c
c	positive polar decomposition of 3x3 matrix  G = R * S
c	forcing positive orthonormal rotation matrix
c
c	INPUTS
c	G = 3x3 general matrix
c
c	OUTPUTS
c	R = 3x3 positive orthonormal matrix
c	S = 3x3 symmetric matrix
c
c	PRECISION:	single
c	COMMONS:	none
c	CALLS:		none
c	FUNCTIONS:	ABS, SQRT
c	REFERENCE:	Veldpaus, F.E., H.J. Woltring, and L.J.M.G. Dortmans,
c				A Least-Squares Algorithm for the Equiform
c				Transformation from Spatial Marker Coordinates, 
c				J. Biomechanics, 21(1):45-54 (1988).
c	DATE:		10/8/92 - HJSIII
c
c
      SUBROUTINE MATPP3(G,R,S)
c
c	declarations
      REAL*4 G(3,3),R(3,3),S(3,3)
      REAL*4 COG(3,3),P(3,3),ADP(3,3),PBI(3,3)
c
c	constants
      EPS=1.0E-5
c
c	cofactors and determinant of g
      COG(1,1)=G(2,2)*G(3,3)-G(2,3)*G(3,2)
      COG(2,1)=G(1,3)*G(3,2)-G(1,2)*G(3,3)
      COG(3,1)=G(1,2)*G(2,3)-G(1,3)*G(2,2)
      COG(1,2)=G(2,3)*G(3,1)-G(2,1)*G(3,3)
      COG(2,2)=G(1,1)*G(3,3)-G(1,3)*G(3,1)
      COG(3,2)=G(1,3)*G(2,1)-G(1,1)*G(2,3)
      COG(1,3)=G(2,1)*G(3,2)-G(2,2)*G(3,1)
      COG(2,3)=G(1,2)*G(3,1)-G(1,1)*G(3,2)
      COG(3,3)=G(1,1)*G(2,2)-G(1,2)*G(2,1)
      G3=G(1,1)*COG(1,1)+G(2,1)*COG(2,1)+G(3,1)*COG(3,1)
c
c	P = trans(G) * G = S * S
      DO 10000 I=1,3
      P(I,1)=G(1,I)*G(1,1)+G(2,I)*G(2,1)+G(3,I)*G(3,1)
      P(I,2)=G(1,I)*G(1,2)+G(2,I)*G(2,2)+G(3,I)*G(3,2)
      P(I,3)=G(1,I)*G(1,3)+G(2,I)*G(2,3)+G(3,I)*G(3,3)
10000 CONTINUE
c
c	adjoint of P
      ADP(1,1)=P(2,2)*P(3,3)-P(2,3)*P(3,2)
      ADP(2,2)=P(1,1)*P(3,3)-P(1,3)*P(3,1)
      ADP(3,3)=P(1,1)*P(2,2)-P(1,2)*P(2,1)
c
c	G invariants
      G1SQ=P(1,1)+P(2,2)+P(3,3)
      G1=SQRT(G1SQ)
      G2SQ=ADP(1,1)+ADP(2,2)+ADP(3,3)
      G2=SQRT(G2SQ)
c
c	initialize iteration
      H1=G2/G1SQ
      H2=G3*G1/G2SQ
      X=1.0
      Y=1.0
c
c	iteration loop
10001 CONTINUE
      DEN=2.0*(X*Y-H1*H2)
      RES1=1.0-X*X+2.0*H1*Y
      RES2=1.0-Y*Y+2.0*H2*X
      DX=(Y*RES1+H1*RES2)/DEN
      DY=(H2*RES1+X*RES2)/DEN
      X=X+DX
      Y=Y+DY
      IF(ABS(DX/X).GT.EPS.OR.ABS(DY/Y).GT.EPS)GO TO 10001
c
c	BETA invariants
      BETA1=X*G1
      BETA2=Y*G2
c
c	invert ( trans(G) * G + BETA2 * identity )
      P(1,1)=P(1,1)+BETA2
      P(2,2)=P(2,2)+BETA2
      P(3,3)=P(3,3)+BETA2
      PBI(1,1)=P(2,2)*P(3,3)-P(2,3)*P(3,2)
      PBI(1,2)=P(1,3)*P(3,2)-P(1,2)*P(3,3)
      PBI(1,3)=P(1,2)*P(2,3)-P(1,3)*P(2,2)
      PBI(2,1)=P(2,3)*P(3,1)-P(2,1)*P(3,3)
      PBI(2,2)=P(1,1)*P(3,3)-P(1,3)*P(3,1)
      PBI(2,3)=P(1,3)*P(2,1)-P(1,1)*P(2,3)
      PBI(3,1)=P(2,1)*P(3,2)-P(2,2)*P(3,1)
      PBI(3,2)=P(1,2)*P(3,1)-P(1,1)*P(3,2)
      PBI(3,3)=P(1,1)*P(2,2)-P(1,2)*P(2,1)
      DETPBI=P(1,1)*PBI(1,1)+P(2,1)*PBI(1,2)+P(3,1)*PBI(1,3)
c
c	R = (cofac(G)+BETA1*G) * inv(trans(G)*G+BETA2*identity)
      DO 10002 I=1,3
      R(I,1)=((COG(I,1)+BETA1*G(I,1))*PBI(1,1)
     1       +(COG(I,2)+BETA1*G(I,2))*PBI(2,1)
     2       +(COG(I,3)+BETA1*G(I,3))*PBI(3,1))/DETPBI
      R(I,2)=((COG(I,1)+BETA1*G(I,1))*PBI(1,2)
     1       +(COG(I,2)+BETA1*G(I,2))*PBI(2,2)
     2       +(COG(I,3)+BETA1*G(I,3))*PBI(3,2))/DETPBI
      R(I,3)=((COG(I,1)+BETA1*G(I,1))*PBI(1,3)
     1       +(COG(I,2)+BETA1*G(I,2))*PBI(2,3)
     2       +(COG(I,3)+BETA1*G(I,3))*PBI(3,3))/DETPBI
10002 CONTINUE
c
c	S = trans(R) * G
      DO 10003 I=1,3
      S(I,1)=R(1,I)*G(1,1)+R(2,I)*G(2,1)+R(3,I)*G(3,1)
      S(I,2)=R(1,I)*G(1,2)+R(2,I)*G(2,2)+R(3,I)*G(3,2)
      S(I,3)=R(1,I)*G(1,3)+R(2,I)*G(2,3)+R(3,I)*G(3,3)
10003 CONTINUE
c
c	done
      RETURN
      END
c
c********1*********2*********3*********4*********5*********6*********7**
c
c	KINFHA(TT,P,UU,S,TH)
c
c	extract finite helical axis (FHA) parameters from displacement
c	transformation matrix
c
c	INPUTS
c	TT = 4x4 coordinate transformation matrix
c
c	OUTPUTS
c	P = helical axis location vector of length 3
c		P will lie at root of perpendicular to FHA through origin
c	UU = helical axis unit direction vector of length 3
c	S = helical axis translation
c	TH = helical axis rotation in radians
c
c	PRECISION:	single
c	COMMONS:	none
c	CALLS:		none
c	FUNCTIONS:	ABS, ACOS, ASIN, SIGN, SQRT
c	REFERENCE:	Beggs, J.S.
c			Advanced Mechanism, p. 29, Macmillan (1966)
c			and
c			Spoor, C.W. and F.E. Veldpaus
c			Rigid Body Motion Calculated from Spatial
c			Co-ordinates of Markers
c			J. Biomechanics, 13(4):391-393 (1980)
c	DATE:		12/5/92 - HJSIII
c
c
	SUBROUTINE KINFHA(TT,P,UU,S,TH)
c
c	dimsenions
	REAL P(3),UU(3),TT(4,4),B(3,3)
c
c	constants
	PI=3.141592653
	SC45=0.7071067
c
c	determine TH
	S4334=TT(4,3)-TT(3,4)
	S2442=TT(2,4)-TT(4,2)
	S3223=TT(3,2)-TT(2,3)
	ST=0.5*SQRT(S4334*S4334+S2442*S2442+S3223*S3223)
	IF(ST.GT.1.0)ST=1.0
	CT=0.5*(TT(2,2)+TT(3,3)+TT(4,4)-1.0)
	IF(ABS(CT).GT.1.0)CT=SIGN(1.0,CT)
	TH=ACOS(CT)
	IF(TH.EQ.0.0)GO TO 10000
	IF(ABS(CT).GT.SC45)GO TO 10001
	UU(1)=0.5*S4334/ST
	UU(2)=0.5*S2442/ST
	UU(3)=0.5*S3223/ST
	GO TO 10003
c
c	determine UU
10001	CONTINUE
	TH=ASIN(ST)
	IF(CT.LT.0.0)TH=PI-TH
	B(1,1)=TT(2,2)-CT
	B(2,1)=0.5*(TT(3,2)+TT(2,3))
	B(3,1)=0.5*(TT(4,2)+TT(2,4))
	B(1,2)=B(2,1)
	B(2,2)=TT(3,3)-CT
	B(3,2)=0.5*(TT(4,3)+TT(3,4))
	B(1,3)=B(3,1)
	B(2,3)=B(3,2)
	B(3,3)=TT(4,4)-CT
	MAX=1
	DO 10002 J=1,3
	P(J)=SQRT(B(1,J)*B(1,J)+B(2,J)*B(2,J)+B(3,J)*B(3,J))
	IF(P(J).GT.P(MAX))MAX=J
10002	CONTINUE
	DOT=S4334*B(1,MAX)+S2442*B(2,MAX)+S3223*B(3,MAX)
	P(MAX)=SIGN(P(MAX),DOT)
	UU(1)=B(1,MAX)/P(MAX)
	UU(2)=B(2,MAX)/P(MAX)
	UU(3)=B(3,MAX)/P(MAX)
c
c	determine S and P
10003	CONTINUE
	S=TT(2,1)*UU(1)+TT(3,1)*UU(2)+TT(4,1)*UU(3)
	Q=ST/2.0/(1.0-CT)
	C1=UU(2)*TT(4,1)-UU(3)*TT(3,1)
	C2=UU(3)*TT(2,1)-UU(1)*TT(4,1)
	C3=UU(1)*TT(3,1)-UU(2)*TT(2,1)
	P(1)=Q*C1-0.5*(UU(2)*C3-UU(3)*C2)
	P(2)=Q*C2-0.5*(UU(3)*C1-UU(1)*C3)
	P(3)=Q*C3-0.5*(UU(1)*C2-UU(2)*C1)
	GO TO 99999
c
c	special case for TH=0
10000	CONTINUE
	TH=0.0
	P(1)=0.0
	P(2)=0.0
	P(3)=0.0
	S=SQRT(TT(2,1)*TT(2,1)+TT(3,1)*TT(3,1)
     1      +TT(4,1)*TT(4,1))
	UU(1)=TT(2,1)/S
	UU(2)=TT(3,1)/S
	UU(3)=TT(4,1)/S 
c
c	done
99999	CONTINUE
	RETURN
	END
c
c********1*********2*********3*********4*********5*********6*********7**
c
c	DATISA(M,XI,VI,AI,FI,SS,WV,VP,P,AV,AQ,Q)
c
c	least squares estimation of Instantaneous Screw Axis (ISA)
c	and Angular Acceleration Axis (AAA) from landmark motion
c
c	INPUTS
c	M = number of landmarks
c	XI = 3xM matrix of landmark coordinates
c		XI(1,j)=x, XI(2,j)=y, XI(3,j)=z
c	VI = 3xM matrix of landmark velocities
c		VI(1,j)=vx, VI(2,j)=vy, VI(3,j)=vz
c	AI = 3xM matrix of landmark acclerations
c		AI(1,j)=ax, AI(2,j)=ay, AI(3,j)=az
c	FI = landmark weighting factors vector of length m
c	SS = 3x3 symmetric expected variance-covariance weighting 
c		matrix for landmark measurements
c
c	OUTPUTS
c	WV = angular velocity vector of length 3
c	VP = velocity vector of length 3 for ISA location
c	P = location vector of length 3 for ISA location
c		P will lie at root of perpendicular to ISA through
c		centroid of landmarks
c	AV = angular acceleration vector of length 3
c	AQ = acceleration vector of length 3 for acceleration pivot
c	Q = location vector of length 3 for acceleration pivot
c
c	PRECISION:	single
c	COMMONS:	none
c	CALLS:		MATIN3
c	FUNCTIONS:	ABS, FLOAT, SQRT
c	REFERENCE:	Sommer, H.J.
c			Determination of First and Second Order Instant
c			Screw Parameters from Landmark Trajectories
c			ASME J. Mechanical Design, 114:274-282 (1992)
c	DATE:		12/4/92 - HJSIII
c
c
	SUBROUTINE DATISA(M,XI,VI,AI,FI,SS,WV,VP,P,AV,AQ,Q)
c
c	dimensions
	REAL*4 XI(3,M),VI(3,M),AI(3,M),FI(M),SS(3,3)
	REAL*4 WV(3),VP(3),P(3),AV(3),AQ(3),Q(3)
	REAL*4 X0(3),V0(3),A0(3),XX(3,3),VV(3,3),AA(3,3)
	REAL*4 BB(3,3),SSI(3,3),XXSS(3,3),VVSS(3,3),BBSS(3,3)
	REAL*4 JAC(3,3),JACI(3,3),RHSW(3),RHSA(3)
	REAL*4 WM(3,3),WM2(3,3),AM(3,3),BM(3,3),BMI(3,3)
	REAL*4 TEMP(3)
	REAL*4 U(3),E(3)
	REAL*4 W2IA(3,3),W2IAI(3,3)
	REAL*4 OV(3),T(3),FN(3),C(3),AC(3)
c
c	constants
	EPS=1.0E-5
	FM=FLOAT(M)
c
c	initialize mean values and summation matrices
	F0=0.0
	DO 10000 I=1,3
	X0(I)=0.0
	V0(I)=0.0
	A0(I)=0.0
	DO 10001 J=1,3
	XX(I,J)=0.0
	VV(I,J)=0.0
	AA(I,J)=0.0
10001	CONTINUE
10000	CONTINUE
c
c	accumulate mean values
	DO 10002 ILM=1,M
	F0=F0+FI(ILM)
	DO 10003 I=1,3
	X0(I)=X0(I)+FI(ILM)*XI(I,ILM)
	V0(I)=V0(I)+FI(ILM)*VI(I,ILM)
	A0(I)=A0(I)+FI(ILM)*AI(I,ILM)
10003	CONTINUE
10002	CONTINUE
c
c	normalize mean values
	F0=F0/FM
	DO 10004 I=1,3
	X0(I)=X0(I)/FM/F0
	V0(I)=V0(I)/FM/F0
	A0(I)=A0(I)/FM/F0
10004	CONTINUE
c
c	accumulate summation matrices over all landmarks
	DO 10005 ILM=1,M
	DO 10006 I=1,3
	DO 10007 J=1,3
	XX(I,J)=XX(I,J)+FI(ILM)*(XI(I,ILM)-X0(I))*(XI(J,ILM)-X0(J))
	VV(I,J)=VV(I,J)+FI(ILM)*VI(I,ILM)*(XI(J,ILM)-X0(J))
	AA(I,J)=AA(I,J)+FI(ILM)*AI(I,ILM)*(XI(J,ILM)-X0(J))
10007	CONTINUE
10006	CONTINUE
10005	CONTINUE
c
c	normalize summation matrices
	DO 10008 I=1,3
	DO 10009 J=1,3
	XX(I,J)=XX(I,J)/FM/F0
	VV(I,J)=VV(I,J)/FM/F0
	AA(I,J)=AA(I,J)/FM/F0
10009	CONTINUE
10008	CONTINUE
c
c	variance-covariance weighting
	CALL MATIN3(SS,SSI,DETSS)
	DO 10010 I=1,3
	DO 10011 J=1,3
	XXSS(I,J)=XX(I,1)*SSI(1,J)+XX(I,2)*SSI(2,J)+XX(I,3)*SSI(3,J)
	JAC(I,J)=-XXSS(I,J)
	VVSS(I,J)=VV(I,1)*SSI(1,J)+VV(I,2)*SSI(2,J)+VV(I,3)*SSI(3,J)
10011	CONTINUE
10010	CONTINUE
c
c	finish modified inertia matrix and velocity right-hand vector
	JAC(1,1)=XXSS(2,2)+XXSS(3,3)
	JAC(2,2)=XXSS(1,1)+XXSS(3,3)
	JAC(3,3)=XXSS(1,1)+XXSS(2,2)
	RHSW(1)=VVSS(3,2)-VVSS(2,3)
	RHSW(2)=VVSS(1,3)-VVSS(3,1)
	RHSW(3)=VVSS(2,1)-VVSS(1,2)
c
c	angular velocity
	CALL MATIN3(JAC,JACI,DETJAC)
	WV(1)=JACI(1,1)*RHSW(1)+JACI(1,2)*RHSW(2)+JACI(1,3)*RHSW(3)
	WV(2)=JACI(2,1)*RHSW(1)+JACI(2,2)*RHSW(2)+JACI(2,3)*RHSW(3)
	WV(3)=JACI(3,1)*RHSW(1)+JACI(3,2)*RHSW(2)+JACI(3,3)*RHSW(3)
	W=SQRT(WV(1)*WV(1)+WV(2)*WV(2)+WV(3)*WV(3))
	WM(1,2)=-WV(3)
	WM(1,3)=WV(2)
	WM(2,3)=-WV(1)
	WM(1,1)=0.0
	WM(2,2)=0.0
	WM(3,3)=0.0
	WM(2,1)=-WM(1,2)
	WM(3,1)=-WM(1,3)
	WM(3,2)=-WM(2,3)
c
c	sliding velocity along ISA and ISA location
	IF(W.GT.EPS)THEN
	   U(1)=WV(1)/W
	   U(2)=WV(2)/W
	   U(3)=WV(3)/W
	   SD=U(1)*V0(1)+U(2)*V0(2)+U(3)*V0(3)
	   VP(1)=SD*U(1)
	   VP(2)=SD*U(2)
	   VP(3)=SD*U(3)
	   P(1)=X0(1)+(WM(1,1)*V0(1)+WM(1,2)*V0(2)+WM(1,3)*V0(3))/W/W
	   P(2)=X0(2)+(WM(2,1)*V0(1)+WM(2,2)*V0(2)+WM(2,3)*V0(3))/W/W
	   P(3)=X0(3)+(WM(3,1)*V0(1)+WM(3,2)*V0(2)+WM(3,3)*V0(3))/W/W
c
c	special case for pure translation
	ELSE
	   SD=SQRT(V0(1)*V0(1)+V0(2)*V0(2)+V0(3)*V0(3))
	   U(1)=V0(1)/SD
	   U(2)=V0(2)/SD
	   U(3)=V0(3)/SD
	   VP(1)=V0(1)
	   VP(2)=V0(2)
	   VP(3)=V0(3)
	   P(1)=X0(1)
	   P(2)=X0(2)
	   P(3)=X0(3)
	ENDIF
c
c	acceleration right-hand side vector
	DO 10012 I=1,3
	WM2(I,1)=WM(I,1)*WM(1,1)+WM(I,2)*WM(2,1)+WM(I,3)*WM(3,1)
	WM2(I,2)=WM(I,1)*WM(1,2)+WM(I,2)*WM(2,2)+WM(I,3)*WM(3,2)
	WM2(I,3)=WM(I,1)*WM(1,3)+WM(I,2)*WM(2,3)+WM(I,3)*WM(3,3)
	BB(I,1)=AA(I,1)-WM2(I,1)*XX(1,1)-WM2(I,2)*XX(2,1)
     1                 -WM2(I,3)*XX(3,1)
	BB(I,2)=AA(I,2)-WM2(I,1)*XX(1,2)-WM2(I,2)*XX(2,2)
     1                 -WM2(I,3)*XX(3,2)
	BB(I,3)=AA(I,3)-WM2(I,1)*XX(1,3)-WM2(I,2)*XX(2,3)
     1                 -WM2(I,3)*XX(3,3)
	BBSS(I,1)=BB(I,1)*SSI(1,1)+BB(I,2)*SSI(2,1)+BB(I,3)*SSI(3,1)
	BBSS(I,2)=BB(I,1)*SSI(1,2)+BB(I,2)*SSI(2,2)+BB(I,3)*SSI(3,2)
	BBSS(I,3)=BB(I,1)*SSI(1,3)+BB(I,2)*SSI(2,3)+BB(I,3)*SSI(3,3)
10012	CONTINUE
	RHSA(1)=BBSS(3,2)-BBSS(2,3)
	RHSA(2)=BBSS(1,3)-BBSS(3,1)
	RHSA(3)=BBSS(2,1)-BBSS(1,2)
c
c	angular acceleration
	AV(1)=JACI(1,1)*RHSA(1)+JACI(1,2)*RHSA(2)+JACI(1,3)*RHSA(3)
	AV(2)=JACI(2,1)*RHSA(1)+JACI(2,2)*RHSA(2)+JACI(2,3)*RHSA(3)
	AV(3)=JACI(3,1)*RHSA(1)+JACI(3,2)*RHSA(2)+JACI(3,3)*RHSA(3)
	A=SQRT(AV(1)*AV(1)+AV(2)*AV(2)+AV(3)*AV(3))
	IF(A.GT.EPS)THEN
	   E(1)=AV(1)/A
	   E(2)=AV(2)/A
	   E(3)=AV(3)/A
	ENDIF
	AM(1,2)=-AV(3)
	AM(1,3)=AV(2)
	AM(2,3)=-AV(1)
	AM(1,1)=0.0
	AM(2,2)=0.0
	AM(3,3)=0.0
	AM(2,1)=-AM(1,2)
	AM(3,1)=-AM(1,3)
	AM(3,2)=-AM(2,3)
	DO 10013 I=1,3
	BM(I,1)=AM(I,1)+WM2(I,1)
	BM(I,2)=AM(I,2)+WM2(I,2)
	BM(I,3)=AM(I,3)+WM2(I,3)
10013	CONTINUE
c
c	solve for acceleration pivot
	CALL MATIN3(BM,BMI,DETBM)
	PARALL=(ABS(DETBM))**(1.0/3.0)
	IF(PARALL.GT.EPS)THEN
	   Q(1)=X0(1)-BMI(1,1)*A0(1)-BMI(1,2)*A0(2)-BMI(1,3)*A0(3)
	   Q(2)=X0(2)-BMI(2,1)*A0(1)-BMI(2,2)*A0(2)-BMI(2,3)*A0(3)
	   Q(3)=X0(3)-BMI(3,1)*A0(1)-BMI(3,2)*A0(2)-BMI(3,3)*A0(3)
	   AQ(1)=0.0
	   AQ(2)=0.0
	   AQ(3)=0.0
c
c	special case 1 - w=0, a=0
	ELSE
	   IF(W.LT.EPS)THEN
	      IF(A.LT.EPS)THEN
	         AQ(1)=A0(1)
	         AQ(2)=A0(2)
	         AQ(3)=A0(3)
	         Q(1)=X0(1)
	         Q(2)=X0(2)
	         Q(3)=X0(3)
c
c	special case 2 - w=0, a>0
	      ELSE
	         D=E(1)*A0(1)+E(2)*A0(2)+E(3)*A0(3)
	         AQ(1)=D*E(1)
	         AQ(2)=D*E(2)
	         AQ(3)=D*E(3)
	         Q(1)=X0(1)+(AM(1,1)*A0(1)+AM(1,2)*A0(2)
     1                      +AM(1,3)*A0(3))/A/A
	         Q(2)=X0(2)+(AM(2,1)*A0(1)+AM(2,2)*A0(2)
     1                      +AM(2,3)*A0(3))/A/A
	         Q(3)=X0(3)+(AM(3,1)*A0(1)+AM(3,2)*A0(2)
     1                      +AM(3,3)*A0(3))/A/A
	      ENDIF
c
c	special case 3 - w>0, a=0
	   ELSE
	      IF(A.LT.EPS)THEN
	         E(1)=U(1)
	         E(2)=U(2)
	         E(3)=U(3)
	         SDD=E(1)*A0(1)+E(2)*A0(2)+E(3)*A0(3)
	         AQ(1)=SDD*E(1)
	         AQ(2)=SDD*E(2)
	         AQ(3)=SDD*E(3)
	         Q(1)=X0(1)+(A0(1)-AQ(1))/W/W
	         Q(2)=X0(2)+(A0(2)-AQ(2))/W/W
	         Q(3)=X0(3)+(A0(3)-AQ(3))/W/W
c
c	special case 4 - w>0, a>0, w parallel to a
	      ELSE
	         SDD=E(1)*A0(1)+E(2)*A0(2)+E(3)*A0(3)
	         AQ(1)=SDD*E(1)
	         AQ(2)=SDD*E(2)
	         AQ(3)=SDD*E(3)
	         DO 10014 I=1,3
	         W2IA(I,1)=-AM(I,1)
	         W2IA(I,2)=-AM(I,2)
	         W2IA(I,3)=-AM(I,3)
	         W2IA(I,I)=W*W
10014	         CONTINUE
	         CALL MATIN3(W2IA,W2IAI,DETW2IA)
	         Q(1)=X0(1)+W2IAI(1,1)*(A0(1)-AQ(1))
     1               +W2IAI(1,2)*(A0(2)-AQ(2))+W2IAI(1,3)*(A0(3)-AQ(3))
	         Q(2)=X0(2)+W2IAI(2,1)*(A0(1)-AQ(1))
     1               +W2IAI(2,2)*(A0(2)-AQ(2))+W2IAI(2,3)*(A0(3)-AQ(3))
	         Q(3)=X0(3)+W2IAI(3,1)*(A0(1)-AQ(1))
     1               +W2IAI(3,2)*(A0(2)-AQ(2))+W2IAI(3,3)*(A0(3)-AQ(3))
	      ENDIF
	   ENDIF
	ENDIF
c
c	axode geometry
	IF(PARALL.GT.EPS)THEN
	   FLAM=SD/W
	   WD=U(1)*AV(1)+U(2)*AV(2)+U(3)*AV(3)
	   OV(1)=(WM(1,1)*AV(1)+WM(1,2)*AV(2)+WM(1,3)*AV(3))/W/W
	   OV(2)=(WM(2,1)*AV(1)+WM(2,2)*AV(2)+WM(2,3)*AV(3))/W/W
	   OV(3)=(WM(3,1)*AV(1)+WM(3,2)*AV(2)+WM(3,3)*AV(3))/W/W
	   O=SQRT(OV(1)*OV(1)+OV(2)*OV(2)+OV(3)*OV(3))
	   T(1)=OV(1)/O
	   T(2)=OV(2)/O
	   T(3)=OV(3)/O
	   FN(1)=-(WM(1,1)*T(1)+WM(1,2)*T(2)+WM(1,3)*T(3))/W
	   FN(2)=-(WM(2,1)*T(1)+WM(2,2)*T(2)+WM(2,3)*T(3))/W
	   FN(3)=-(WM(3,1)*T(1)+WM(3,2)*T(2)+WM(3,3)*T(3))/W
	   TEMP(1)=T(1)*BM(1,1)+T(2)*BM(2,1)+T(3)*BM(3,1)
	   TEMP(2)=T(1)*BM(1,2)+T(2)*BM(2,2)+T(3)*BM(3,2)
	   TEMP(3)=T(1)*BM(1,3)+T(2)*BM(2,3)+T(3)*BM(3,3)
	   D=(TEMP(1)*(Q(1)-P(1))+TEMP(2)*(Q(2)-P(2))
     1       +TEMP(3)*(Q(3)-P(3)))
     2	    /(TEMP(1)*U(1)+TEMP(2)*U(2)+TEMP(3)*U(3))
	   C(1)=P(1)+D*U(1)
	   C(2)=P(2)+D*U(2)
	   C(3)=P(3)+D*U(3)
	   AC(1)=BM(1,1)*(C(1)-Q(1))+BM(1,2)*(C(2)-Q(2))
     1          +BM(1,3)*(C(3)-Q(3))
	   AC(2)=BM(2,1)*(C(1)-Q(1))+BM(2,2)*(C(2)-Q(2))
     1          +BM(2,3)*(C(3)-Q(3))
	   AC(3)=BM(3,1)*(C(1)-Q(1))+BM(3,2)*(C(2)-Q(2))
     1          +BM(3,3)*(C(3)-Q(3))
	   SSD=(FN(1)*AC(1)+FN(2)*AC(2)+FN(3)*AC(3))/W-SD*O/W
	   FLLAM=SSD/O
c
c	special cases 1 & 2 for axode geometry - w=0
c	second order screw is not defined
	ELSE
	   IF(W.LT.EPS)THEN
	      FLAM=0.0
	      O=0.0
	      T(1)=0.0
	      T(2)=0.0
	      T(3)=0.0
	      FN(1)=0.0
	      FN(2)=0.0
	      FN(3)=0.0
	      C(1)=P(1)
	      C(2)=P(2)
	      C(3)=P(3)
	      SSD=0.0
	      FLLAM=0.0
c
c	special cases 3 & 4 for axode geometry - w>0
	   ELSE
	      FLAM=SD/W
	      O=0.0
	      C(1)=P(1)
	      C(2)=P(2)
	      C(3)=P(3)
	      AC(1)=BM(1,1)*(C(1)-Q(1))+BM(1,2)*(C(2)-Q(2))
     1             +BM(1,3)*(C(3)-Q(3))
	      AC(2)=BM(2,1)*(C(1)-Q(1))+BM(2,2)*(C(2)-Q(2))
     1             +BM(2,3)*(C(3)-Q(3))
	      AC(3)=BM(3,1)*(C(1)-Q(1))+BM(3,2)*(C(2)-Q(2))
     1             +BM(3,3)*(C(3)-Q(3))
	      TEMP(1)=WM(1,1)*AC(1)+WM(1,2)*AC(2)+WM(1,3)*AC(3)/W/W
	      TEMP(2)=WM(2,1)*AC(1)+WM(2,2)*AC(2)+WM(2,3)*AC(3)/W/W
	      TEMP(3)=WM(3,1)*AC(1)+WM(3,2)*AC(2)+WM(3,3)*AC(3)/W/W
	      SSD=SQRT(TEMP(1)*TEMP(1)+TEMP(2)*TEMP(2)+TEMP(3)*TEMP(3))
	      T(1)=TEMP(1)/SSD
	      T(2)=TEMP(2)/SSD
	      T(3)=TEMP(3)/SSD
	      FN(1)=-(WM(1,1)*T(1)+WM(1,2)*T(2)+WM(1,3)*T(3))/W
	      FN(2)=-(WM(2,1)*T(1)+WM(2,2)*T(2)+WM(2,3)*T(3))/W
	      FN(3)=-(WM(3,1)*T(1)+WM(3,2)*T(2)+WM(3,3)*T(3))/W
	      FLLAM=0.0
	   ENDIF
	ENDIF
c
c	done
	RETURN
	END
c
c********1*********2*********3*********4*********5*********6*********7**
c
c	MATIN3(A,B,DET)
c
c	invert 3x3 matrix  B=INV(A)
c
c	INPUTS
c	A = 3x3 matrix
c
c	OUTPUTS
c	B = 3x3 inverse matrix
c	DET = determinant of A
c
c	PRECISION:	single
c	COMMONS:	none
c	CALLS:		none
c	FUNCTIONS:	none
c	REFERENCE:	none
c	DATE:		9/23/92 - HJSIII
c
c
      SUBROUTINE MATIN3(A,B,DET)
c
c     dimensions
      REAL A(3,3),B(3,3)
c
c     determinant
      B(1,1)=A(2,2)*A(3,3)-A(2,3)*A(3,2)
      B(1,2)=A(1,3)*A(3,2)-A(1,2)*A(3,3)
      B(1,3)=A(1,2)*A(2,3)-A(1,3)*A(2,2)
      DET=A(1,1)*B(1,1)+A(2,1)*B(1,2)+A(3,1)*B(1,3)
      IF(DET.EQ.0.0)GO TO 99999
c
c     finish adjoint
      B(2,1)=A(2,3)*A(3,1)-A(2,1)*A(3,3)
      B(2,2)=A(1,1)*A(3,3)-A(1,3)*A(3,1)
      B(2,3)=A(1,3)*A(2,1)-A(1,1)*A(2,3)
      B(3,1)=A(2,1)*A(3,2)-A(2,2)*A(3,1)
      B(3,2)=A(1,2)*A(3,1)-A(1,1)*A(3,2)
      B(3,3)=A(1,1)*A(2,2)-A(1,2)*A(2,1)
c
c     divide by determinant
      DO 10000 I=1,3
      B(I,1)=B(I,1)/DET
      B(I,2)=B(I,2)/DET
      B(I,3)=B(I,3)/DET
10000 CONTINUE
c
c     done
99999 CONTINUE
      RETURN
      END