c                                        
c	CalcJCSAnglesRef  
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Calculate the joint JCS angles using a set of reference
c				angles.  Essentially what this means is that the nonsagittal
c				joint angles are forced to be a specific combination at 
c				at specific flexion angle.  A transformation which has
c				the net effect of rotating the distal segment LCS is 
c				applied to all frames of data for the a given joint during
c				the calculation of the joint's JCS angles.  This concept
c				originated from the desire to try to remove some of the
c				variability observed in nonsagittal plane joint angles as
c				compared with sagittal angles.  For certain joints, like
c				the knee,  the ab/adduction and in/external joint angles
c				should have the same value everytime the leg is at a certain
c				flexion angle.  For example, at full extension of the knee,
c				the ab/ad and in/ex angles might both be 0.  This is a 
c				structural restriction- under normal circumstances these
c				two angles can not be voluntarily changed.  For other joints
c				this idea cannot apply, such as the hip and ankle.  There
c				no guarantee that everytime I fully extended my hip that
c				the ab/ad and in/ex angles will always be at the same 
c				value.  Thus, this correction probably has limited 
c				applicability and shouldn't be used wholesale on every
c				joint.  The default reference joint angles are 0,0,0 but
c				a file containing joint reference angles can be read in
c				and this file may as few or as many joints as desired.
c				NOTE: To not do any correction of a specific joint's
c				angles set the reference angles for that joint to
c				-999,-999,-999 in the file.  The file format is:
c
c				A line starting with the letter c or C is a comment
c
c				Line with reference angles has 4 entries:
c
c				jointname  fl/extension  ab/adduction  in/external
c
c				If no file name is given with /FIL the file is assumed
c				to have the name 'ref_jnt_ang.dat'
c
c				See the derivation manual for a proper discussion of the
c				mathematics and concepts used here...
c
c	Routines called:
c
c	CalcJCSAnglesSimple
c	FindJntRefAng
c	HandleAngleError
c	InterpAngles
c	JcsAnglesRefAng
c	JCSAngtoRotMat
c	MatMult
c	MatTran
c	ReadSetRefAng
c	RotMattoJCSAng
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Joint			AngleSignJCS
c	Param			GP.jnt_num
c	Param			GP.num_jnt
c	Names			jnt_names
c	Joint			JntSegNum
c	Segment			SgpGCS
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Joint			JntAngRefCorAng
c	Joint			JntRefAng
c	Joint			JangJCS
c	Joint			JaxesJCS
c
      subroutine CalcJCSAnglesRef( CMND,SF,JF,OF )
      implicit none

      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_prm.inc'
      include 'a_seg.inc'
      include 'a_jnt.inc'
      include 'a_names.inc'

      record /CmndLine/     CMND
      record /segment_flag/ SF
      record /joint_flag/   JF
      record /option_flag/  OF  
      
      
      integer*4 ierr,i,j,k,jnt,error,start,DistSeg,ProxSeg,frm,
     &          NumError(3,MAX_JNT),JntNum
      real*4    ExtrapAngMax,AnkleFlexion,ExpRefMat(3,3),
     &          RefRotMat(3,3),AliRotMat(3,3),ExpRefMatTran(3,3)
      logical*4 lerror,CorrectAnkleFlexion,CorrectJntAng(MAX_JNT),
     &          SkipLine,InterpFlag,JntInterp,
     &          InterpError,TempUpDateFlag
      
      JF.angle_JCS= .false.         
      if ( .not. SF.position ) goto 1040     
      JntInterp= .true.
      InterpFlag= .false.

c
c     check for options 
c
      CorrectAnkleFlexion= .false.
      AnkleFlexion= -6.0
      ExtrapAngMax= 10.0*atan(1.0)/45.0
      if ( CMND.NumOpt(2) .ne. 0 ) then
        do i= 1, CMND.NumOpt(2)
          if ( CMND.OptText(i,2) .eq. 'ETH' ) then
            if ( .not. CMND.OptNumOK(i,2) ) goto 1020 ! error
            if ( CMND.OptIntVal(i,2) .lt. 0 ) goto 1020 ! error
            ExtrapAngMax= CMND.OptRealVal(i,2)*atan(1.0)/45.0
            call RemoveCMNDOpt( CMND,2,i )
          else if ( CMND.OptText(i,2) .eq. 'NAF' ) then
            CorrectAnkleFlexion= .false.
          else if ( CMND.OptText(i,2) .eq. 'AFL' ) then
            if ( CMND.OptNumOK(i,2) ) then
              if ( abs(CMND.OptIntVal(i,2)) .gt. 30.0 ) goto 1030
              AnkleFlexion= CMND.OptIntVal(i,2)
            end if
            CorrectAnkleFlexion= .true.
          end if
        end do
      end if
c
c     Check for reference angle file
c
      call ReadSetRefAng( CMND,OF,JntRefAng,lerror )
      if ( lerror ) return
c
c     NOTE:  There is a provision to allow skipping of a joint so
c            that its angles will not be changed.
c            This 'trick' is used by setting the joint reference angles
c            all equal to -999
c
      do i= 1, GP.num_jnt
        jnt= GP.jnt_num(i)
        CorrectJntAng(jnt)= .false.
        if ( (abs(JntRefAng(1,jnt) + 999.0) .gt. 1.0e-3) .or.
     &       (abs(JntRefAng(2,jnt) + 999.0) .gt. 1.0e-3) .or.
     &       (abs(JntRefAng(3,jnt) + 999.0) .gt. 1.0e-3)      ) 
     &    CorrectJntAng(jnt)= .true.
      end do
c
c     Calculate simple JCS angles so that they may be searched for
c     reference positions
c
      TempUpDateFlag= OF.Update
      OF.Update= .false.
      write(6,200)
      if ( OF.ToLog ) write(3,200)
200   format(' Calculating original JCS angles...'/)
      call CalcJCSAnglesSimple( CMND,SF,JF,OF )
      OF.Update= TempUpDateFlag
      if ( .not. JF.angle_JCS ) goto 1050
c
c     Process each joint
c
      do i= 1, GP.num_jnt
        jnt= GP.jnt_num(i)
        if ( CorrectJntAng(jnt) ) then
          if ( (jnt .ne. pvlb) .and. (jnt .ne. neck) ) then
            if ( OF.UpDate ) write(6,210) i,GP.num_jnt,jnt_names(jnt)
210         format('+Calculating angles for joint # ',i2,'/',i2,': ',a4)
        
c
c       Calculate the correction matrix
c
            call FindJntRefAng( jnt,ExtrapAngMax,JntRefAng(1,jnt),
     &                          ExpJntRefAng(1,jnt),lerror )
            if ( lerror ) goto 1060
            call JCSAngtoRotMat( ExpJntRefAng(1,jnt),AngleSignJCS(1,jnt),ExpRefMat )
            call JCSAngtoRotMat( JntRefAng(1,jnt),AngleSignJCS(1,jnt),RefRotMat )
            call MatTran( ExpRefMat,ExpRefMatTran )
            call MatMult( ExpRefMatTran,RefRotMat,AliRotMat )
            call RotMattoJCSAng( AliRotMat,JntAngRefCorAng(1,jnt),error )
            DistSeg= JntSegNum(1,jnt)                       
            ProxSeg= JntSegNum(2,jnt)
c
c           Use the correction matrix to calculate JCS angles
c
            do frm= 1, MP.nfrm         
              call JcsAnglesRefAng( AliRotMat,
     &                              SgpGCS(x,x,frm,DistSeg),
     &                              SgpGCS(x,x,frm,ProxSeg),     
     &                              JangJCS(flex,frm,jnt),
     &                              JaxesJCS(x,flex,frm,jnt),error )
              if ( error .ne. 0 ) then ! error
                InterpFlag= .true.
                SkipLine=   .true.
                call HandleAngleError( error,frm,jnt,NumError(1,jnt),
     &                                 ErrFrm(1,1,jnt),JntInterp,OF  )
              else
                do k= x, z
                  JangJCS(k,frm,jnt)= AngleSignJCS(k,jnt)*
     &                                      JangJCS(k,frm,jnt)
                end do
              end if
c
c           Subtract 6 deg (default) from ankle flexion (plantarflexion) to
c           make the angle calculated the same as in previous studies
c                     
              if ( ( (jnt .eq. rtak) .or. (jnt .eq. lfak) ) .and.
     &             CorrectAnkleFlexion )
     &          JangJCS(flex,frm,jnt)= JangJCS(flex,frm,jnt) +
     &                                       AnkleFlexion
            end do ! frm
          end if ! not pvlb joint
        end if ! Is joint angle correction to be done....
      end do ! jnt
c
c     If any angles are missing due to errors in JCSAngles calculation,
c     interpolate the the missing data.
c
      JF.angle_JCS= .true.                         
      if ( InterpFlag ) then
        JntInterp= .true.
        call InterpAngles( NumError,ErrFrm,JangJCS,JaxesJCS,
     &                     JntInterp,OF,InterpError )
        if ( InterpError ) JF.angle_JCS= .false.
      end if
      JF.RefCorrectAng= .true.
      do i= 1, GP.num_jnt
        jnt= GP.jnt_num(i)
        if ( .not. CorrectJntAng(jnt) ) then
          write(6,300) jnt_names(jnt)
          if ( OF.ToLog ) write(3,300) jnt_names(jnt)
300       format(' NOTE: ',a4,' joint angles not corrected')
        end if
      end do
      return                                    

1020  write(6,1025) CMND.OptText(i,2)
      if ( OF.ToLog ) write(3,1025) CMND.OptText(i,2)
1025  format(' Error- ',a3,' is not a valid extrapolation threshold angle'/
     &       '        Valid angles are >0')
      return

1030  write(6,1035)
      if ( OF.ToLog ) write(3,1035)
1035  format(' Error- Incorrect amount of ankle flexion correction'/
     &       '        Correction must be >-30 and <30')
      return

1040  write(6,1045)
      if ( OF.ToLog ) write(3,1045)
1045  format(' Error- Segment position has not been calculated yet')
      return
                                                        
1050  write(6,1055)       
      if ( OF.ToLog ) write(3,1055)
1055  format(' Error- JCS angles with reference correction failed...')
      return

1060  write(6,1065) jnt_names(jnt)   
      if ( OF.ToLog ) write(3,1065) jnt_names(jnt)
1065  format(' Error- Interpolation of data reference angles of ',a4,' failed'/
     &       '        JCS angles with reference correction failed...')
      return
      end
c                                        
c	FindJntRefAng  
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Find the set of joint angles in the experimental data
c				which most closely correspond to the flexion angle given
c				in the joint reference angles.  This will probably require
c				an interpolation of the joint angles to the extact reference
c				flexion angle.  E.G., if the reference angles are 0,0,0
c				and there is experimental joint angle data at flexion
c				angles of 1.3 deg and -0.3 deg then interpolate between
c				these angles to find the ab/ad and in/ex angles when the
c				experimental joint angle would become 0 deg.  This is a 
c				little messy but necessary.  Look at the derivation notes
c				for ANZ for more details and some diagrams to make more
c				clear as to what is going on here...
c
c	Routines called:
c
c	Lagrange_Interp
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Joint			JangJCS
c	Joint			jnt_names
c	Param			MP.nfrm
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	None
c
      subroutine FindJntRefAng( jnt,ExtrapAngMax,FlexRefAng,
     &                          ExpRefAng,error )
      implicit none
      
      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_prm.inc'
      include 'a_names.inc'
      include 'a_jnt.inc'
      
      integer*4 jnt
      real*4    ExtrapAngMax,FlexRefAng,ExpRefAng(3)
      logical*4 error
      
      integer*4 frm,i,j,PolyOrder,start,half,MinFlexFrm,MaxFlexFrm
      real*4    MinFlexAng,MaxFlexAng,Sign,AngData(4),TimeData(4),
     &          InterpTime,InterpAng
c
c     Determine whether reference flexion angle is within the range
c     of flexion angle values calculated in the experimental data
c
      error= .false.
      PolyOrder= 3
      MinFlexAng= JangJCS(1,1,jnt)
      MinFlexFrm= 1
      MaxFlexAng= JangJCS(1,1,jnt)
      MaxFlexFrm= 1
      do frm= 2, MP.nfrm
        if ( JangJCS(1,frm,jnt) .lt. MinFlexAng ) then
          MinFlexAng= JangJCS(1,frm,jnt)
          MinFlexFrm= frm
        end if
        if ( JangJCS(1,frm,jnt) .gt. MaxFlexAng ) then
          MaxFlexAng= JangJCS(1,frm,jnt)
          MaxFlexFrm= frm
        end if
      end do
      if ( FlexRefAng .lt. MinFlexAng ) then
        PolyOrder= 2
        start= 1
        if ( FlexRefAng .lt. MinFlexAng - ExtrapAngMax ) then
          write(6,100) FlexRefAng,MinFlexAng,jnt_names(jnt)
100       format(' Warning- Reference angle is not within data''s joint'/
     &           '          angle range   Reference Angle    :',f10.5/
     &           '                        Min Measured Angle :',f10.5/
     &           '          Using ',a4,' angles at minimum flexion')
          do i= 1, 3
            ExpRefAng(i)= JangJCS(i,MinFlexFrm,jnt)
          end do
          return
        end if
      else if ( FlexRefAng .gt. MaxFlexAng ) then
        PolyOrder= 2
        start= MP.nfrm - 2
        if ( FlexRefAng .gt. MaxFlexAng + ExtrapAngMax ) then
          write(6,110) FlexRefAng,MaxFlexAng,jnt_names(jnt)
110       format(' Warning- Reference angle is not within data''s joint'/
     &           '          angle range   Reference Angle    :',f10.5/
     &           '                        Max Measured Angle :',f10.5/
     &           '          Using ',a4,' angles at maximum flexion')
          do i= 1, 3
            ExpRefAng(i)= JangJCS(i,MaxFlexFrm,jnt)
          end do
          return
        end if
      end if
c
c     Interpolate/Extrapolate the experimental data joint angles at 
c     the reference flexion angle
c
c     NOTE: Use linear interpolation to get the time at which the
c           reference flexion angle occured to avoid problems at
c           sections of the angle vs time curve where a min/max
c           has occured.  Go ahead and use polynomial interpolation
c           for the other two angles however.
c
      if ( PolyOrder .ne. 2 ) then ! find interpolation frames
        Sign= 1.0
        if ( JangJCS(1,1,jnt) .lt. FlexRefAng ) Sign= -1.0
        frm= 2
        do while ( (Sign*(JangJCS(1,frm,jnt)-FlexRefAng) .gt. 0.0) .and.
     &             (frm .le. MP.nfrm) )
          frm= frm + 1
        end do
        frm= frm - 1
        half= PolyOrder/2
        if ( (frm - half + 1) .lt. 1) then
          start= 1
        else if ( (frm+PolyOrder+1-half) .gt. MP.nfrm ) then
          start= MP.nfrm - PolyOrder
        else
          start= frm - half + 1
        end if
      end if
      do j= 1, PolyOrder + 1
        AngData(j)=  JangJCS(1,start+j-1,jnt)
        TimeData(j)= float(start+j-1)/rMtnFrmRate
      end do
      call Lagrange_Interp( AngData(half),TimeData(half),2,
     &                      FlexRefAng,InterpTime,error )
      if ( error ) return
      ExpRefAng(1)= FlexRefAng
      do i= 2, 3
        do j= 1, PolyOrder + 1
          AngData(j)= JangJCS(i,start+j-1,jnt)
        end do
        call Lagrange_Interp( TimeData,AngData,PolyOrder+1,
     &                        InterpTime,InterpAng,error )
        if ( error ) return
        ExpRefAng(i)= InterpAng
      end do
      return
      end
c                                        
c	JcsAnglesRefAng  
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Calculate the JCS joint angles (really just YXZ floating
c				axis euler angles) between two body segments while using
c				a correction rotation of the LCS1 (distal) segment. 
c				LCS1 is rotated first by AliRotMat and then the JCS angles
c				required to rotate it to the LCS2 orientation are calculated. 
c				This routine is essentially identical to subroutine JCSAngles
c				contained in math_sub.for except for the rotation 
c				correction.
c             
c     Error Codes: 0 - no error
c                  1 - Angle 1 (flexion/extension) bad
c                  2 - Angle 2 (abduction/adduction) bad
c                  4 - Angle 3 (internal/external) bad
c                  3 - Angles 1 and 2 bad
c                  5 - Angles 1 and 3 bad
c                  6 - Angles 2 and 3 bad
c                  7 - Angles 1, 2, and 3 bad
c
c	Routines called:
c
c	MatMult
c	MatTran
c	VecNorm
c	VecX
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	None
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	None
c
      subroutine JcsAnglesRefAng( AliRotMat,LCS1,LCS2,
     &                            angles,axes,error )
      implicit none

      real*4    LCS1(3,3),LCS2(3,3),angles(3),AliRotMat(3,3),axes(3,3)
      integer*4 error
                               
      real*4    TempMat(3,3),RotMat(3,3),cos2,rtod,
     &          RotMatA(3,3),LCS1Adj(3,3)
      integer*4 i
      logical*4 lerror,VecNorm
                       

      error= 0                     
      rtod= 45.0/atan(1.0)
      call MatTran( LCS2, TempMat ) ! Invert matrix
      call MatMult( TempMat, LCS1, RotMatA ) ! now calc lcs1 relative to lcs2
      call MatMult( RotMatA,AliRotMat,RotMat )
      cos2= sqrt( RotMat(2,1)**2 + RotMat(2,2)**2 )
      if ( cos2 .lt. 1.0e-5 ) then        ! special case...
        if ( RotMat(2,3) .lt. 0.0 ) then
          angles(1)= atan2( RotMat(1,2),RotMat(1,1) )
          angles(2)= 90.0
        else
          angles(1)= atan2( -RotMat(1,2),RotMat(1,1) )
          angles(2)= -90.0
        end if
        angles(3)= 0.0
        error= 7
        return
      else
        angles(2)= atan2( -RotMat(2,3),cos2 )
      end if
      if ( (abs(RotMat(1,3)) .lt. 1.0e-5) .and.
     &     (abs(RotMat(3,3)) .lt. 1.0e-5)       ) then
        angles(1)= 0.0
        error= error + 1       
      else  
        angles(1)= atan2( RotMat(1,3)/cos2,RotMat(3,3)/cos2 )
      end if
      if ( (abs(RotMat(2,1)) .lt. 1.0e-5) .and.
     &     (abs(RotMat(2,2)) .lt. 1.0e-5)       ) then
        angles(3)= 0.0
        error= error + 2
      else
        angles(3)= atan2( RotMat(2,1)/cos2,RotMat(2,2)/cos2 )
      end if
      do i= 1, 3
        angles(i)= angles(i)*rtod 
      end do   
c
c     If there are no angle calculation errors than calculate the JCS
c     axes with the correction applied
c
c     I'm not really sure why the axes are zd X yp when it seems to me
c     that they ought to be yp X zd- it is the same in CalcJCSAngleSimple
c     I guess I have a sign correction in the later calculations....
c     This damn program has gotten so big I can't recall what I did !!
c     7/30/90
c       
      call MatMult( LCS2,RotMat,LCS1Adj )
      do i= 1, 3
        axes(i,1)= LCS2(i,2)
        axes(i,3)= LCS1Adj(i,3)
      end do
      call VecX( axes(1,3),axes(1,1),axes(1,2) )
      lerror= VecNorm( axes(1,2),axes(1,2) )
      return
      end                  
c                                        
c	RotMattoJCSAng  
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Calculate the JCS joint angles (really just YXZ floating
c				axis euler angles) between two body segments.  The
c				rotation matrix input is the rotation of the distal segment
c				relative to the proximal segment in terms of the proximal
c				segment's LCS.
c				This routine is essentially identical to subroutine JCSAngles
c				contained in math_sub.for except that the joint axes are
c				not calculated and the input is not the segment rotation
c				matrices relative to the lab GCS.
c             
c     Error Codes: 0 - no error
c                  1 - Angle 1 (flexion/extension) bad
c                  2 - Angle 2 (abduction/adduction) bad
c                  4 - Angle 3 (internal/external) bad
c                  3 - Angles 1 and 2 bad
c                  5 - Angles 1 and 3 bad
c                  6 - Angles 2 and 3 bad
c                  7 - Angles 1, 2, and 3 bad
c
c	Routines called:
c
c	None
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	None
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	None
c
      subroutine RotMattoJCSAng( RotMat,angles,error )
      implicit none

      real*4    angles(3),RotMat(3,3)
      integer*4 error
                               
      real*4    cos2,rtod
      integer*4 i
                       

      error= 0                     
      rtod= 45.0/atan(1.0)
      cos2= sqrt( RotMat(2,1)**2 + RotMat(2,2)**2 )
      if ( cos2 .lt. 1.0e-5 ) then        ! special case...
        if ( RotMat(2,3) .lt. 0.0 ) then
          angles(1)= atan2( RotMat(1,2),RotMat(1,1) )
          angles(2)= 90.0
        else
          angles(1)= atan2( -RotMat(1,2),RotMat(1,1) )
          angles(2)= -90.0
        end if
        angles(3)= 0.0
        error= 7
        return
      else
        angles(2)= atan2( -RotMat(2,3),cos2 )
      end if
      if ( (abs(RotMat(1,3)) .lt. 1.0e-5) .and.
     &     (abs(RotMat(3,3)) .lt. 1.0e-5)       ) then
        angles(1)= 0.0
        error= error + 1       
      else  
        angles(1)= atan2( RotMat(1,3)/cos2,RotMat(3,3)/cos2 )
      end if
      if ( (abs(RotMat(2,1)) .lt. 1.0e-5) .and.
     &     (abs(RotMat(2,2)) .lt. 1.0e-5)       ) then
        angles(3)= 0.0
        error= error + 2
      else
        angles(3)= atan2( RotMat(2,1)/cos2,RotMat(2,2)/cos2 )
      end if
      do i= 1, 3
        angles(i)= angles(i)*rtod 
      end do   
      return
      end                  
c                                        
c	JCSAngtoRotMat  
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Calculate the rotation matrix expressing the orientation
c				of the distal segment across a joint relative to the 
c				proximal segment in terms of the proximal segment's LCS.
c				The JCS angles of the joint are used to do this.
c				This is essentially the inverse of the subroutine
c				JCSAngtoRotMat.  NOTE: The angle sign correction is
c				needed because the JCS angles are stored with signs that
c				correspond to orthopedic conventions which do not always
c				agree with the mathematical definition of rotation
c				(right hand rule positive rotation or CCW is +).
c
c	Routines called:
c
c	None
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	None
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	None
c
      subroutine JCSAngtoRotMat( Angles,AngleSignJCS,RotMat )
      implicit none
      
      real*4 Angles(3),AngleSignJCS(3),RotMat(3,3)
      
      real*4 cy,sy,cx,sx,cz,sz,dtor

      dtor= atan(1.0)/45.0

      cy= cos(AngleSignJCS(1)*Angles(1)*dtor)        
      sy= sin(AngleSignJCS(1)*Angles(1)*dtor)
      cx= cos(AngleSignJCS(2)*Angles(2)*dtor)
      sx= sin(AngleSignJCS(2)*Angles(2)*dtor)
      cz= cos(AngleSignJCS(3)*Angles(3)*dtor)
      sz= sin(AngleSignJCS(3)*Angles(3)*dtor)
      RotMat(1,1)=  cz*cy + sz*sx*sy
      RotMat(1,2)= -sz*cy + cz*sx*sy
      RotMat(1,3)=  cx*sy
      RotMat(2,1)=  sz*cx
      RotMat(2,2)=  cz*cx
      RotMat(2,3)= -sx
      RotMat(3,1)= -cz*sy + sz*sx*cy
      RotMat(3,2)=  sz*sy + cz*sx*cy
      RotMat(3,3)=  cx*cy
      return
      end  
c                                        
c	ReadSetRefAng  
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Read in the joint reference angles from a file.  The 
c               file has the following format:
c
c				A line starting with the letter c or C is a comment
c
c				Line with reference angles has 4 entries:
c
c				jointname  fl/extension  ab/adduction  in/external
c
c				If no file name is given with /FIL the file is assumed
c				to have the name 'ref_jnt_ang.dat'
c
c	Routines called:
c
c	GetJntNum
c	OpenTextFile
c	ParseLine
c	RemoveCMNDOpt
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Param			GP.jnt_num
c	Param			GP.num_jnt
c	Names			jnt_names
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	None
c
      subroutine ReadSetRefAng( CMND,OF,RefAng,error )
      implicit none
      
      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_names.inc'
      include 'a_prm.inc'
      
      record /CmndLine/    CMND
      record /option_flag/ OF
      
      real*4    RefAng(3,MAX_JNT)
      logical*4 error

      record /CmndLine/ LineParts
      
      integer*4 i,j,start,ierr,JntNum,LineNum,end
      logical*4 FileNameGiven,GetJntNum,OpenTextFile,ReadAngles
      character line*80,FileText*80,JntText*3,filename*40

      error= .true.
      do i= 1, MAX_JNT
        do j= 1, 3
          RefAng(j,i)= 0.0
        end do
      end do
      FileNameGiven= .false.
      ReadAngles= .false.
      if ( CMND.NumOpt(2) .ne. 0 ) then
        do i= 1, CMND.NumOpt(2)
          if ( CMND.OptText(i,2) .eq. 'FIL' ) then
            FileNameGiven= .true.
            start= CMND.OptStart(i,2)
            do while ( (start .lt. CMND.OptEnd(i,2)) .and.
     &                 (CMND.Text(1)(start:start) .ne. '=') )
              start= start + 1               
            end do
            start= start + 1
            end= CMND.OptEnd(i,2)
            FileText= CMND.Text(1)
            call RemoveCMNDOpt( CMND,2,i )
            ReadAngles= .true.
          end if
        end do
      end if
      if ( FileNameGiven ) then
        if ( .not. OpenTextFile(FileText,start,.false.) ) return
        ierr= 0
        filename= CMND.Text(1)(start:end)
      else if ( ReadAngles ) then ! default file name
        open( unit=2,file='ref_jnt_ang.dat',status='old',
     &        carriagecontrol='none',iostat=ierr )
        filename= 'ref_jnt_ang.dat'
      end if
      if ( (ierr .eq. 0) .and. ReadAngles ) then ! file is present
        LineNum= 0
        do while ( ierr .eq. 0 ) 
          read(2,100,iostat=ierr) line
100       format(a80)
          if ( ierr .eq. 0 ) then
            LineNum= LineNum + 1
            call ParseLine( line,LineParts )
            JntText= LineParts.CmndText(1)
            if ( JntText(1:1) .ne. 'C' ) then ! not a comment...
              if ( .not. GetJntNum(JntText,JntNum) ) goto 1000
              do i= 1, 3      
                if ( .not. LineParts.CmndNumOK(i+1) ) goto 1010
                RefAng(i,JntNum)= LineParts.CmndRealVal(i+1)
              end do
            end if ! line is not a comment
          end if
        end do
        close(unit=2)
      else if ( ReadAngles ) then
        write(6,110)
        if ( OF.ToLog ) write(3,110)
110     format(' Using default reference angles- reference angle file'/
     &         '                               ''ref_jnt_ang.dat'' not found'/)
      else
        write(6,120)
        if ( OF.ToLog ) write(3,120)
120     format(' Using default reference angles- All 0.0''s')
      end if
      error= .false.
      return

1000  write(6,1005) JntText,filename,LineNum,line,
     &              (jnt_names(GP.jnt_num(j)),j=1,GP.num_jnt)
      if ( OF.ToLog ) write(3,1005) JntText,filename,LineNum,line,
     &              (jnt_names(GP.jnt_num(j)),j=1,GP.num_jnt)
1005  format(' Error- ',a3,' is not a valid joint name in file'//
     &       '        File : ',a40/
     &       '        Line # ',i2/
     &       '        Text as read : ',a80/
     &       '        Valid names are : ',5a5/2(26x,5a5/)/)
      close(unit=2)
      return

1010  write(6,1015) jnt_names(JntNum),i,LineParts.CmndText(i+1),
     &              LineNum,line
      if ( OF.ToLog ) write(3,1015) jnt_names(JntNum),
     &                                      i,LineParts.CmndText(i+1),
     &                                      LineNum,line
1015  format(' Error- ',a4,' does not have a valid number for angle',
     &       ' component # ',i1/
     &       '        ',a3,' was read from file'/
     &       '        Invalid text on line # ',i2/
     &       '        Text as read : ',a80)
      close(unit=2)
      return
      end
c                                        
c	CalcAnatSgpRef  
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Calculate the correction rotations for each segment's
c				LCS such they line up with a set of joint reference
c				angles at one position.  This is intended to be used with
c				a set of marker data taken with a person in a standing position 
c				with all the joints except the ankles fully extended
c				(other reference positions can also be used).  Assuming
c				that the joint angles should be what the reference angles
c				are, calculate corrections to orientations of the segment
c				LCS's in this position.  The idea behind this is to correct
c				for twists in the segment orientations induced by the
c				markers being placed on the body such that the calculated
c 				segment LCS's do not align with anatomic Coordinate Systems.
c				These anatomic CS's are somewhat nebulous since they cannot
c				be observed except by xray but I feel the concept is
c				valid.  This correction is especially vital if rigid bodies
c				are place on the segments since it will be quite easy to
c				place them in a skewed orientation.  This correction will
c				NOT work if a patient cannot be measured in a stationary
c				position where the joint angles are approximately known.
c				This correction has the potential to make nonsagittal plane
c				segment and joint kinematics much more consistent between
c				observation sessions than they have been up to now.
c				However, if used blindly this method can also really make
c				a mess out of useful data.  THUS, DO NOT USE THIS UNLESS
c				YOU UNDERSTAND WHAT IT IS DOING !!!!!!!
c
c				The corrections are calculated in a chained sequence starting
c				at the pelvis and moving distally- first the legs, then the
c				trunk and head and lastly the arms.  See the derivation
c				notes for ANZ to get the mathematical details...
c
c	Routines called:
c
c	JCSAngtoRotMat
c	MatMult
c	MatTran
c	ReadSetRefAng
c	RotMattoJCSAng
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Joint			AngleSignJCS
c	Segment			SgpAvg
c	Joint			JntSegNum
c	Names			seg_names
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Segment			SgpRefCorAng
c
      subroutine CalcAnatSgpRef( CMND,AF,OF ) 
      implicit none
      
      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_prm.inc'          
      include 'a_seg.inc'
      include 'a_jnt.inc'
      include 'a_names.inc'

      record /CmndLine/     CMND
      record /avg_pos_flag/ AF
      record /option_flag/  OF
      
      integer*4 JntNum(15),ProxSeg,DistSeg,i,j,jnt,ierr,seg
      real*4    AvgJntRefAng(3,15),NoSign(3),MatA(3,3),MatB(3,3),
     &          MatC(3,3),CorMat(3,3),RefRotMat(3,3),LabAxes(3,3)
      logical*4 lerror
      
c     pvlb,rthp,rtkn,rtak,lfhp,lfkn,lfak,pvtk,neck,rtsh,rtel,lfsh,lfel
      data JntNum / 7,3,2,1,6,5,4,8,13,10,9,12,11,0,0 /
      
      data NoSign / 1.0,1.0,1.0 /

      data LabAxes / 1.0,0.0,0.0, 0.0,1.0,0.0, 0.0,0.0,1.0 /

      if ( .not. AF.position ) goto 1000 ! error
      if ( GP.analysis_type .ne. FULL_ANL ) goto 1010 ! error
      if ( .not. AF.angle_JCS ) goto 1020 ! error
c
c     Check for reference angle file
c
      call ReadSetRefAng( CMND,OF,AvgJntRefAng,lerror )
      if ( lerror ) return
c
c     Carry out reference correction sequentially
c     NOTE:  There is a provision to allow skipping of a segment so
c            that later in SEG POS/REF its position will not be changed
c            because the correction angles will be set to (0,0,0).
c            This 'trick' is used by setting the joint reference angles
c            for the joint proximal to the segment all equal to -999
c
      do i= 1, 13
        jnt= JntNum(i)
        DistSeg= JntSegNum(1,jnt)                       
        ProxSeg= JntSegNum(2,jnt)
        if ( (abs(AvgJntRefAng(1,jnt) + 999.0) .le. 1.0e-3) .and.
     &       (abs(AvgJntRefAng(2,jnt) + 999.0) .le. 1.0e-3) .and.
     &       (abs(AvgJntRefAng(3,jnt) + 999.0) .le. 1.0e-3)      ) then
          do j= 1, 3                        ! no correction
            SgpRefCorAng(j,DistSeg)= 0.0
          end do
          write(6,200) seg_names(DistSeg)
          if ( OF.ToLog ) write(3,200) seg_names(DistSeg)
200       format(' NOTE: ',a4,' orientation correction not calculated')
        else ! do segment orientation correction
          do j= 1, 3
            if ( abs(AvgJntRefAng(j,jnt) + 999.0) .lt. 1.0e-3 ) 
     &         AvgJntRefAng(j,jnt)= JangAvgJCS(j,jnt) ! use avg jnt comp
          end do 
          if ( i .eq. 1 ) then ! special case
            call JCSAngtoRotMat( AvgJntRefAng(1,jnt),
     &                           AngleSignJCS(1,jnt),RefRotMat )
            call MatTran( SgpAvg(1,1,DistSeg),MatA )
            call MatMult( MatA,RefRotMat,CorMat )
            call RotMattoJCSAng( CorMat,SgpRefCorAng(1,DistSeg),ierr )
          else
            call JCSAngtoRotMat( AvgJntRefAng(1,jnt),
     &                           AngleSignJCS(1,jnt),RefRotMat )
            call MatTran( SgpAvg(1,1,DistSeg),MatA )
            call JCSAngtoRotMat( SgpRefCorAng(1,ProxSeg),
     &                           NoSign,CorMat )
            call MatMult( CorMat,RefRotMat,MatB )
            call MatMult( SgpAvg(1,1,ProxSeg),MatB,MatC )
            call MatMult( MatA,MatC,CorMat )
            call RotMattoJCSAng( CorMat,SgpRefCorAng(1,DistSeg),ierr )
          end if
        end if
      end do
      write(6,300)
      if ( OF.ToLog ) write(3,300)
300   format(' Joint reference angles used for reference correction'//
     &       ' Joint   Flex/Ext   Ab/Ad   Int/Ext'//)
      do i= 1, GP.num_jnt
        jnt= GP.jnt_num(i)
        write(6,310) jnt_names(jnt),(AvgJntRefAng(j,jnt),j=1,3)
        if ( OF.ToLog ) write(3,310) jnt_names(jnt),
     &                                       (AvgJntRefAng(j,jnt),j=1,3)
310     format(1x,a4,3(1x,f10.4))
      end do
c
c     Lastly, apply these corrections to the average segment position
c     data.  This allows the results of the correction to be readily
c     apparent when the body is viewed in 3D in TELIO.  The average
c     JCS joint angles can also be calculated from this data to see if
c     they are the same as the reference angles as a double check of
c     the reference matrix calculations...
c
      do i= 1, GP.num_seg
        seg= GP.seg_num(i)
        call JCSAngtoRotMat( SgpRefCorAng(1,seg),NoSign,CorMat )
        call MatMult( SgpAvg(1,1,seg),CorMat,
     &                SgpAvg(1,1,seg)         )
      end do
      do i= 1, GP.num_jnt
        jnt= GP.jnt_num(i)
        DistSeg= JntSegNum(1,jnt)                       
        ProxSeg= JntSegNum(2,jnt)
        if ( jnt .eq. pvlb ) then ! special case for pelvis  
          call JCSAngles( SgpAvg(x,x,DistSeg),
     &                    LabAxes,JangAvgJCS(flex,jnt),lerror )
        else
          call JCSAngles( SgpAvg(x,x,DistSeg),
     &                    SgpAvg(x,x,ProxSeg),     
     &                    JangAvgJCS(flex,jnt),lerror )
        end if
      end do
      AF.RefCorrectAng= .true.
      return

1000  write(6,1005)
      if ( OF.ToLog ) write(3,1005)
1005  format(' Error- Average segment position has not been calculated yet')
      return
 
1010  write(6,1015)
      if ( OF.ToLog ) write(3,1015)
1015  format(' Error- This calculation is only valid for full body analysis')
      return

1020  write(6,1025)
      if ( OF.ToLog ) write(3,1025)
1025  format(' Error- Average joint JCS angle has not been calculated yet')
      return
      end
                                                      
