c                                        
c	do_SEG_calc  
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Controlling routine that calls the segment kinematic 
c				and kinetic calculation routines
c
c	Routines called:
c		CalcSegParams 
c		CalcMrkDiagnostics
c		CalcFullBodySegPos
c		CalcUpperBodySegPos
c		CalcSegVelAcc
c		CalcSegEnergy
c		CalcSegFiniteScrews
c		CalcSegInstantScrews
c		CalcBodyCM
c		CalcSegMomentum
c		CalcSegGCSAngles
c		DigFilterSeg
c
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	None
c
c	Common Block Variable MODIFIED by routine:
c	Block Name		Variable Name
c
c	None
c
      subroutine do_SEG_calc( CMND,MF,SF,JF,AF,OF )
      implicit none                                                   

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

      record /CmndLine/      CMND
      record /marker_flag/   MF
      record /segment_flag/  SF
      record /avg_pos_flag/  AF
      record /joint_flag/    JF
      record /option_flag/   OF

      if ( CMND.CmndText(2) .eq. 'PRM' ) then
        call CalcSegParams( CMND,MF,SF,OF )
                      
      else if ( CMND.CmndText(2) .eq. 'MRK' ) then
        call CalcMrkDiagnostics( CMND,MF,SF,OF )

      else if ( CMND.CmndText(2) .eq. 'POS' ) then
        if ( (GP.analysis_type .eq. FULL_ANL) .or. 
     &       (GP.analysis_type .eq. HH_ANL)        ) then ! Full Body
          call CalcFullBodySegPos( CMND,MF,SF,AF,OF )
        else if ( GP.analysis_type .eq. UPR_ANL ) then ! Upper Extremity
          call CalcUpperBodySegPos( MF,SF,OF )
        else if ( GP.analysis_type .eq. CKC_ANL ) then ! CKC
          call CalcCKCBodySegPos( CMND,MF,SF,OF )
        else if ( (GP.analysis_type .eq. CC_ANL)   .or. ! Cleveland Clinic
     &            (GP.analysis_type .eq. MAYO_ANL) ) then ! Mayo Markers
          call CalcCCBodySegPos( CMND,MF,SF,OF )
        else if ( (GP.analysis_type .eq. F3DR_ANL) .or.
     &            (GP.analysis_type .eq. F3DL_ANL)      ) then ! Foot3d
          call CalcF3dSegPos( CMND,MF,SF,OF )

        else
          write(6,110) ANLNames(GP.analysis_type)
          if ( OF.ToLog ) write(3,110) ANLNames(GP.analysis_type)
110       format(' Error- ',a20,'analysis is not supported yet')
        end if

      else if ( CMND.CmndText(2) .eq. 'VAA' ) then
        call CalcSegVelAcc( CMND,SF,MF,OF )

      else if ( CMND.CmndText(2) .eq. 'ENR' ) then
        call CalcSegEnergy( SF,JF,OF )

      else if ( CMND.CmndText(2) .eq. 'FSC' ) then
        call CalcSegFiniteScrews( CMND,SF,JF,OF )

      else if ( CMND.CmndText(2) .eq. 'ISC' ) then
        call CalcSegInstantScrews( SF,JF,OF )
        
      else if ( CMND.CmndText(2) .eq. 'BCM' ) then
        call CalcBodyCM( CMND,SF,OF )

      else if ( CMND.CmndText(2) .eq. 'MOM' ) then
        call CalcSegMomentum( SF,OF )

      else if ( CMND.CmndText(2) .eq. 'ANG' ) then
        call CalcSegGCSAngles( CMND,SF,OF )

      else if ( CMND.CmndText(2) .eq. 'FIL' ) then
        call DigFilterSeg( CMND,SF,OF )

      else
        write(6,100) CMND.CmndText(2)
        if ( OF.ToLog ) write(3,100) CMND.CmndText(2)
100     format(' ',a3,' is an invalid SEG command.'/
     &         ' Valid commands are: PaRaMeters MaRKers   POSition     VelAndAccel'/
     &         '                     ENeRgy     FinSCrew  BodyCntrMass MOMentum'/
     &         '                     ANGles     InsSCrew  FILter' )
      end if
      return
      end
c                                        
c	CalcSegParams  
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Controlling routine that calls the segment parameter 
c				calculation routines.  Segment parameters are 1) location
c				of the joints relative to the segment center of mass
c				which is also assumed to be the location of the segment
c				LCS, 2) The outlines of the segments relative to the
c				segment LCS used with Telio to allow 3D display, 3) The
c				mass and moment of inertia of the individual segments.
c				NOTE the assumption of fixed joint location.  This may be
c				enhanced in a future version of ANZ since both finite and
c				instantaneous kinematic screw calculations of the relative
c				3D motion of the segments on either side of a joint are 
c				available here and hence a 'virtual' joint center can be 
c				determined at each time frame.
c
c	Routines called:
c		CalcFullBodySegEnds 
c		CalcFullBodySegShape
c		CalcUpperBodySegEnds
c		CalcUpperBodySegShape
c		CalcFullBodySegMassInertia
c
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Param			GP.analysis_type
c
c	Common Block Variable MODIFIED by routine:
c	Block Name		Variable Name
c
c	None
c
      subroutine CalcSegParams( CMND,MF,SF,OF )
      implicit none

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

      record /CmndLine/      CMND
      record /marker_flag/   MF
      record /segment_flag/  SF
      record /option_flag/   OF                         
                      
      integer*4 StartFrm,NumFrm,i
      logical*4 error,ZeroMass,ZeroInertia,ZeroNonSagittal,UseAdult
                      
      error= .false.
      SF.param= .false.
      ZeroMass= .false.
      ZeroInertia= .false.
      ZeroNonSagittal= .false.
      UseAdult= .false.
      StartFrm= 1
      NumFrm= MP.nfrm
      if ( CMND.NumOpt(2) .ne. 0 ) then
        do i= 1, CMND.NumOpt(2)
          if ( CMND.OptText(i,2) .eq. 'ZMI' ) then
            ZeroMass= .true.
            ZeroInertia= .true.
          else if ( CMND.OptText(i,2) .eq. 'ZIN' ) then
            ZeroInertia= .true.
          else if ( CMND.OptText(i,2) .eq. 'ZNS' ) then
            ZeroNonSagittal= .true.
          else if ( CMND.OptText(i,2) .eq. 'STA' ) then   
            if ( CMND.OptNumOK(i,2) .and.                       
     &           (CMND.OptIntVal(i,2) .gt. 0) .and.
     &           (CMND.OptIntVal(i,2) .le. MP.nfrm) ) then
              StartFrm= CMND.OptIntVal(i,2)
            else
              goto 1040 ! error
            end if
          else if ( CMND.OptText(i,2) .eq. 'NUM' ) then
            if ( CMND.OptNumOK(i,2) .and. 
     &           (CMND.OptIntVal(i,2) .gt. 0) .and.
     &           (CMND.OptIntVal(i,2)+StartFrm-1 .le. MP.nfrm) ) then
              NumFrm= CMND.OptIntVal(i,2)
            else
              goto 1050 ! error
            end if
          else if ( CMND.OptText(i,2) .eq. 'ADU' ) then
            UseAdult= .true.
          else if ( CMND.OptText(i,2) .eq. 'CHI' ) then
            UseAdult= .false.
          else
            goto 1010 ! error
          end if
        end do ! i
      end if                     
      if ( (GP.analysis_type .eq. FULL_ANL) .or. 
     &     (GP.analysis_type .eq. HH_ANL)       ) then ! full body
        call CalcFullBodySegEnds( StartFrm,NumFrm,SF,error )
        if ( error ) goto 1030
        call CalcFullBodySegShape( StartFrm,NumFrm )
      else if ( GP.analysis_type .eq. UPR_ANL ) then ! upper extremity
        call CalcUpperBodySegEnds( StartFrm,NumFrm,SF,error )
        if ( error ) goto 1030
        call CalcUpperBodySegShape( StartFrm,NumFrm )
      else if ( GP.analysis_type .eq. CKC_ANL ) then ! CKC
        call CalcCKCBodySegEnds( StartFrm,NumFrm,SF,error )
        if ( error ) goto 1030
        call CalcCKCBodySegShape( StartFrm,NumFrm )
      else if ( (GP.analysis_type .eq. CC_ANL) .or.  ! Cleveland Clinic
     &          (GP.analysis_type .eq. MAYO_ANL)  ) then ! Mayo Markers
        if ( (.not. SF.RightSideAlign) .or.
     &       (.not. SF.LeftSideAlign)       ) goto 1060
        call CalcCCBodySegEnds( StartFrm,NumFrm,SF,error )
        if ( error ) goto 1030
        call CalcCCBodySegShape( StartFrm,NumFrm )

      else if ( (GP.analysis_type .eq. F3DR_ANL) .or.
     &          (GP.analysis_type .eq. F3DL_ANL)      ) then
        write(6,110)
110     format(' NOTE- This step is not necessary for Foot3d marker sets')
        goto 999

      else 
        write( 6,100 ) ANLNames(GP.analysis_type)
100     format(' Error- ',a20,' analysis is not supported')
        return
      end if                                      
      if ( (.not. MF.adjusted) .and.
     &     ( (GP.analysis_type .ne. CC_ANL) .and.
     &       (GP.analysis_type .ne. MAYO_ANL)    ) ) goto 1000  ! error
      call CalcFullBodySegMassInertia( ZeroMass,ZeroInertia,ZeroNonSagittal,
     &                                 UseAdult,OF,error )
      if ( error ) goto 1020
999   SF.param= .true.
      return

1000  write(6,1005)                                         
      if ( OF.ToLog ) write(3,1005)
1005  format(' Error- Marker data has not been adjusted yet.  Marker data must be read,'/
     &       '        interpolated, and adjusted before segment parameters can be'/
     &       '        calculated.')
      return

1010  write(6,1015)
      if ( OF.ToLog ) write(3,1015)
1015  format(' Error- Invalid option for SEG PRM.'/
     &       '        The valid options are: /ZeroMassInertia'/
     &       '                               /ZeroINertia'/
     &       '                               /ZeroNonSagittal'/
     &       '                               /STArtframe /NUMframe'/
     &       '        SEG PRM/MAN will be available in the future.')
      return                                   
                                              
1020  write(6,1025)
      if ( OF.ToLog ) write(3,1025)
1025  format(' Error- the patient''s body weight and height must be specified'/
     &       '        before the segment inertia and mass can be calculated.')
      return

1030  write(6,1035)
      if ( OF.ToLog ) write(3,1035)
1035  format(' Error- Segment position not calculated.  This must be done'/
     &       '        before joint position can be calculated.')
      return
  
1040  write(6,1045) CMND.OptText(i,2),MP.nfrm
      if ( OF.ToLog ) write(3,1045) CMND.OptText(i,2),MP.nfrm
1045  format(' Error- 'a3', is not a valid start frame number for calculating'/
     &       '        the average position of the segment ends'/
     &       '        Valid frame numbers are from 1 to ',i3)
      return

1050  write(6,1055) CMND.OptText(i,2),MP.nfrm+StartFrm-1
      if ( OF.ToLog ) write(3,1055) CMND.OptText(i,2),
     &                                      MP.nfrm+StartFrm-1
1055  format(' Error- 'a3', is not a valid number of frames for calculating'/
     &       '        the average position of the segment ends'/
     &       '        Valid numbers of frames are from 1 to ',i3)
      return

1060  write(6,1065)
      if ( OF.ToLog ) write(3,1065)
1065  format(' Error- Segment coordinate systems have not been initialized for'/
     &       '        the Cleveland Clinic marker set.  This must be done'/
     &       '        before the segment parameters can be calculated.')
      return
      end
c                                        
c	CalcFullBodySegShape  
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Determine the outlines of the segments relative to the
c				segment LCS for use with Telio.  This is what allows the
c				3D display of the segments as parallepipeds.
c
c	Routines called:
c		VecLth 
c		VecMag
c
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Param			GP.num_seg
c	Param			GP.seg_num
c	Marker			MrkData
c	Param			MP.nfrm
c	Param			SP.mrk_num
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Segment			SP.length
c	Segment			SP.shape
c
      subroutine CalcFullBodySegShape( frm1,nfrm )
      implicit none          

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

      integer*4 frm1,nfrm

      real*4    scale(5),VecLth,asis_width,shldr_width,foot_height(2),
     &          mid_shldr(3),mid_asis(3),mid_trnk(3),vecA(3),trnk_length,
     &          length,VecMag
      integer*4 i,j,k,axis,frm,side,seg,foot,calf,thgh,uarm,larm
    
c                  foot   calf   thigh  uparm  lwarm
      data scale / 0.571, 0.567, 0.567, 0.564, 0.570 / ! from Dempster (1955)

c    
c     compute the average distance between markers which will be used to
c     calculate the location of the points defining the 3D parallelapipeds
c     which will represent the body segments.  These shapes are viewed using
c     SHOW- the companion results display program for ANALYZE.
c
      do i= 1, GP.num_seg
        seg= GP.seg_num(i)
        if ( (seg .ne. rtft) .and. (seg .ne. lfft) ) then         
          SP(seg).length= VecLth( SP(seg).end(x,distal),
     &                                 SP(seg).end(x,proximal) )
        else
          length= 0.0
          do frm= frm1, frm1+nfrm-1
            length= length + VecLth(MrkData(x,frm,SP(seg).mrk_num(1)),
     &                              MrkData(x,frm,SP(seg).mrk_num(2)))
          end do
          SP(seg).length= length/float(nfrm)
        end if        
      end do
      asis_width=      0.0
      shldr_width=     0.0
      foot_height(rt)= 0.0
      foot_height(lf)= 0.0
      do frm= frm1, frm1+nfrm-1
        asis_width= asis_width + VecLth( MrkData(x,frm,MRK_NUM(rt).asis),
     &                                   MrkData(x,frm,MRK_NUM(lf).asis)  )
        shldr_width= shldr_width +
     &               VecLth( MrkData(x,frm,MRK_NUM(rt).shldr),
     &                       MrkData(x,frm,MRK_NUM(lf).shldr)  )
        do side= rt, lf                                        
          foot_height(side)= foot_height(side) +
     &               VecLth( MrkData(x,frm,MRK_NUM(side).heel),
     &                       MrkData(x,frm,MRK_NUM(side).ankle) )
        end do ! side
      end do ! frm
      asis_width=  asis_width/float(nfrm)
      shldr_width= shldr_width/float(nfrm)
      foot_height(rt)= foot_height(rt)/float(nfrm)
      foot_height(lf)= foot_height(lf)/float(nfrm)
c             
c     first do the extremities (foot,calf,thigh,upper-lower arms) since
c     the location the points describing the segment parallelapiped shapes
c     are easily calculated.  NOTE!!!!!!  These points are computed relative
c     to the local coordinate system of the segment.
c                                   
c     The size of the segments is based upon ratios of segment width at
c     the proximal and distal ends relative to segment length.  These ratios
c     were calculated from a sample size of 1- namely myself.  They seem
c     to look OK, but obviously it will make fat people look slim and extremely
c     thin people look bulkier than they really are (I'm not real thin,
c     but at the same time not fat either...).  Besides, the segment shapes
c     are for illustration purposes only- they allow a better perspective
c     of the persons motion- no subsequent evaluations are based upon the
c     persons segment size/shape.
c
c     The order of the points describing the segment shape is: the first
c     four points describe the distal face of the shape while last four
c     define the proximal face.  The first point in each face is that in
c     the positive x, positive y quadrant of the LCS.  Subsequent points
c     are defined clockwise around the LCS positive z axis.
c
      do side= rt, lf
        if ( side .eq. rt ) then
          foot= rtft
          calf= rtcf
          thgh= rtth
          larm= rtla
          uarm= rtua
        else
          foot= lfft
          calf= lfcf
          thgh= lfth
          larm= lfla
          uarm= lfua
        end if
        call CalcFootShape( scale(1),SP(calf).length,
     &                      foot_height,SP(foot) )
        call CalcCalfShape( scale(2),SP(calf) )
        call CalcThighShape( scale(3),SP(thgh) )
        call CalcUarmShape( scale(4),SP(uarm) )
        call CalcLarmShape( scale(5),SP(larm) )
      end do ! side
c
c     Now calculate the points to represent the pelvis segment.  This is
c     based upon the ratio of the lengths of the sides of a triangle formed
c     by the asis markers and the sacral marker.  The length of the sacral
c     stick was taken into account and the pelvis is considered to be starting
c     at the level of the asis markers and extending distally from there.
c     These measurments are based upon my pelvis dimensions.
c
c     calculate pelvis segment points
c
      call CalcPelvisShape( asis_width,SP(pelv) )
c
c     Calculate the segment shape point locations for the trunk.  
c
      do axis= x, z
        mid_shldr(axis)=  0.0
        mid_asis(axis)=   0.0
        do frm= frm1, frm1+nfrm-1
          mid_shldr(axis)= mid_shldr(axis) + 
     &                     ( MrkData(axis,frm,MRK_NUM(rt).shldr) +
     &                       MrkData(axis,frm,MRK_NUM(lf).shldr)  )/2.0
          mid_asis(axis)=  mid_asis(axis) + 
     &                     ( MrkData(axis,frm,MRK_NUM(rt).asis) +
     &                       MrkData(axis,frm,MRK_NUM(lf).asis)  )/2.0
        end do ! frm
        mid_shldr(axis)= mid_shldr(axis)/float(nfrm)
        mid_asis(axis)=  mid_asis(axis)/float(nfrm)
        mid_trnk(axis)=  ( mid_shldr(axis)+mid_asis(axis) )/2.0
        vecA(axis)= mid_shldr(axis) - mid_asis(axis)
      end do ! axis                                     
      trnk_length= VecMag( vecA )
c
c     calculate trunk segment points
c
      call CalcTrunkShape( trnk_length,shldr_width,SP(trnk),SP(pelv) )
c
c     calculate head segment points
c
      call CalcHeadShape( shldr_width,SP(head) )      
      return
      end
c
c
c                         
      subroutine CalcFootShape(sf,cflen,ftht,SP)
      implicit none
      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_names.inc'

      record /seg_param/ SP

      real*4   sf,cflen,ftht
                        
      SP.shape(x,1)=  sf*SP.length
      SP.shape(y,1)=  0.1*cflen
      SP.shape(z,1)= -ftht/1.2
      SP.shape(x,2)=  SP.shape(x,1)
      SP.shape(y,2)= -SP.shape(y,1)
      SP.shape(z,2)=  SP.shape(z,1)
      SP.shape(x,3)=  SP.shape(x,1) - SP.length
      SP.shape(y,3)=  SP.shape(y,2)
      SP.shape(z,3)=  SP.shape(z,1)
      SP.shape(x,4)=  SP.shape(x,3)
      SP.shape(y,4)=  SP.shape(y,1)
      SP.shape(z,4)=  SP.shape(z,1)
      SP.shape(x,5)=  SP.shape(x,1)
      SP.shape(y,5)=  SP.shape(y,1)
      SP.shape(z,5)=  SP.shape(z,1) + 0.4*ftht
      SP.shape(x,6)=  SP.shape(x,2)
      SP.shape(y,6)=  SP.shape(y,2) 
      SP.shape(z,6)=  SP.shape(z,5)
      SP.shape(x,7)=  SP.shape(x,3)
      SP.shape(y,7)=  SP.shape(y,3)
      SP.shape(z,7)=  0.7*ftht
      SP.shape(x,8)=  SP.shape(x,4)
      SP.shape(y,8)=  SP.shape(y,4)           
      SP.shape(z,8)=  SP.shape(z,7)
      return
      end
c
c
c                         
      subroutine CalcCalfShape(sf,SP)
      implicit none
      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_names.inc'

      record /seg_param/ SP
      real*4   sf
                        
      SP.shape(x,1)=  0.1*SP.length
      SP.shape(y,1)=  0.1*SP.length
      SP.shape(z,1)= -sf*SP.length*0.9
      SP.shape(x,2)=  SP.shape(x,1)
      SP.shape(y,2)= -SP.shape(y,1)
      SP.shape(z,2)=  SP.shape(z,1)
      SP.shape(x,3)= -SP.shape(x,1)
      SP.shape(y,3)=  SP.shape(y,2)
      SP.shape(z,3)=  SP.shape(z,1)
      SP.shape(x,4)=  SP.shape(x,3)
      SP.shape(y,4)=  SP.shape(y,1)
      SP.shape(z,4)=  SP.shape(z,1)
      SP.shape(x,5)=  0.15*SP.length
      SP.shape(y,5)=  0.15*SP.length
      SP.shape(z,5)=  SP.shape(z,1) + SP.length*0.9
      SP.shape(x,6)=  SP.shape(x,5)
      SP.shape(y,6)= -SP.shape(y,5) 
      SP.shape(z,6)=  SP.shape(z,5)
      SP.shape(x,7)= -SP.shape(x,5)
      SP.shape(y,7)=  SP.shape(y,6)
      SP.shape(z,7)=  SP.shape(z,5)
      SP.shape(x,8)=  SP.shape(x,7)
      SP.shape(y,8)=  SP.shape(y,5)
      SP.shape(z,8)=  SP.shape(z,5)
      return
      end
c
c
c                         
      subroutine CalcThighShape(sf,SP)
      implicit none
      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_names.inc'

      record /seg_param/ SP
      real*4   sf
 
      SP.shape(x,1)=  0.138*SP.length
      SP.shape(y,1)=  0.138*SP.length
      SP.shape(z,1)= -sf*SP.length*0.9
      SP.shape(x,2)=  SP.shape(x,1)
      SP.shape(y,2)= -SP.shape(y,1)
      SP.shape(z,2)=  SP.shape(z,1)
      SP.shape(x,3)= -SP.shape(x,1)
      SP.shape(y,3)=  SP.shape(y,2)
      SP.shape(z,3)=  SP.shape(z,1)
      SP.shape(x,4)=  SP.shape(x,3)
      SP.shape(y,4)=  SP.shape(y,1)
      SP.shape(z,4)=  SP.shape(z,1)
      SP.shape(x,5)=  0.2*SP.length
      SP.shape(y,5)=  0.2*SP.length
      SP.shape(z,5)=  SP.shape(z,1) + SP.length*0.9
      SP.shape(x,6)=  SP.shape(x,5)
      SP.shape(y,6)= -SP.shape(y,5) 
      SP.shape(z,6)=  SP.shape(z,5)
      SP.shape(x,7)= -SP.shape(x,5)
      SP.shape(y,7)=  SP.shape(y,6)
      SP.shape(z,7)=  SP.shape(z,5)
      SP.shape(x,8)=  SP.shape(x,7)
      SP.shape(y,8)=  SP.shape(y,5)
      SP.shape(z,8)=  SP.shape(z,5)
      return
      end
c
c
c                         
      subroutine CalcUarmShape(sf,SP)
      implicit none
      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_names.inc'

      record /seg_param/ SP
      real*4   sf

      SP.shape(x,1)=  0.15*SP.length
      SP.shape(y,1)=  0.15*SP.length
      SP.shape(z,1)= -sf*SP.length*0.9
      SP.shape(x,2)=  SP.shape(x,1)
      SP.shape(y,2)= -SP.shape(y,1)
      SP.shape(z,2)=  SP.shape(z,1)
      SP.shape(x,3)= -SP.shape(x,1)
      SP.shape(y,3)=  SP.shape(y,2)
      SP.shape(z,3)=  SP.shape(z,1)
      SP.shape(x,4)=  SP.shape(x,3)
      SP.shape(y,4)=  SP.shape(y,1)
      SP.shape(z,4)=  SP.shape(z,1)
      SP.shape(x,5)=  0.225*SP.length
      SP.shape(y,5)=  0.225*SP.length
      SP.shape(z,5)=  SP.shape(z,1) + SP.length*0.9
      SP.shape(x,6)=  SP.shape(x,5)
      SP.shape(y,6)= -SP.shape(y,5) 
      SP.shape(z,6)=  SP.shape(z,5)
      SP.shape(x,7)= -SP.shape(x,5)
      SP.shape(y,7)=  SP.shape(y,6)
      SP.shape(z,7)=  SP.shape(z,5)
      SP.shape(x,8)=  SP.shape(x,7)
      SP.shape(y,8)=  SP.shape(y,5)
      SP.shape(z,8)=  SP.shape(z,5)
      return
      end
c
c
c                         
      subroutine CalcLarmShape(sf,SP)
      implicit none
      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_names.inc'

      record /seg_param/ SP
      real*4   sf

      SP.shape(x,1)=  0.1*SP.length
      SP.shape(y,1)=  0.1*SP.length
      SP.shape(z,1)= -sf*SP.length 
      SP.shape(x,2)=  SP.shape(x,1)
      SP.shape(y,2)= -SP.shape(y,1)
      SP.shape(z,2)=  SP.shape(z,1)
      SP.shape(x,3)= -SP.shape(x,1)
      SP.shape(y,3)=  SP.shape(y,2)
      SP.shape(z,3)=  SP.shape(z,1)
      SP.shape(x,4)=  SP.shape(x,3)
      SP.shape(y,4)=  SP.shape(y,1)
      SP.shape(z,4)=  SP.shape(z,1)
      SP.shape(x,5)=  0.165*SP.length
      SP.shape(y,5)=  0.165*SP.length
      SP.shape(z,5)=  SP.shape(z,1) + SP.length*0.95
      SP.shape(x,6)=  SP.shape(x,5)
      SP.shape(y,6)= -SP.shape(y,5) 
      SP.shape(z,6)=  SP.shape(z,5)
      SP.shape(x,7)= -SP.shape(x,5)
      SP.shape(y,7)=  SP.shape(y,6)
      SP.shape(z,7)=  SP.shape(z,5)
      SP.shape(x,8)=  SP.shape(x,7)
      SP.shape(y,8)=  SP.shape(y,5)
      SP.shape(z,8)=  SP.shape(z,5)
      return
      end
c
c
c                         
      subroutine CalcPelvisShape(width,SP)
      implicit none
      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_names.inc'

      record /seg_param/ SP        
      real*4   width
                        
      SP.shape(x,1)=  (5./12.)*width
      SP.shape(y,1)=  0.55*width - 0.0254 ! subtract marker radius
      SP.shape(z,1)= -width/3.0
      SP.shape(x,2)=  SP.shape(x,1)
      SP.shape(y,2)= -SP.shape(y,1)
      SP.shape(z,2)=  SP.shape(z,1)
      SP.shape(x,3)= -0.5*(7./12.)*width
      SP.shape(y,3)=  SP.shape(y,2)
      SP.shape(z,3)=  SP.shape(z,1)
      SP.shape(x,4)=  SP.shape(x,3)
      SP.shape(y,4)=  SP.shape(y,1)
      SP.shape(z,4)=  SP.shape(z,1)
      SP.shape(x,5)=  SP.shape(x,1) 
      SP.shape(y,5)=  SP.shape(y,1)
      SP.shape(z,5)=  0.0
      SP.shape(x,6)=  SP.shape(x,2)
      SP.shape(y,6)=  SP.shape(y,2)
      SP.shape(z,6)=  SP.shape(z,5)
      SP.shape(x,7)=  SP.shape(x,3)
      SP.shape(y,7)=  SP.shape(y,3)
      SP.shape(z,7)=  SP.shape(z,5)
      SP.shape(x,8)=  SP.shape(x,4)
      SP.shape(y,8)=  SP.shape(y,4)
      SP.shape(z,8)=  SP.shape(z,5)
      return               
      end
c
c
c                         
      subroutine CalcTrunkShape(tlen,swidth,SP,SPP)
      implicit none
      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_names.inc'

      record /seg_param/ SP,SPP
      real*4   tlen,swidth

      SP.shape(x,1)=  SPP.shape(x,1)
      SP.shape(y,1)=  SPP.shape(y,1)
      SP.shape(z,1)= -0.5*tlen
      SP.shape(x,2)=  SP.shape(x,1)
      SP.shape(y,2)= -SP.shape(y,1)                   
      SP.shape(z,2)=  SP.shape(z,1)
      SP.shape(x,3)=  SPP.shape(x,3)
      SP.shape(y,3)=  SPP.shape(y,3)
      SP.shape(z,3)=  SP.shape(z,1)
      SP.shape(x,4)=  SP.shape(x,3)
      SP.shape(y,4)=  SP.shape(y,1)
      SP.shape(z,4)=  SP.shape(z,1)
      SP.shape(x,5)=  0.25*swidth
      SP.shape(y,5)=  0.45*swidth
      SP.shape(z,5)=  0.5*tlen
      SP.shape(x,6)=  SP.shape(x,5)
      SP.shape(y,6)= -SP.shape(y,5)
      SP.shape(z,6)=  SP.shape(z,5)
      SP.shape(x,7)= -SP.shape(x,5)
      SP.shape(y,7)=  SP.shape(y,6)
      SP.shape(z,7)=  SP.shape(z,5)
      SP.shape(x,8)=  SP.shape(x,7)
      SP.shape(y,8)=  SP.shape(y,5)
      SP.shape(z,8)=  SP.shape(z,5)
      return
      end   
c
c
c
      subroutine CalcHeadShape(swidth,SP)
      implicit none         
      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_names.inc'

      record /seg_param/ SP
      real*4   swidth

      SP.shape(x,1)=  0.2*swidth
      SP.shape(y,1)=  0.225*swidth
      SP.shape(z,1)=  0.05*swidth
      SP.shape(x,2)=  SP.shape(x,1)
      SP.shape(y,2)= -SP.shape(y,1)                     
      SP.shape(z,2)=  SP.shape(z,1)
      SP.shape(x,3)= -SP.shape(x,1)
      SP.shape(y,3)=  SP.shape(y,2)
      SP.shape(z,3)=  SP.shape(z,1)
      SP.shape(x,4)=  SP.shape(x,3)
      SP.shape(y,4)=  SP.shape(y,1)
      SP.shape(z,4)=  SP.shape(z,1)
      SP.shape(x,5)=  SP.shape(x,1)
      SP.shape(y,5)=  SP.shape(y,1)
      SP.shape(z,5)=  0.65*swidth
      SP.shape(x,6)=  SP.shape(x,2)
      SP.shape(y,6)=  SP.shape(y,2)
      SP.shape(z,6)=  SP.shape(z,5)
      SP.shape(x,7)=  SP.shape(x,3)
      SP.shape(y,7)=  SP.shape(y,3)
      SP.shape(z,7)=  SP.shape(z,5)
      SP.shape(x,8)=  SP.shape(x,4)
      SP.shape(y,8)=  SP.shape(y,4)
      SP.shape(z,8)=  SP.shape(z,5)
      return
      end
c                                        
c	CalcFullBodySegMassInertia  
c                                             
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	
c		This subroutine calculates the mass and moment of inertia of the body
c		segments based upon the height and weight of the subject.  The moments
c		of inertia calculated are the principal moments and as a result are
c		assumed to be in the same direction as the local coordinate system
c		of the segment.  Separate calculation methods are included for the
c		calculation of adult male and female bady segment parameters (BSP) as
c		well as a method for children (both sexes under age of 15). 
c		The adult male calculations are based upon the work of
c		McConville et al. (1980) at Wright-Patterson AFB, the female BSP's
c		from the work of Young et al. (1983) also of Wright-Patterson and
c		the children BSP's use the formulas of Jensen (1989).  NOTE the
c		default calculation method is adult male and the other methods
c		are slected automatically based upon sex and age.  However, the
c		calculation can also be manually specified.
c

c	Routines called:
c		None 
c
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Param			GP.Age
c	Param			GP.height
c	Param			GP.num_seg
c	Param			GP.seg_num
c	Param			GP.Sex
c	Param			GP.weight
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Segment			SP.inertia
c	Segment			SP.mass
c
      subroutine CalcFullBodySegMassInertia( ZeroMass,ZeroInertia,
     &                                       ZeroNonSagittal,UseAdult,
     &                                       OF,error )
      implicit none

      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_prm.inc'
      include 'a_names.inc'
      include 'a_seg.inc'
 
      record /option_flag/   OF
                                                                        
      logical*4 ZeroMass,ZeroInertia,ZeroNonSagittal,error,UseAdult
     
      real*4    den_water,sp_grav(15),ix_scale_m(3,15),iy_scale_m(3,15),
     &          iz_scale_m(3,15),kg,meter,mass_scale_m(3,15),est_mass,MtoCm,
     &          KgtoLbs,ix_scale_f(3,15),iy_scale_f(3,15),iz_scale_f(3,15),
     &          mass_scale_f(3,15),mass_scale_c(3,15),ixy_scale_c(3,15),
     &          cntr_scale_c(3,15)
      integer*4 i,j,ht,wt,const,seg                               
                                               
c                         ht     wt    const     ht     wt    const
      data mass_scale_m /  9.31,  1.87,  -994.0, 26.72, 11.84, -2912.0, !rtft,rtcf
     &                  35.19, 45.30, -4083.0,  9.31,  1.87,  -994.0, !rtth,lfft
     &                  26.72, 11.84, -2912.0, 35.19, 45.30, -4083.0, !lfcf,lfth
     &                 -68.97, 98.98,  6765.0,-48.01,185.00,  3950.0, !pelv,trnk
     &                  -2.14, 13.25,    76.0, -0.36, 10.01,   267.0, !rtua,rtla
     &                  -2.14, 13.25,    76.0, -0.36, 10.01,   267.0, !lfua,lfla
     &                   1.34,  8.37,  3749.0,                        !head
     &                   1.69,  1.60,   -62.0,  2.18,  1.23,   -83.0/ !rthd,lfhd
                                                                    
c                    rtft  rtcf  rtth  lfft  lfcf  lfth  pelv  trnk     
      data sp_grav / 1.10, 1.09, 1.05, 1.10, 1.09, 1.05, 1.01, 0.92,
     &               1.07, 1.13, 1.07, 1.13, 1.11, 1.13, 1.13 /
c                    rtua  rtla  lfua  lfla  head  rthd  lfhd
  
c              ht     wt      const      ht     wt      const    
      data ix_scale_m /
     &         110.,    30.,   -16550., 12323.,  1702., -1907428., ! rtft,rtcf
     &       28839.,  6407., -4659953.,   110.,    30.,   -16550., ! rtth,lfft
     &       12323.,  1702., -1907428., 28839.,  6407., -4659953., ! lfcf,lfth
     &      -10283., 14215.,   396174.,  8183., 47484., -3393924., ! pelv,trnk
     &         627.,  1304.,  -198020.,  4058.,  1316.,  -642117., ! rtua,rtla
     &         627.,  1304.,  -198020.,  4058.,  1316.,  -642117., ! lfua,lfla
     &         859.,    86.,    20735.,                            ! head
     &         133.,    48.,   -18865.,   181.,    27.,    -23856./! rthd,lfhd
      data iy_scale_m /
     &         948.,    71.,  -137353., 12429.,  1730., -1922497., ! rtft,rtcf
     &       28559.,  7298., -4679995.,   948.,    71.,  -137353., ! rtth,lfft
     &       12429.,  1730., -1922497., 28559.,  7298., -4679995., ! lfcf,lfth
     &      -10851., 13750.,   492711.,  3738., 36636., -4768961., ! pelv,trnk
     &         934.,  1094.,  -224626.,  4086.,  1316.,  -645877., ! rtua,rtla
     &         934.,  1094.,  -224626.,  4086.,  1316.,  -645877., ! lfua,lfla
     &        1097.,   103.,    36972.,                            ! head
     &         124.,    34.,   -17258.,   167.,    17.,   -21834./ ! rthd,lfhd
      data iz_scale_m /                         
     &         954.,    86.,  -138369.,   291.,   477.,   -66811., ! rtft,rtcf
     &       -1587.,  4537.,   -72496.,   954.,    86.,  -138369., ! rtth,lfft
     &         291.,   477.,   -66811., -1587.,  4537.,   -72496., ! lfcf,lfth
     &      -14684., 17498.,   772875.,-14325., 36254.,  -675725., ! pelv,trnk
     &        -338.,   391.,    19102.,  -139.,   200.,     7973., ! rtua,rtla
     &        -338.,   391.,    19102.,  -139.,   200.,     7973., ! lfua,lfla
     &         216.,   168.,    83847.,                            ! head
     &          12.,    26.,    -2404.,    22.,    19.,    -3028./ ! rthd,lfhd
c
c                         ht     wt    const     ht     wt    const
      data mass_scale_f /  8.43,  1.51,  -908.0, 10.40, 17.54, -1037.0, !rtft,rtcf
     &                    60.01, 65.04, -8770.0,  9.22,  1.50, -1016.0, !rtth,lfft
     &                     9.49, 19.41, -1115.0, 56.00, 65.65, -8236.0, !lfcf,lfth
     &                   -97.57,118.96,  9097.0,-115.0,153.17, 17954.0, !pelv,trnk
     &                     2.45, 11.91,  -518.0,  2.76,  6.99,  -150.0, !rtua,rtla
     &                     0.91, 13.06,  -431.0,  1.11,  6.89,   108.0, !lfua,lfla
     &                    -1.24,  4.45,  3469.0,                        !head
     &                     1.87,  1.05,   105.0,  0.92,  0.83,    70.0/ !rthd,lfhd
c                                                               
c              ht     wt      const      ht     wt      const       
      data ix_scale_f /                                             
     &          94.,    19.,   -12709.,  4595.,  2187.,  -680711., ! rtft,rtcf
     &       28916.,  9314., -4584794.,   106.,    19.,   -14585., ! rtth,lfft
     &        4402.,  2381.,  -672548., 26130.,  9759., -4201311., ! lfcf,lfth
     &      -10829., 16383.,   338759.,  7231., 27698., -2278454., ! pelv,trnk
     &        1386.,   671.,  -230521.,  1822.,   795.,  -254637., ! rtua,rtla
     &        1052.,   894.,  -192530.,  1572.,   784.,  -215823., ! lfua,lfla
     &        -384.,   476.,   155137.,                            ! head
     &          94.,    36.,   -12623.,    58.,    23.,    -5140. /! rthd,lfhd
      data iy_scale_f /                                         
     &         505.,    82.,   -70275.,  4767.,  2121.,  -700348., ! rtft,rtcf
     &       27798., 11131., -4587938.,   536.,    77.,   -74036., ! rtth,lfft
     &        4546.,  2299.,  -685242., 24743., 11623., -4163722., ! lfcf,lfth
     &      -19910., 17024.,  1538661., 10639., 21608., -2619378., ! pelv,trnk
     &        1162.,   805.,  -208801.,  1834.,   767.,  -255501., ! rtua,rtla
     &        1010.,   894.,  -196666.,  1587.,   755.,  -216740., ! lfua,lfla
     &         -25.,   357.,   143627.,                            ! head
     &          90.,    30.,   -12185.,    55.,    18.,    -5091./ ! rthd,lfhd
      data iz_scale_f /                                           
     &         512.,    87.,   -71115.,   -83.,   592.,   -20903., ! rtft,rtcf
     &         706.,  7299.,  -625303.,   522.,    80.,   -71333., ! rtth,lfft
     &        -158.,   688.,   -20769.,  1394.,  7215.,  -722096., ! lfcf,lfth
     &      -27129., 26546.,  1875223.,-13444., 23963.,  -650051., ! pelv,trnk
     &        -139.,   319.,     3337.,   -45.,   107.,     2058., ! rtua,rtla
     &        -196.,   359.,      409.,   -62.,   106.,     4514., ! lfua,lfla
     &         220.,    88.,    92585.,                            ! head
     &          10.,    11.,     1062.,     5.,     9.,      -55./ ! rthd,lfhd
     
      data mass_scale_c / 0.01355,  0.14661e-02, -0.71030e-04, ! rtft
     &                    0.02177,  0.48532e-02, -0.19003e-03, ! rtcf
     &                    0.04309,  0.88978e-02, -0.27425e-03, ! rtth
     &                    0.01355,  0.14661e-02, -0.71030e-04, ! lfft
     &                    0.02177,  0.48532e-02, -0.19003e-03, ! lfcf
     &                    0.04309,  0.88978e-02, -0.27425e-03, ! lfth
     &                    0.27330,  0.0,          0.0,         ! pelv
     &                    0.15837, -0.12533e-01,  0.0,         ! trnk
     &                    0.02344,  0.69558e-03,  0.0,         ! rtua
     &                    0.02280,  0.26100e-03,  0.0,         ! rtla
     &                    0.02344,  0.69558e-03,  0.0,         ! lfua
     &                    0.02280,  0.26100e-03,  0.0,         ! lfla
     &                    0.27881, -0.21152e-01,  0.53168e-03, ! head
     &                    0.00880,  0.0,          0.0,         ! rthd
     &                    0.00880,  0.0,          0.0        / ! lfhd

      data ixy_scale_c /  0.24370,  0.0,          0.0,         ! rtft
     &                    0.29271, -0.67104e-02,  0.0,         ! rtcf
     &                    0.29090,  0.0,          0.0,         ! rtth
     &                    0.24370,  0.0,          0.0,         ! lfft
     &                    0.29271, -0.67104e-02,  0.0,         ! lfcf
     &                    0.29090,  0.0,          0.0,         ! lfth
     &                    0.34610,  0.0,          0.0,         ! pelv
     &                    0.34580,  0.0,          0.0,         ! trnk
     &                    0.31939,  0.11138e-02,  0.0,         ! rtua
     &                    0.25947,  0.36529e-02, -0.16206e-03, ! rtla
     &                    0.31939,  0.11138e-02,  0.0,         ! lfua
     &                    0.25947,  0.36529e-02, -0.16206e-03, ! lfla
     &                    0.30750,  0.0,          0.0,         ! head
     &                    0.23860,  0.0,          0.0,         ! rthd
     &                    0.23860,  0.0,          0.0        / ! lfhd

      data cntr_scale_c /  0.41610,  0.0,          0.0,         ! rtft
     &                     0.44429, -0.21059e-02,  0.0,         ! rtcf
     &                     0.46090,  0.0,          0.0,         ! rtth
     &                     0.41610,  0.0,          0.0,         ! lfft
     &                     0.44429, -0.21059e-02,  0.0,         ! lfcf
     &                     0.46090,  0.0,          0.0,         ! lfth
     &                     0.47043,  0.92785e-02, -0.34945e-03, ! pelv
     &                     0.55480,  0.0,          0.0,         ! trnk
     &                     0.44180,  0.0,          0.0,         ! rtua
     &                     0.39052,  0.57871e-02, -0.33220e-03, ! rtla
     &                     0.44180,  0.0,          0.0,         ! lfua
     &                     0.39052,  0.57871e-02, -0.33220e-03, ! lfla
     &                     0.50440,  0.0,          0.0,         ! head
     &                     0.40850,  0.0,          0.0,         ! rthd
     &                     0.40850,  0.0,          0.0        / ! lfhd

      data ht,wt,const / 1,2,3 /                                 
      data MtoCm,KgtoLbs,den_water / 100.0,2.2048,0.001 / ! cm/m,lbs/kg,kg/cm**3
      data kg,meter / 0.001,0.01 /
      
c                               
c     Calculated masses are in kilograms, while segment inertias are in kg*m**2
c     The input weight is in kg while the regression equations require lbs
c     and the input height is in m while the regression equations require cm.
c     Hence, unit corrections are made in the calculations.
c             
        if ( (abs(GP.weight) .lt. 0.001) .or.
     &       (abs(GP.height) .lt. 0.001)      ) goto 1000
        est_mass= 0.0
        if ( (GP.Age .lt. 15) .and. (.not. UseAdult) ) then
          write(6,500) 
          if ( OF.ToLog ) write(3,500)
500       format(' NOTE - Child mass/inertia parameters used')
          do i= 1, GP.num_seg
            seg= GP.seg_num(i)
            SP(seg).Mass= ( mass_scale_c(1,seg) +
     &                           mass_scale_c(2,seg)*GP.Age +
     &                           mass_scale_c(3,seg)*GP.Age**2 )*
     &                         GP.weight
            est_mass= est_mass + SP(seg).mass
            SP(seg).inertia(x)= SP(seg).Mass*
     &                              ((( ixy_scale_c(1,seg) +
     &                                  ixy_scale_c(2,seg)*GP.Age +
     &                                  ixy_scale_c(3,seg)*GP.Age**2 )*
     &                                 SP(seg).length)**2)
            SP(seg).inertia(y)= SP(seg).inertia(x)
            SP(seg).inertia(z)= 0.0
          end do ! seg
        else ! Use Adult regression equations
          if ( GP.Sex .eq. 'M' ) then       
            write(6,510) 
            if ( OF.ToLog ) write(3,510)
510         format(' NOTE - Adult male mass/inertia parameters used')
            do i= 1, GP.num_seg
              seg= GP.seg_num(i)
              SP(seg).mass= ( mass_scale_m(ht,seg)*GP.height*MtoCm   +
     &                             mass_scale_m(wt,seg)*GP.weight*KgtoLbs +
     &                             mass_scale_m(const,seg)      ) 
     &                             *sp_grav(seg)*den_water
              est_mass= est_mass + SP(seg).mass
              SP(seg).inertia(x)= ix_scale_m(ht,seg)*GP.height*MtoCm   +
     &                                 ix_scale_m(wt,seg)*GP.weight*KgtoLbs +
     &                                 ix_scale_m(const,seg)         
              SP(seg).inertia(y)= iy_scale_m(ht,seg)*GP.height*MtoCm   +
     &                                 iy_scale_m(wt,seg)*GP.weight*KgtoLbs +
     &                                 iy_scale_m(const,seg)
              SP(seg).inertia(z)= iz_scale_m(ht,seg)*GP.height*MtoCm   +
     &                                 iz_scale_m(wt,seg)*GP.weight*KgtoLbs +
     &                                 iz_scale_m(const,seg)
c                                                         
c       the following unit conversion is necessary because the regression
c       eqs return values in terms of gm*cm**2 whereas the subsequent analysis
c       subroutines of this package expect units of kg*m**2
c              
              do j= x, z
                SP(seg).inertia(j)= SP(seg).inertia(j)*kg*meter*meter
              end do
            end do                                                       
          else ! Female
            write(6,520) 
            if ( OF.ToLog ) write(3,520)
520         format(' NOTE - Adult female mass/inertia parameters used')
            do i= 1, GP.num_seg
              seg= GP.seg_num(i)
              SP(seg).mass= ( mass_scale_f(ht,seg)*GP.height*MtoCm   +
     &                             mass_scale_f(wt,seg)*GP.weight*KgtoLbs +
     &                             mass_scale_f(const,seg)      ) 
     &                             *sp_grav(seg)*den_water
              est_mass= est_mass + SP(seg).mass
              SP(seg).inertia(x)= ix_scale_f(ht,seg)*GP.height*MtoCm   +
     &                                 ix_scale_f(wt,seg)*GP.weight*KgtoLbs +
     &                                 ix_scale_f(const,seg)
              SP(seg).inertia(y)= iy_scale_f(ht,seg)*GP.height*MtoCm   +
     &                                 iy_scale_f(wt,seg)*GP.weight*KgtoLbs +
     &                                 iy_scale_f(const,seg)
              SP(seg).inertia(z)= iz_scale_f(ht,seg)*GP.height*MtoCm   +
     &                                 iz_scale_f(wt,seg)*GP.weight*KgtoLbs +
     &                                 iz_scale_f(const,seg)
c                                                         
c       the following unit conversion is necessary because the regression
c       eqs return values in terms of gm*cm**2 whereas the subsequent analysis
c       subroutines of this package expect units of kg*m**2
c              
              do j= x, z
                SP(seg).inertia(j)= SP(seg).inertia(j)*kg*meter*meter
              end do
            end do                                                       
          end if
        end if ! Adult or Child
        write(6,100) GP.weight,GP.weight*2.22,est_mass,est_mass*2.22
        if ( OF.ToLog ) write(3,100) GP.weight,GP.weight*2.22,
     &                                       est_mass,est_mass*2.22
100     format(' Actual weight   : ',f8.3,' kg   ',f8.3,' lbs'/
     &         ' Estimated weight: ',f8.3,' kg   ',f8.3,' lbs')
c
c     Zero out specified quantities in the BSP's.  This is useful for
c     fooling around with calculation of the joint resultant loads to
c     see the relative significance of the full 3D calculation of the JRL's.
c
      if ( ZeroInertia ) then
        do i= 1, GP.num_seg
          seg= GP.seg_num(i)
          do j= x, z
            SP(seg).inertia(j)= 0.0
          end do
        end do               
      end if
      if ( ZeroMass ) then
        do i= 1, GP.num_seg
          seg= GP.seg_num(i)
          SP(seg).Mass= 0.0
        end do               
      end if
      if ( ZeroNonSagittal ) then
        do i= 1, GP.num_seg
          seg= GP.seg_num(i)
          SP(seg).inertia(x)= 0.0
          SP(seg).inertia(z)= 0.0
        end do               
      end if
      return                                             

1000  error= .true.
      return
      end
c                                        
c	CalcFullBodySegEnds  
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	
c		Calculate the locations of the ends of the segments relative to the
c		local coordinate systems attachead to those segments.  These ends are
c		the attachment points between the segments and subsequently are the
c		locations at which the intersegmental resultant loads ( i.e. joint
c		forces and moments) will be calculated.                    
c
c
c	Routines called:
c		PntGCStoLCS 
c
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Param			GP.num_seg
c	Param			GP.seg_num
c	Marker			MrkData
c	Segment			SgpGCS
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Segment			SP.avg_cntr
c	Segment			SP.end
c
      subroutine CalcFullBodySegEnds( StartFrm,NumFrm,SF,error )
      implicit none

      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_prm.inc'

      record /segment_flag/   SF

      integer*4 StartFrm,NumFrm
      logical*4 error
                                              
      include 'a_mrk.inc'
      include 'a_seg.inc'
      include 'a_names.inc'
      
      real*4    temp_pnt(3),avg_pos(3)
      integer*4 i,j,k,axis,seg,frm

      error= .true.
      if ( .not. SF.position ) return
      error= .false.
c                          
c     Calculate the average position of the joint markers relative the
c     segment LCS's.  These positions will be the joint "ends".  NOTE that
c     by using this method of calculation, a set of motion data could be
c     used to find the segment ends if a set of standing position data is
c     not available.
c
      do i= 1, GP.num_seg
        seg= GP.seg_num(i)
        do k= 1, 3
          do j= 1, 4
            SP(seg).end(k,j)= 0.0
            SP(seg).TCStoLCS(k,j)= 0.0
          end do
          do j= 1, 5
            SP(seg).mpnLCS(k,j)= 0.0
          end do
        end do            
        SP(seg).nmTCS= 0
        do j= 1, 5
          SP(seg).mnTCS(j)= 0
        end do
      end do

      do frm= StartFrm, StartFrm+NumFrm-1
c                   
c       Ankle - rt & lf - both relative to foot and relative to calf
c
        call PntGCStoLCS( SgpGCS(x,x,frm,rtft),
     &                    MrkData(x,frm,MRK_NUM(rt).ankle), temp_pnt )
        do axis= x, z
          SP(rtft).end(axis,proximal)= SP(rtft).end(axis,proximal) +
     &                                      temp_pnt(axis)
        end do
        call PntGCStoLCS( SgpGCS(x,x,frm,lfft),
     &                    MrkData(x,frm,MRK_NUM(lf).ankle), temp_pnt )
        do axis= x, z
          SP(lfft).end(axis,proximal)= SP(lfft).end(axis,proximal) +
     &                                      temp_pnt(axis)
        end do
        call PntGCStoLCS( SgpGCS(x,x,frm,rtcf),
     &                    MrkData(x,frm,MRK_NUM(rt).ankle), temp_pnt )
        do axis= x, z
          SP(rtcf).end(axis,distal)= SP(rtcf).end(axis,distal) +
     &                                      temp_pnt(axis)
        end do
        call PntGCStoLCS( SgpGCS(x,x,frm,lfcf),
     &                    MrkData(x,frm,MRK_NUM(lf).ankle), temp_pnt )
        do axis= x, z
          SP(lfcf).end(axis,distal)= SP(lfcf).end(axis,distal) +
     &                                      temp_pnt(axis)
        end do
c
c       Knee - rt & lf - relative to both calf and thigh segments
c
        call PntGCStoLCS( SgpGCS(x,x,frm,rtcf),
     &                    MrkData(x,frm,MRK_NUM(rt).knee), temp_pnt )
        do axis= x, z
          SP(rtcf).end(axis,proximal)= SP(rtcf).end(axis,proximal) +
     &                                      temp_pnt(axis)
        end do      
        call PntGCStoLCS( SgpGCS(x,x,frm,lfcf),
     &                    MrkData(x,frm,MRK_NUM(lf).knee), temp_pnt )
        do axis= x, z
          SP(lfcf).end(axis,proximal)= SP(lfcf).end(axis,proximal) +
     &                                      temp_pnt(axis)
        end do
        call PntGCStoLCS( SgpGCS(x,x,frm,rtth),
     &                    MrkData(x,frm,MRK_NUM(rt).knee), temp_pnt )
        do axis= x, z                                   
          SP(rtth).end(axis,distal)= SP(rtth).end(axis,distal) +
     &                                    temp_pnt(axis)
        end do
        call PntGCStoLCS( SgpGCS(x,x,frm,lfth),
     &                    MrkData(x,frm,MRK_NUM(lf).knee), temp_pnt )
        do axis= x, z
          SP(lfth).end(axis,distal)= SP(lfth).end(axis,distal) +
     &                                    temp_pnt(axis)
        end do                                            
c
c       Hip - rt & lf - both relative to thigh and pelvis
c
        call PntGCStoLCS( SgpGCS(x,x,frm,rtth),
     &                    MrkData(x,frm,MRK_NUM(rt).hip), temp_pnt )
        do axis= x, z
          SP(rtth).end(axis,proximal)= SP(rtth).end(axis,proximal) +
     &                                      temp_pnt(axis)
        end do
        call PntGCStoLCS( SgpGCS(x,x,frm,lfth),    
     &                    MrkData(x,frm,MRK_NUM(lf).hip), temp_pnt )
        do axis= x, z
          SP(lfth).end(axis,proximal)= SP(lfth).end(axis,proximal) +
     &                                      temp_pnt(axis) 
        end do
        call PntGCStoLCS( SgpGCS(x,x,frm,pelv),
     &                    MrkData(x,frm,MRK_NUM(rt).hip), temp_pnt )
        do axis= x, z                                     
          SP(pelv).end(axis,rt)= SP(pelv).end(axis,rt) +
     &                                temp_pnt(axis)
        end do
        call PntGCStoLCS( SgpGCS(x,x,frm,pelv),
     &                    MrkData(x,frm,MRK_NUM(lf).hip), temp_pnt )
        do axis= x, z
          SP(pelv).end(axis,lf)= SP(pelv).end(axis,lf) +
     &                                temp_pnt(axis)
        end do
c           
c       Trunk-Pelvis - relative to pelvis and trunk.  NOTE that joint relative
c       to pelvis is at (0,0,0) by definition so calculation of pelvis LCS
c       center relative to trunk LCS is equivalent to finding the joint
c       relative to the trunk LCS.
c
        call PntGCStoLCS( SgpGCS(x,x,frm,trnk),
     &                    SgpGCS(x,cntr,frm,pelv), temp_pnt )
        do axis= x, z
          SP(trnk).end(axis,3)= SP(trnk).end(axis,3) +
     &                               temp_pnt(axis)
        end do
c       
c       Neck - relative to trunk
c
        call PntGCStoLCS( SgpGCS(x,x,frm,trnk),
     &                    MrkData(x,frm,MRK_NUM(rt).neck), temp_pnt )
        do axis= x, z
          SP(trnk).end(axis,4)= SP(trnk).end(axis,4) +
     &                               temp_pnt(axis)
        end do                                        
        call PntGCStoLCS( SgpGCS(x,x,frm,head),
     &                    MrkData(x,frm,MRK_NUM(rt).neck), temp_pnt )
        do axis= x, z
          SP(head).end(axis,proximal)= SP(head).end(axis,proximal) +
     &                                    temp_pnt(axis)
        end do                                        
c
c       Elbow - rt & lf - relative to lower and upper arms
c
        call PntGCStoLCS( SgpGCS(x,x,frm,rtla),
     &                    MrkData(x,frm,MRK_NUM(rt).elbow), temp_pnt )
        do axis= x, z
          SP(rtla).end(axis,proximal)= SP(rtla).end(axis,proximal) +
     &                                      temp_pnt(axis)
        end do
        call PntGCStoLCS( SgpGCS(x,x,frm,lfla),    
     &                    MrkData(x,frm,MRK_NUM(lf).elbow), temp_pnt )
        do axis= x, z
          SP(lfla).end(axis,proximal)= SP(lfla).end(axis,proximal) +
     &                                      temp_pnt(axis) 
        end do                                               
        call PntGCStoLCS( SgpGCS(x,x,frm,rtua),
     &                    MrkData(x,frm,MRK_NUM(rt).elbow), temp_pnt )
        do axis= x, z                                     
          SP(rtua).end(axis,distal)= SP(rtua).end(axis,distal) +
     &                                    temp_pnt(axis)
        end do
        call PntGCStoLCS( SgpGCS(x,x,frm,lfua),
     &                    MrkData(x,frm,MRK_NUM(lf).elbow), temp_pnt )
        do axis= x, z
          SP(lfua).end(axis,distal)= SP(lfua).end(axis,distal) +
     &                                    temp_pnt(axis)
        end do
c
c       Shoulder - rt & lf - relative to upper arm and trunk
c
        call PntGCStoLCS( SgpGCS(x,x,frm,rtua),
     &                    MrkData(x,frm,MRK_NUM(rt).shldr), temp_pnt )
        do axis= x, z
          SP(rtua).end(axis,proximal)= SP(rtua).end(axis,proximal) +
     &                                      temp_pnt(axis)
        end do
        call PntGCStoLCS( SgpGCS(x,x,frm,lfua),    
     &                    MrkData(x,frm,MRK_NUM(lf).shldr), temp_pnt )
        do axis= x, z
          SP(lfua).end(axis,proximal)= SP(lfua).end(axis,proximal) +
     &                                      temp_pnt(axis) 
        end do                                               
        call PntGCStoLCS( SgpGCS(x,x,frm,trnk),
     &                    MrkData(x,frm,MRK_NUM(rt).shldr), temp_pnt )
        do axis= x, z                                     
          SP(trnk).end(axis,rt)= SP(trnk).end(axis,rt) +
     &                                temp_pnt(axis)
        end do
        call PntGCStoLCS( SgpGCS(x,x,frm,trnk),
     &                    MrkData(x,frm,MRK_NUM(lf).shldr), temp_pnt )
        do axis= x, z
          SP(trnk).end(axis,lf)= SP(trnk).end(axis,lf) +
     &                                temp_pnt(axis)
        end do
c
c       Wrist - rt & lf - relative to lower arms
c
        call PntGCStoLCS( SgpGCS(x,x,frm,rtla),
     &                    MrkData(x,frm,MRK_NUM(rt).wrist), temp_pnt )
        do axis= x, z
          SP(rtla).end(axis,distal)= SP(rtla).end(axis,distal) +
     &                                    temp_pnt(axis)
        end do
        call PntGCStoLCS( SgpGCS(x,x,frm,lfla),    
     &                    MrkData(x,frm,MRK_NUM(lf).wrist), temp_pnt )
        do axis= x, z
          SP(lfla).end(axis,distal)= SP(lfla).end(axis,distal) +
     &                                    temp_pnt(axis) 
        end do                                               
      end do ! frm
      do i= 1, GP.num_seg
        seg= GP.seg_num(i)
        do j= 1, 4
          do axis= x, z
            SP(seg).end(axis,j)= SP(seg).end(axis,j)/
     &                                float(NumFrm)
          end do ! axis
        end do ! j
      end do ! i
      do axis= x, z
        SP(pelv).end(axis,3)= 0.0 ! pelvis-trunk joint
      end do ! axis
c
c     Calculate the average segment center position.  This is used as the
c     reference position for calculating segment potential energies later
c     on.  Note that it would be preferable to use standing position data
c     to find these positions but that motion can be used to get and average
c     height of the segment.  
c
      do i= 1, GP.num_seg
        seg= GP.seg_num(i)
        do axis= x, z
          avg_pos(axis)= 0.0
        end do ! axis
        do frm= StartFrm, StartFrm+NumFrm-1
          do axis= x, z
            avg_pos(axis)= avg_pos(axis) + SgpGCS(axis,cntr,frm,seg)
          end do ! axis
        end do ! frm
        do axis= x, z
          SP(seg).avg_cntr(axis)= avg_pos(axis)/float(NumFrm)
        end do ! axis
      end do ! i
      return                  
      end   
c                                        
c	CalcFullBodySegPos  
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	
c		Calculate the position and orientation of the local coordinate systems
c		associated with each of the body segments relative to the global (lab)
c		coordinate system.  These coordinate systems are located at the center
c		of mass of the lower limb segments ( foot,calf,thigh ) and are aligned
c		with the principal inertia axes of the segment.  This makes it easier
c		to calculate the joint loads and segement energies latter on in the
c		program.  NOTE: There is an option included here to correct the 
c		segment orientation using a fixed orientation correction calculated
c		with the command AVG REF.  Look at the subroutine CalcAnatSgpRef
c		in the file anz_jnt_ang_ref.for for more details.  Also, check 
c		out my dissertation....
c
c	Routines called:
c		CalcLCSAntMrk
c		CalcLCSLatMrk
c		CalcLCSFoot 
c		CalcLCSHead
c		CalcLCSPelvis
c		CalcLCSTrunk
c		HandleSegPosError
c		JCSAngtoRotMat
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Marker			MrkData
c	Param			GP.num_seg
c	Param			GP.seg_num
c	Segment			SgpRefCorAng
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Segment			SgpGCS
c
      subroutine CalcFullBodySegPos( CMND,MF,SF,AF,OF )
      implicit none

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

      record /CmndLine/      CMND
      record /marker_flag/   MF
      record /segment_flag/  SF
      record /avg_pos_flag/  AF
      record /option_flag/   OF                       
    
      integer*4 frm,seg,side,foot,calf,thgh,uarm,larm,error,NumError(MAX_SEG),
     &          ErrFrm(MAX_MTN,MAX_SEG),i,j
      real*4    lcs_pos_z_scale(5),RM(3,3),NoSign(3),PrevFootVec(3,2),VecDot,
     &          SegAngleGCS(3,2),PrevSegAngleGCS(3,2),AngThreshold,stklth
      logical*4 InterpFlag,InterpError,SkipLine,AnatRefCorrect,CheckFtFlip(2),
     &          NegFtFrm1(2),CheckDir
            
      data foot,calf,thgh,uarm,larm / 1,2,3,4,5 /
c                           foot  calf  thgh  uarm  larm    
      data lcs_pos_z_scale/ 0.571,0.567,0.567,0.570,0.564 /   
      
      data NoSign / 1.0,1.0,1.0 /

      SF.position= .false.
      if ( .not. MF.adjusted ) goto 1000 ! error
c
c     Check for anatomical reference correction option
c
      AnatRefCorrect= .false.
      do i= rt, lf
        CheckFtFlip(i)= .false.
        NegFtFrm1(i)= .false.
      end do     
      AngThreshold= 50.0 ! degrees
      CheckDir= .true.
      if ( CMND.NumOpt(2) .gt. 0 ) then
        do i= 1, CMND.NumOpt(2)
          if ( CMND.OptText(i,2) .eq. 'REF' ) then
            if ( .not. AF.RefCorrectAng ) goto 2020 ! error
            AnatRefCorrect= .true.
          else if ( CMND.OptText(i,2) .eq. 'NFF' ) then
            CheckFtFlip(rt)= .false.
            CheckFtFlip(lf)= .false.
          else if ( CMND.OptText(i,2) .eq. 'NRF' ) then
            NegFtFrm1(rt)= .true.
          else if ( CMND.OptText(i,2) .eq. 'NLF' ) then
            NegFtFrm1(lf)= .true.
          else if ( CMND.OptText(i,2) .eq. 'CFR' ) then
            CheckFtFlip(rt)= .true.
          else if ( CMND.OptText(i,2) .eq. 'CFL' ) then
            CheckFtFlip(lf)= .true.
          else if ( CMND.OptText(i,2) .eq. 'CFB' ) then
            CheckFtFlip(rt)= .true.
            CheckFtFlip(lf)= .true.
          else if ( CMND.OptText(i,2) .eq. 'ATH' ) then
            CheckDir= .false.
            if ( CMND.OptNumOK(i,2) ) AngThreshold= CMND.OptRealVal(i,2)
          else   
            goto 2010 ! error
          end if
        end do
      end if
c
c     Initialize the error counting array
c
      do i= 1, MAX_SEG
        NumError(i)= 0
      end do ! i
c
c    Set sacral stick length according to analysis
c
      if ( GP.analysis_type .eq. HH_ANL ) then
        stklth= 8.0*.0254 ! Mayo sacral stick
      else
        stklth= 6.0*.0254 ! Ohio State sacral stick
      end if
c
c     Do the calculations frame by frame                                 
c
      SkipLine=   .false.
      InterpFlag= .false.
      do frm= 1, MP.nfrm
        if ( OF.UpDate .and. (mod(frm,10) .eq. 0) )  then
          if ( SkipLine ) write(6,90)
90        format(' ')        
          write(6,100) frm,MP.nfrm
100       format('+Processing frame # ',i3,'/',i3,'.')
          SkipLine= .false.
        end if
                
c
c     First calculate the location and orientation of the principal inertia
c     local coordinate systems for the lower limbs- foot, calf, thigh. 
c     These are used for the joint load calculations as well as for 3D
c     animation of the gait.
c
        do side= rt, lf
c                 
c       foot principal inerita coordinate system is located on the line
c       between the toe and ankle and is oriented such that the z axis is
c       along this line the x axis is lateral (medial) for the right (left)
c       foot and the y axis is mutually perpendicular to these two.  The
c       location is based upon Dempster's data (1955) which has been supported
c       by more recent studies.
c
c       10/8/90 - DAM
c       NOTE: There is a special error trapping section of code below for the 
c             case of the line between the toe and heel marker being above the
c             ankle marker.  This occurs occaisionally with very difficult 
c             cases with the OSU marker set.  This trap fixes the resulting
c             flip in the coordinate system
c
          seg= rtft              
          if ( side .eq. lf )  seg= lfft                             
          Call CalcLCSFoot( MrkData(x,frm,MRK_NUM(side).toe),
     &                      MrkData(x,frm,MRK_NUM(side).ankle),
     &                      MrkData(x,frm,MRK_NUM(side).heel), 
     &                      lcs_pos_z_scale(foot),
     &                      SgpGCS(x,x,frm,seg),error        )
          if ( error .ne. 0 ) then
            call HandleSegPosError( error,frm,seg,NumError(seg),
     &                              ErrFrm(1,seg),OF )
            InterpFlag= .true.
            SkipLine=   .true.              
          else if ( (frm .gt. 1) .and. CheckFtFlip(side) ) then ! check for flipped foot
            if ( CheckDir ) then
              if ( VecDot(PrevFootVec(x,side),SgpGCS(x,y,frm,seg))
     &             .lt. 0.0 ) then ! foot has flipped
                do j= y, z
                  do i= 1, 3                  
                    SgpGCS(i,j,frm,seg)= -SgpGCS(i,j,frm,seg)
                  end do
                end do
                write(6,500) seg_names(seg),frm
                if ( OF.ToLog ) write(3,500) seg_names(seg),frm
500             format(' NOTE- Direction Check says ',a4,' flipped at frame # ',i3/)
              end if
              do i= 1, 3                            
                PrevFootVec(i,side)= SgpGCS(i,y,frm,seg)
              end do
            else ! check for magnitude of change in angle from frame to frame
              call RotMattoJCSAng( SgpGCS(x,x,frm,seg),
     &                             SegAngleGCS(1,side),error )
              if ( abs(PrevSegAngleGCS(1,side)-SegAngleGCS(1,side)) .gt. 
     &             AngThreshold ) then
                do j= y, z
                  do i= 1, 3                  
                    SgpGCS(i,j,frm,seg)= -SgpGCS(i,j,frm,seg)
                  end do
                end do
                write(6,503) seg_names(seg),frm
                if ( OF.ToLog ) write(3,503) seg_names(seg),frm
503             format(' NOTE- Angle Check says ',a4,' flipped at frame # ',i3/)
                call RotMattoJCSAng( SgpGCS(x,x,frm,seg),
     &                               PrevSegAngleGCS(1,side),error )
              else
                do i= 1, 3
                  PrevSegAngleGCS(i,side)= SegAngleGCS(i,side)
                end do
              end if
            end if ! which check method...
          else if ( (frm .eq. 1) .and. CheckFtFlip(side) ) then
            if ( NegFtFrm1(side) ) then
              do j= y, z
                do i= 1, 3
                  SgpGCS(i,j,frm,seg)= -SgpGCS(i,j,frm,seg)
                end do                   
              end do                
            end if
            do i= 1, 3
              PrevFootVec(i,side)= SgpGCS(i,y,1,seg)
            end do
            call RotMattoJCSAng( SgpGCS(x,x,frm,seg),
     &                           PrevSegAngleGCS(1,side),error )
          end if
c                                                       
c       calf principal inertia coordinate system is located along the line
c       from the ankle to the knee by a distance from the ankle scaled using
c       a factor developed by Dempster in 1955 (more recent studies support this
c       number).  The axes are aligned such that the z axis goes
c       from ankle toward knee, x axis is perpedicular to the plane formed
c       by the ankle, shank and knee markers ( lateral (medial) for the
c       right (left) calf) , and the y axis is mutually perpendicular to
c       the x and z axes.                                    
c
          seg= rtcf
          if ( side .eq. lf )  seg= lfcf
          Call CalcLCSLatMrk( MrkData(x,frm,MRK_NUM(side).ankle),
     &                        MrkData(x,frm,MRK_NUM(side).calf),
     &                        MrkData(x,frm,MRK_NUM(side).knee), 
     &                        side,lcs_pos_z_scale(calf),
     &                        SgpGCS(x,x,frm,seg),error        )
          if ( error .ne. 0 ) then
            call HandleSegPosError( error,frm,seg,NumError(seg),
     &                              ErrFrm(1,seg),OF )
            InterpFlag= .true.
            SkipLine=   .true.
          end if
c                                                        
c       thigh principal inertia coordinate system is located along the line
c       from the knee to the hip and its distance above the knee is determined
c       from Dempster's Data.  The z axis is aligned with this line, the
c       x axis is perpendicular to the plane defined by the knee, stick
c       and hip markers such that it is lateral (medial) for the right (left)
c       thigh, and the y axis is mutually perpendicular to the x and z axes.
c
          seg= rtth
          if ( side .eq. lf )  seg= lfth
          if ( GP.analysis_type .eq. FULL_ANL ) then
            Call CalcLCSAntMrk( MrkData(x,frm,MRK_NUM(side).knee),
     &                          MrkData(x,frm,MRK_NUM(side).thigh),
     &                          MrkData(x,frm,MRK_NUM(side).hip),
     &                          lcs_pos_z_scale(thgh),        
     &                          SgpGCS(x,x,frm,seg),error       )
           else if ( GP.analysis_type .eq. HH_ANL ) then
            Call CalcLCSLatMrk( MrkData(x,frm,MRK_NUM(side).knee),
     &                          MrkData(x,frm,MRK_NUM(side).thigh),
     &                          MrkData(x,frm,MRK_NUM(side).hip),
     &                          side,lcs_pos_z_scale(thgh),        
     &                          SgpGCS(x,x,frm,seg),error       )
           end if
          if ( error .ne. 0 ) then
            call HandleSegPosError( error,frm,seg,NumError(seg),
     &                              ErrFrm(1,seg),OF )
            InterpFlag= .true.
            SkipLine=   .true.
          end if
c                                                       
c       Now calculate a local coordinate system for the upper and lower
c       arms.  These are not aligned with the principal inertia axes!!!!!
c       Their purpose is to allow the upper arm to properly displayed during 
c       a 3D animation of the gait motion.
c       The upper system is located at midway
c       along the line between the elbow and shoulder markers.  It is aligned
c       such that the z axis is along this line, the x axis is perpendicular
c       to the plane defined by the shoulder, elbow and wrist, and the y
c       axis is mutually perpedicular to these.      
c
          seg= rtua                       
          if ( side .eq. lf )  seg= lfua
          Call CalcLCSAntMrk( MrkData(x,frm,MRK_NUM(side).elbow),
     &                        MrkData(x,frm,MRK_NUM(side).wrist),
     &                        MrkData(x,frm,MRK_NUM(side).shldr),
     &                        lcs_pos_z_scale(uarm),
     &                        SgpGCS(x,x,frm,seg),error        )
          if ( error .ne. 0 ) then
            call HandleSegPosError( error,frm,seg,NumError(seg),
     &                              ErrFrm(1,seg),OF )
            InterpFlag= .true.
            SkipLine=   .true.
          end if
c
c       the lower arm system is located at the midpoint between the wrist
c       and the elbow.  It is aligned such that the z axis is along this
c       line, the x axis is perpendicular to the plane formed by the wrist,
c       elbow and shoulder, and the y axis is mutually perpendicular to
c       these axes.
c
          seg= rtla
          if ( side .eq. lf )  seg= lfla
          Call CalcLCSAntMrk( MrkData(x,frm,MRK_NUM(side).wrist),
     &                        MrkData(x,frm,MRK_NUM(side).shldr),
     &                        MrkData(x,frm,MRK_NUM(side).elbow),
     &                        lcs_pos_z_scale(larm),
     &                        SgpGCS(x,x,frm,seg),error        )
          if ( error .ne. 0 ) then
            call HandleSegPosError( error,frm,seg,NumError(seg),
     &                              ErrFrm(1,seg),OF )
            InterpFlag= .true.
            SkipLine=   .true.
          end if
        end do ! side
c
c     now create local coordinate systems for the pelvis, trunk, and head.
c     Right now, these are to be used mainly for the purpose of generating 3D animation
c     of gait motion.  Thus, they are not aligned with principal inertia
c     axes and are placed in positions that allow a representative paralla-
c     piped to be drawn easily.
c     First the pelvis...    Its LCS lies in the middle of the spatial triangle
c     defined by the rt asis, sacral stick, and lf asis.  Its z axis is
c     perpendicular to this plane while the y axis is parallel to the line
c     running from rt asis to lf asis.  The x axis is the mutually
c     perpendicular to these axes  NOTE- rt & lf sacrum and neck
c     markers are the same number and are represented this way just
c     to make easier to pass their marker numbers around with the
c     rest of the bodies marker numbers.
c
        call CalcLCSPelvis( MrkData(x,frm,MRK_NUM(rt).asis),
     &                      MrkData(x,frm,MRK_NUM(rt).sacrum),
     &                      MrkData(x,frm,MRK_NUM(lf).asis),stklth,
     &                      SgpGCS(x,x,frm,pelv),error     )
        if ( error .ne. 0 ) then
          call HandleSegPosError( error,frm,seg,NumError(seg),
     &                            ErrFrm(1,seg),OF )
          InterpFlag= .true.
          SkipLine=   .true.
        end if
c                                   
c     Now do the trunk.  The LCS will lie at the geometric center of the spatial
c     rectangle formed by the right and left shoulder markers and the right and
c     left asis.  The x axis will be perpendicular to this plane, the
c     y axis will be parallel to a line drawn from the right to the left 
c     shoulder markers, and the z axis will be mutually perpendicular to these.
c                                
        call CalcLCSTrunk( MrkData(x,frm,MRK_NUM(rt).shldr),
     &                     MrkData(x,frm,MRK_NUM(lf).shldr),
     &                     MrkData(x,frm,MRK_NUM(rt).asis),
     &                     MrkData(x,frm,MRK_NUM(lf).asis),
     &                     MrkData(x,frm,MRK_NUM(rt).sacrum),
     &                     SgpGCS(x,x,frm,trnk),error     )
        if ( error .ne. 0 ) then
          call HandleSegPosError( error,frm,seg,NumError(seg),
     &                            ErrFrm(1,seg),OF )
          InterpFlag= .true.
          SkipLine=   .true.
        end if        
c                                       
c     Lastly, do the head.  Since only one marker is provided ( neck marker )
c     to locate the head, its orientation will be assumed to be aligned with
c     trunk coordinate system at all times while its location will be at the 
c     neck marker.
c
        call CalcLCSHead( MrkData(x,frm,MRK_NUM(rt).neck),
     &                    SgpGCS(x,x,frm,trnk),
     &                    SgpGCS(x,x,frm,head) )
      end do ! frm
c
c     Check for error in calculation of the segment positions and orientations
c     and if there are any interpolate the segment information for the missing
c     frames
c
      SF.position= .true.
      if ( InterpFlag ) then
        call InterpSegPos( NumError,ErrFrm,SgpGCS,OF,InterpError )
        if ( InterpError ) SF.position= .false.
      end if
c
c     Do correction for orientation of segment LCS based upon corrections
c     calculated using reference joint angles and stationary position
c     data corresponding to these reference angles.  These corrections
c     are calculated in AVG REF and shouldn't be used unless you know
c     exactly what you are doing !!!!   They are powerful in adjusting
c     data to be more accurate and just as powerful in turning marginal
c     but useful data into total junk...
c      
      if ( AnatRefCorrect ) then
        do i= 1, GP.num_seg
          seg= GP.seg_num(i)
          call JCSAngtoRotMat( SgpRefCorAng(1,seg),NoSign,RM )
          do j= 1, MP.nfrm
            call MatMult( SgpGCS(1,1,j,seg),RM,
     &                    SgpGCS(1,1,j,seg)     )
          end do
        end do
        write(6,200)
        if ( OF.ToLog ) write(3,200)
200     format(/' Anatomic reference correction used...')
        SF.RefCorrect= .true.
      end if
      return
                  
1000  write(6,1010)
      if ( OF.ToLog ) write(3,1010)
1010  format(' Error- MaRKer data has not been ADJusted.'/
     &       '        This must be done before segment position can be calculated.')
      return

2000  write(6,2005)
      if ( OF.ToLog ) write(3,2005)
2005  format(' Error- More than one option has been given for SEG POS'/
     &       '        The only option allowed is /REFerence position')
      return

2010  write(6,2015) CMND.OptText(1,2)
      if ( OF.ToLog ) write(3,2015) CMND.OptText(1,2)
2015  format(' Error- ',a3,' is not a valid option for SEG POS'/
     &       '        The valid options are: /REFerence position'/
     &       '                               /No Foot Flip check'/
     &       '                               /Negate Right Foot first frame'/
     &       '                               /Negate Left Foot first frame'/
     &       '                               /Check foot Flip Right'/
     &       '                               /Check foot Flip Left'/
     &       '                               /Check foot Flip Both feet'/
     &       '                               /use Angle THreshold= # (50.0 deg)')
      return

2020  write(6,2025)
      if ( OF.ToLog ) write(3,2025)
2025  format(' Error- Anatomic reference corrections have not been calculated yet'/
     &       '        This is done with AVG REF')
      return
      end       
c                                        
c	CalcLCSFoot  
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Calculate location/orientaiton of foot LCS from toe,
c				ankle and heel markers
c
c	Error Codes:	0 - no error
c					1 - X axis bad
c					2 - Y axis bad
c					4 - Z axis bad
c					3 - X and Y axes bad
c					5 - X and Z axes bad
c					6 - Y and Z axes bad
c					7 - X, Y, and Z axes bad
c
c	Routines called:
c		Tricen
c		VecNorm
c		VecSub
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 CalcLCSFoot( toe,ankle,heel,pos_scale,local_coord_sys,error )
      implicit none              

      real*4    toe(3),ankle(3),heel(3),pos_scale,local_coord_sys(3,4)
      integer*4 error
           
      real*4    vector1(3),vector2(3),LCS_Xaxis(3),LCS_Yaxis(3),LCS_Zaxis(3)
      integer*4 axis,x,y,z,cntr,NormError,VecNorm,CntrError

      data x,y,z,cntr / 1,2,3,4 /

      error= 0
      Call VecSub( ankle,toe,vector1 )
      Call VecSub( heel,toe,vector2 )
      Call VecX( vector2, vector1, LCS_Yaxis )
      do axis= x,z
        LCS_Xaxis(axis)= -vector2(axis)
      end do      
      Call VecX( LCS_Xaxis, LCS_Yaxis, LCS_Zaxis )
      NormError= VecNorm( LCS_Xaxis, local_coord_sys(x,x) )
      if ( NormError .ne. 0 ) error= error + 1
      NormError= VecNorm( LCS_Yaxis, local_coord_sys(x,y) )
      if ( NormError .ne. 0 ) error= error + 2
      NormError= VecNorm( LCS_Zaxis, local_coord_sys(x,z) )
      if ( NormError .ne. 0 ) error= error + 4
      Call Tricen( toe,ankle,heel,local_coord_sys(x,cntr),CntrError )
      if ( CntrError .ne. 0 ) error= 7
      return
      end
c                                        
c	CalcLCSAntMrk
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Calculate location/orientaiton of segment LCS from the
c				postions of three markers
c
c	Error Codes:	0 - no error
c					1 - X axis bad
c					2 - Y axis bad
c					4 - Z axis bad
c					3 - X and Y axes bad
c					5 - X and Z axes bad
c					6 - Y and Z axes bad
c					7 - X, Y, and Z axes bad
c
c	Routines called:
c		VecNorm
c		VecSub
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 CalcLCSAntMrk( mrk1,mrk2,mrk3,pos_scale,local_coord_sys,error )
      implicit none     
 
      real*4    mrk1(3),mrk2(3),mrk3(3),pos_scale,local_coord_sys(3,4)
      integer*4 error

      real*4    vector1(3),vector2(3),LCS_Xaxis(3),LCS_Yaxis(3),LCS_Zaxis(3)
      integer*4 axis,x,y,z,cntr,NormError,VecNorm
                                       
      data x,y,z,cntr / 1,2,3,4 /

      error= 0
      Call VecSub( mrk2,mrk1,vector1 )          
      Call VecSub( mrk3,mrk1,vector2 )
      Call VecX( vector2, vector1, LCS_Yaxis )
      do axis= x,z
        LCS_Zaxis(axis)= vector2(axis)
      end do                                  
      Call VecX( LCS_Yaxis, LCS_Zaxis, LCS_Xaxis )
      NormError= VecNorm( LCS_Xaxis, local_coord_sys(x,x) )
      if ( NormError .ne. 0 ) error= error + 1
      NormError= VecNorm( LCS_Yaxis, local_coord_sys(x,y) )
      if ( NormError .ne. 0 ) error= error + 2
      NormError= VecNorm( LCS_Zaxis, local_coord_sys(x,z) )
      if ( NormError .ne. 0 ) error= error + 4
      do axis= x,z
        local_coord_sys(axis,cntr)= mrk1(axis) + pos_scale*vector2(axis)
      end do              
      return              
      end
c                                        
c	CalcLCSLatMrk
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Calculate location/orientaiton of calf LCS from ankle,
c				tibia stick and knee markers. Also applies to thigh for
c           the Helen Hayes marker set
c
c	Error Codes:	0 - no error
c					1 - X axis bad
c					2 - Y axis bad
c					4 - Z axis bad
c					3 - X and Y axes bad
c					5 - X and Z axes bad
c					6 - Y and Z axes bad
c					7 - X, Y, and Z axes bad
c
c	Routines called:
c		VecNorm
c		VecSub
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 CalcLCSLatMrk( mrk1,mrk2,mrk3,side,pos_scale,
     &                          local_coord_sys,error )
      implicit none     

      real*4    mrk1(3),mrk2(3),mrk3(3),pos_scale,local_coord_sys(3,4)
      integer*4 side,error

      real*4    vector1(3),vector2(3),LCS_Xaxis(3),LCS_Yaxis(3),LCS_Zaxis(3)
      integer*4 axis,x,y,z,cntr,lf,NormError,VecNorm

      data x,y,z,cntr,lf / 1,2,3,4,2 /

      error= 0
      Call VecSub( mrk2,mrk1,vector1 )          
      Call VecSub( mrk3,mrk1,vector2 )
      Call VecX( vector2, vector1, LCS_Xaxis )
      do axis= x,z
        LCS_Zaxis(axis)= vector2(axis)
      end do
      Call VecX( LCS_Zaxis, LCS_Xaxis, LCS_Yaxis )
      NormError= VecNorm( LCS_Xaxis, local_coord_sys(x,x) )
      if ( NormError .ne. 0 ) error= error + 1
      NormError= VecNorm( LCS_Yaxis, local_coord_sys(x,y) )
      if ( NormError .ne. 0 ) error= error + 2
      NormError= VecNorm( LCS_Zaxis, local_coord_sys(x,z) )
      if ( NormError .ne. 0 ) error= error + 4
      do axis= x,z
        local_coord_sys(axis,cntr)= mrk1(axis) + pos_scale*vector2(axis)
        if ( side .eq. lf ) then
          local_coord_sys(axis,x)= -local_coord_sys(axis,x)
          local_coord_sys(axis,y)= -local_coord_sys(axis,y)
        end if
      end do              
      return
      end      
c                                        
c	CalcLCSPelvis
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Calculate location/orientaiton of pelvis LCS from right ASIS,
c				left ASIS and sacral stick markers
c
c	Error Codes:	0 - no error
c					1 - X axis bad
c					2 - Y axis bad
c					4 - Z axis bad
c					3 - X and Y axes bad
c					5 - X and Z axes bad
c					6 - Y and Z axes bad
c					7 - X, Y, and Z axes bad
c
c	Routines called:
c		VecNorm
c		VecSub
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 CalcLCSPelvis( rasis, sacrum, lasis, stklth,
     &                          local_coord_sys,error )
      implicit none  

      real*4    rasis(3),sacrum(3),lasis(3),local_coord_sys(3,4),StkLth
      integer*4 error
                  
      real*4    vector1(3),vector2(3),LCS_Xaxis(3),LCS_Yaxis(3),LCS_Zaxis(3),
     &          z_avg
      integer*4 axis,x,y,z,cntr,NormError,VecNorm
                     
      data x,y,z,cntr / 1,2,3,4 / ! StkLth- Length of sacral stick

      error= 0
      Call VecSub( lasis, rasis,vector1 )
      Call VecSub( sacrum,rasis,vector2 )
      Call VecX( vector1, vector2, LCS_Zaxis )
      do axis= x,z   
        LCS_Yaxis(axis)= vector1(axis)
      end do
      Call VecX( LCS_Yaxis, LCS_Zaxis, LCS_Xaxis )
      NormError= VecNorm( LCS_Xaxis, local_coord_sys(x,x) )
      if ( NormError .ne. 0 ) error= error + 1
      NormError= VecNorm( LCS_Yaxis, local_coord_sys(x,y) )
      if ( NormError .ne. 0 ) error= error + 2
      NormError= VecNorm( LCS_Zaxis, local_coord_sys(x,z) )
      if ( NormError .ne. 0 ) error= error + 4
      do axis= x, z
        local_coord_sys(axis,cntr)=
     &           ( ( lasis(axis) + rasis(axis) )/2.0 +
     &             sacrum(axis) + StkLth*local_coord_sys(axis,x) )/ 2.0
      end do
      return         
      end                                  
c                                        
c	CalcLCSTrunk
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Calculate location/orientaiton of trunk LCS from right ASIS,
c				left ASIS, sacral stick, right shoulder and left shoulder
c				markers
c
c	Error Codes:	0 - no error
c					1 - X axis bad
c					2 - Y axis bad
c					4 - Z axis bad
c					3 - X and Y axes bad
c					5 - X and Z axes bad
c					6 - Y and Z axes bad
c					7 - X, Y, and Z axes bad
c
c	Routines called:
c		VecNorm
c		VecSub
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 CalcLCSTrunk( rsh,lsh,ras,las,sacrum,local_coord_sys,error )
      implicit none

      real*4    rsh(3),lsh(3),ras(3),las(3),sacrum(3),local_coord_sys(3,4)
      integer*4 error
                                                
      real*4    vector1(3),vector2(3),LCS_Xaxis(3),LCS_Yaxis(3),LCS_Zaxis(3),
     &          MidPelv(3),MidShldr(3),VecA(3),VecB(3),VecC(3),VecD(3),StkLth
      integer*4 x,y,z,cntr,i,NormError,VecNorm
             
      data x,y,z,cntr,StkLth / 1,2,3,4,6.0 / ! StkLth- Length of sacral stick, in
            
      error= 0
      call VecSub( lsh, rsh, vector1 )
      call VecSub( las, ras, VecA )
      call VecSub( sacrum, ras, VecB )
      call VecX( VecA,VecB,VecC )
      call VecX( VecC,VecA,VecD )
      NormError= VecNorm( VecD,VecD )
      if ( NormError .ne. 0 ) then ! bomb out of subroutine
        error= 7
        return
      end if
      do i= x, z     
        MidShldr(i)= ( rsh(i)+lsh(i) )/2.0
        MidPelv(i)= ( (ras(i)+las(i))/2.0 + 
     &                (sacrum(i)-0.0254*StkLth*VecD(i)) )/2.0
        LCS_Yaxis(i)= vector1(i)
      end do
      call VecSub( MidShldr, MidPelv, vector2 )
      call VecX( vector1, vector2, LCS_Xaxis )             
      call VecX( LCS_Xaxis, LCS_Yaxis, LCS_Zaxis )
      NormError= VecNorm( LCS_Xaxis, local_coord_sys(x,x) )
      if ( NormError .ne. 0 ) error= error + 1
      NormError= VecNorm( LCS_Yaxis, local_coord_sys(x,y) )
      if ( NormError .ne. 0 ) error= error + 2
      NormError= VecNorm( LCS_Zaxis, local_coord_sys(x,z) )
      if ( NormError .ne. 0 ) error= error + 4
c      call Tricen( rsh, lsh,  asis_avg, cntr1 ) ! old method
c      call Tricen( ras, las, shldr_avg, cntr2 )
      do i= x, z
        local_coord_sys(i,cntr)= ( MidShldr(i) + MidPelv(i) )/2.0
      end do
      return 
      end
c                                        
c	CalcLCSHead
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Calculate location/orientaiton of head LCS from trunk
c				LCS and location of neck marker.  This calculation exists
c				purely so that Telio may display 3D figures with heads !!!
c				The asthetics of this are quite important.  People really
c				don't like seeing headless figures....
c
c	Routines called:
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 CalcLCSHead( neck, trunk_rot_matrix, local_coord_sys )
      implicit none
 
      real*4    neck(3),local_coord_sys(3,4),trunk_rot_matrix(3,3)
   
      integer*4 x,y,z,cntr,i,j,error
      
      data x,z,cntr / 1,3,4 /

      do i= x, z
        do j= x, z
          local_coord_sys(i,j)= trunk_rot_matrix(i,j)
        end do
        local_coord_sys(i,cntr)= neck(i)
      end do
      return
      end                           
c                                        
c	HandleSegPosError
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Display message that error in segment LCS calculation
c				has occurred.
c
c	Routines called:
c	None
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Names			seg_names
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	None
c
      subroutine HandleSegPosError( error,frm,seg,NumError,ErrFrm,OF )
      implicit none

      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_names.inc'

      record /option_flag/ OF
      integer*4 error,NumError,ErrFrm(MAX_MTN),frm,seg,i

      NumError= NumError + 1
      ErrFrm(NumError)= frm
      if ( error .eq. 1 ) then ! x axis only
        write(6,100) seg_names(seg),frm
        if ( OF.ToLog ) write(3,100) seg_names(seg),frm
100     format(' Error- x     axis ',a4,' frame # ',i3)
      else if ( error .eq. 2 ) then ! y axis only
        write(6,110) seg_names(seg),frm
        if ( OF.ToLog ) write(3,110) seg_names(seg),frm
110     format(' Error-   y   axis ',a4,' frame # ',i3)
      else if ( error .eq. 4 ) then ! z axis only
        write(6,120) seg_names(seg),frm
        if ( OF.ToLog ) write(3,120) seg_names(seg),frm
120     format(' Error-     z axis ',a4,' frame # ',i3)
      else if ( error .eq. 3 ) then ! x and y axes 
        write(6,130) seg_names(seg),frm
        if ( OF.ToLog ) write(3,130) seg_names(seg),frm
130     format(' Error- x y   axes ',a4,' frame # ',i3)
      else if ( error .eq. 5 ) then ! x and z axes
        write(6,140) seg_names(seg),frm
        if ( OF.ToLog ) write(3,140) seg_names(seg),frm
140     format(' Error- x   z axes ',a4,' frame # ',i3)
      else if ( error .eq. 6 ) then ! x and z axes
        write(6,150) seg_names(seg),frm
        if ( OF.ToLog ) write(3,150) seg_names(seg),frm
150     format(' Error-   y z axes ',a4,' frame # ',i3)
      else if ( error .eq. 7 ) then ! x, y, and z axes
        write(6,160) seg_names(seg),frm
        if ( OF.ToLog ) write(3,160) seg_names(seg),frm
160     format(' Error- x y z axes ',a4,' frame # ',i3)
      end if
      error= 0
      return
      end
c                                        
c	InterpSegPos
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	If an error was detected in calculating a segment's LCS
c				then use interpolation to fill in the missing frames.
c				This capability is quite useful in clinical situations
c				in which a marker was occluded briefly and the marker 
c				trajectory interpolator did not do a good job of estimating
c				it's position.  In this case if this routine were not used
c				the data of the segments would be useless for calculating
c				the JRL's during these frames or more due to the numerical
c				differentiation of the segment positions.  NOTE:  Those
c				positions interpolated should be noted and kept in mind
c				during subsequent analysis.  Don't base clinical decisions
c				upon data purely from these frames...
c
c	Routines called:
c	find_interp_frm
c	lagrange_interp
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Param			GP.int_poly_order
c	Param			GP.num_seg
c	Param			GP.seg_num
c	Param			rMtnFrmRate
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	None
c
      subroutine InterpSegPos( NumError,ErrFrm,SegPos,OF,InterpError )
      implicit none
         
      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_prm.inc'
      include 'a_names.inc'

      integer*4 NumError(MAX_SEG),ErrFrm(MAX_MTN,MAX_SEG)
      real*4    SegPos(3,4,MAX_MTN,MAX_SEG)       
      logical*4 InterpError  

      record /option_flag/ OF

      real*4    time(10),PosData(10),InterpTime,InterpData
      integer*4 i,j,k,axis1,axis2,frm,seg,ErrorNum,NumGoodFrm,GoodFrm(MAX_MTN),
     &          InterpFrm(10),PolyOrder
      character axis_names(4)*4

      data axis_names / '   x','   y','   z','cntr' /
                             
      PolyOrder= GP.int_poly_order
      write(6,100)                                        
      if ( OF.ToLog ) write(3,100)
100   format(' Interpolating missing segment position/orientation data...')
      do k= 1, GP.num_seg
        seg= GP.seg_num(k)
        if ( NumError(seg) .gt. 0 ) then ! interpolate
          ErrorNum=1
          NumGoodFrm= 0
          do frm= 1, MP.nfrm
            if ( (frm .ne. ErrFrm(ErrorNum,seg)) .or.
     &           (ErrorNum .gt. NumError(seg))          ) then
              NumGoodFrm= NumGoodFrm + 1
              GoodFrm(NumGoodFrm)= frm
            else                
              ErrorNum= ErrorNum + 1 
            end if        
          end do ! frm                
          do i= 1, NumError(seg)           
            call find_interp_frm( GoodFrm,NumGoodFrm,ErrFrm(i,seg),
     &                            PolyOrder+1,InterpFrm )
            do j= 1, PolyOrder+1
              time(j)= float(InterpFrm(j))/rMtnFrmRate
            end do ! j
            InterpTime= float(ErrFrm(i,seg))/rMtnFrmRate
            do axis1= x, cntr
              do axis2= x, z
                do j= 1, PolyOrder+1
                  PosData(j)= SegPos(axis2,axis1,InterpFrm(j),seg)
                end do ! j
                call lagrange_interp( time,PosData,PolyOrder+1,
     &                                InterpTime,InterpData,InterpError )
                if ( InterpError ) goto 1000   
                SegPos(axis2,axis1,ErrFrm(i,seg),seg)= InterpData
              end do ! axis2
            end do ! axis1
          end do ! i => error number
        end if
      end do ! k => jnt
      write(6,110)
      if ( OF.ToLog ) write(3,110)
110   format('+Interpolating missing segment position/orientation data...',
     &       '   Done')
      return
                       
1000  write(6,1005) seg_names(seg),axis_names(axis2),axis_names(axis1),
     &              ErrFrm(i,seg)
      if ( OF.ToLog ) write(3,1005) seg_names(seg),axis_names(axis2),
     &                                      axis_names(axis1),ErrFrm(i,seg)
1005  format(' Error- Interpolating algorithm bombed at ',a4,' ',a4,' ',a4,
     &       ' frame #',i3)
      return
      end                                                                   
c                                        
c	CalcSegFiniteScrews
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:
c		Calculate screws describing the motion of each body segment from one
c		frame of kinematic data to the next ( in terms of GCS ).  Also, calculate
c		the screws describing motion of the various segments relative to one
c		another about their respective joints. (e.g., the calf relative to
c		thigh- knee joint screw ).  NOTE two different types of kinematic
c		screw calculation methods are available here because I've found that
c		Waldron's method seems to be less 'noisy' than Woltring's.  Also,
c		screws may be calculated for the positioning of the segments realtive
c		to the lab GCS or for the relative change in segment position from
c		one frame to the next 
c
c	Routines called:
c	CalcFSAWaldron
c	CalcFSAWoltring
c	MatTran
c	PntGCStoLCS
c	VecChangeCS
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Param			GP.analysis_type
c	Param			GP.jnt_num
c	Param			GP.num_jnt
c	Param			GP.num_seg
c	Param			GP.seg_num
c	Names			jnt_names
c	Names			JntSegNum
c	Param			MP.nfrm
c	Names			seg_names
c	Segment			SgpGCS
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c   Joint			JFinScr
c						screw(1-3) - direction cosines of screw axis
c						screw(4-6) - vector from CS origin to screw axis
c						screw(7)   - rotation angle about screw axis ( radians )
c						screw(8)   - translation along screw axis
c	Segment			SgFinScrGCS
c
      subroutine CalcSegFiniteScrews( CMND,SF,JF,OF )
      implicit none

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

      record /CmndLine/      CMND
      record /segment_flag/  SF
      record /joint_flag/    JF
      record /option_flag/   OF
                                     
      integer*4 frm,i,j,seg,jnt,DistSeg,ProxSeg,axis,NumFrm
      real*4    SegPosLCS(3,4,2),GCStoLCS(3,3),RefPosOrient(3,4)
      logical*4 PrevScrew,UseWaldronMethod,UseRelativeMotion,SkipLine

      data RefPosOrient / 1.0,0.0,0.0,
     &                    0.0,1.0,0.0,
     &                    0.0,0.0,1.0,
     &                    0.0,0.0,0.0  /

      SF.FiniteScrew= .false.
      JF.FiniteScrew= .false.         
      if ( .not. SF.position ) goto 1000 ! error
c
c     Check for option indicating which finite screw calculation technique
c     to use- Waldron or Woltring
c
      UseRelativeMotion= .false.
      UseWaldronMethod= .true.
      if ( CMND.NumOpt(2) .gt. 0 ) then
        do i= 1, CMND.NumOpt(2)                            
          if ( CMND.OptText(i,2) .eq. 'WAL' ) then
            UseWaldronMethod= .true.
          else if ( CMND.OptText(i,2) .eq. 'WOL' ) then
            UseWaldronMethod= .false.
          else if ( CMND.OptText(i,2) .eq. 'ABS' ) then
            UseRelativeMotion= .false.
          else if ( CMND.OptText(i,2) .eq. 'REL' ) then
            UseRelativeMotion= .true.
          else
            goto 1010 ! error
          end if
        end do
      end if                    
c                                                    
c     First calculate the screw of the segments through space from one frame
c     to the next relative to the lab GCS
c             
      NumFrm= MP.nfrm
      if ( UseRelativeMotion ) NumFrm= NumFrm - 1
      SkipLine= .false.                                          
      do frm= 1, NumFrm
        if ( OF.UpDate .and. (mod(frm,10) .eq. 0) )  then
          if ( SkipLine ) write(6,90)
90        format('  ')
          write(6,101) frm,MP.nfrm
101       format('+Processing frame # ',i3,'/',i3)      
          SkipLine= .false.
        end if
        do i= 1, GP.num_seg
          seg= GP.seg_num(i)
          if ( UseWaldronMethod ) then
            if ( UseRelativeMotion ) then
              call CalcFSAWaldron( SgpGCS(x,x,frm,seg),
     &                             SgpGCS(x,x,frm+1,seg),
     &                             SgFinScrGCS(1,frm,seg),
     &                             PrevScrew        )
            else
              call CalcFSAWaldron( RefPosOrient,
     &                             SgpGCS(x,x,frm,seg),
     &                             SgFinScrGCS(1,frm,seg),
     &                             PrevScrew        )
            end if                                                   
          else
            if ( UseRelativeMotion ) then
              call CalcFSAWoltring( SgpGCS(x,x,frm,seg),
     &                              SgpGCS(x,x,frm+1,seg),
     &                              SgFinScrGCS(1,frm,seg),
     &                              PrevScrew        )
            else
              call CalcFSAWoltring( RefPosOrient,
     &                              SgpGCS(x,x,frm,seg),
     &                              SgFinScrGCS(1,frm,seg),
     &                              PrevScrew        )
            end if
          end if                                      
          if ( PrevScrew ) then
            write(6,200) seg_names(seg),frm
            if ( OF.ToLog ) write(3,200) seg_names(seg),frm
200         format(' WARNING- Pure translation or error in calculation at ',
     &             a4,' frame #',i4)
            SkipLine= .true.
            if ( frm .gt. 1 ) then
              do j= 1, 8        
                SgFinScrGCS(j,frm,seg)= SgFinScrGCS(j,frm-1,seg)
              end do
            else
              do j= 1, 8
                SgFinScrGCS(j,frm,seg)= 0.0
              end do
            end if
          end if
        end do ! i
c                  
c       Calculate the screw of the distal segment relative to the proximal
c       segment at each joint from one frame to the next.
c
        do i= 1, GP.num_jnt                               
          jnt= GP.jnt_num(i)                     
          if ( jnt .ne. pvlb ) then ! this calculation doesn't make sense for
            DistSeg= JntSegNum(distal,jnt)  ! the pelvis relative to lab GCS
            ProxSeg= JntSegNum(proximal,jnt)
            do j= 1, 2
              call PntGCStoLCS( SgpGCS(x,x,frm+j-1,ProxSeg),
     &                          SgpGCS(x,cntr,frm+j-1,DistSeg),
     &                          SegPosLCS(x,cntr,j) )          
              call MatTran( SgpGCS(x,x,frm+j-1,ProxSeg),GCStoLCS )
              do axis= x, z
                call VecChangeCS( GCStoLCS,
     &                            SgpGCS(x,axis,frm+j-1,DistSeg),
     &                            SegPosLCS(x,axis,j) )
              end do ! axis
            end do ! j     
            if ( UseWaldronMethod ) then           
              if ( UseRelativeMotion ) then
                call CalcFSAWaldron( SegPosLCS(x,x,1),
     &                               SegPosLCS(x,x,2),
     &                               JFinScr(1,frm,jnt),
     &                               PrevScrew )          
              else
                call CalcFSAWaldron( RefPosOrient,
     &                               SegPosLCS(x,x,1),
     &                               JFinScr(1,frm,jnt),
     &                               PrevScrew )          
              end if
            else
              if ( UseRelativeMotion ) then
                call CalcFSAWoltring( SegPosLCS(x,x,1),
     &                                SegPosLCS(x,x,2),
     &                                JFinScr(1,frm,jnt),
     &                                PrevScrew )
              else
                call CalcFSAWoltring( RefPosOrient,
     &                                SegPosLCS(x,x,1),
     &                                JFinScr(1,frm,jnt),
     &                                PrevScrew )
              end if
            end if
          end if ! jnt not pelvis-lab                      
          if ( PrevScrew ) then
            if ( (GP.analysis_type .ne. FULL_ANL) .or.
     &           (jnt .ne. neck)                             ) then
              write(6,210) jnt_names(jnt),frm
              if ( OF.ToLog ) write(3,210) jnt_names(jnt),frm
210           format(' WARNING- Pure translation or error in calculation at ',
     &               a4,' frame #',i4)
              SkipLine= .true.
            end if
            if ( frm .gt. 1 ) then
              do j= 1, 8
                JFinScr(j,frm,jnt)= JFinScr(j,frm-1,jnt)
              end do
            else     
              do j= 1, 8
                SgFinScrGCS(j,frm,jnt)= 0.0
              end do    
            end if
          end if
        end do ! i => jnt
      end do ! frm
      SF.FiniteScrew= .true.
      JF.FiniteScrew= .true.
      return

1000  write(6,1005)
      if ( OF.ToLog ) write(3,1005)
1005  format(' Error- Segment position has not been calculated yet.')
      return

1010  write(6,1015) CMND.OptText(1,2)
      if ( OF.ToLog ) write(3,1015) CMND.OptText(1,2)
1015  format(' Error- ',a3,' is not a valid SEG FSC option'/
     &       '        Valid options are: WALdron WOLtring RELative ABSolute')
      return                    
      end
c                                        
c	CalcFSAWaldron
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:
c    Calculate the screw parameters describing the change in position of
c    a rigid body from location1 to location2.  These parameters include
c    the direction of the screw axis, the direction of a vector from the
c    coordinate system origin to the screw axis, the displacement along
c    the screw axis by the rigid body, and the rotation angle of the rigid
c    body about the screw axis.  These calculations are based upon algorithms
c    derived by Dr. K. Waldron at The Ohio State University, Department
c    of Mechanical Engineering.
c    NOTE!!! No assumptions as to the coordinate system are made within this
c    subroutine, so it applies equally well to calculations of the screw
c    in both global and local coordinate systems.
c
c  Data passed into subroutine 
c   - location1 - first position of rigid body( location and orientation )
c        - consists of 4 three dimensional vectors:
c        - loc(1-3,1-3) - direction cosines of local x,y,z of rigid body
c        - loc(1-3,4)   - location of point at LCS center on rigid body
c   - location2 - second position of rigid body
c                
c  Data returned from subroutine
c   - screw - screw parameters describing rigid motion
c           - consists of 8 real values:
c           - screw(1-3) - direction cosines of screw axis
c           - screw(4-6) - vector from CS origin to screw axis
c           - screw(7)   - rotation angle about screw axis ( degrees )
c           - screw(8)   - translation along screw axis ( meters )
c
c  - Error  -    a flag indicating that either pure translation
c                or minimal rotation and/or translation is present
c                in the screw, so assign the previous time steps
c                screw direction and location to this screw.
c
c	Routines called:
c	MatMult
c	MatTran
c	VecChangeCS
c	VecDot	
c	VecSub
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 CalcFSAWaldron( location1,location2,Screw,Error )
      implicit none

      real*4    location1(3,4),location2(3,4),screw(8)
      logical*4 Error

      real*4    matrix1(3,3),mmatrix(3,3),matrix3(3,3),vector1(3),mvec(3),
     &          lvec(3),vector4(3),theta,ctheta,mldot,VecDot,rtod
      integer*4 i,j,x,cntr

      data x,cntr / 1,4 /

      Error= .false.
      rtod= 45.0/atan(1.0)
      call MatTran( location1(x,x),matrix1 )
      call MatMult( location2(x,x),matrix1,mmatrix )
      call VecChangeCS( mmatrix,location1(x,cntr),vector1 )
      call VecSub( location2(x,cntr),vector1,mvec )
c        
c     Calculate the angle of rotation about the screw axis
c
      ctheta= 0.5*( mmatrix(1,1) + mmatrix(2,2) + mmatrix(3,3) - 1.0 )
      if ( abs(ctheta) .gt. 1.0 ) then
        Error= .true.
        return   
      end if
      theta= abs( acos( ctheta ) )
      lvec(1)= mmatrix(2,3) - mmatrix(3,2)
      lvec(2)= mmatrix(3,1) - mmatrix(1,3)
      lvec(3)= mmatrix(1,2) - mmatrix(2,1)
      mldot= VecDot( mvec,lvec )
      if ( mldot .lt. 0.0 ) theta= -theta
      screw(7)= theta
c
c     Check magnitude of rotation- if it is too small than assume
c     that the screw represents essentially pure translation.
c
      if ( abs(sin(theta)) .gt. 1.0e-5 ) then ! rotation is present
c
c       Calculate the unit vector in the direction of the screw axis
c
        do i= 1, 3
          screw(i)= -lvec(i)/(2.0*sin(theta))
        end do
c
c       Calculate the translation along the screw axis
c
        screw(8)= -mldot/(2.0*sin(theta))
c
c       Calculate normal vector from CS origin to the screw axis
c
        if ( abs(1.0 - ctheta) .gt. 1.0e-5 ) then
          do i= 1, 3    
            do j= 1, 3
              if ( i .eq. j ) then
                matrix3(i,j)= 1.0 - mmatrix(j,i)
              else
                matrix3(i,j)= -mmatrix(j,i)
              end if
            end do
          end do
          call VecChangeCS( matrix3,lvec,vector4 )
          do i= 1, 3      
            screw(i+3)= vector4(i)/(1.0-ctheta)
          end do
        else
          do i= 4, 6
            screw(i)= 0.0
          end do
        end if
      else
        Error= .true.                                               
      end if
      screw(7)= screw(7)*rtod
      return         
      end
c                                        
c	CalcFSAWoltring
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:
c    Calculate the screw parameters describing the change in position of
c    a rigid body from location1 to location2.  These parameters include
c    the direction of the screw axis, the direction of a vector from the
c    coordinate system origin to the screw axis, the displacement along
c    the screw axis by the rigid body, and the rotation angle of the rigid
c    body about the screw axis.  These calculations are based upon algorithms
c    derived by Herman Woltring
c    
c    NOTE!!! No assumptions as to the coordinate system are made within this
c    subroutine, so it applies equally well to calculations of the screw
c    in both global and local coordinate systems.
c
c  Data passed into subroutine 
c   - location1 - first position of rigid body( location and orientation )
c        - consists of 4 three dimensional vectors:
c        - loc(1-3,1-3) - direction cosines of local x,y,z of rigid body
c        - loc(1-3,4)   - location of point at LCS center on rigid body
c   - location2 - second position of rigid body
c                
c  Data returned from subroutine
c   - screw - screw parameters describing rigid motion
c           - consists of 8 real values:
c           - screw(1-3) - direction cosines of screw axis
c           - screw(4-6) - vector from CS origin to screw axis
c           - screw(7)   - rotation angle about screw axis ( degrees )
c           - screw(8)   - translation along screw axis ( meters )
c
c  - Error  -    a flag indicating that either pure translation
c                or minimal rotation and/or translation is present
c                in the screw, so assign the previous time steps
c                screw direction and location to this screw.
c
c	Routines called:
c	MatMult
c	MatTran
c	VecDot
c	VecMag	
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 CalcFSAWoltring( location1,location2,Screw,Error )
      implicit none

      real*4    location1(3,4),location2(3,4),screw(8)
      logical*4 Error

      real*4    GCStoLCS1(3,3),RotMat(3,3),theta,ctheta,stheta,rtod,pi,VecD(3),
     &          VecE(3),VecP(3),VecDot,VecMag,b1(3),b2(3),b3(3),Magb1,Magb2,
     &          Magb3
      integer*4 i,j,x,cntr

      data x,cntr / 1,4 /
                                              
      Error= .true.
      rtod= 45.0/atan(1.0)
      pi= 4.0*atan(1.0)
      call MatTran( location1(x,x),GCStoLCS1 )
      call MatMult( GCStoLCS1,location2(x,x),RotMat )                 
c        
c     Calculate the angle of rotation about the screw axis
c
      ctheta= 0.5*( RotMat(1,1)+RotMat(2,2)+RotMat(3,3)-1.0 )
      stheta= 0.5*sqrt( (RotMat(3,2)-RotMat(2,3))**2 +
     &                  (RotMat(1,3)-RotMat(3,1))**2 +
     &                  (RotMat(2,1)-RotMat(1,2))**2 )
      if ( (abs(ctheta) .lt. 1.0e-5) .and. (stheta .lt. 1.0e-5) )  return   
      if ( stheta .lt. 1.41 ) then
        if ( abs(ctheta) .gt. 1.0 ) return
        theta= acos( ctheta )
      else
        if ( stheta .gt. 1.0 ) return
        theta= asin( stheta )
      end if
      screw(7)= theta
c      theta= atan2( stheta,ctheta )
c          
c     Now find the axis of rotation
c
      if ( stheta .lt. sin(0.75*pi) ) then
        b1(1)= RotMat(1,1) - ctheta
        b1(2)= 0.5*(RotMat(2,1)+RotMat(1,2)) - ctheta
        b1(3)= 0.5*(RotMat(3,1)+RotMat(1,3)) - ctheta
        b2(1)= 0.5*(RotMat(2,1)+RotMat(1,2)) - ctheta
        b2(2)= RotMat(2,2) - ctheta
        b2(3)= 0.5*(RotMat(3,2)+RotMat(2,3)) - ctheta
        b3(1)= 0.5*(RotMat(3,1)+RotMat(1,3)) - ctheta
        b3(2)= 0.5*(RotMat(3,2)+RotMat(2,3)) - ctheta
        b3(3)= RotMat(3,3) - ctheta
        Magb1= VecMag(b1)
        Magb2= VecMag(b2)
        Magb3= VecMag(b3)   
        if ( (Magb1 .ge. Magb2) .and. (Magb1 .ge. Magb3) ) then
          if ( Magb1 .lt. 1.0e-5 ) return
          do i= 1, 3
            screw(i)= b1(i)/Magb1
          end do
        else if ( (Magb2 .ge. Magb1) .and. (Magb2 .ge. Magb3) ) then
          if ( Magb2 .lt. 1.0e-5 ) return
          do i= 1, 3
            screw(i)= b2(i)/Magb2
          end do
        else        
          if ( Magb3 .lt. 1.0e-5 ) return
          do i= 1, 3
            screw(i)= b3(i)/Magb3
          end do    
        end if
c        if ( (screw(1) .gt. 0.0) .and. 
c     &       (RotMat(3,2)-RotMat(2,3) .lt. 0.0) ) then ! change direction of
c          do i= 1, 3                                   ! screw because otherwise
c            screw(i)= -screw(i)                        ! theta must be negative
c          end do                                       ! which by definition is
c          screw(7)= abs(screw(7))                      ! not allowed
c        end if
      else              
        screw(1)= 0.5*(RotMat(3,2)-RotMat(2,3))/stheta
        screw(2)= 0.5*(RotMat(1,3)-RotMat(3,1))/stheta
        screw(3)= 0.5*(RotMat(2,1)-RotMat(1,2))/stheta
      end if
c
c     Calculate point on screw axis and translation along axis
c
      do i= 1, 3
        VecD(i)= location2(i,cntr) - location1(i,cntr)
        VecP(i)= 0.5*( location2(i,cntr) + location1(i,cntr) )
      end do
      call VecX( screw(1),VecD,VecE )
      do i= 1, 3
        screw(i+3)= VecP(i) + 2.0*tan(screw(7))*VecE(i)
      end do
      screw(8)= VecDot(screw(1),VecD)
      screw(7)= screw(7)*rtod
      Error= .false.
      return         
      end
c                                        
c	CalcSegInstantScrews
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:
c    Calculate screws describing the motion of each body segment from one
c    frame of kinematic data to the next ( in terms of GCS ).  Also, calculate
c    the screws describing motion of the various segments relative to one
c    another about their respective joints. (e.g., the calf relative to
c    thigh- knee joint screw ). 
c
c	Routines called:
c	CalcInstantScrewAxis
c	MatTran
c	PntGCStoLCS
c	VecChangeCS
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	Param			GP.num_seg
c	Param			GP.seg_num
c	Names			JntSegNum
c	Param			MP.nfrm
c	Segment			SgpGCS
c	Segment			SvlGCS
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c   Joint			JInsScr
c						screw(1-3) - direction cosines of screw axis
c						screw(4-6) - vector from CS origin to screw axis
c						screw(7)   - rotation velocity about screw axis (r/s)
c						screw(8)   - translation velocity along screw axis (m/s)
c	Segment			SgInsScrGCS
c
c
      subroutine CalcSegInstantScrews( SF,JF,OF )
      implicit none

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

      record /segment_flag/  SF 
      record /joint_flag/    JF
      record /option_flag/   OF
                                     
      integer*4 frm,i,j,seg,jnt,DistSeg,ProxSeg,axis
      real*4    SegCntrLCS(3),GCStoLCS(3,3),SegVelLCS(6)

      SF.InstantScrew= .false.
      JF.InstantScrew= .false.
      if ( .not. SF.position ) goto 1000 ! error
      if ( .not. SF.vel_acc  ) goto 1010 ! error
c                
c     First calculate the screw of the segments through space from one frame
c     to the next relative to the lab GCS
c                                                       
      do frm= 1, MP.nfrm
        if ( OF.UpDate .and. (mod(frm,10) .eq. 0) )  
     &    write(6,101) frm,MP.nfrm
101     format('+Processing frame # ',i3,'/',i3)      
        do i= 1, GP.num_seg
          seg= GP.seg_num(i)
          call CalcInstantScrewAxis( SvlGCS(vx,frm,seg),
     &                               SgpGCS(x,cntr,frm,seg),
     &                               SgInsScrGCS(1,frm,seg) )
        end do ! i
c
c       Calculate the screw of the distal segment relative to the proximal
c       segment at each joint from one frame to the next.
c
        do i= 1, GP.num_jnt
          jnt= GP.jnt_num(i)                     
          if ( jnt .ne. pvlb ) then ! this calculation doesn't make sense for
            DistSeg= JntSegNum(distal,jnt)  ! the pelvis relative to lab GCS
            ProxSeg= JntSegNum(proximal,jnt)
            call MatTran( SgpGCS(x,x,frm,ProxSeg),GCStoLCS )
            call VecChangeCS( GCStoLCS,SvlGCS(vx,frm,DistSeg),SegVelLCS(vx))
            call VecChangeCS( GCStoLCS,SvlGCS(wx,frm,DistSeg),SegVelLCS(wx))
            call PntGCStoLCS( SgpGCS(x,x,frm,ProxSeg),
     &                        SgpGCS(x,cntr,frm,DistSeg),
     &                        SegCntrLCS )
            call CalcInstantScrewAxis( SegVelLCS,SegCntrLCS,
     &                                 JInsScr(1,frm,jnt) )
          end if ! seg is not pvlb
        end do ! i
      end do ! frm
      SF.InstantScrew= .true.
      JF.InstantScrew= .true.
      return
      
1000  write(6,1005)
      if ( OF.ToLog ) write(3,1005)
1005  format(' Error- Segment position has not been calculated yet.')
      return
      
1010  write(6,1015)
      if ( OF.ToLog ) write(3,1015)
1015  format(' Error- Segment velocity has not been calculated yet.')
      return
      end
c                                        
c	CalcInstantScrewAxis
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Calculate the instantaneous kinematic screw from the 
c				segment's rotation/translational velocity and segment
c				position.  NOTE that this routine can also be used to
c				calculate a load wrench by replacing the rotational vel
c				with the force and and the translational vel with the
c				applied moment and the segment position with the point
c				of application of the load.  This is true because the
c				instantaneous kinematic screw is the dual of the load 
c				wrench...
c
c	Routines called:
c	VecMag
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 CalcInstantScrewAxis( Vel,Pos,Screw )
      implicit none

      include 'a_names.inc'           

      real*4    Vel(6),Pos(3),Screw(8)
                                                                        
      integer*4 axis
      real*4    VecMag,rtod

      Screw(7)= VecMag(Vel(wx))
      rtod= 45.0/atan(1.0)
      if ( Screw(7) .gt. 1.0e-5 ) then ! screw has a rotational velocity
        do axis= x, z
          Screw(axis)= Vel(wx+axis-1)/Screw(7) ! screw axis direction cosines
        end do
        Screw(4)= ( ( Pos(x)*Vel(wy) + Vel(vz) - Pos(y)*Vel(wx) )*Vel(wy) +
     &              ( Pos(x)*Vel(wz) - Vel(vy) - Pos(z)*Vel(wx) )*Vel(wz) )/
     &              Screw(7)**2 ! x position of screw
        Screw(5)= ( ( Pos(y)*Vel(wx) + Vel(vz) - Pos(x)*Vel(wy) )*Vel(wx) +
     &              ( Pos(y)*Vel(wz) - Vel(vx) - Pos(z)*Vel(wy) )*Vel(wz) )/
     &              Screw(7)**2 ! y position of screw 
        Screw(6)= ( ( Pos(z)*Vel(wx) + Vel(vy) - Pos(x)*Vel(wz) )*Vel(wx) +
     &              ( Pos(z)*Vel(wy) - Vel(vx) - Pos(y)*Vel(wz) )*Vel(wy) )/
     &              Screw(7)**2 ! z position of screw                 
        Screw(8)= ( Vel(vx)*Vel(wx) + Vel(vy)*Vel(wy) + Vel(vz)*Vel(wz) )/
     &              Screw(7)**2  ! translational velocity
      else ! screw is purely translational
        Screw(8)= VecMag(Vel(vx))
        if ( Screw(8) .gt. 1.0e-5 ) then
          do axis= x, z
            Screw(axis)= Vel(axis)/Screw(8)
            Screw(axis+3)= 0.0
          end do
        else ! rigid body is pretty much stationary
          do axis= 1, 6
            Screw(axis)= 0.0
          end do
        end if
      end if
      Screw(7)= Screw(7)*rtod
      return
      end
c                                        
c	CalcSegEnergy
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Calculate the mechanical energy of the body segments and
c				the total body.  These energies are the kinetic, potential,
c				total (sum of KE and PE) energies.  The kinetic energy is
c				separated into translational and rotational components
c				as well as the magnitude sum of the two.
c
c	Routines called:
c	VecDot
c	VecMag
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Param			GP.num_seg
c	Param			GP.seg_num
c	Param			MP.nfrm
c	Segment			Sgp_LCS
c	Param			SP.inertia
c	Param			SP.mass
c	Segment			SvlLCS
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Energy			BodyKE
c	Energy			BodyPE
c	Energy			BodyTE
c	Energy			SegKE
c	Energy			SegPE
c	Energy			SegTE
c
      subroutine CalcSegEnergy( SF,JF,OF )
      implicit none

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

      record /joint_flag/     JF
      record /segment_flag/   SF
      record /option_flag/    OF
                               
      real*4    gravity,vel_mag,VecMag,VecDot,jnt_mom_LCS(3),GCStoLCS(3,3)
      integer*4 frm,segment,axis,rot,jnt,seg,sign,KE_trans,KE_rot,KE_total,i,j
                         
      data rot,KE_trans,KE_rot,KE_total / 3,1,2,3 /
      data gravity / 9.8071 / 

      if ( .not. SF.position ) goto 1000
      if ( .not. SF.param )    goto 1010
      if ( .not. SF.vel_acc )  goto 1020
      do frm= 1, MP.nfrm
        if ( OF.UpDate .and. (mod(frm,10) .eq. 0) )  
     &    write(6,101) frm,MP.nfrm
101     format('+Processing frame # ',i3,'/',i3)      
        do i= 1, 3
          BodyKE(i,frm)= 0.0
        end do
        BodyPE(frm)= 0.0                            
        BodyTE(frm)= 0.0
        do i= 1, GP.num_seg
          seg= GP.seg_num(i)
          vel_mag= VecMag( SvlLCS(x,frm,seg) )
          SegKE(KE_trans,frm,seg)= 0.5*SP(seg).mass*vel_mag**2
          SegKE(KE_rot,frm,seg)= 0.0
          do axis= x, z  
            SegKE(KE_rot,frm,seg)= SegKE(KE_rot,frm,seg) +
     &                              0.5*SP(seg).inertia(axis)*
     &                              SvlLCS(axis+rot,frm,seg)**2
          end do ! axis
          SegKE(KE_total,frm,seg)= SegKE(KE_trans,frm,seg) +
     &                              SegKE(KE_rot,frm,seg)
          SegPE(frm,seg)= SP(seg).mass*gravity*
     &                               SgpGCS(z,cntr,frm,seg)
c     &                           -  SP(seg).avg_cntr(z)      )
          SegTE(frm,seg)= SegKE(KE_total,frm,seg) + SegPE(frm,seg)
          do j= 1, 3
            BodyKE(j,frm)= BodyKE(j,frm) + SegKE(j,frm,seg)
          end do ! j
          BodyPE(frm)= BodyPE(frm) + SegPE(frm,seg)
          BodyTE(frm)= BodyTE(frm) + SegTE(frm,seg)
        end do ! i 
      end do ! frm
      SF.energy= .true.
      return                                
      
1000  write(6,1005)
      if ( OF.ToLog ) write(3,1005)
1005  format('Error- Segment position has not been calculated.')
      return
      
1010  write(6,1015)
      if ( OF.ToLog ) write(3,1015)
1015  format('Error- Segment parameters have not been calculated.')
      return
      
1020  write(6,1025)                         
      if ( OF.ToLog ) write(3,1025)
1025  format('Error- Segment velocity has not been calculated.')
      return
      end
c                                        
c	CalcMrkDiagnostics
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Calculate the position of the markers attached to a 
c				segment relative to the segment LCS.  These should be
c				constants if the markers were attached rigidly to the
c				segment, but of course this is reality and they are not.
c				thus this information functions as a diagnotic to tell
c				how well the markers approximate this at a given time
c				frame.  Also, calculate the distance between markers.
c				The same remarks above apply to this but the resulting
c				information is easier to interpret though less useful.
c
c	Routines called:
c	PntGCStoLCS
c	VecLth
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Param			GP.num_seg
c	Param			GP.seg_num
c	Marker			MrkData                
c	Param			MP.nfrm
c	Segment			SgpGCS
c	Param			SP.inertia
c	Param			SP.mass
c	Param			SP.mrk_num
c	Param			SP.num_mrk
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Marker			MrkDist
c	Marker			MpnLCS
c	Param			SP.dist_mrk_num
c	Param			SP.num_MrkDist
c
      subroutine CalcMrkDiagnostics( CMND,MF,SF,OF )
      implicit none          

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

      record /CmndLine/      CMND
      record /marker_flag/   MF           
      record /segment_flag/  SF
      record /option_flag/   OF
                               
      integer*4 i,j,k,seg,mrk,num_dist,frm
      real*4    VecLth,VecMag,VecDot,rtod,vecA(3),vecB(3),vecC(3),PrevVecC(3),
     &          MagA,MagB,Angle
      logical*4 DoMrkDist
c                                
c     Check for option to calculate the angles between the markers
c
      if ( .not. SF.position ) goto 1000 ! error- seg pos must be present
      rtod= 45.0/atan(1.0)
      DoMrkDist= .true.
      if ( CMND.NumOpt(2) .gt. 0 ) then
        do i= 1, CMND.NumOpt(2)          
          if ( CMND.OptText(i,2) .eq. 'ANG' ) then
            DoMrkDist= .false.
          else
            goto 1010
          end if
        end do   
      end if
c                             
c     First build array containing marker numbers defining the line segments
c     between markers in the same segment
c  
      do i= 1, GP.num_seg
        seg= GP.seg_num(i)
        num_dist= 0
        do j= 1, SP(seg).num_mrk-1
          do k= j+1, SP(seg).num_mrk
            num_dist= num_dist + 1
            if ( num_dist .le. 10 ) then
              SP(seg).dist_mrk_num(1,num_dist)= SP(seg).mrk_num(j)
              SP(seg).dist_mrk_num(2,num_dist)= SP(seg).mrk_num(k)
            end if
          end do ! k           
        end do ! j
c
c      Due to my dimensioning MrkDist to (6,MAX_FRM,MAX_SEG) only 6 distances
c      can be stored at present - I'll fix this when I update the ANZ file
c      format to version 'c' For now limit the number of distances to 6 to
c      prevent a crash...
c
        if ( num_dist .gt. 6 ) then
          write(6,110) num_dist,seg_names(seg)
          if ( OF.ToLog ) write(3,110) num_dist,seg_names(seg)
110       format(' NOTE- The actual number of intermarker distances (',i2,
     &           ') for ',a4,' is'/
     &           '       being limited to 6 due current limitations in the program')
          num_dist= 6
        end if
        SP(seg).num_MrkDist= num_dist
c
c       Now calculate the intermarker distances
c
        if ( DoMrkDist ) then
          do frm= 1, MP.nfrm
            do j= 1, num_dist    
              MrkDist(j,frm,seg)= 
     &                VecLth( MrkData(x,frm,SP(seg).dist_mrk_num(1,j)),
     &                        MrkData(x,frm,SP(seg).dist_mrk_num(2,j))  )
            end do ! j
          end do ! frm
c
c       Calculate the angles between the markers on the segment
c
        else
          do frm= 1, MP.nfrm
            do j= 1, num_dist
              if ( j .lt. num_dist ) then
                call VecSub( MrkData(x,frm,SP(seg).dist_mrk_num(1,j)),
     &                       MrkData(x,frm,SP(seg).dist_mrk_num(2,j)),
     &                       vecA )
                call VecSub( MrkData(x,frm,SP(seg).dist_mrk_num(1,j+1)),
     &                       MrkData(x,frm,SP(seg).dist_mrk_num(2,j+1)),
     &                       vecB )
              else
                call VecSub( MrkData(x,frm,SP(seg).dist_mrk_num(1,j)),
     &                       MrkData(x,frm,SP(seg).dist_mrk_num(2,j)),
     &                       vecA )
                call VecSub( MrkData(x,frm,SP(seg).dist_mrk_num(1,1)),
     &                       MrkData(x,frm,SP(seg).dist_mrk_num(2,1)),
     &                       vecB )
              end if
              call VecX( vecA,vecB,vecC )
              MagA= VecMag(vecA)
              MagB= VecMag(vecB)
              if ( (MagA .lt. 1.0e-5) .or. (MagB .lt. 1.0e-5) ) then
                Angle= 0.0
              else
                Angle= VecDot(vecA,vecB)/(MagA*MagB)
                if ( Angle .gt. 1.0  ) Angle=  1.0
                if ( Angle .lt. -1.0 ) Angle= -1.0
                Angle= rtod*acos(Angle)
                if ( (frm .gt. 1) .and. (VecDot(vecC,PrevVecC) .lt. 0.0) ) 
     &            Angle= -Angle
                do k= 1, 3
                  PrevVecC(k)= vecC(k)
                end do
              end if
              MrkDist(j,frm,seg)= angle
            end do ! j 
          end do ! frm                                                
        end if
      end do ! i
c
c     Calculate marker position relative to segment LCS in each frame
c
      write(6,100)
100   format(/)
      do frm= 1, MP.nfrm
        if ( OF.UpDate .and. (mod(frm,10) .eq. 0) )  
     &    write(6,101) frm,MP.nfrm
101     format('+Processing frame # ',i3,'/',i3)      
        do i= 1, GP.num_seg
          seg= GP.seg_num(i)
          do j= 1, SP(seg).num_mrk
            mrk= SP(seg).mrk_num(j)
            call PntGCStoLCS( SgpGCS(x,x,frm,seg),
     &                        MrkData(x,frm,mrk),
     &                        MpnLCS(x,j,frm,seg)    )
          end do ! j
        end do ! i
      end do ! frm
      SF.param= .true.
      MF.diagnostics= .true.
      return

1000  write(6,1005)
      if ( OF.ToLog ) write(3,1005)
1005  format(' Error- Segment position has not been calculated yet.'/
     &       '        This must be done before marker diagnostics may be done.')
      return

1010  write(6,1015) CMND.OptText(i,2)
      if ( OF.ToLog ) write(3,1015) CMND.OptText(i,2)
1015  format(' Error- ', a3,' is not a valid option for SEG MRK'/
     &       '        The only valid option is /ANGle')
      return
      end
c                                        
c	CalcBodyCM
c                
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Calculate the body center of mass using the positions 
c				and estimated masses of the individual segments.  Also,
c				optionally estimate the velocity of the center of mass
c				by differentiating the center of mass path.
c
c	Routines called:
c	filter
c	RemoveCMNDOpt
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Param			GP.num_seg
c	Param			GP.seg_num
c	Param			MP.nfrm
c	Segment			SgpGCS
c	Param			SP.mass
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Segment			BodyCM
c	Segment			BCMVel
c
      subroutine CalcBodyCM( CMND,SF,OF )
      implicit none
                             
      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_prm.inc'
      include 'a_seg.inc'
      include 'a_names.inc'
    
      record /CmndLine/      CMND
      record /segment_flag/  SF
      record /option_flag/   OF

      integer*4 i,j,seg,axis,frm,status
      real*4    total_mass,temp(MAX_MTN),vel(MAX_MTN),acc(MAX_MTN),
     &          time_inc
      character axis_names(3)*1
      logical*4 CalcBCMVel,Smooth

      data axis_names / 'x','y','z' /

      SF.cntr_mass= .false.
      if ( .not. SF.position ) goto 1000
      if ( .not. SF.param )    goto 1010
c
c     Check for option requesting center of mass velocity to be
c     calculated
c
      CalcBCMVel= .false.
      if ( CMND.NumOpt(2) .ne. 0 ) then
        do i= 1, CMND.NumOpt(2)
          if ( CMND.OptText(i,2) .eq. 'VEL' ) then
            CalcBCMVel= .true.
            call RemoveCMNDOpt( CMND,2,i )
          end if
        end do
        if ( .not. CalcBCMVel ) goto 1020 ! error
      end if
c
c     estimate total body mass
c
      total_mass= 0.0
      do i= 1, GP.num_seg
        seg= GP.seg_num(i)
        total_mass= total_mass + SP(seg).mass
      end do ! i
c
c     calculate the body center of mass from a weighted average of the segment
c     mass centers   
c
      do frm= 1, MP.nfrm
        if ( OF.UpDate .and. (mod(frm,10) .eq. 0) )  
     &    write(6,101) frm,MP.nfrm
101     format('+Processing frame # ',i3,'/',i3)      
        do axis= x, z
          BodyCM(axis,frm)= 0.0
        end do  
        do i= 1, GP.num_seg
          seg= GP.seg_num(i)
          do axis= x, z
            BodyCM(axis,frm)= BodyCM(axis,frm) +
     &                         SP(seg).mass*SgpGCS(axis,cntr,frm,seg)
          end do
        end do ! i => seg
        do axis= x, z
          BodyCM(axis,frm)= BodyCM(axis,frm)/total_mass
        end do ! axis
      end do ! frm
      SF.cntr_mass= .true.
c
c     If requested now calculate the velocity of the center of mass
c      
      if ( CalcBCMVel ) then
        time_inc= 1.0/rMtnFrmRate
        Smooth= .false.  ! differentiation will take place
        do axis= x, z
          if ( OF.UpDate )  write(6,200) axis_names(axis)
200       format('+Differentiating center of mass path : ',a1)
          do frm= 1, MP.nfrm
            temp(frm)= BodyCM(axis,frm)
          end do ! frm
          call filter( CMND,Smooth,MP.nfrm,temp,time_inc,OF,
     &                 vel,acc,status )                                   
          if ( status .lt. 0 ) then
            goto 1030 ! fatal error in filter...
          else if ( status .eq. 2 ) then       
            write(6,300) axis_names(axis)
300         format('+Differentiating center of mass path : ',a1,
     &             '    Warning- Nearly Constant'/)
            if ( OF.ToLog ) write(3,300) axis_names(axis)
          else if ( status .eq. 3 ) then   
            write(6,310) axis_names(axis)
310         format('+Differentiating center of mass path : ',a1,
     &             '    Warning- Data very small'/)
            if ( OF.ToLog ) write(3,310) axis_names(axis)
          else if ( status .eq. 4 ) then   
            write(6,320) axis_names(axis)
320         format('+Differentiating center of mass path : ',a1,
     &             '    Filter error- Used Finite Dif'/)
            if ( OF.ToLog ) write(3,320) axis_names(axis)
          else if ( status .ge. 10 ) then   
            write(6,330) axis_names(axis),status-10
330         format('+Differentiating center of mass path : ',a1,
     &             '    GCVSPL error #',i2,'- Used Finite Dif'/)
            if ( OF.ToLog ) write(3,330) axis_names(axis),status-10
          end if
          do frm= 1, MP.nfrm
            BodyCM(axis,frm)= temp(frm)
            BCMVel(axis,frm)= vel(frm)
          end do ! frm
        end do ! axis
        SF.BCMVel= .true.
      end if
      return

1000  write(6,1005)
      if ( OF.ToLog ) write(3,1005)
1005  format(' Error- Segment position has not been calculated yet.')
      return

1010  write(6,1015)
      if ( OF.ToLog ) write(3,1015)
1015  format(' Error- Segment parameters have not been calculated yet.')
      return

1020  write(6,1025)
      if ( OF.ToLog ) write(3,1025)
1025  format(' Error- Options must include /VEL to be valid here')
      return

1030  write(6,1035) axis_names(axis)
      if ( OF.ToLog ) write(3,1035) axis_names(axis)
1035  format(' Error- Smoothing/differentiating filter has bombed out on'/
     &       '        center of mass ',a1,' while doing the calculations'/
     &       '        for ceneter of mass translational velocity')
      return
      end
c                                        
c	CalcSegMomentum
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Calculate the segment and total body momentum relative
c				to the lab GCS. Compute the components of both the
c				translational and rotational momentum as well as the 
c				magnitude of these.
c
c	Routines called:
c	VecMag
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Param			GP.num_seg
c	Param			GP.seg_num
c	Param			MP.nfrm
c	Param			SP.inertia
c	Param			SP.mass
c	Segment			SvlGCS
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Segment			BdMom
c	Segment			SgMom
c
      subroutine CalcSegMomentum( SF,OF )
      implicit none
                             
      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_prm.inc'
      include 'a_seg.inc'
      include 'a_names.inc'
    
      record /segment_flag/  SF
      record /option_flag/   OF
                             
      integer*4 i,j,seg,axis,frm,rot
      real*4    VecMag

      data rot / 3 /

      SF.momentum= .false.
      if ( .not. SF.vel_acc ) goto 1000
      if ( .not. SF.param )   goto 1010
      do frm= 1, MP.nfrm
        if ( OF.UpDate .and. (mod(frm,10) .eq. 0) )  
     &    write(6,101) frm,MP.nfrm
101     format('+Processing frame # ',i3,'/',i3)      
        do i= 1, 8
          BdMom(i,frm)= 0.0
        end do
        do i= 1, GP.num_seg
          seg= GP.seg_num(i)
          do axis= x, z
            SgMom(axis,frm,seg)= SP(seg).mass*
     &                                  SvlGCS(axis,frm,seg)
            SgMom(axis+rot,frm,seg)= SP(seg).inertia(axis)*
     &                                      SvlGCS(axis+rot,frm,seg)
          end do ! axis
          SgMom(7,frm,seg)= VecMag( SgMom(x,frm,seg) )
          SgMom(8,frm,seg)= VecMag( SgMom(rx,frm,seg) )
          do j= 1, 8
            BdMom(j,frm)= BdMom(j,frm) + SgMom(j,frm,seg)
          end do ! j
        end do ! i => seg
      end do ! frm
      SF.momentum= .true.
      return
      
1000  write(6,1005)
      if ( OF.ToLog ) write(3,1005)
1005  format(' Error- Segment velocity has not been calculated yet.')
      return

1010  write(6,1015)
      if ( OF.ToLog ) write(3,1015)
1015  format(' Error- Segment parameters have not been calculated yet.')
      return
      end
c                                        
c	CalcSegGCSAngles
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Calculate the YXZ euler angle of the segment orientation
c				relative to the lab GCS.  This is an alternative way of
c				expressing segment orientation and is used to compute the
c				segment rotational velocity more efficiently than using
c				the rotation matrix contained in SgpGCS.  NOTE however
c				that there is the definition problem when exchanging 
c				the angle data with others whereas the rotation matrix, though
c				taking more storage space, is unambiguous.
c
c	Routines called:
c                
c  CalcEulAccYXZ
c  CalcEulVelYXZ
c	HandleAngleError
c	InterpAngles
c	JCSAngles
c  odeinit
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c                        
c	Param			GP.analysis_type
c	Param			GP.num_seg
c	Param			GP.seg_num
c	Param			MP.nfrm
c  Param       rMtnFrmRate
c	Segment			SangGCS
c	Segment			SacGCS
c	Segment			SgpGCS
c	Segment			SvlGCS
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Segment			SangGCS
c
      subroutine CalcSegGCSAngles( CMND,SF,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 /option_flag/   OF

      real*4    Accel(3)
      common / Acceleration / Accel

      integer*4 i,j,k,seg,frm,NumError(3,MAX_SEG),nvar,nok,nbad
      real*4    TempRotMat(3,3),yt(6),TimeInc,t1,t2,EulerAngYXZ(3,MAX_MTN),
     &          EulerVelYXZ(3,MAX_MTN),EulerAccYXZ(3,MAX_MTN),rtod
      logical*4 error,SkipLine,InterpFlag,JntInterp,InterpError,IntegrateAngle
                        
c
c     Check for /INTegrate option
c
      IntegrateAngle= .false.
      if ( CMND.NumOpt(2) .gt. 0 ) then
        do i= 1, CMND.NumOpt(2)
          if ( CMND.OptText(i,2) .eq. 'INT' ) then
            IntegrateAngle= .true.
          else
            goto 1020 ! error
          end if
       end do                                                  
      end if                    
c
c     If integral then integrate the segment rotational acceleration relative\
c     to the lab GCS to find the segment angle at each frame
c                   
      if ( IntegrateAngle ) then              
        if ( .not. SF.Angle ) goto 1030 ! error
        if ( .not. SF.Vel_Acc ) goto 1040 ! error
        TimeInc= 1.0/rMtnFrmRate
        nvar= 6
        rtod= 45.0/atan(1.0)
        do i= 1, GP.num_seg      
          seg= GP.seg_num(i)
          if ( OF.UpDate ) write(6,107) i,GP.num_seg,seg_names(seg)
107       format('+Integrating segment # ',i2,'/',i2,' : ',a4)
          do j= 1, 3  
            EulerAngYXZ(j,1)= SangGCS(j,1,seg)/rtod
          end do
          call CalcEulVelYXZ( EulerAngYXZ(1,1),SvlGCS(4,1,seg),
     &                           EulerVelYXZ(1,1) )
          do frm= 1, MP.nfrm  
            call CalcEulAccYXZ( EulerAngYXZ(1,frm),EulerVelYXZ(1,frm),
     &                          SacGCS(4,frm,seg),EulerAccYXZ(1,frm) )
            do j= 1, 3
              yt(j)= EulerVelYXZ(j,frm)
              yt(j+3)= EulerAngYXZ(j,frm)
              Accel(j)= EulerAccYXZ(j,frm)
            end do         
            t1= float(frm-1)*TimeInc           
            t2= float(frm)*TimeInc
            call odeinit( yt,nvar,t1,t2,1.0e-5,TimeInc/100.0,
     &                    1.0e-15,nok,nbad,error )
            if ( error ) goto 1050
            do j= 1, 3     
              EulerVelYXZ(j,frm+1)= yt(j)
              EulerAngYXZ(j,frm+1)= yt(j+3)   
            end do              
          end do ! frm
          do frm= 1, MP.nfrm
            do j= 1, 3
              SangGCS(j,frm,seg)= EulerAngYXZ(j,frm)*rtod
            end do ! j
          end do ! frm
        end do ! seg                                             
        return                 
      end if          
c                                                              
c     Initialize error handling before starting calculations
c
      error= .false.
      SF.angle= .false.
      if ( .not. SF.position ) goto 1000
      InterpFlag= .false.
      JntInterp= .false.
      do i= 1, MAX_SEG                                              
        do j= 1, 3
          NumError(j,i)= 0                                                 
        end do ! j
      end do ! i   
      do frm= 1, MP.nfrm
        if ( OF.UpDate .and. (mod(frm,10) .eq. 0) ) then
          if ( SkipLine ) write(6,90)
90        format(' ')                     
          write(6,101) frm,MP.nfrm
101       format('+Processing frame # ',i3,'/',i3)            
          SkipLine= .false.
        end if
        do i= 1, GP.num_seg
          seg= GP.seg_num(i)
          if ( GP.analysis_type .eq. UPR_ANL ) then
            do j= 1, 3
              do k= 1, 2
                TempRotMat(j,k)= -SgpGCS(j,k,frm,seg)
              end do
              TempRotMat(j,3)= SgpGCS(j,3,frm,seg)
            end do
            call RotMattoJCSAng( TempRotMat,
     &                           SangGCS(x,frm,seg),error )
          else    
            call RotMattoJCSAng( SgpGCS(x,x,frm,seg),
     &                           SangGCS(x,frm,seg),error )
          end if
          if ( error ) then ! error
            InterpFlag= .true.
            SkipLine=   .true.
            call HandleAngleError( error,frm,seg,NumError(1,seg),
     &                             ErrFrm(1,1,seg),JntInterp,OF )
          end if                        
        end do ! i => seg
      end do ! frm         
c
c     If any angles are missing due to errors in Euler angle calculation,
c     interpolate the the missing data.
c
      SF.angle= .true.
      if ( InterpFlag ) then
        call InterpAngles( NumError,ErrFrm,SangGCS,JaxesJCS,
     &                     JntInterp,OF,InterpError )
        if ( InterpError ) SF.angle= .false.
      end if
      return

1000  write(6,1005)
      if ( OF.ToLog ) write(3,1005)
1005  format(' Error- Segment position has not been calculated yet.')
      return

1010  write(6,1015) seg_names(seg),frm
      if ( OF.ToLog ) write(3,1015) seg_names(seg),frm
1015  format(' Error- An error has occured while calculating the euler angles between'/
     &       '        the lab coord system and the segment: ',a4,' at frame # ',i3,'.')
      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 option for SEG ANG'/
     &       '        The only valid option is /INTegrate')
      return

1030  write(6,1035)                                       
      if ( OF.ToLog ) write(3,1035)
1035  format(' Error- Segment angle has not been calculated yet')
      return

1040  write(6,1045)
      if ( OF.ToLog ) write(3,1045)
1045  format(' Error- Segment acceleration has not been calculated yet')
      return

1050  write(6,1055) seg_names(seg),frm
      if ( OF.ToLog ) write(3,1055) seg_names(seg),frm
1055  format(' Error- An error has occured while integrateing the segment angles'/
     &       '        for segment ',a4,' at frame # ',i3)
      SF.angle= .false.
      return
      end
c                                        
c	DigFilterSeg
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Provide digitial filtering of segment quantities if desired.
c				This is an optional facilty provided primarily for 
c				'cleaning up' the appearance of data being used for
c				presentation.  I don't recommend its use with clinical
c				data.  This is controlling routine which builds the 
c				frequency domain filter function and then drives the
c				actual fft and ifft routines used for the filter.  Right
c				now only segment position and velocity/acceleration
c				filtering is supported...
c
c	Routines called:
c	BuildFilter
c	DigFilterDataSet
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Param			GP.num_seg
c	Param			GP.seg_num
c	Param			MP.nfrm
c	Param			rMtnFrmRate
c	Names			seg_names
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Segment			SacGCS
c	Segment			SacLCS
c	Segment			SgpGCS
c	Segment			SvlGCS
c	Segment			SvlLCS
c
      subroutine DigFilterSeg( CMND,SF,OF )
      implicit none
                           
      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_prm.inc'
      include 'a_names.inc'
      include 'a_seg.inc'

      record /CmndLine/     CMND
      record /segment_flag/ SF
      record /option_flag/  OF
c
c     Zero Pad data so that the number of data points is a power of 2, apply
c     a window function to the data since it is assumed to be nonperiodic,
c     take the fourier transform of the result then multiply it with the filter
c     function, and take the inverse transform.
c                                                             
      real*4    smplfreq,Filter(256),TempData(MAX_MTN),NoSign(3)
      integer*4 SegVarNum,i,j,n,NumFilPnts,NumBeginZero,NumEndZero,seg,
     &          axis1,axis2
      logical*4 SegVarFound,DoWindow,Error
      character SegVarName(2)*3,AxisName(4)*4,VaaName(6)*4,AngleName(3)*4
                               
      data NoSign / 1.0,1.0,1.0 /
      data SegVarName / 'POS','VAA' /
      data AxisName / 'x   ','y   ','z   ','cntr' /
      data AngleName / 'flex','abad','inex' /                   
      data VaaName  / 'x   ','y   ','z   ','rx  ','ry  ','rz  ' /
c
c     check whether a proper segment variable has been specified to be filtered
c
      SegVarFound= .false.
      i= 1
      Do while ( (i .le. 2) .and. (.not. SegVarFound) )
        if ( CMND.CmndText(3) .eq. SegVarName(i) ) then
          SegVarFound= .true.
          SegVarNum= i
        end if                                          
        i= i + 1
      end do
      if ( .not. SegVarFound ) goto 1000 ! error
      if ( (SegVarNum .eq. 1) .and. (.not. SF.position) )   goto 1010 ! error
      if ( (SegVarNum .eq. 1) .and. (.not. SF.angle) )   goto 1060 ! error 
      if ( (SegVarNum .eq. 2) .and. (.not. SF.vel_acc) ) goto 1020 ! error
      if ( rMtnFrmRate .lt. 1.0e-5 ) goto 1030 ! error  
      if ( MP.nfrm .lt. 2 )       goto 1040 ! error
c
c     Figure out how many zeros must be added to data.  Zeros are added
c     symmetrically to the beginning and end of the array.  This should
c     take care of most of the end effects resulting from fourier transform
c     process.             
c                               
      n= MP.nfrm                                
      i= 1    
      do while( (n - 2**i) .gt. 0 )
        i= i + 1
      end do         
      i= i + 1  ! increase order of zero padding 10/12/90
      if ( i .gt. 9 ) goto 1050 ! error- too many samples, must be <=512
      NumFilPnts= 2**i                                                
      NumBeginZero= (NumFilPnts-n)/2
      NumEndZero=   (NumFilPnts-n) - NumBeginZero                          
c                              
c     Build filter function                             
c                                                       
      smplfreq= rMtnFrmRate
      call BuildFilter( CMND,2,NumFilPnts,smplfreq,
     &                  Filter,DoWindow,Error,OF )
      if ( Error ) return ! error
c
c     Do filtering of segment variables 
c     
      if ( SegVarNum .eq. 1 ) then ! filter segment position/orientation
        do i= 1, GP.num_seg
          seg= GP.seg_num(i)
          do axis1= x, z
            if ( OF.UpDate ) 
     &        write(6,100) seg,GP.num_seg,Seg_Names(seg),AxisName(axis1)
100         format('+ Filtering segment # ',i2,'/',i2,' ',a4,' ',a4)
            do j= 1, MP.nfrm
              TempData(j)= SgpGCS(axis1,cntr,j,seg)
            end do
            call DigFilterDataSet( TempData,n,
     &                             NumFilPnts,NumBeginZero,NumEndZero,
     &                             Filter,DoWindow )
            do j= 1, MP.nfrm
              SgpGCS(axis1,cntr,j,seg)= TempData(j)
            end do
          end do ! axis 1
          do axis1= x, z
            if ( OF.UpDate ) 
     &        write(6,100) seg,GP.num_seg,Seg_Names(seg),AngleName(axis1)
            do j= 1, MP.nfrm
              TempData(j)= SangGCS(axis1,j,seg)
            end do
            call DigFilterDataSet( TempData,n,
     &                             NumFilPnts,NumBeginZero,NumEndZero,
     &                             Filter,DoWindow )
            do j= 1, MP.nfrm
              SangGCS(axis1,j,seg)= TempData(j)
            end do
          end do ! axis 1
          do j= 1, MP.nfrm
            call JCSAngtoRotMat( SangGCS(1,j,seg),NoSign,
     &                           SgpGCS(1,1,j,seg)         )
          end do
        end do ! i => seg   
                                             
      else ! do segment vel/accel
        do i= 1, GP.num_seg
          seg= GP.seg_num(i)
          do axis1= x, rz
            if ( OF.UpDate ) 
     &        write(6,110) seg,GP.num_seg,Seg_Names(seg),VaaName(axis1)
110         format('+ Filtering segment vel/acc # ',i2,'/',i2,' ',a4,' ',a4)
            do j= 1, MP.nfrm
              TempData(j)= SvlGCS(axis1,j,seg)
            end do
            call DigFilterDataSet( TempData,n,
     &                             NumFilPnts,NumBeginZero,NumEndZero,
     &                             Filter,DoWindow )
            do j= 1, MP.nfrm
              SvlGCS(axis1,j,seg)= TempData(j)
            end do
            do j= 1, MP.nfrm
              TempData(j)= SacGCS(axis1,j,seg)
            end do
            call DigFilterDataSet( TempData,n,
     &                             NumFilPnts,NumBeginZero,NumEndZero,
     &                             Filter,DoWindow )
            do j= 1, MP.nfrm
              SacGCS(axis1,j,seg)= TempData(j)
            end do
            do j= 1, MP.nfrm
              TempData(j)= SvlLCS(axis1,j,seg)
            end do
            call DigFilterDataSet( TempData,n,
     &                             NumFilPnts,NumBeginZero,NumEndZero,
     &                             Filter,DoWindow )
            do j= 1, MP.nfrm
              SvlLCS(axis1,j,seg)= TempData(j)
            end do
            do j= 1, MP.nfrm
              TempData(j)= SacLCS(axis1,j,seg)
            end do
            call DigFilterDataSet( TempData,n,
     &                             NumFilPnts,NumBeginZero,NumEndZero,
     &                             Filter,DoWindow )
            do j= 1, MP.nfrm
              SacLCS(axis1,j,seg)= TempData(j)
            end do          
          end do ! axis 1
        end do ! i => seg 
      end if
      if ( SF.NumProcOps .le. 4 ) then
        SF.NumProcOps= SF.NumProcOps + 1
        SF.ProcOp(SF.NumProcOps)= CMND.Text(1)(1:40)
      end if
      return
                         
1000  write(6,1005) CMND.CmndText(3)
      if ( OF.ToLog ) write(3,1005) CMND.CmndText(3)
1005  format(' Error- ',a3,' is not valid segment variable that can be filtered'/
     &       '        Valid variables are: POS VAA')
      return
      
1010  write(6,1015) 
      if ( OF.ToLog ) write(3,1015) 
1015  format(' Error- Segment positions cannot be filtered because they have not'/
     &       '        been calculated yet.')
      return
                           
1020  write(6,1025) 
      if ( OF.ToLog ) write(3,1025) 
1025  format(' Error- Segment vel/accel cannot be filtered because they have not'/
     &       '        been calculated yet.')
      return        
      
1030  write(6,1035) rMtnFrmRate
      if ( OF.ToLog ) write(3,1035) rMtnFrmRate
1035  format(' Error- Sample rate is ',f10.6,' Hz which is too small.'/
     &       '        Something has gone wrong in reading the data file')
      return                
      
1040  write(6,1045) MP.nfrm
      if ( OF.ToLog ) write(3,1045) MP.nfrm
1045  format(' Error- There are only ',i4,' samples of data for each segment'/
     &       '        There must be at least 2 samples to perform filtering')
      return                             
                            
1050  write(6,1055) MP.nfrm
      if ( OF.ToLog ) write(3,1055) MP.nfrm
1055  format(' Error- There are ',i6,' samples per segment, but at most'/
     &       '        512 samples/segment can be filtered')
      return

1060  write(6,1065) 
      if ( OF.ToLog ) write(3,1065) 
1065  format(' Error- Segment positions cannot be filtered because segment angles'/
     &       '        have not been calculated yet.')
      return
      end      
c
c	Derivs
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Assign the derivatives to be integrated by the runga-kutta routine
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 Derivs( t,y,dy )
      implicit none

      real*4    Accel(3)
      common / Acceleration / Accel

      real*4    t,y(1),dy(1)
                 
      integer*4 i

      do i= 1, 3
        dy(i)=   Accel(i) ! y(1-3)= angular velocity
        dy(i+3)= y(i)     ! y(4-6)= rotation angle
      end do
      return
      end
c
c	OdeInit
c
c   Purpose:
c     Runge-Kutta driver with adaptive step size control.
c     Integrate nvar equations from x1 to x2 with starting values ystart
c     with accuracy eps, storing the intermediate results in common block
c     /PATH/ (this is optional- primarily for debugging and monitoring the
c     integration process). 
c
c     NOTE: Routine copied from Numerical Recipes
c           
c     ystart : Starting values of y which is replaced with values at x2
c     nvar   : Number of y's
c     x1     : Starting point of integration
c     x2     : Ending point of integration
c     eps    : Accuracy criteria
c     h1     : Guess of first step size for x
c     hmin   : Minimum allowed step size (can be 0.0)
c     nok    : Number of good steps taken to reach x2
c     nbad   : Number of bad steps taken to reach x2
c     derivs : Routine which calculates the derivatives of y at a given x
c     rkqc   : Stepper routine used 
c     error  : Error flag
c
c	Routines called:
c
c	derivs
c  rkqc
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 Odeinit( ystart,nvar,x1,x2,eps,h1,hmin,nok,nbad,error )
      implicit none

      integer*4 MAXSTP,NMAX
      real*4    TWO,ZERO,TINY

      parameter (MAXSTP=10000)
      parameter (NMAX=25)
      parameter (TWO=2.0) 
      parameter (ZERO=0.0)
      parameter (TINY=1.0e-30)

      ! this for tracking intermediate results, it's optional
      integer*4 kmax,kount
      real*4    dxsav,xp(200),yp(10,200)
      common /PATH/ kmax,kount,dxsav,xp,yp 
                   
      integer*4 nvar,nok,nbad,i,nstp
      real*4    ystart(nvar),x1,x2,eps,h1,hmin,yscal(NMAX),y(NMAX),dydx(NMAX),
     &          x,h,xsav,hdid,hnext
      logical*4 error

      x= x1
      h= sign(h1,x2-x1)
      nok= 0
      nbad= 0
      kount= 0
      do i= 1, nvar
        y(i)= ystart(i)
      end do                         
      if ( kmax .gt. 0 ) xsav= x - dxsav*TWO
      do nstp= 1, MAXSTP
        call derivs( x,y,dydx )
        do i= 1, nvar                                  ! setup scaling criteria
          yscal(i)= abs(y(i)) + abs(h*dydx(i)) + TINY
        end do 
        if ( (kmax .gt. 0) .and.     ! this for tracking intermediate results
     &       (abs(x-xsav) .gt. abs(dxsav)) .and.
     &       (kount .lt. kmax-1)                 ) then
          kount= kount + 1
          xp(kount)= x
          do i= 1, nvar
            yp(i,kount)= y(i)
          end do
          xsav= x
        end if
        if ( (x+h-x2)*(x+h-x1) .gt. ZERO ) h= x2 - x ! make h does not overshoot 
                                                     ! the interval x1 to x2
        call rkqc( y,dydx,nvar,x,h,eps,yscal,hdid,hnext )
        if ( hdid .eq. h ) then
          nok= nok + 1
        else
          nbad= nbad + 1
        end if
        if ( (x-x2)*(x2-x1) .ge. ZERO ) then  ! Done
          do i= 1, nvar
            ystart(i)= y(i)
          end do
          if ( kmax .ne. 0 ) then ! update tracking info...
            kount= kount + 1   
            xp(kount)= x
            do i= 1, nvar  
              yp(i,kount)= y(i)
            end do                                           
          end if
          error= .false.
          return      ! sucessfully completed integration
        end if
        if ( abs(hnext) .lt. hmin ) write(6,100) hnext,hmin,x
100     format(' Warning- Step size (',e12.5,') is less than hmin (',e12.5,')'/
     &         '          at x= ',e12.5)
        h= hnext
      end do                 
      error= .true.
      write(6,110) MAXSTP,x
110   format(' Error- The maximum number of steps (',i5,') has been exceeded'/
     &       '        without finishing the integration at x= ',e12.5)
      return
      end
c           
c     rkqc                              
c
c     Fifth-order Runge-Kutta step with monitoring of local truncation 
c     error to ensure accuracy and adjust step size.
c     This routine will integrate the set of equations defined by derivs 
c     starting at y with derivatives dydx for a single step in which the
c     desired accuracy of the solution will better than or equal to eps.
c     To be useful, this routine must be driven by another routine which
c     continues the calls to this routine until the target step size has
c     been reached.  The routine odeinit does just this.
c
c     NOTE: Routine copied from Numerical Recipes
c
c     y      : Starting values of y's
c     dydx   : Derivative of y's at x
c     n      : Number of equations
c     x      : Independent variable at which above y's are known
c     htry   : Suggested step size of x
c     eps    : Required accuracy as fraction of yscal's for each y
c     yscal  : Scaling for error criteria to allow y's of different magnitudes
c     hdid   : Actually step size of x which allowed eps to be satisfied
c     hnext  : Suggested stepsize of x for next integration step
c     derivs : Routine with equations defining derivatives of y
c
c	Routines called:
c
c	derivs
c  rk4
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 rkqc( y,dydx,n,x,htry,eps,yscal,hdid,hnext )
      implicit none

      integer*4 NMAX
      real*4    PGROW,PSHRINK,FCOR,ONE,SAFETY,ERRCON

      parameter (NMAX=25)
      parameter (PGROW=-0.20)
      parameter (PSHRINK=-0.25)
      parameter (FCOR=1.0/15.0)
      parameter (ONE=1.0)
      parameter (SAFETY=0.9)
      parameter (ERRCON=6.0e-4)
	  
c      external  derivs
      integer*4 n,i
      real*4    y(n),dydx(n),x,htry,eps,yscal(n),hdid,hnext,ytemp(NMAX),
     &          ysav(NMAX),dysav(NMAX),xsav,h,errmax,hh

      xsav= x
      do i= 1, n
        ysav(i)=  y(i)
        dysav(i)= dydx(i)
      end do
      h= htry
      errmax= 2.0
      do while ( errmax .gt. ONE )
        hh= 0.5*h
        call rk4( ysav,dysav,n,xsav,hh,ytemp ) ! first 1/2 step
        x= xsav + hh
        call derivs( x,ytemp,dydx )
        call rk4(  ytemp,dydx,n,x,hh,y )       ! second 1/2 step
        x= xsav + h
        if ( x .eq. xsav ) write(6,100) h,x
100     format(' Warning- Step size (',e12.5,') not significant in RKQC'/
     &         '          at x= ',e12.5)
        call rk4( ysav,dysav,n,xsav,h,ytemp )  ! full step
        errmax= 0.0
        do i= 1, n
          ytemp(i)= y(i) - ytemp(i)  ! ytemp is now the error estimate
          if ( abs(ytemp(i)/yscal(i)) .gt. errmax ) 
     &      errmax= abs(ytemp(i)/yscal(i))
        end do
        errmax= errmax/eps
        if ( errmax .gt. ONE ) h= SAFETY*h*(errmax**PSHRINK)
      end do
      hdid= h                                          
      if ( errmax .gt. ERRCON ) then
        hnext= SAFETY*h*(errmax**PGROW)
      else
        hnext= 4.0*h
      end if
      do i= 1, n
        y(i)= y(i) + ytemp(i)*FCOR
      end do
      return
      end
c
c     rk4
c
c     This routine is the classic, simple 4th order runge-kutta routine.
c     Given the values of n variables y with derivatives dydx at x, calculate
c     the values of y at x+h
c
c     NOTE: Routine copied from Numerical Recipes
c           
c     y      : Values of y at x
c     dydx   : Value of derivatives of y at x
c     n      : Number of equations (i.e., y's)
c     x      : Starting point independent variable value
c     h      : Step size of x
c     yout   : Values of y at x+h 
c     derivs : Routine which returns the derivatives of y's at a known x
c
c
c	Routines called:
c
c	derivs
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 rk4( y,dydx,n,x,h,yout )
      implicit none

      integer*4 NMAX
      parameter (NMAX=25)
	  
      integer*4 n,i
      real*4    y(n),dydx(n),yout(n),yt(NMAX),dyt(NMAX),dym(NMAX),x,h,hh,h6,
     &          xh                                             
 
      hh= 0.5*h
      h6= h/6.0
      xh= x + hh
      do i= 1, n                  ! First step
        yt(i)= y(i) + hh*dydx(i)
      end do                     
      call derivs( xh,yt,dyt )    ! Second step
      do i= 1, n
        yt(i)= y(i) + hh*dyt(i)
      end do
      call derivs( xh,yt,dym )    ! Third step
      do i= 1, n
        yt(i)=  y(i) + h*dym(i)
        dym(i)= dyt(i) + dym(i)
      end do
      call derivs( x+h,yt,dyt )   ! Fourth step
      do i= 1, n
        yout(i)= y(i) + h6*(dydx(i) + dyt(i) + 2.0*dym(i))
      end do
      return
      end       
