c                                        
c	do_JNT_calc  
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Controlling routine that calls the joint kinematic 
c				and kinetic calculation routines
c
c	Routines called:
c		CalcJCSAngles 
c		CalcJCSVelAcc
c		CalcEULAngles
c		CalcEULVelAcc
c		CalcJntLoad
c		CalcJntWrench
c		CalcJntPower
c		CalcJntTranslation
c		CalcJntScrewAngle
c		CalcJntMomentArm
c		CalcJntWrenchArm
c		FilterJnt
c     CalcJexment
c     CalcJntWork
c     CalcJrlAng
c     CalcDynamJcn
c     CalcAvgJcn
c
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Param			GP.analysis_type
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	None
c
      subroutine do_JNT_calc( CMND,SF,FF,JF,OF )
      implicit none

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

      record /CmndLine/      CMND
      record /segment_flag/  SF
      record /force_flag/    FF 
      record /joint_flag/    JF
      record /option_flag/   OF

      if ( CMND.CmndText(2) .eq. 'JAN' ) then
        call CalcJCSAngles( CMND,SF,JF,OF )

      else if ( CMND.CmndText(2) .eq. 'JVA' ) then
        call CalcJCSVelAcc( CMND,SF,JF,OF )
c
c      else if ( CMND.CmndText(2) .eq. 'EAN' ) then
c        call CalcEULAngles( CMND,SF,JF,OF )
c
c      else if ( CMND.CmndText(2) .eq. 'EVA' ) then
c        call CalcEULVelAcc( CMND,SF,JF,OF )
c                                           
      else if ( CMND.CmndText(2) .eq. 'LOA' ) then
        if ( (GP.analysis_type .eq. FULL_ANL) .or.
     &       (GP.analysis_type .eq. CC_ANL)   .or.
     &       (GP.analysis_type .eq. MAYO_ANL) .or.
     &       (GP.analysis_type .eq. HH_ANL)        ) then
          call CalcJntLoad( CMND,SF,FF,JF,OF )
        else if ( GP.analysis_type .eq. CKC_ANL ) then
          call CalcCKCJntLoad( CMND,SF,FF,JF,OF )
        else
          write(6,110) ANLNames(GP.analysis_type)
          if ( OF.ToLog ) write(3,110) ANLNames(GP.analysis_type)
110       format(' Error- This calculation is not supported yet for ',
     &          a20,' analysis')
        end if

      else if ( CMND.CmndText(2) .eq. 'WRE' ) then
        if ( (GP.analysis_type .eq. FULL_ANL) .or.
     &       (GP.analysis_type .eq. HH_ANL)   .or.
     &       (GP.analysis_type .eq. CC_ANL)   .or.
     &       (GP.analysis_type .eq. MAYO_ANL) .or.
     &       (GP.analysis_type .eq. CKC_ANL)      ) then
          call CalcJntWrench( JF,SF,OF )
        else
          write(6,110) ANLNames(GP.analysis_type)
          if ( OF.ToLog ) write(3,110) ANLNames(GP.analysis_type)
        end if

      else if ( CMND.CmndText(2) .eq. 'POW' ) then
        if ( (GP.analysis_type .eq. FULL_ANL) .or.
     &       (GP.analysis_type .eq. HH_ANL)   .or.
     &       (GP.analysis_type .eq. CC_ANL)   .or.
     &       (GP.analysis_type .eq. MAYO_ANL) .or.
     &       (GP.analysis_type .eq. CKC_ANL)      ) then
          call CalcJntPower( CMND,SF,JF,OF )         
        else
          write(6,110) ANLNames(GP.analysis_type)
          if ( OF.ToLog ) write(3,110) ANLNames(GP.analysis_type)
        end if

      else if ( CMND.CmndText(2) .eq. 'TRA' ) then
        call CalcJntTranslation( SF,JF,OF )
     
      else if ( CMND.CmndText(2) .eq. 'SAN' ) then
        call CalcJntScrewAngle( JF,OF )

      else if ( CMND.CmndText(2) .eq. 'MAR' ) then
        call CalcJntMomentArm( JF,OF )

      else if ( CMND.CmndText(2) .eq. 'WAR' ) then
        call CalcJntWrenchArm( JF,OF )

      else if ( CMND.CmndText(2) .eq. 'EXM' ) then
        call CalcJexment( JF,FF,OF )

      else if ( CMND.CmndText(2) .eq. 'WRK' ) then
        call CalcJntWork( JF,OF )

      else if ( CMND.CmndText(2) .eq. 'LAN' ) then
        call CalcJrlAng( JF,OF )

      else if ( CMND.CmndText(2) .eq. 'CEN' ) then
        call CalcDynamJcn( CMND,JF,OF )

      else if ( CMND.CmndText(2) .eq. 'ACN' ) then
        call CalcAvgJcn( CMND,JF,OF )

      else if ( CMND.CmndText(2) .eq. 'FIL' ) then
        call FilterJnt( CMND,JF,OF )               
                                   
      else
        write(6,100) CMND.CmndText(2)
        if ( OF.ToLog ) write(3,100) CMND.CmndText(2)
100     format(' Error- ',a3,' is not a valid JNT command.'/
     &         '         Valid commands are: JcsANgle JcsVelAcc LOAd   POWer'/
c     &         29x,'EulANgle EulVelAcc WREnch FILter'/
     &         29x,'WREnch             FILter'/
     &         29x,'TRAnslation        ScrewANgle'/
     &         29x,'MomentARm          WrenchARm'/
     &         29x,'EXternalMoment     WoRK'/
     &         29x,'Load ANgle         CENter'/
     &         29x,'AvgCeNter')
      end if
      return
      end
c                                        
c	CalcJCSAngles  
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Controlling routine to call either the simple calculation
c				of JCS joint angles or the more complex method which
c				uses a reference position (angle) to try to correct for
c				marker placement on the surface of the body segments as
c				opposed to directly to the underlying bone which normally
c				cannot be observed.       
c
c	Routines called:
c	CalcJCSAnglesRef
c	CalcJCSAnglesSimple
c	RemoveCMNDOpt
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 CalcJCSAngles( CMND,SF,JF,OF )
      implicit none

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

      record /CmndLine/     CMND
      record /segment_flag/ SF
      record /joint_flag/   JF
      record /option_flag/  OF   
      
      integer*4 i
      logical*4 UseRefAngle,IntegrateAngle 
c
c     check for option to call reference angle calculation technique
c
      IntegrateAngle= .false.
      UseRefAngle= .false.
      if ( CMND.NumOpt(2) .ne. 0 ) then
        do i= 1, CMND.NumOpt(2)
          if ( CMND.OptText(i,2) .eq. 'REF' ) then
            UseRefAngle= .true.
            call RemoveCMNDOpt( CMND,2,i )
          else if ( CMND.OptText(i,2) .eq. 'INT' ) then
            IntegrateAngle= .true.
            call RemoveCMNDOpt( CMND,2,i )
          end if
        end do
      end if
      if ( UseRefAngle ) then
        call CalcJCSAnglesRef( CMND,SF,JF,OF )
      else
        if ( IntegrateAngle ) then
          call IntegrateJCSAngles( JF,OF )
        else
          call CalcJCSAnglesSimple( CMND,SF,JF,OF )
        end if
      end if
      return
      end
c
c   IntegrateJCSAngles
c
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:   Calculate the JCS joint angles by integrating the JCS joint angle
c            velocities.  This provides a way of checking the angular velocity
c            calculation equations since the result of this operation should be
c            virtually identical to the original JCS angles calculated directly
c            from the segment orientations.
c
c	Routines called:
c
c   IntegrateData
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Param			GP.num_jnt
c	Param			GP.jnt_num
c	Param			MP.nfrm
c	Segment			SgpGCS
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Joint			JangJCS
c
      subroutine IntegrateJCSAngles( JF,OF )
      implicit none

      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_prm.inc'
      include 'a_jnt.inc'
      include 'a_names.inc'
                 
      record /joint_flag/  JF
      record /option_flag/ OF
     
      integer*4 i,jnt,frm,axis,NumFrm
      real*4    AngVel(MAX_MTN),IntAng(MAX_MTN),TimeInc

      if ( .not. JF.angle_JCS ) goto 1000
      if ( .not. JF.vel_acc_JCS ) goto 1010
      NumFrm= MP.nfrm
      TimeInc= 1.0/float(MP.frm_rate)
      do i= 1, GP.num_jnt
        jnt= GP.jnt_num(i)
        do axis= flex, inex
          do frm= 1, NumFrm
            AngVel(frm)= JvelJCS(axis,frm,jnt)
          end do
          call IntegrateData( AngVel,NumFrm,TimeInc,IntAng )
          do frm= 2, NumFrm
            JangJCS(axis,frm,jnt)= JangJCS(axis,1,jnt) + IntAng(frm)
          end do
        end do ! axis                                  
      end do ! i
      return

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

1010  write(6,1015)
      if ( OF.ToLog ) write(3,1015)
1015  format(' Error- JCS joint velocity has not been calculated yet')
      return
      end 
c                                        
c	CalcJCSAnglesSImple  
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Calculate the YXZ FLOATING axis euler angles describing
c				the orientation of the distal segment relative to the
c				proximal segment across a joint.  These angles are
c				refered to here as the joint coordinate system (JCS)
c				angles since they are defined relative to lines which
c				orthopedists would commonly consider reference directions
c				for describing joint motion.
c
c	Routines called:
c
c	HandleAngleError
c	InterpAngles
c	JCSAngles
c	ShldrAngles
c	VecNorm
c	VecX
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Joint			AngleSignJCS
c	Param			GP.analysis_type
c	Param			GP.num_jnt
c	Param			GP.jnt_num
c	Joint			JntSegNum
c	Param			MP.nfrm
c	Segment			SgpGCS
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Joint			JangJCS
c	Joint			JaxesJCS
c
      subroutine CalcJCSAnglesSimple( CMND,SF,JF,OF )
      implicit none

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

      record /CmndLine/     CMND
      record /segment_flag/ SF
      record /joint_flag/   JF
      record /option_flag/  OF      
                                                                           
      integer*4 frm,jnt,axis1,num_left_jnts,left_jnt_num(5),i,j,error,
     &          prox_seg,dist_seg,VecNorm,NumError(3,MAX_JNT)
      real*4    lab_axes(3,3),vecA(3),AnkleFlexion,temp_axes(3,3)
      logical*4 left_jnt,InterpFlag,InterpError,SkipLine,CorrectAnkleFlexion,
     &          UseShldrPolar,JntInterp,WalkInvDir
                           
      data lab_axes / 1.,0.,0.,0.,1.,0.,0.,0.,1. /
      data num_left_jnts / 5 /
      data left_jnt_num / 4,5,6,11,12 /
                                         
      error= 0
      JF.angle_JCS= .false.         
      if ( .not. SF.position ) goto 1000     
c
c     Initialize error handling before starting calculations
c
      JntInterp= .true.
      InterpFlag= .false.
      do i= 1, MAX_JNT                                              
        do j= 1, 3
          NumError(j,i)= 0                                                 
        end do ! j
      end do ! i
c                                                 
c     Check for angle options
c
      UseShldrPolar= .false.
      CorrectAnkleFlexion= .false.
      AnkleFlexion= -6.0
      WalkInvDir= .false.
      if ( CMND.NumOpt(2) .gt. 0 ) then
        do i= 1, CMND.NumOpt(2)
          if ( CMND.OptText(i,2) .eq. 'NAF' ) then
            CorrectAnkleFlexion= .false.
          else if ( CMND.OptText(i,2) .eq. 'AFL' ) then
            if ( CMND.OptNumOK(i,2) ) then
              if ( abs(CMND.OptIntVal(i,2)) .gt. 30.0 ) goto 1030
              AnkleFlexion= CMND.OptIntVal(i,2)
            end if
            CorrectAnkleFlexion= .true.
          else if ( CMND.OptText(i,2) .eq. 'SHP' ) then
            UseShldrPolar= .true.
          else if ( CMND.OptText(i,2) .eq. 'INV' ) then
            WalkInvDir= .true.
          else        
            goto 1040 ! error
          end if                  
        end do
      end if
      if ( WalkInvDir ) then
        lab_axes(x,x)= -1.0
        lab_axes(y,y)= -1.0
      end if
c                                        
c     Do angle calculations
c                     
      SkipLine= .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,101) frm,MP.nfrm
101       format('+Processing frame # ',i3,'/',i3)
          SkipLine= .false.
        end if        
        do i= 1, GP.num_jnt
          jnt= GP.jnt_num(i)        
c                           
c         The pelvis is a special case because its angle is measured relative
c         to the lab coordinate system, whereas all other angles are measured
c         relative to the more proximal segment.
c                                                                    
          if ( jnt .eq. pvlb .and. ! special case for pelvis  
     &         ( (GP.analysis_type .eq. FULL_ANL) .or.
     &           (GP.analysis_type .eq. HH_ANL)   .or.
     &           (GP.analysis_type .eq. CC_ANL)   .or.
     &           (GP.analysis_type .eq. CKC_ANL)  .or.
     &           (GP.analysis_type .eq. MAYO_ANL)      ) ) then 
            if ( GP.analysis_type .eq. UPR_ANL )  lab_axes(z,z)= -1.0
            call JCSAngles( SgpGCS(x,x,frm,JntSegNum(1,jnt)),
     &                      lab_axes,JangJCS(flex,frm,jnt),error )
            if ( error .ne. 0 ) then ! error 
              InterpFlag= .true.
              SkipLine=   .true.                               
              call HandleAngleError( error,frm,jnt,NumError(1,jnt),
     &                               ErrFrm(1,1,jnt),JntInterp,OF  )
            else       
              do axis1= flex, inex
                JangJCS(axis1,frm,jnt)= AngleSignJCS(axis1,jnt)*
     &                                        JangJCS(axis1,frm,jnt)
              end do
              do axis1= x, z               
                JaxesJCS(axis1,flex,frm,jnt)= lab_axes(axis1,y)       
                JaxesJCS(axis1,inex,frm,jnt)= SgpGCS(axis1,z,frm,jnt)
              end do                                
              call VecX( lab_axes(x,y),SgpGCS(x,z,frm,jnt),vecA )
              error= VecNorm( vecA,JaxesJCS(x,abad,frm,jnt) )      
              if ( error .ne. 0 ) goto 1010
           end if ! error in calculating angle
           if ( GP.analysis_type .eq. UPR_ANL )  lab_axes(z,z)= 1.0
         else
c                                        
c           Check the joint to segment connectivity array, JntSegNum,
c           to see which segments are to calculate which joint angles. 
c           This array is defined in anz_joint.inc and is filled in the beginning
c           of the main ANALYZE program.
c
            dist_seg= JntSegNum(1,jnt)                       
            prox_seg= JntSegNum(2,jnt)         
            if ( (GP.analysis_type .eq. UPR_ANL) .and. 
     &           ( (jnt .eq. rtsh) .or. (jnt .eq. lfsh) ) .and.
     &           ( UseShldrPolar )    ) then
              call ShldrAngles( SgpGCS(x,x,frm,dist_seg),
     &                          SgpGCS(x,x,frm,prox_seg),
     &                          JangJCS(flex,frm,jnt),error )
            else
              call JcsAngles( SgpGCS(x,x,frm,dist_seg),
     &                        SgpGCS(x,x,frm,prox_seg),     
     &                        JangJCS(flex,frm,jnt),error )
            end if
            if ( error .ne. 0 ) then ! error
              InterpFlag= .true.
              SkipLine=   .true.
              call HandleAngleError( error,frm,jnt,NumError(1,jnt),
     &                               ErrFrm(1,1,jnt),JntInterp,OF  )
            else                
              do axis1= x, z
                JangJCS(axis1,frm,jnt)= AngleSignJCS(axis1,jnt)*
     &                                        JangJCS(axis1,frm,jnt)
                JaxesJCS(axis1,flex,frm,jnt)= SgpGCS(axis1,y,frm,prox_seg)
                JaxesJCS(axis1,inex,frm,jnt)= SgpGCS(axis1,z,frm,dist_seg)
              end do ! axis1               
              call VecX( SgpGCS(x,y,frm,prox_seg),
     &                   SgpGCS(x,z,frm,dist_seg),vecA )         
              error= VecNorm( vecA,JaxesJCS(x,abad,frm,jnt) )
              if ( error .ne. 0 ) goto 1010
            end if
c
c           Subtract 6 deg (default) from ankle flexion (plantarflexion) to
c           make the angle calculated the same as in previous studies
c                     
            if ( ( (jnt .eq. rtak) .or. (jnt .eq. lfak) ) .and.
     &           CorrectAnkleFlexion )
     &        JangJCS(flex,frm,jnt)= JangJCS(flex,frm,jnt) +
     &                                     AnkleFlexion
          end if ! special case of pelvis-lab joint
        end do ! i => jnt                                              
      end do ! frame
c
c     If any angles are missing due to errors in JCSAngles calculation,
c     interpolate the the missing data.
c
      JF.angle_JCS= .true.                         
      if ( InterpFlag ) then
        JntInterp= .true.
        call InterpAngles( NumError,ErrFrm,JangJCS,JaxesJCS,
     &                     JntInterp,OF,InterpError )
        if ( InterpError ) JF.angle_JCS= .false.
      end if
      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) jnt_names(jnt),frm
      if ( OF.ToLog ) write(3,1015) jnt_names(jnt),frm
1015  format(' Error- An error has occured in the calculating the Ab/Ad axis'/
     &       '        for the ',a4,' at frame # ',i3,' ...')
      return
      
1020  write(6,1025) jnt_names(jnt),frm
      if ( OF.ToLog ) write(3,1025) jnt_names(jnt),frm
1025  format(' Error- An error has occured in the calculating the JCS angles'/
     &       '        for the ',a4,' at frame # ',i3,' ...')
      return
                 
1030  write(6,1035)
      if ( OF.ToLog ) write(3,1035)
1035  format(' Error- Incorrect amount of ankle flexion correction'/
     &       '        Correction must be >-30 and <30')
      return

1040  write(6,1045) CMND.OptText(i,2)
      if ( OF.ToLog ) write(3,1045) CMND.OptText(i,2)
1045  format(' Error- ',a3,' is an invalid option for JNT JAN'/
     &       '        Valid options are: SHoulderPolarangle INVersewalkdirection'/
     &       '                           NoAnkleFlexadjust AFL= #')
      return
      end
c                                        
c	HandleAngleError  
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Display an error message if an error has occured while
c				trying to calculate a joint angle.  Also, record this 
c				error so that the angle and/or joint axes can be
c				interpolated in later on.  This capability is quite
c				useful with clinical data which has large gaps in it 
c				and allows marker trajectory that might otherwise be
c				unusable to at least provide some information.  NOTE
c				that the log file from a file which had its angles
c				interpolated should be kept and used to apply a critical
c				evaluation to resulting joint angle vs gait cycle graphs
c				which might interpreted for clinical evaluation.  
c				Basically anything interpolated must be regarded with
c				some suspicion.  Don't base clinical decisions exclusively
c				upon data points which have been interpolated.
c
c	Routines called:
c	None
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Names			jnt_names
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	None
c
      subroutine HandleAngleError( error,frm,jnt,NumError,ErrFrm,
     &                             JntInterp,OF )
      implicit none

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

      record /option_flag/ OF
      integer*4 error,NumError(3),ErrFrm(3,MAX_MTN),frm,jnt,i
      logical*4 JntInterp

      if ( error .eq. 1 ) then ! angle 1 only
        NumError(1)= NumError(1) + 1
        ErrFrm(1,NumError(1))= frm
        if ( JntInterp ) then
          write(6,100) jnt_names(jnt),frm
          if ( OF.ToLog ) write(3,100) jnt_names(jnt),frm
100       format(' Error- flex/ext               ',a4,' frame # ',i3)
        else
          write(6,200) seg_names(jnt),frm
          if ( OF.ToLog ) write(3,200) seg_names(jnt),frm
200       format(' Error- y axis                 ',a4,' frame # ',i3)
        end if
      else if ( error .eq. 2 ) then ! angle 2 only
        NumError(2)= NumError(2) + 1
        ErrFrm(2,NumError(2))= frm
        if ( JntInterp ) then   
          write(6,110) jnt_names(jnt),frm        
          if ( OF.ToLog ) write(3,110) jnt_names(jnt),frm
110       format(' Error-          int/ext       ',a4,' frame # ',i3)
        else
          write(6,210) seg_names(jnt),frm        
          if ( OF.ToLog ) write(3,210) seg_names(jnt),frm
210       format(' Error-          x axis        ',a4,' frame # ',i3)
        end if
      else if ( error .eq. 4 ) then ! angle 2 only
        NumError(3)= NumError(3) + 1              
        ErrFrm(3,NumError(3))= frm
        if ( JntInterp ) then
          write(6,120) jnt_names(jnt),frm
          if ( OF.ToLog ) write(3,120) jnt_names(jnt),frm
120       format(' Error-                  ab/ad ',a4,' frame # ',i3)
        else 
          write(6,220) seg_names(jnt),frm
          if ( OF.ToLog ) write(3,220) seg_names(jnt),frm
220       format(' Error-                 z axis ',a4,' frame # ',i3)
        end if
      else if ( error .eq. 3 ) then ! angle 1 & 2
        NumError(1)= NumError(1) + 1
        NumError(2)= NumError(2) + 1
        ErrFrm(1,NumError(1))= frm          
        ErrFrm(2,NumError(2))= frm
        if ( JntInterp ) then
          write(6,130) jnt_names(jnt),frm
          if ( OF.ToLog ) write(3,130) jnt_names(jnt),frm
130       format(' Error- flex/ext int/ext       ',a4,' frame # ',i3)
        else
          write(6,230) seg_names(jnt),frm
          if ( OF.ToLog ) write(3,230) seg_names(jnt),frm
230       format(' Error- y axis   x axis        ',a4,' frame # ',i3)
        end if
      else if ( error .eq. 5 ) then ! angle 1 & 3
        NumError(1)= NumError(1) + 1
        NumError(3)= NumError(3) + 1     
        ErrFrm(1,NumError(1))= frm
        ErrFrm(3,NumError(3))= frm
        if ( JntInterp ) then
          write(6,140) jnt_names(jnt),frm       
          if ( OF.ToLog ) write(3,140) jnt_names(jnt),frm
140       format(' Error- flex/ext         ab/ad ',a4,' frame # ',i3)
        else
          write(6,240) seg_names(jnt),frm       
          if ( OF.ToLog ) write(3,240) seg_names(jnt),frm
240       format(' Error- y axis          z axis ',a4,' frame # ',i3)
        end if
      else if ( error .eq. 6 ) then ! angle 2 & 3
        NumError(2)= NumError(2) + 1
        NumError(3)= NumError(3) + 1
        ErrFrm(2,NumError(2))= frm          
        ErrFrm(3,NumError(3))= frm
        if ( JntInterp ) then
          write(3,150) jnt_names(jnt),frm
          if ( OF.ToLog ) write(3,150) jnt_names(jnt),frm
150       format(' Error-          int/ext ab/ad ',a4,' frame # ',i3)
        else 
          write(3,250) seg_names(jnt),frm
          if ( OF.ToLog ) write(3,250) seg_names(jnt),frm
250       format(' Error-          y axis z axis ',a4,' frame # ',i3)
        end if
      else if ( error .eq. 7 ) then ! angle 1, 2, & 3
        do i= 1, 3
          NumError(i)= NumError(i) + 1
          ErrFrm(i,NumError(i))= frm
        end do ! i
        if ( JntInterp ) then
          write(6,160) jnt_names(jnt),frm
          if ( OF.ToLog ) write(3,160) jnt_names(jnt),frm
160       format(' Error- flex/ext int/ext ab/ad ',a4,' frame # ',i3)
        else
          write(6,260) seg_names(jnt),frm
          if ( OF.ToLog ) write(3,260) seg_names(jnt),frm
260       format(' Error- y axis  x axis  z axis ',a4,' frame # ',i3)
        endif
      end if
      error= 0
      return
      end
c                                        
c	InterpAngles  
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Interpolate joint angles.  This capability is quite
c				useful with clinical data which has large gaps in it 
c				and allows marker trajectory that might otherwise be
c				unusable to at least provide some information.  NOTE
c				that the log file from a file which had its angles
c				interpolated should be kept and used to apply a critical
c				evaluation to resulting joint angle vs gait cycle graphs
c				which might interpreted for clinical evaluation.  
c				Basically anything interpolated must be regarded with
c				some suspicion.  Don't base clinical decisions exclusively
c				upon data points which have been interpolated.
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.jnt_num
c	Param			GP.num_jnt
c	Names			jnt_names
c	Param			rMtnFrmRate
c	Param			MP.nfrm
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	None
c
      subroutine InterpAngles( NumError,ErrFrm,Angles,JntAxes,
     &                         JntInterp,OF,InterpError )
      implicit none
         
      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_prm.inc'
      include 'a_names.inc'

      integer*4 NumError(3,MAX_JNT),ErrFrm(3,MAX_MTN,MAX_JNT)
      real*4    Angles(3,MAX_MTN,MAX_JNT),JntAxes(3,3,MAX_MTN,MAX_JNT)       
      logical*4 InterpError,JntInterp

      record /option_flag/ OF

      real*4    time(10),AngleData(10),InterpTime,InterpAngle,InterpAxis,
     &          AxisData(10)
      integer*4 i,j,k,axis,frm,jnt,ErrorNum,NumGoodFrm,GoodFrm(MAX_MTN),         
     &          InterpFrm(10),PolyOrder,axis1,axis2
      character axis_names(3)*4,axis_names2(4)*4
      logical*4 AxesDone
   
      data axis_names  / 'flex','inex','abad' /
      data axis_names2 / '   x','   y','   z','cntr' /

      PolyOrder= GP.int_poly_order
      write(6,100)
      if ( OF.ToLog ) write(3,100)
100   format(' Interpolating missing angle data...')        
      do k= 1, GP.num_jnt
        jnt= GP.jnt_num(k)
        AxesDone= .false.
        do axis= flex, inex
          if ( NumError(axis,jnt) .gt. 0 ) then ! interpolate
            ErrorNum=1
            NumGoodFrm= 0
            do frm= 1, MP.nfrm
              if ( (frm .ne. ErrFrm(axis,ErrorNum,jnt)) .or.
     &             (ErrorNum .gt. NumError(axis,jnt))          ) then
                NumGoodFrm= NumGoodFrm + 1
                GoodFrm(NumGoodFrm)= frm
              else                
                ErrorNum= ErrorNum + 1 
              end if
            end do ! frm                
            do i= 1, NumError(axis,jnt)           
              call find_interp_frm( GoodFrm,NumGoodFrm,ErrFrm(axis,i,jnt),
     &                              PolyOrder+1,InterpFrm )
              do j= 1, PolyOrder+1
                time(j)= float(InterpFrm(j))/rMtnFrmRate
                AngleData(j)= Angles(axis,InterpFrm(j),jnt)
              end do ! j
              InterpTime= float(ErrFrm(axis,i,jnt))/rMtnFrmRate
              call lagrange_interp( time,AngleData,PolyOrder+1,
     &                              InterpTime,InterpAngle,InterpError )
              if ( InterpError ) goto 1000   
              Angles(axis,ErrFrm(axis,i,jnt),jnt)= InterpAngle
              if ( (.not. AxesDone) .and. JntInterp ) then ! interpolate JntAxes
                do axis1= x, z
                  do axis2= x, z
                    do j= 1, PolyOrder+1
                      AxisData(j)= JntAxes(axis2,axis1,InterpFrm(j),jnt)
                    end do
                    call lagrange_interp( time,AxisData,PolyOrder+1,
     &                                    InterpTime,InterpAxis,InterpError )
                    if ( InterpError ) goto 1010
                    JntAxes(axis2,axis1,ErrFrm(axis,i,jnt),jnt)= InterpAxis
                  end do ! axis2     
                end do ! axis1
                AxesDone= .true.
              end if ! Angle CS axes have been interpolated
            end do ! i
          end if
        end do ! axis  
      end do ! k => jnt
      write(6,110)
      if ( OF.ToLog ) write(3,110)
110   format('+Interpolating missing angle data...   Done')        
      return
                       
1000  write(6,1005) jnt_names(jnt),axis_names(axis),ErrFrm(axis,i,jnt)
      if ( OF.ToLog ) write(3,1005) jnt_names(jnt),axis_names(axis),
     &                                      ErrFrm(axis,i,jnt)
1005  format(' Error- Interpolating algorithm bombed at ',a4,' ',a4,
     &       ' frame #',i3)
      return
      
1010  write(6,1015) jnt_names(jnt),axis_names2(axis2),axis_names2(axis1),
     &              ErrFrm(axis,i,jnt)
      if ( OF.ToLog ) write(3,1015) jnt_names(jnt),axis_names2(axis2),
     &                                      axis_names2(axis1),
     &                                      ErrFrm(axis,i,jnt)
1015  format(' Error- Interpolating algorithm bombed at ',a4,' ',a4,' ',a4,
     &       ' frame #',i3)
      return
      end
c                                        
c	CalcJCSVelAcc  
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Calculate the joint velocities and accelerations in
c				terms of the JCS.  Two methods are provided: 1) Just
c				numerically differentiate the JCS joint angles (NOTE
c				this definitely wrong from and kinematics point of
c				view has been carried out in other works) 2) Find them
c				using the relative rotational velocities and accelerations
c				between the distal and proximal joint segments and the
c				transformation formulas for euler vel/accel.  This is
c				correct method and the default.  The simple method has
c				been included for comparision purposes.
c
c	Routines called:
c	AngFilterStatus
c	CalcEulAccYXZ
c  CalcEulVelYXZ
c	filter
c	MatTran
c	RemoveCMNDOpt
c	VecChangeCS
c	VecSub
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Joint			AngleSignJCS
c	Param			GP.jnt_num
c	Param			GP.num_jnt
c	Joint			JangJCS
c	Names			jnt_names
c	Joint			JntSegNum
c	Param			rMtnFrmRate
c	Param			MP.nfrm
c	Segment		   SacGCS
c	Segment			SgpGCS
c	Segment			SvlGCS
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Joint			JaccJCS
c	Joint			JangJCS	! in the simple case...
c	Joint			JvelJCS
c
      subroutine CalcJCSVelAcc( 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 i,j,jnt,axis,frm,status,DistSeg,ProxSeg
      real*4    time_inc,temp(MAX_MTN),vel(MAX_MTN),acc(MAX_MTN),VecA(3),VecB(3),rtod,
     &          RotMat(3,3),RotVelLCS(3),RotAccLCS(3),VecDot,temp2(MAX_MTN),
     &          angle(3)
      character axis_names(3)*4,FilterOption(6)*3
      logical*4 Smooth,SimpleMethod,OptionOK

      data axis_names / 'flex','inex','abad' /
      data FilterOption / 'DPF','GCV','FDF','NOI','VAR','NON' /
                      
      JF.vel_acc_JCS= .false.
      if ( .not. JF.angle_JCS ) goto 1000
      rtod= 45.0/atan(1.0)
c                                                           
c     Check for options
c               
      SimpleMethod= .false.         
      Smooth= .false.
      if ( CMND.NumOpt(2) .gt. 0 ) then
        i= 1
        do while ( (i .lt. CMND.NumOpt(2)) .and. 
     &             (CMND.OptText(i,2) .ne. 'SIM') )
          i= i + 1
        end do
        if ( i .le. CMND.NumOpt(2) ) then
          if( CMND.OptText(i,2) .eq. 'SIM' ) then
            SimpleMethod= .true.
            call RemoveCMNDOpt( CMND,2,j )
          end if
        end if         
        Smooth= .false.
        do i= 1, CMND.NumOpt(2)
          OptionOK= .false.
          do j= 1, 6
            if ( CMND.OptText(i,2) .eq. FilterOption(j) ) OptionOK= .true.
          end do
          if ( .not. OptionOK ) goto 1020 ! error
          if ( (CMND.OptText(i,2) .eq. 'DPF') .or.
     &         (CMND.OptText(i,2) .eq. 'GCV')      ) Smooth= .true.
        end do ! i
      end if ! there are any options
      if ( (.not. SimpleMethod) .and. (.not. SF.angle) ) goto 1040
c             
c     Calculate joint velocities and accels by just differentiating
c     the joint angles - the simple method
c
      if ( SimpleMethod ) then     
        Smooth= .false. 
        time_inc= 1.0/rMtnFrmRate
        do i= 1, GP.num_jnt
          jnt= GP.jnt_num(i)  
          do axis= flex, inex
            if ( OF.UpDate ) write(6,100) i,GP.num_jnt,jnt_names(jnt),axis_names(axis)
100         format('+Currently differentiating joint # ',i2,'/',i2,': ',a4,' ',a4)
            do frm= 1, MP.nfrm
              temp(frm)= JangJCS(axis,frm,jnt)       
            end do ! frm
            call filter( CMND,Smooth,MP.nfrm,temp,time_inc,OF,
     &                   vel,acc,status )
            if ( status .lt. 0 ) goto 1010               
            call AngFilterStatus( status,i,GP.num_jnt,jnt_names(jnt),
     &                            axis_names(axis),OF )
            do frm= 1, MP.nfrm
              JangJCS(axis,frm,jnt)= temp(frm)  
              JvelJCS(axis,frm,jnt)= vel(frm)
              JaccJCS(axis,frm,jnt)= acc(frm)
            end do ! frm                 
          end do ! axis
        end do ! i => jnt    
        if ( JF.NumProcOps .le. 4 ) then
          JF.NumProcOps= JF.NumProcOps + 1
          JF.ProcOp(JF.NumProcOps)= CMND.Text(1)(1:40)
        end if
        JF.vel_acc_JCS= .true.
c
c     Calculate the joint velocities and accels using the correct
c     kinematic method
c                                            
      else ! use the proper method
        if ( .not. SF.vel_acc ) goto 1030 ! error
        do i= 1, GP.num_jnt      
          jnt= GP.jnt_num(i)
          if ( OF.UpDate ) write(6,110) i,GP.num_jnt,jnt_names(jnt)
110       format('+Currently doing joint # ',i2,'/',i2,': ',a4)
          if ( jnt .ne. pvlb ) then
            DistSeg= JntSegNum(distal,jnt)
            ProxSeg= JntSegNum(proximal,jnt)     
            do frm= 1, MP.nfrm
              call VecSub( SvlGCS(wx,frm,DistSeg),
     &                     SvlGCS(wx,frm,ProxSeg),VecA )
              call VecSub( SacGCS(wx,frm,DistSeg),
     &                     SacGCS(wx,frm,ProxSeg),VecB )
              call MatTran( SgpGCS(x,x,frm,ProxSeg),RotMat )
              call VecChangeCS( RotMat,VecA,RotVelLCS )
              call VecChangeCS( RotMat,VecB,RotAccLCS )
              do j= 1, 3
                angle(j)= AngleSignJCS(j,jnt)*JangJCS(j,frm,jnt)/rtod
              end do
              call CalcEulVelYXZ( angle,RotVelLCS,vecA )
              do j= 1, 3
                JvelJCS(j,frm,jnt)= AngleSignJCS(j,jnt)*vecA(j)*rtod
              end do
              call CalcEulAccYXZ( angle,vecA,RotAccLCS,vecB )
              do j= 1, 3
                JaccJCS(j,frm,jnt)= AngleSignJCS(j,jnt)*vecB(j)*rtod
              end do
c              do j= flex, inex		! old method...
c                 JvelJCS(j,frm,jnt)= 
c     &              VecDot(JaxesJCS(x,j,frm,jnt),RotVelLCS )*rtod
c                 JaccJCS(j,frm,jnt)=   
c     &              VecDot(JaxesJCS(x,j,frm,jnt),RotAccLCS )*rtod
c                 JvelJCS(j,frm,jnt)= AngleSignJCS(j,jnt)*
c     &                                   JvelJCS(j,frm,jnt)
c                 JaccJCS(j,frm,jnt)= AngleSignJCS(j,jnt)*
c     &                                   JaccJCS(j,frm,jnt)
c              end do ! j
            end do ! frm                
          else ! jnt = pvlb
            do frm= 1, MP.nfrm
              do j= 1, 3
                angle(j)= SangGCS(j,frm,pelv)/rtod
              end do
              call CalcEulVelYXZ( angle,SvlGCS(wx,frm,pelv),vecA )
              do j= 1, 3
                JvelJCS(j,frm,jnt)= vecA(j)*rtod
              end do
              call CalcEulAccYXZ( angle,vecA,SacGCS(wx,frm,pelv),vecB )
              do j= 1, 3
                JaccJCS(j,frm,jnt)= vecB(j)*rtod
              end do
c              do j= flex, inex		! old method...
c                 JvelJCS(j,frm,jnt)= 
c     &              VecDot(JaxesJCS(x,j,frm,jnt),
c     &                     SvlGCS(wx,frm,pelv) )*rtod
c                 JaccJCS(j,frm,jnt)=  
c     &              VecDot(JaxesJCS(x,j,frm,jnt),
c     &                     SacGCS(wx,frm,pelv) )*rtod
c                 JvelJCS(j,frm,jnt)= AngleSignJCS(j,jnt)*
c     &                                   JvelJCS(j,frm,jnt)
c                 JaccJCS(j,frm,jnt)= AngleSignJCS(j,jnt)*
c     &                                   JaccJCS(j,frm,jnt)
c              end do ! j
            end do ! frm
          end if
        end do ! jnt
c
c       Smooth joint velocities and accelerations if desired
c                                  
        if ( Smooth ) then                 
          time_inc= 1.0/rMtnFrmRate
          do i= 1, GP.num_jnt      
            jnt= GP.jnt_num(i)
            do axis= flex, inex
              if ( OF.UpDate ) write(6,100) i,GP.num_jnt,jnt_names(jnt),axis_names(axis)
120           format('+Smoothing vel/acc joint # ',i2,'/',i2,': ',a4,' ',a4)
              do frm= 1, MP.nfrm
                temp(frm)=  JvelJCS(axis,frm,jnt)       
                temp2(frm)= JaccJCS(axis,frm,jnt)       
              end do ! frm
              call filter( CMND,Smooth,MP.nfrm,temp,time_inc,OF,
     &                     vel,acc,status )
              if ( status .lt. 0 ) goto 1010               
              call AngFilterStatus( status,i,GP.num_jnt,jnt_names(jnt),
     &                              axis_names(axis),OF )
              call filter( CMND,Smooth,MP.nfrm,temp2,time_inc,OF,
     &                     vel,acc,status )
              if ( status .lt. 0 ) goto 1010               
              call AngFilterStatus( status,i,GP.num_jnt,jnt_names(jnt),
     &                              axis_names(axis),OF )
              do frm= 1, MP.nfrm
                JvelJCS(axis,frm,jnt)= temp(frm)  
                JaccJCS(axis,frm,jnt)= temp2(frm)  
              end do ! frm                 
            end do ! axis          
          end do ! i => jnt        
        end if ! Smoothing of vel/acc desired
        JF.vel_acc_JCS= .true.
      end if                  
      return                          
                    
1000  write(6,1005)
      if ( OF.ToLog ) write(3,1005)
1005  format(' Error- JCS joint angles have not been calculated yet.')
      return
                                      
1010  write(6,1015)
      if ( OF.ToLog ) write(3,1015)
1015  format(' Error- Filter/Smoothing operation unsuccessful.')
      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 JNT JVA'/
     &       '        Valid options are: /SIM /DPF /GCV /FDF /NOI /VAR /NON')
      return
      
1030  write(6,1035)
      if ( OF.ToLog ) write(3,1035)
1035  format(' Error- Segment velocity and acceleration has not been'/
     &       '        calculated yet.')
      return

1040  write(6,1045)
      if ( OF.ToLog ) write(3,1045)
1045  format(' Error- Segment angles have not been calculated yet')
      return
      end
c
c	CalcEulVelYXZ
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Calculate the JCS velocity from the velocity
c				of the distal segment relative to the 
c				proximal segment in terms of the proximal segment LCS.
c				These formulas are messy- see the derivation notes for
c			  	ANZ to see the details.
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 CalcEulVelYXZ( EulAng,RotVelGCS,EulVel )
      implicit none

      real*4    EulAng(3),RotVelGCS(3),EulVel(3),pi
           
      pi= 4.0*atan(1.0)      
      if ( abs(abs(EulAng(2))-pi/2.0) .gt. 1.0e-5 ) then
        EulVel(1)=  RotVelGCS(2) 
     &             + RotVelGCS(1)*(sin(EulAng(1))*tan(EulAng(2)))
     &             + RotVelGCS(3)*(cos(EulAng(1))*tan(EulAng(2)))
        EulVel(2)=  cos(EulAng(1))*RotVelGCS(1) 
     &             - sin(EulAng(1))*RotVelGCS(3)
        EulVel(3)=  RotVelGCS(1)*(sin(EulAng(1))/cos(EulAng(2)))
     &             + RotVelGCS(3)*(cos(EulAng(1))/cos(EulAng(2)))
      else
        EulVel(1)= RotVelGCS(2)
        EulVel(3)= 0.0
        if ( abs(EulAng(1)-pi/2.0) .lt. 1.0e-5 ) then
          EulVel(2)= -RotVelGCS(3)
        else if ( abs(EulAng(1)+pi/2.0) .lt. 1.0e-5 ) then
          EulVel(2)=  RotVelGCS(3)
        else
          EulVel(2)= RotVelGCS(1)/cos(EulAng(1))
        end if
      end if
      return
      end
c
c	CalcEulAccYXZ
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Calculate the JCS acceleration from the acceleration
c				of the distal segment relative to the 
c				proximal segment in terms of the proximal segment LCS.
c				These formulas are messy- see the derivation notes for
c			  	ANZ to see the details.
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 CalcEulAccYXZ( EulAng,EulVel,RotAccGCS,EulAcc )     
      implicit none

      real*4    EulAng(3),EulVel(3),RotAccGCS(3),EulAcc(3),at(3)
           
      at(1)=  RotAccGCS(1)
     &      + sin(EulAng(1))*EulVel(1)*EulVel(2)
     &      - cos(EulAng(1))*cos(EulAng(2))*EulVel(1)*EulVel(3)
     &      + sin(EulAng(1))*sin(EulAng(2))*EulVel(2)*EulVel(3)
     
      at(2)=  RotAccGCS(2) + cos(EulAng(2))*EulVel(2)*EulVel(3)
     
      at(3)=  RotAccGCS(3)
     &      + cos(EulAng(1))*EulVel(1)*EulVel(2)
     &      + sin(EulAng(1))*cos(EulAng(2))*EulVel(1)*EulVel(3)
     &      + cos(EulAng(1))*sin(EulAng(2))*EulVel(2)*EulVel(3)
     
      call CalcEulVelYXZ( EulAng,at,EulAcc )
      return
      end
c                                        
c	AngFilterStatus  
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Display message about warning or error that has occured
c				while the smoothing filter was operating
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 AngFilterStatus( status,jnt,NumJnt,JntName,AxisName,OF )
      implicit none

      include 'a_struct.inc'

      record /option_flag/ OF
         
      integer*4 status,jnt,NumJnt
      character JntName*4,AxisName*4
          
      if ( status .eq. 2 ) then
        write(6,100) jnt,NumJnt,JntName,AxisName
        if ( OF.ToLog ) write(3,100) jnt,NumJnt,JntName,AxisName
100     format('+Currently differentiating joint # ',i2,'/',i2,': ',
     &           a4,' ',a4,'   Warning- Nearly constant'/)
      else if ( status .eq. 3 ) then 
        write(6,110) jnt,NumJnt,JntName,AxisName
        if ( OF.ToLog ) write(3,110) jnt,NumJnt,JntName,AxisName
110     format('+Currently differentiating joint # ',i2,'/',i2,': ',
     &           a4,' ',a4,'   Avg too small- Used Finite Dif'/)
      else if ( status .eq. 4 ) then 
        write(6,120) jnt,NumJnt,JntName,AxisName
        if ( OF.ToLog ) write(3,120) jnt,NumJnt,JntName,AxisName
120     format('+Currently differentiating joint # ',i2,'/',i2,': ',
     &           a4,' ',a4,'   Filter error- Used Finite Dif'/)
      else if ( status .ge. 10 ) then
        write(6,130) jnt,NumJnt,JntName,AxisName,status-10
        if ( OF.ToLog ) write(3,130) jnt,NumJnt,JntName,AxisName,
     &                                       status-10
130     format('+Currently differentiating joint # ',i2,'/',i2,': ',
     &           a4,' ',a4,'   GCVSPL error #',i2,'- Used Finite Dif'/)
      end if
      return                     
      end           
c                                        
c	CalcEULAngles  
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Calculate the YXZ FIXED axis euler angles describing
c				the orientation of the distal segment relative to the
c				proximal segment across a joint.  These angles are included
c				purely for compatability with OUT, the postprocessing
c				program previously used by the Ohio Sate Gait Lab.
c				Don't use these unless you absolutely have to...
c
c	Routines called:
c
c	HandleAngleError
c	InterpAngles
c	EulerAngles
c	ShldrAngles
c	VecNorm
c	VecX
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Joint			AngleSignJCS
c	Param			GP.analysis_type
c	Param			GP.num_jnt
c	Param			GP.jnt_num
c	Joint			JntSegNum
c	Param			MP.nfrm
c	Segment			SgpGCS
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Joint			JangEUL
c	Joint			JaxesEUL
c
      subroutine CalcEULAngles( CMND,SF,JF,OF )
      implicit none

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

      record /CmndLine/     CMND
      record /segment_flag/ SF
      record /joint_flag/   JF
      record /option_flag/  OF             
                                                                           
      integer*4 frm,jnt,axis1,axis2,num_left_jnts,left_jnt_num(5),i,j,error,
     &          NumError(3,MAX_JNT),dist_seg,prox_seg
      real*4    lab_axes(3,3),AnkleFlexion
      logical*4 left_jnt,InterpFlag,InterpError,SkipLine,CorrectAnkleFlexion,
     &          UseShldrPolar,JntInterp
                               
      data lab_axes / 1.,0.,0.,0.,1.,0.,0.,0.,1. /
      data num_left_jnts / 5 /
      data left_jnt_num / 4,5,6,11,12 /
      
      error= 0
      JF.angle_EUL= .false.
      if ( .not. SF.position ) goto 1000     
c
c     Initialize error handling before starting calculations
c
      JntInterp= .true.
      InterpFlag= .false.
      do i= 1, MAX_JNT
        do j= 1, 3
          NumError(j,i)= 0                                                 
        end do ! j         
      end do ! i      
c                                                 
c     Check for angle options
c
      UseShldrPolar= .false.
      CorrectAnkleFlexion= .false.
      AnkleFlexion= -6.0
      if ( CMND.NumOpt(2) .gt. 0 ) then
        do i= 1, CMND.NumOpt(2)
          if ( CMND.OptText(i,2) .eq. 'NAF' ) then
            CorrectAnkleFlexion= .false.
          else if ( CMND.OptText(i,2) .eq. 'AFL' ) then
            if ( CMND.OptNumOK(i,2) ) then
              if ( abs(CMND.OptIntVal(i,2)) .gt. 30.0 ) goto 1030
              AnkleFlexion= CMND.OptIntVal(i,2)
            end if
            CorrectAnkleFlexion= .true.
          else if ( CMND.OptText(i,2) .eq. 'SHP' ) then
            UseShldrPolar= .true.
          else 
            goto 1040 ! error
          end if
        end do
      end if
c                                        
c     Do angle calculations
c                
      SkipLine= .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,101) frm,MP.nfrm
101       format('+Processing frame # ',i3,'/',i3)
          SkipLine= .false.
        end if               
        do i= 1, GP.num_jnt
          jnt= GP.jnt_num(i)
c                           
c         The pelvis is a special case because its angle is measured relative
c         to the lab coordinate system, whereas all other angles are measured
c         relative to the more proximal segment.
c
          if ( jnt .eq. pvlb ) then ! special case for pelvis
            if ( GP.analysis_type .eq. UPR_ANL )  lab_axes(z,z)= -1.0
            call EulerAngles( SgpGCS(x,x,frm,JntSegNum(1,jnt)),
     &                        lab_axes,JangEUL(flex,frm,jnt),error   )
            if ( error .ne. 0 ) then ! error 
              InterpFlag= .true.
              SkipLine=   .true.
              call HandleAngleError( error,frm,jnt,NumError(1,jnt),
     &                               ErrFrm(1,1,jnt),JntInterp,OF  )
            end if
            do axis1= x, z
              JangEUL(axis1,frm,jnt)= AngleSignJCS(axis1,jnt)*
     &                                      JangEUL(axis1,frm,jnt)
              do axis2= x, z
                JaxesEUL(axis1,axis2,frm,jnt)= lab_axes(axis1,axis2)
              end do
            end do
            if ( GP.analysis_type .eq. UPR_ANL )  lab_axes(z,z)= 1.0
          else
c
c           Check the joint to segment connectivity array, JntSegNum,
c           to see which segments are to calculate which joint angles. 
c           This array is defined in a_jnt.inc and is filled in the beginning
c           of the main ANALYZE program.
c
            dist_seg= JntSegNum(distal,jnt)
            prox_seg= JntSegNum(proximal,jnt)
            if ( (GP.analysis_type .eq. UPR_ANL) .and. 
     &           ( (jnt .eq. rtsh) .or. (jnt .eq. lfsh) ) .and.
     &           ( UseShldrPolar )    ) then
              call ShldrAngles( SgpGCS(x,x,frm,dist_seg),
     &                          SgpGCS(x,x,frm,prox_seg),
     &                          JangJCS(flex,frm,jnt),error )
            else
              call EulerAngles( SgpGCS(x,x,frm,dist_seg),
     &                          SgpGCS(x,x,frm,prox_seg),
     &                          JangEUL(flex,frm,jnt),error )
            end if
            if ( error .ne. 0 ) then ! error 
              InterpFlag= .true.
              SkipLine=   .true.
              call HandleAngleError( error,frm,jnt,NumError(1,jnt),
     &                               ErrFrm(1,1,jnt),JntInterp,OF  )
            else
              do axis1= x, z
                JangEUL(axis1,frm,jnt)= AngleSignJCS(axis1,jnt)*
     &                                        JangEUL(axis1,frm,jnt)
                do axis2= x, z
                  JaxesEUL(axis1,axis2,frm,jnt)=
     &                      SgpGCS(axis1,axis2,frm,JntSegNum(2,jnt))
                end do
              end do
            end if
c
c           Subtract 6 deg (default) from ankle flexion (plantarflexion) to
c           make the angle calculated the same as in previous studies
c
            if ( ( (jnt .eq. rtak) .or. (jnt .eq. lfak) ) .and.
     &           CorrectAnkleFlexion )
     &        JangEUL(flex,frm,jnt)= JangEUL(flex,frm,jnt) +
     &                                     AnkleFlexion
          end if ! special case of pelvis-lab joint
        end do ! i => jnt
      end do ! frame
c
c     If any angles are missing due to errors in JCSAngles calculation,
c     interpolate the the missing data.
c
      JF.angle_EUL= .true.
      if ( InterpFlag ) then 
        call InterpAngles( NumError,ErrFrm,JangEUL,JaxesEUL,
     &                     JntInterp,OF,InterpError )
        if ( InterpError ) JF.angle_EUL= .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) jnt_names(jnt),frm
      if ( OF.ToLog ) write(3,1015) jnt_names(jnt),frm
1015  format(' Error- An error has occured in the calculating the euler angles'/
     &       '        for the ',a4,' at frame # ',i3,' ...')
      return
      
1030  write(6,1035)
      if ( OF.ToLog ) write(3,1035)
1035  format(' Error- Incorrect amount of ankle flexion correction'/
     &       '        Correction must be >-30 and <30')
      return

1040  write(6,1045) CMND.OptText(i,2)
      if ( OF.ToLog ) write(3,1045) CMND.OptText(i,2)
1045  format(' Error- ',a3,' is an invalid option for JNT EAN'/
     &       '        Valid options are: SHP NAF AFL=')
      return
      end
c                                        
c	CalcEULVelAcc  
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Calculate the joint velocities and accelerations in
c				terms of the EULer angles.  Two methods are provided: 1) Just
c				numerically differentiate the EULer joint angles (NOTE
c				this definitely wrong from and kinematics point of
c				view has been carried out in other works) 2) Find them
c				using the relative rotational velocities and accelerations
c				between the distal and proximal joint segments.  This is
c				the correct method and the default.  The simple method has
c				been included for comparision purposes.
c
c	Routines called:
c
c	AngFilterStatus
c	filter
c	MatTran
c	RemoveCMNDOpt
c	VecChangeCS
c	VecDot
c	VecSub
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Joint			AngleSignJCS
c	Param			GP.jnt_num
c	Param			GP.num_jnt
c	Joint			JangEUL
c	Names			jnt_names
c	Joint			JntSegNum
c	Param       rMtnFrmRate
c	Param			MP.nfrm
c	Segment			SacGCS
c	Segment			SgpGCS
c	Segment			SvlGCS
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Joint			JaccEUL
c	Joint			JangEUL	! in the simple case...
c	Joint			JvelEUL
c
      subroutine CalcEULVelAcc( 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 i,j,jnt,axis,frm,iterm,NumJnt,status,DistSeg,ProxSeg
      real*4    time_inc,temp(MAX_MTN),vel(MAX_MTN),acc(MAX_MTN),VecA(3),VecB(3),rtod,
     &          RotMat(3,3),RotVelLCS(3),RotAccLCS(3),VecDot,temp2(MAX_MTN)
      character axis_names(3)*4,FilterOption(6)*3
      logical*4 Smooth,SimpleMethod,OptionOK

      data axis_names / 'flex','inex','abad' /
      data FilterOption / 'DPF','GCV','FDF','NOI','VAR','NON' /
                           
      JF.vel_acc_EUL= .false.                                        
      if ( .not. JF.angle_EUL ) goto 1000
      rtod= 45.0/atan(1.0)
c
c     Check for options
c
      Smooth= .false.
      SimpleMethod= .false.         
      if ( CMND.NumOpt(2) .gt. 0 ) then
        i= 1
        do while ( (i .lt. CMND.NumOpt(2)) .and. 
     &             (CMND.OptText(i,2) .ne. 'SIM') )
          i= i + 1
        end do
        if ( CMND.OptText(i,2) .eq. 'SIM' ) then
          SimpleMethod= .true.
          call RemoveCMNDOpt( CMND,2,j )
        end if         
        Smooth= .false.
        do i= 1, CMND.NumOpt(2)                                        
          OptionOK= .false.
          do j= 1, 6
            if ( CMND.OptText(i,2) .eq. FilterOption(j) ) OptionOK= .true.
          end do
          if ( .not. OptionOK ) goto 1020 ! error
          if ( (CMND.OptText(i,2) .eq. 'DPF') .or.
     &         (CMND.OptText(i,2) .eq. 'GCV')      ) Smooth= .true.
        end do ! i
      end if ! there any options
c
c     Calculate joint velocities and accels by just differentiating
c     the joint angles - the simple method        
c
      if ( SimpleMethod ) then     
        Smooth= .false.
        time_inc= 1.0/rMtnFrmRate
        NumJnt= GP.num_jnt
        if ( JF.TRU_angle ) NumJnt= 7
        do i= 1, NumJnt              
          jnt= GP.jnt_num(i)
          do axis= flex, inex
            if ( OF.UpDate ) write(6,100) i,GP.num_jnt,jnt_names(jnt),axis_names(axis)
100         format('+Currently differentiating joint # ',i2,'/',i2,': ',a4,' ',a4)
            do frm= 1, MP.nfrm
              temp(frm)= JangEUL(axis,frm,jnt)
            end do ! frm
            call filter( CMND,Smooth,MP.nfrm,temp,time_inc,OF,
     &                   vel,acc,status )
            if ( status .lt. 0 ) goto 1010
            call AngFilterStatus( status,i,GP.num_jnt,jnt_names(jnt),
     &                            axis_names(axis),OF )
            do frm= 1, MP.nfrm                                     
              JangEUL(axis,frm,jnt)= temp(frm)
              JvelEUL(axis,frm,jnt)= vel(frm)
              JaccEUL(axis,frm,jnt)= acc(frm)
            end do ! frm
          end do ! axis    
        end do ! i => jnt  
        JF.vel_acc_EUL= .true.
c
c     Calculate the joint velocities and accels using the correct
c     kinematic method
c                                            
      else ! use the proper method
        if ( .not. SF.vel_acc ) goto 1030 ! error
        do i= 1, GP.num_jnt      
          jnt= GP.jnt_num(i)
          if ( OF.UpDate ) write(6,110) i,GP.num_jnt,jnt_names(jnt)
110       format('+Currently doing joint # ',i2,'/',i2,': ',a4)
          if ( jnt .ne. pvlb ) then
            DistSeg= JntSegNum(distal,jnt)
            ProxSeg= JntSegNum(proximal,jnt)
            do frm= 1, MP.nfrm    
              call VecSub( SvlGCS(wx,frm,DistSeg),
     &                     SvlGCS(wx,frm,ProxSeg),VecA )
              call VecSub( SacGCS(wx,frm,DistSeg),
     &                     SacGCS(wx,frm,ProxSeg),VecB )
              call MatTran( SgpGCS(x,x,frm,ProxSeg),RotMat )
              call VecChangeCS( RotMat,VecA,RotVelLCS )
              call VecChangeCS( RotMat,VecB,RotAccLCS )
              do j= flex, inex
                 JvelEUL(j,frm,jnt)= 
     &              VecDot(JaxesEUL(x,j,frm,jnt),RotVelLCS )*rtod
                 JaccEUL(j,frm,jnt)=  
     &              VecDot(JaxesEUL(x,j,frm,jnt),RotAccLCS )*rtod
                 JvelEUL(j,frm,jnt)= AngleSignJCS(j,jnt)*
     &                                   JvelEUL(j,frm,jnt)
                 JaccEUL(j,frm,jnt)= AngleSignJCS(j,jnt)*
     &                                   JaccEUL(j,frm,jnt)
              end do ! j
            end do ! frm
          else ! jnt = pvlb
            do frm= 1, MP.nfrm
              do j= flex, inex
                 JvelJCS(j,frm,jnt)= 
     &              VecDot(JaxesEUL(x,j,frm,jnt),
     &                     SvlGCS(wx,frm,pelv) )*rtod
                 JaccJCS(j,frm,jnt)=  
     &              VecDot(JaxesEUL(x,j,frm,jnt),
     &                     SacGCS(wx,frm,pelv) )*rtod
                 JvelEUL(j,frm,jnt)= AngleSignJCS(j,jnt)*
     &                                   JvelEUL(j,frm,jnt)
                 JaccEUL(j,frm,jnt)= AngleSignJCS(j,jnt)*
     &                                   JaccEUL(j,frm,jnt)
              end do ! j
            end do ! frm
          end if
        end do ! jnt
c
c       Smooth joint velocities and accelerations if desired
c                                  
        if ( Smooth ) then                 
          time_inc= 1.0/rMtnFrmRate
          do i= 1, GP.num_jnt      
            jnt= GP.jnt_num(i)
            do axis= flex, inex
              if ( OF.UpDate ) write(6,100) i,GP.num_jnt,jnt_names(jnt),axis_names(axis)
120           format('+Smoothing vel/acc joint # ',i2,'/',i2,': ',a4,' ',a4)
              do frm= 1, MP.nfrm
                temp(frm)=  JvelEUL(axis,frm,jnt)       
                temp2(frm)= JaccEUL(axis,frm,jnt)       
              end do ! frm
              call filter( CMND,Smooth,MP.nfrm,temp,time_inc,OF,
     &                     vel,acc,status )
              if ( status .lt. 0 ) goto 1010               
              call AngFilterStatus( status,i,GP.num_jnt,jnt_names(jnt),
     &                              axis_names(axis),OF )
              call filter( CMND,Smooth,MP.nfrm,temp2,time_inc,OF,
     &                     vel,acc,status )
              if ( status .lt. 0 ) goto 1010               
              call AngFilterStatus( status,i,GP.num_jnt,jnt_names(jnt),
     &                              axis_names(axis),OF )
              do frm= 1, MP.nfrm
                JvelEUL(axis,frm,jnt)= temp(frm)  
                JaccEUL(axis,frm,jnt)= temp2(frm)  
              end do ! frm                 
            end do ! axis          
          end do ! i => jnt        
        end if ! Smoothing of vel/acc desired
        JF.vel_acc_EUL= .true.
      end if                  
      return           
                           
1000  write(6,1005)
      if ( OF.ToLog ) write(3,1005)     
1005  format(' Error- Euler joint angles have not been calculated yet.')

      
1010  write(6,1015)
      if ( OF.ToLog ) write(3,1015)
1015  format(' Error- Filter/Smoothing operation unsuccessful.')
      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 JNT EVA'/
     &       '        Valid options are: /SIM /DPF /GCV /FDF /NOI /VAR /NON')
      return
      
1030  write(6,1035)
      if ( OF.ToLog ) write(3,1035)
1035  format(' Error- Segment velocity and acceleration has not been'/
     &       '        calculated yet.')
      return
      end                     
c                                        
c	CalcJntLoad  
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Calculate the joint loads.  This is the main control
c				routine which decides what calculation chain scheme
c				will be used at each time frame of data and then calls
c				the routines that do the actual joint load calculation.
c				This routine bases its decisions as to which scheme to
c				use exclusively on the foot contact conditions and 
c				whether there is valid GRL data at a given time frame.
c
c		NOTE:	There is a lengthy section of code after the joint load
c				calculation is finished which is used to detect anomilies
c				in the JRL calculation at or near the gait events and to
c				interpolate across these trouble points.  That these
c				problems exist at all is a result of slight misalignment
c				between the force plate data and the motion data and the
c				use of a minimum fz threshold cutoff which makes the GRL
c				data on the feet invalid once the fz as become too small.
c				This works very well for allowing calculation schemes to be
c				selected and invalid frames not to be displayed in TELIO
c				but has the side effect of causing the the problems here.
c				I have tried a number of things to try to prevent this from 
c				occurring but have had no luck.  Its the GRL validity
c				that is the main culprit not the roundoff error in
c				determining the GRL frame that corresponds to the motion
c				data frame.  The detection/interpolation trick seems to
c				work but I'm not real proud of having to place this 
c				KLUDGE in here...
c
c	Routines called:
c
c	CalcAllJntLoads
c	find_interp_frm
c	lagrange_interp
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Force			FlrCntct
c	Force			FtGrlOK
c	Param			GP.jnt_num
c	Param			GP.num_jnt
c	Param			JNT_BU
c	Param			JNT_DBL
c	Param			JNT_TD
c	Param			MP.cycle_length
c	Param		   rMtnFrmRate
c	Param			MP.lhs_frm
c	Param			MP.lto_frm
c	Param			MP.nfrm
c	Param			MP.num_lhs
c	Param			MP.num_lto
c	Param			MP.num_rhs
c	Param			MP.num_rto
c	Param			MP.rhs_frm
c	Param			MP.rto_frm
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Joint			event
c	Joint			JRLegGCS
c	Joint			JRLegJCS
c	Joint			JntCalcOK
c	Joint			jnt_calc_type
c	Joint			JrlDCS
c	Joint			JrlGCS
c	Joint			JrlJCS
c	Joint			JrlPCS
c
      subroutine CalcJntLoad( CMND,SF,FF,JF,OF )
      implicit none                 

      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_prm.inc'
      include 'a_gls2.inc'
      include 'a_frc.inc'
      include 'a_jnt.inc'
      include 'a_names.inc'

      record /CmndLine/      CMND
      record /segment_flag/  SF
      record /force_flag/    FF
      record /joint_flag/    JF
      record /option_flag/   OF
                        
      integer*4 i,j,k,jnt,NumLegJnt,LegJnt(6),frm,PolyOrder,NumGoodJRLFrm(MAX_JNT),
     &          GoodJRLFrm(MAX_MTN,MAX_JNT),inc1,inc2,InterpFrm(10),axis,
     &          NumChange,EventFrm(5),FtOrder(2)
      real*4    TimeData(10),JRLData(10,8),InterpTime,InterpVal
      character EventName(4)*3
      logical*4 LimitedJntCalc,InterpJntLoad,DoInterp,error,DataOK,
     &          OverRideFrcOK

      data EventName / 'RHS','LHS','RTO','LTO' /
 
      data LegJnt / 1,2,3,4,5,6 / ! rtak,rtkn,rthp,lfak,lfkn,lfhp      
c                
c     Check whether the required information has been read in and/or calculated
c

      if ( .not. SF.position )        goto 1000 ! errors...
      if ( .not. SF.vel_acc )         goto 1010
      if ( .not. SF.param )           goto 1020
      if ( .not. JF.angle_JCS )       goto 1070
      LimitedJntCalc= .true.
      if ( .not. FF.individual_feet ) then
        LimitedJntCalc= .false.
        do j= 1, 2
          do i= 1, MP.nfrm
            FtGrlOK(i,j)= .false.
          end do 
        end do
        write(6,107)
107     format(' Joint load calculation being carried without force plate data'/)
      else
        if ( .not. FF.cpr_LCS )         goto 1040
        if ( .not. FF.DataClipped )     goto 1050
      end if 
c
c     Check for limitedjntcalc option- this limits the joint laod calculation
c     scheme to only those that use ground reaction loads
c                                                               
      PolyOrder= 3
      InterpJntLoad= .false.
      OverRideFrcOK= .true.
      if ( CMND.NumOpt(2) .gt. 0 ) then
        do i= 1, CMND.NumOpt(2)
          if ( CMND.OptText(1,2) .eq. 'FUL' ) then
            LimitedJntCalc= .false.
          else if ( CMND.OptText(i,2) .eq. 'INT' ) then
            InterpJntLoad= .true.
            if ( CMND.OptNumOK(i,2) ) then
              if ( (CMND.OptIntVal(i,2) .lt. 1) .or.
     &             (CMND.OptIntVal(i,2) .gt. 9)      ) goto 1080
              PolyOrder= CMND.OptIntVal(i,2)
            end if
          else if ( CMND.OptText(i,2) .eq. 'NOI' ) then
            InterpJntLoad= .false.
          else if ( CMND.OptText(i,2) .eq. 'OVE' ) then
            OverRideFrcOK= .true.
          else if ( CMND.OptText(i,2) .eq. 'NOV' ) then
            OverRideFrcOK= .false.
          else if ( CMND.OptText(i,2) .eq. 'POL' ) then
            if ( CMND.OptNumOK(i,2) ) then
              if ( (CMND.OptIntVal(i,2) .lt. 1) .or.
     &             (CMND.OptIntVal(i,2) .gt. 9)      ) goto 1080
              PolyOrder= CMND.OptIntVal(i,2)
            end if
          else                                           
            goto 1060
          end if
        end do
      end if
      if ( LimitedJntCalc ) then
        write(6,104)
      else
        write(6,106)
      end if
104   format(' Limited joint load calculation scheme being used'/)
106   format(' Full joint load calculation scheme being used'/)
c
c     Zero out the joint calc OK flags and the indicator of which type of
c     joint calculation order was used
c                
      do frm= 1, MP.cycle_length(1)
        JrlCalcType(frm)= 0
        do i= 1, GP.num_jnt
          JntCalcOK(frm,GP.jnt_num(i))= .false.
        end do
      end do
c                             
c     This section only applies to gait based joint load calculations
c     Override the FtGrlOK array if option says so...
c     This is a hack to get around the round off error problem when the time
c     resolution of the force plate data is decreased by the FRC CLIp command
c
        if ( OverRideFrcOK ) then      
          if ( MP.first_ft .eq. rt ) then
            EventFrm(1)= MP.rhs_frm(1)
            EventFrm(2)= MP.lto_frm(1)
            EventFrm(3)= MP.lhs_frm(1)
            EventFrm(4)= MP.rto_frm(1)
            EventFrm(5)= MP.rhs_frm(2)
            FtOrder(1)= rt
            FtOrder(2)= lf
          else
            EventFrm(1)= MP.lhs_frm(1)
            EventFrm(2)= MP.rto_frm(1)
            EventFrm(3)= MP.rhs_frm(1)
            EventFrm(4)= MP.lto_frm(1)
            EventFrm(5)= MP.lhs_frm(2)
            FtOrder(1)= lf
            FtOrder(2)= rt
          end if
c
c         Kludge added by dam 2/93 because of trouble with nongait joint load
c         calculations. Jumping in particular...
c
          do i= 1, 5
            if ( (EventFrm(i) .le. 0) .or. 
     &           (EventFrm(i) .gt. MP.nfrm) ) then
              write(6,300) i
300           format(' NOTE- Gait event override being ignored because event'/
     &               '       number ',i2,' is invalid')
              goto 400
            end if
          end do

          DataOK= .false.
          do frm= EventFrm(1), EventFrm(4)
            if ( FtGrlOK(frm,FtOrder(1)) ) DataOK= .true.
          end do
          if ( DataOK ) then
            do frm= EventFrm(1), EventFrm(1)+3
              FtGrlOK(frm,FtOrder(1))= .true.
            end do
            do frm= EventFrm(4)-3, EventFrm(4)
              FtGrlOK(frm,FtOrder(1))= .true.
            end do
            FtGrlOK(EventFrm(5),FtOrder(1))= .true.
          end if
          DataOK= .false.
          do frm= EventFrm(3), EventFrm(5)
            if ( FtGrlOK(frm,FtOrder(2)) ) DataOK= .true.
          end do                           
          if ( DataOK ) then
            do frm= EventFrm(3), EventFrm(3)+3
              FtGrlOK(frm,FtOrder(2))= .true.
            end do
            do frm= EventFrm(5)-3, EventFrm(5)
              FtGrlOK(frm,FtOrder(2))= .true.
            end do
          end if
        end if                            
c           
c     Check whether all 6 leg joints are present
c   
400   JF.legsummed= .false.     
      NumLegJnt= 0
      do i= 1, GP.num_jnt
        jnt= GP.jnt_num(i)
        do j= 1, 6
          if ( jnt .eq. LegJnt(j) ) NumLegJnt= NumLegJnt + 1
        end do ! j            
      end do ! i
      if ( NumLegJnt .ge. 6 ) JF.legsummed= .true.
      if ( JF.legsummed ) write(6,103)
103   format(' NOTE- Summed leg loads will be calculated in addition to'/
     &       '       individual leg joint loads'//)
c 
c     Do joint load calculation for each joint given in GP.jnt_num
c     at each frame of motion data within the gait cycle.  Note that the
c     way in which this routine is written it is specific to gait, but that
c     by changing the logic examining what part of the gait cycle is occuring
c     and making it more general, other activities could be analyzed with
c     this code.  I made the code gait cycle specific because of time
c     constraints and I couldn't come up with a general enough formulation
c     so that the code would apply to all activities.  One possibility,
c     however, is to only do top down analyses (which do not require ground
c     reaction information) and just don't do the legs when/if double stance
c     occurs.  This is essentially what occurs here except that the possibility
c     ground reaction data for either one or both feet is allowed in which
c     case the ground loads can be calculated...
c
      do frm= 1, MP.cycle_length(1)
        if ( OF.UpDate .and. (mod(frm,10) .eq. 0) ) 
     &    write(6,101) frm,MP.nfrm
101     format('+Processing frame # ',i3,'/',i3)
c
c       This section applies only to gait based joint load calculations
c       Determine what part of the gait cycle is occuring and choose the
c       proper analysis method based upon the availability of ground reaction
c       data for the individual feet.
c
c       First- either of the double stance phases      
c
          if ( FlrCntct(frm,rt) .and. FlrCntct(frm,lf) ) then ! double stance
            if ( FtGrlOK(frm,rt) .and. FtGrlOK(frm,lf) ) then ! bottom up
              call CalcAllJntLoads( frm,JNT_DBL(3),JF.legsummed )
              JrlCalcType(frm)= 5                      
                                                                  
            else if ( FtGrlOK(frm,rt) ) then
              if ( .not. LimitedJntCalc ) then ! modified bottom up calc order
                call CalcAllJntLoads( frm,JNT_DBL(rt),JF.legsummed )  
                JrlCalcType(frm)= 6
              else ! simple bottom up order on right leg only
                call CalcAllJntLoads( frm,JNT_DBL(5),JF.legsummed )  
                JrlCalcType(frm)= 10
              end if 
           
            else if ( FtGrlOK(frm,lf) ) then 
              if ( .not. LimitedJntCalc ) then ! modified bottom up calc order
                call CalcAllJntLoads( frm,JNT_DBL(lf),JF.legsummed )
                JrlCalcType(frm)= 7
              else ! simple botton up order on left leg only
                call CalcAllJntLoads( frm,JNT_DBL(6),JF.legsummed )
                JrlCalcType(frm)= 11
              end if
                                                                
            else if (.not. LimitedJntCalc) then ! no way to do leg joint loads, but do upper body
              call CalcAllJntLoads( frm,JNT_DBL(4),JF.legsummed )
              JrlCalcType(frm)= 8
            end if
c
c      Now, the single support phases
c
          else if ( FlrCntct(frm,rt) ) then ! rt single support
            if ( FtGrlOK(frm,rt) ) then ! do bottom up calc order
              call CalcAllJntLoads( frm,JNT_BU(rt),JF.legsummed )
              JrlCalcType(frm)= 3
            
            else if ( .not. LimitedJntCalc ) then ! do top down calc order
              call CalcAllJntLoads( frm,JNT_TD(rt),JF.legsummed )
              JrlCalcType(frm)= 1

            else ! simple botton up order on left leg only
              call CalcAllJntLoads( frm,JNT_DBL(6),JF.legsummed )
              JrlCalcType(frm)= 11
            end if
          
          else if ( FlrCntct(frm,lf) ) then ! lf single support
            if ( FtGrlOK(frm,lf) ) then ! do bottom up calc order
              call CalcAllJntLoads( frm,JNT_BU(lf),JF.legsummed )
              JrlCalcType(frm)= 4
            
            else if ( .not. LimitedJntCalc ) then ! do top down calc order
              call CalcAllJntLoads( frm,JNT_TD(lf),JF.legsummed )
              JrlCalcType(frm)= 2

            else ! simple bottom up order on right leg only
              call CalcAllJntLoads( frm,JNT_DBL(5),JF.legsummed )  
              JrlCalcType(frm)= 10
            end if
c
c       Lastly, any flight phases- there shouldn't be any in gait of course.
c       I just put this in here for the fun of it.  Maybe ANALYZE will be
c       used for running analysis by someone someday...
c                 
          else if ( .not. LimitedJntCalc ) then ! neither foot touching- do top down order
            call CalcAllJntLoads( frm,JNT_TD(rt),JF.legsummed )
            JrlCalcType(frm)= 9
          end if
*$ifdef DEBUG
          write(4,511) frm,JrlCalcType(frm),JntCalcOK(frm,1),JrlJCS(fz,frm,1)
 511      format(1x,i4,1x,i2,1x,l1,1x,f10.4)
*$endif
      end do ! frm
c
c     This section only applies to gait based joint load calculations
c     If InterpJntLoad has been set then scan the resulting joint loads and
c     look for cases where interpolation across the gait event frames is
c     necessary
c
      JF.load= .true.                            
      if ( InterpJntLoad .and.
     &     ( (GP.analysis_type .eq. FULL_ANL) .or.
     &       (GP.analysis_type .eq. HH_ANL)       )  ) then
        do i= 1, GP.num_jnt
          NumGoodJRLFrm(GP.jnt_num(i))= 0
        end do
        do frm= 1, MP.nfrm
          do i= 1, GP.num_jnt
            jnt= GP.jnt_num(i)        
            if ( JntCalcOK(frm,jnt) ) then
              NumGoodJRLFrm(jnt)= NumGoodJRLFrm(jnt) + 1
              GoodJRLFrm(NumGoodJRLFrm(jnt),jnt)= frm
            end if
          end do
          GaitEvent(frm)= 0
        end do ! frm          
        do i= 1, MP.num_rhs                                 
          GaitEvent(MP.rhs_frm(i))= 1
        end do                        
        do i= 1, MP.num_lhs
          GaitEvent(MP.lhs_frm(i))= 2
        end do
        do i= 1, MP.num_rto
          GaitEvent(MP.rto_frm(i))= 3
        end do
        do i= 1, MP.num_lto
          GaitEvent(MP.lto_frm(i))= 4
        end do                    
        do frm= 1, MP.nfrm
          if ( GaitEvent(frm) .ne. 0 ) then
            DoInterp= .false.
            if ( frm .eq. 1 ) then
              if ( JrlCalcType(1) .ne. JrlCalcType(2) ) DoInterp= .true.
              if ( ((.not. JntCalcOK(1,rtak)) .and. JntCalcOK(2,rtak)) .or.
     &             ((.not. JntCalcOK(1,lfak)) .and. JntCalcOK(2,lfak)) )
     &          DoInterp= .true.
              inc1= 1
              inc2= 2
            else if ( frm .eq. MP.nfrm ) then
              if ( JrlCalcType(frm-1) .ne. JrlCalcType(frm) ) DoInterp= .true.
              if ( (.not. JntCalcOK(frm,rtak)) .and. JntCalcOK(frm-1,rtak) )
     &             DoInterp= .true.
              if ( (.not. JntCalcOK(frm,lfak)) .and. JntCalcOK(frm-1,lfak) )
     &             DoInterp= .true.
              inc1= -1
              inc2= -2
            else
              NumChange= 0
              do k= frm-3, frm+3
                if ( k .ne. frm ) then
                  if ( JrlCalcType(k) .ne. JrlCalcType(frm) ) 
     &              NumChange= NumChange + 1
                end if
              end do
              if ( NumChange .gt. 1 ) then
                DoInterp= .true.
                do k= frm-3, frm+3
                  if ( JrlCalcType(k) .eq. JrlCalcType(frm) ) then
                    do i= 1, GP.num_jnt
                      jnt= GP.jnt_num(i)        
                      do j= k, MP.nfrm-1
                        GoodJRLFrm(j,jnt)= GoodJRLFrm(j+1,jnt)
                      end do ! j
                      NumGoodJRLFrm(jnt)= NumGoodJRLFrm(jnt) - 1
                    end do ! i
                  end if
                end do ! k    
              else
                if ( (JntCalcOK(frm-1,rtak) .or. JntCalcOK(frm+1,rtak)) .and.
     &               (.not. JntCalcOK(frm,rtak)) ) DoInterp= .true.
                if ( (JntCalcOK(frm-1,lfak) .or. JntCalcOK(frm+1,lfak)) .and.
     &               (.not. JntCalcOK(frm,lfak)) ) DoInterp= .true.
              end if
              inc1= -1
              inc2=  1
            end if
            if ( DoInterp ) then
              write(6,500) frm,EventName(GaitEvent(frm))
              if ( OF.ToLog ) write(3,500) frm,EventName(GaitEvent(frm))
500           format(' Interpolating joint loads at frame # ',i4,1x,a3)
              do i= 1, GP.num_jnt
                jnt= GP.jnt_num(i)
                if ( JntCalcOK(frm+inc1,jnt) .or.
     &               JntCalcOK(frm+inc2,jnt)       ) then
                  j= 1
                  do while ( (GoodJRLFrm(j,jnt) .ne. frm) .and.
     &                       (j .lt. NumGoodJRLFrm(jnt))       )
                    j= j + 1
                  end do
                  if ( j .lt. NumGoodJRLFrm(jnt) ) then
                    NumGoodJRLFrm(jnt)= NumGoodJRLFrm(jnt) - 1
                    do k= j, NumGoodJRLFrm(jnt)
                      GoodJRLFrm(k,jnt)= GoodJRLFrm(k+1,jnt)
                    end do
                  end if
                  call find_interp_frm( GoodJRLFrm(1,jnt),NumGoodJRLFrm(jnt),
     &                                  frm,PolyOrder+1,InterpFrm )
                  do j= 1, PolyOrder+1  
                    TimeData(j)= float(InterpFrm(j))/rMtnFrmRate
                    do axis= 1, 8
                      JRLData(j,axis)= JrlJCS(axis,InterpFrm(j),jnt)
                    end do ! axis
                  end do ! j
                  InterpTime= float(frm)/rMtnFrmRate
                  do axis= 1, 8
                    call lagrange_interp( TimeData,JRLData(1,axis),
     &                                    PolyOrder+1,
     &                                    InterpTime,InterpVal,error )
                    if ( error ) goto 1090                          
                    JrlJCS(axis,frm,jnt)= InterpVal     
                  end do ! axis
                  do j= 1, PolyOrder+1  
                    do axis= 1, 6
                      JRLData(j,axis)= JrlGCS(axis,InterpFrm(j),jnt)
                    end do ! axis
                  end do ! j
                  do axis= 1, 6
                    call lagrange_interp( TimeData,JRLData(1,axis),
     &                                    PolyOrder+1,
     &                                    InterpTime,InterpVal,error )
                    if ( error ) goto 1090              
                    JrlGCS(axis,frm,jnt)= InterpVal     
                  end do ! axis
                  do j= 1, PolyOrder+1  
                    do axis= 1, 6
                      JRLData(j,axis)= JrlDCS(axis,InterpFrm(j),jnt)
                    end do ! axis
                  end do ! j            
                  do axis= 1, 6
                    call lagrange_interp( TimeData,JRLData(1,axis), 
     &                                    PolyOrder+1,
     &                                    InterpTime,InterpVal,error )
                    if ( error ) goto 1090              
                    JrlDCS(axis,frm,jnt)= InterpVal           
                  end do ! axis
                  do j= 1, PolyOrder+1  
                    do axis= 1, 6
                      JRLData(j,axis)= JrlPCS(axis,InterpFrm(j),jnt)
                    end do ! axis
                  end do ! j                                    
                  do axis= 1, 6
                    call lagrange_interp( TimeData,JRLData(1,axis),
     &                                    PolyOrder+1,
     &                                    InterpTime,InterpVal,error )
                    if ( error ) goto 1090              
                    JrlPCS(axis,frm,jnt)= InterpVal     
                  end do ! axis
                  JntCalcOK(frm,jnt)= .true.
                end if ! is the joint allowed to be interpolated
              end do ! i => jnt
              do i= rt, lf
                do j= 1, PolyOrder+1               
                  do axis= 1, 6
                    JRLData(j,axis)= JRLegJCS(axis,InterpFrm(j),i)
                  end do ! axis
                end do ! i            
                do axis= 1, 6
                  call lagrange_interp( TimeData,JRLData(1,axis),
     &                                  PolyOrder+1,
     &                                  InterpTime,InterpVal,error )
                  if ( error ) goto 1090              
                  JRLegJCS(axis,frm,i)= InterpVal     
                end do ! axis
                do j= 1, PolyOrder+1  
                  do axis= 1, 6
                    JRLData(j,axis)= JRLegGCS(axis,InterpFrm(j),i)
                  end do ! axis
                end do ! i            
                do axis= 1, 6
                  call lagrange_interp( TimeData,JRLData(1,axis),
     &                                  PolyOrder+1,
     &                                  InterpTime,InterpVal,error )
                  if ( error ) goto 1090              
                  JRLegGCS(axis,frm,i)= InterpVal     
                end do ! axis
              end do ! i => side
            end if ! DoInterpolation
          end if ! gait event has occured
        end do ! frm
      end if ! Check jointload for interpolation
      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 acceleration has not been calculated yet.')
      return
      
1020  write(6,1025)
      if ( OF.ToLog ) write(3,1025)
1025  format(' Error- Segment parameters have not been calculated yet.')
      return
      
1030  write(6,1035)
      if ( OF.ToLog ) write(3,1035)
1035  format(' Error- Ground reaction load data has not been broken up between'/
     &       '        the individual feet yet.')
      return
      
1040  write(6,1045)
      if ( OF.ToLog ) write(3,1045)
1045  format(' Error- Individual foot centers of pressure in the foot LCS'/
     &       '        have not been calculated yet.')
      return

1050  write(6,1055)
      if ( OF.ToLog ) write(3,1055)
1055  format(' Error- Force and motion data has been clipped to a single'/
     &       '        gait cycle yet.')
      return      

1060  write(6,1065) CMND.OptText(1,2)
      if ( OF.ToLog ) write(3,1065) CMND.OptText(1,2)
1065  format(' Error- ',a3,' is not a valid option for JNT LOA'/
     &       '        Valid Options are : FULl INTerpolate=  NOInterpolate'/
     &       '                            OVErride NOoVerride')
      return

1070  write(6,1075)
      if ( OF.ToLog ) write(3,1075)
1075  format(' Error- JCS Coord System Axes (angles) have not been calculated yet')
      return

1080  write(6,1085) CMND.OptIntVal(i,2)
      if ( OF.ToLog ) write(3,1085) CMND.OptIntVal(i,2)
1085  format(' Error- ',i4,' is not a valid interpolation polynomial order'/
     &       '        Valid polynomials are from 1 to 9')
      return

1090  write(6,1095)
      if ( OF.ToLog ) write(3,1095) 
1095  format(' Error- Trouble has occured during joint load interpolation')
      return
      end                                  
c
c	CalcAllJntLoads
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Calculate the joint resultant loads (JRL) for the 
c				chain of segments defined by the connectivity 
c				structure passed into the routine.  This a general
c				chain calculation driving routine.  ALso, sum the JRL
c				of the legs if all the joints have valid JRL's to obtain
c				net Leg Load.  A sub component of this, the fl/extension
c				moment, is what Winter has called the support moment.
c				I've included it purely out of curiosity...
c
c	Routines called:
c
c	CalcAJntLoad
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Force			FtGRLLCS
c	Force			FtCpLCS
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Joint			JRLegGCS
c	Joint			JRLegJCS
c	Joint			JntCalcOK
c	Joint			JrlDCS
c	Joint			JrlGCS
c	Joint			JrlJCS
c	Joint			JrlPCS
c
      subroutine CalcAllJntLoads( frm,CONNECT,SumLegs )
      implicit none
      
      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_prm.inc'            
      include 'a_names.inc'
      include 'a_gls2.inc'
      include 'a_frc.inc'
      include 'a_jnt.inc'
      include 'a_seg.inc'

      record /connectivity/ CONNECT
                                           
      integer*4 frm
      logical*4 SumLegs
                             
      integer*4 i,load,axis,jnt
      real*4    ext_load(6,3),ext_load_pos(3,3)
      logical*4 JntSumOK

      do i= 1, CONNECT.num_jnt
        if ( CONNECT.num_ext_load(i) .ne. 0 ) then
          if ( CONNECT.ext_load_num(1,i) .eq. rt ) then ! rtft ground reaction
            do load= fx, mz
              ext_load(load,1)= FtGRLLCS(load,frm,rt)
            end do
            do axis= x, z
              ext_load_pos(axis,1)= FtCpLCS(axis,frm,rt)
            end do                          
          else ! lf foot ground reaction
            do load= fx, mz
              ext_load(load,1)= FtGRLLCS(load,frm,lf)
            end do
            do axis= x, z
              ext_load_pos(axis,1)= FtCpLCS(axis,frm,lf)
            end do             
          end if      
        end if ! any externally applied loads to segment
        call CalcAJntLoad( frm,CONNECT.jnt_order(i),CONNECT.jnt_seg(i),
     &                     CONNECT.calc_jnt_num(i),  
     &                     CONNECT.num_conn_jnt(i),CONNECT.conn_jnt_num(1,i),
     &                     CONNECT.conn_jnt_seg(1,i),
     &                     CONNECT.num_ext_load(i),ext_load,ext_load_pos,
     &                     SP(CONNECT.jnt_seg(i)) )
        JntCalcOK(frm,CONNECT.jnt_order(i))= .true.
      end do ! i => jnt
c
c     Now that all the chain calculations are done, correct the signs of the
c     joint resultant loads of those joints whose chain calculation passed from
c     proximal to distal
c
      do i= 1, CONNECT.num_jnt
        if ( CONNECT.CalcOrder(i) .eq. ProxtoDist ) then
          jnt= CONNECT.jnt_order(i)
          do load= fx, mz
            JrlDCS(load,frm,jnt)= -JrlDCS(load,frm,jnt)
            JrlPCS(load,frm,jnt)= -JrlPCS(load,frm,jnt)
            JrlGCS(load,frm,jnt)= -JrlGCS(load,frm,jnt)
            JrlJCS(load,frm,jnt)= -JrlJCS(load,frm,jnt)
          end do ! load
        end if
      end do ! i
c                
c     Check for the presence of all six leg joints and if they are here
c     then sum the left and right flex-ext moments.  This is what Winter
c     calls the support moments.  I don't know exactly whether this is good
c     for anything, but Winter sure thinks so- so I added it for the heck
c     of it.
c
      if ( SumLegs ) then
        JntSumOK= .true.   ! check right side first
        do i= rtak, rthp
          if ( .not. JntCalcOK(frm,i) ) JntSumOK= .false.
        end do         
        if ( JntSumOK ) then
          do load= fx, mz
            JRLegJCS(load,frm,rt)= 0.0
            JRLegGCS(load,frm,rt)= 0.0
            do jnt= rtak, rthp
              JRLegJCS(load,frm,rt)= JRLegJCS(load,frm,rt) +
     &                                    JrlJCS(load,frm,jnt)
              JRLegGCS(load,frm,rt)= JRLegGCS(load,frm,rt) +
     &                                    JrlGCS(load,frm,jnt)
            end do ! jnt
          end do ! load
        end if
        JntSumOK= .true.   ! now check the left side
        do i= lfak, lfhp
          if ( .not. JntCalcOK(frm,i) ) JntSumOK= .false.
        end do
        if ( JntSumOK ) then              
          do load= fx, mz
            JRLegJCS(load,frm,lf)= 0.0
            JRLegGCS(load,frm,lf)= 0.0
            do jnt= lfak, lfhp
              JRLegJCS(load,frm,lf)= JRLegJCS(load,frm,lf) +
     &                                    JrlJCS(load,frm,jnt)
              JRLegGCS(load,frm,lf)= JRLegGCS(load,frm,lf) +
     &                                    JrlGCS(load,frm,jnt)
            end do ! jnt
          end do ! load
        end if
      end if 
      return                                
      end                   
c 
c	CalcAJntLoad                                
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Generic joint load calculation routine.  Basically solve
c				the 3D equations for rigid body with a known position/
c				orientation, velocity and acceleration plus known applied
c				loads (either external or from other joints) for a single
c				location (i.e., the joint resultant).  This routine is
c				critically dependent upon the connectivity information
c				passed into it for its correct operation.  Initially
c				solve for the JRL in the segment's LCS then transform
c				the JRL into the CS of the connected segment as well as
c				the JCS and GCS.
c                          
c
c	Routines called:
c
c	LoadGCStoJCS
c	LoadLCStoGCS
c	LoadLCStoLCS
c	VecX
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Joint			AngleSignJCS
c	Joint			JaxesJCS
c	Joint			JntSegNum
c	Segment			SacLCS
c	Segment			SgpGCS
c	Segment			SvlLCS
c	Segment			SP.end
c	Segment			SP.inertia
c	Segment			SP.mass
c	Joint			TransSignJCS
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Joint			JrlDCS
c	Joint			JrlGCS
c	Joint			JrlJCS
c	Joint			JrlPCS
c
      subroutine CalcAJntLoad( frm,calc_jnt,calc_seg,seg_jnt,num_conn,
     &                         conn_jnt,conn_jnt_seg,num_ext_load,ext_load,
     &                         ext_load_pos,SP )
      implicit none
                 
      include 'a_const.inc'      
      include 'a_struct.inc'
      include 'a_seg.inc'
      include 'a_jnt.inc'
      include 'a_names.inc'
                                        
      record /seg_param/ SP            

      integer*4 frm,calc_jnt,calc_seg,seg_jnt,num_conn,conn_jnt(3),
     &          conn_jnt_seg(3),num_ext_load
      real*4    ext_load(6,3),ext_load_pos(3,3)
                           
      integer*4 i,axis
      real*4    force(3),moment(3),add_moment(3),temp_load(6),gravity(3),
     &          gravLCS(3),TmpRM(3,3)
                             
      data gravity / 0.0,0.0,-9.816 / ! m/s^2
                             
      do axis= x, z  
        moment(axis)= 0.0
        force(axis)= 0.0
      end do
c
c     Sum external loads              
c                              
      do i= 1, num_ext_load
        call VecX( ext_load_pos(x,i),ext_load(x,i),add_moment )
        do axis= x, z
          force(axis)= force(axis) + ext_load(axis,i)
          moment(axis)= moment(axis) + ext_load(axis+3,i) + add_moment(axis)
        end do ! axis
      end do ! i            
c
c     Sum joint reaction loads from other joints attached to segment.  NOTE
c     that the joint loads are made negative to account for the fact that
c     they were calculated previously as the other segment affecting upon
c     the segment whose joint load was being calculated, but now the situation
c     is the opposite.                        
c
      do i= 1, num_conn
        if ( JntSegNum(distal,conn_jnt(i)) .eq. calc_seg ) then
          do axis= fx, mz 
            temp_load(axis)= -JrlDCS(axis,frm,conn_jnt(i))
          end do ! axis      
        else ! use proximal segment load
          do axis= fx, mz
            temp_load(axis)= -JrlPCS(axis,frm,conn_jnt(i))
          end do ! axis
        end if
        call VecX( SP.end(x,conn_jnt_seg(i)),temp_load(fx),add_moment )
        do axis= x, z
          force(axis)= force(axis) + temp_load(axis)
          moment(axis)= moment(axis) + temp_load(axis+3) + add_moment(axis)
        end do ! axis                                   
      end do ! i
c
c     Calculate the gravity term relative to the segment orientation
c
      call MatTran( SgpGCS(1,1,frm,calc_seg),TmpRM )
      call VecChangeCS( TmpRM,gravity,gravLCS )
c
c     Now include the inertial forces and moments.  The joint resultant
c     is now calculated in the segment (seg) LCS (temp_load).
c
      temp_load(fx)= SP.mass*( SacLCS(x,frm,calc_seg) 
     &             + SvlLCS(vz,frm,calc_seg)*SvlLCS(wy,frm,calc_seg)
     &             - SvlLCS(vy,frm,calc_seg)*SvlLCS(wz,frm,calc_seg) )
     &             - force(x) - SP.mass*gravLCS(x)
      temp_load(fy)= SP.mass*( SacLCS(y,frm,calc_seg) 
     &             + SvlLCS(vx,frm,calc_seg)*SvlLCS(wz,frm,calc_seg)
     &             - SvlLCS(vz,frm,calc_seg)*SvlLCS(wx,frm,calc_seg) )
     &             - force(y) - SP.mass*gravLCS(y)
      temp_load(fz)= SP.mass*( SacLCS(z,frm,calc_seg) 
     &             + SvlLCS(vy,frm,calc_seg)*SvlLCS(wx,frm,calc_seg)
     &             - SvlLCS(vx,frm,calc_seg)*SvlLCS(wy,frm,calc_seg) )
     &             - force(z) - SP.mass*gravLCS(z)
      temp_load(mx)= SP.inertia(x)*SacLCS(rx,frm,calc_seg)
     &             + (SP.inertia(z)-SP.inertia(y))
     &             * SvlLCS(ry,frm,calc_seg)*SvlLCS(rz,frm,calc_seg)
     &             - moment(x)
      temp_load(my)= SP.inertia(y)*SacLCS(ry,frm,calc_seg)
     &             + (SP.inertia(x)-SP.inertia(z))
     &             * SvlLCS(rx,frm,calc_seg)*SvlLCS(rz,frm,calc_seg)
     &             - moment(y)
      temp_load(mz)= SP.inertia(z)*SacLCS(rz,frm,calc_seg)
     &             + (SP.inertia(y)-SP.inertia(x))
     &             * SvlLCS(rx,frm,calc_seg)*SvlLCS(ry,frm,calc_seg)
     &             - moment(z)
c
c     Now remove the moment due to the joint reaction force about the segment
c     coordinate system.                              
c
      call VecX( SP.end(x,seg_jnt),temp_load(fx),add_moment )
      do axis= x, z
        temp_load(axis+3)= temp_load(axis+3) - add_moment(axis)
      end do
c
c     Tranform this joint load into the Global and Joint Coord Systems as
c     well as the LCS of the segment on the other side of the joint.  This
c     maybe distal or proximal relative to the joint- depending upon what
c     direction the calculation is progressing across the body.  Check the
c     JntSegNum(distal,jnt) to figure out which it is.
c
      if ( JntSegNum(distal,calc_jnt) .eq. calc_seg ) then
        do axis= fx, mz
          JrlDCS(axis,frm,calc_jnt)= temp_load(axis)
        end do ! axis  
        call loadLCStoLCS( SgpGCS(x,x,frm,JntSegNum(distal,calc_jnt)),
     &                     SgpGCS(x,x,frm,JntSegNum(proximal,calc_jnt)),
     &                     JrlDCS(fx,frm,calc_jnt),
     &                     JrlPCS(fx,frm,calc_jnt)  )
      else ! proximal segment relative to joint
        do axis= fx, mz
          JrlPCS(axis,frm,calc_jnt)= temp_load(axis)
        end do ! axis
        call LoadLCStoLCS( SgpGCS(x,x,frm,JntSegNum(proximal,calc_jnt)),
     &                     SgpGCS(x,x,frm,JntSegNum(distal,calc_jnt)),
     &                     JrlPCS(fx,frm,calc_jnt),
     &                     JrlDCS(fx,frm,calc_jnt)  )
      end if         
      call LoadLCStoGCS( SgpGCS(x,x,frm,JntSegNum(distal,calc_jnt)),
     &                   JrlDCS(fx,frm,calc_jnt),
     &                   JrlGCS(fx,frm,calc_jnt)       )
      call LoadGCStoJCS( JaxesJCS(x,x,frm,calc_jnt),
     &                   AngleSignJCS(1,calc_jnt),
     &                   TransSignJCS(1,calc_jnt),
     &                   JrlGCS(fx,frm,calc_jnt),         
     &                   JrlJCS(fx,frm,calc_jnt)  )
      return                                                                   
      end   
c 
c	LoadLCStoLCS                                
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Transform Joint resultant joint (JRL) from one segment
c				LCS to another segment LCS.
c      
c	NOTE that the transformation performed here is not a full transformation
c	of the load from LCS1 to LCS2 but rather a reexpression of the load
c	rotated parallel to LCS2.  Another way to view this is that the LCS's
c	both have the same origin (being the joint location in this case)
c	and therefore the transformation of the load from LCS1 to LCS2 is
c	merely a rotational transformation as a result.
c
c
c	Routines called:
c
c	MatTran
c	VecChangeCS
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 LoadLCStoLCS( LCS1, LCS2, load_LCS1, load_LCS2 )
      implicit none
              
      real*4 LCS1(3,3),LCS2(3,3),load_LCS1(6),load_LCS2(6)
                   
      integer*4 fx,mx
      real*4    temp_load(6),GCStoLCS(3,3)
                 
      data fx,mx / 1,4 /

      call VecChangeCS( LCS1,load_LCS1(fx),temp_load(fx) )
      call VecChangeCS( LCS1,load_LCS1(mx),temp_load(mx) )
      call MatTran( LCS2,GCStoLCS )
      call VecChangeCS( GCStoLCS,temp_load(fx),load_LCS2(fx) )
      call VecChangeCS( GCStoLCS,temp_load(mx),load_LCS2(mx) )
      return
      end
c 
c	LoadGCStoLCS                                
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Transform Joint resultant joint (JRL) from lab GCS
c				to segment LCS.
c
c	Routines called:
c
c	MatTran
c	VecChangeCS
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 LoadGCStoLCS( LCS, load_GCS, load_LCS )
      implicit none

      real*4 LCS(3,3),load_LCS(6),load_GCS(6)

      integer*4 fx,mx
      real*4    GCStoLCS(3,3)
      
      data fx,mx / 1,4 /

      call MatTran( LCS,GCStoLCS )
      call VecChangeCS( GCStoLCS,load_GCS(fx),load_LCS(fx) )
      call VecChangeCS( GCStoLCS,load_GCS(mx),load_LCS(mx) )
      return                 
      end        
c 
c	LoadLCStoGCS                                
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Transform Joint resultant joint (JRL) from segment LCS
c				to lab GCS
c
c	Routines called:
c
c	MatTran
c	VecChangeCS
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 LoadLCStoGCS( LCS, load_LCS, load_GCS )
      implicit none

      real*4 LCS(3,3),load_LCS(6),load_GCS(6)

      integer*4 fx,mx
      
      data fx,mx / 1,4 /

      call VecChangeCS( LCS,load_LCS(fx),load_GCS(fx) )
      call VecChangeCS( LCS,load_LCS(mx),load_GCS(mx) )
      return
      end
c 
c	LoadGCStoJCS                                
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Transform Joint resultant joint (JRL) from lab GCS
c				to joint coordinate system (JCS).  NOTE that this 
c				JCS is nonorthogonal so that components cannot be
c				transformed using the usual rotation matrix X vector
c				method.
c
c	Routines called:
c
c	VecDot
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 LoadGCStoJCS( JCS, MomSign, ForceSign, load_GCS, load_JCS )
      implicit none

      real*4    JCS(3,3),load_GCS(6),load_JCS(8),MomSign(3),ForceSign(3)
  
      integer*4 x,z,fx,mx,axis                               
      real*4    VecDot,VecMag

      data x,z,fx,mx / 1,3,1,4 /

      do axis= x, z
        load_JCS(axis)=   ForceSign(axis)*VecDot( JCS(x,axis),load_GCS(fx) )
        load_JCS(axis+3)= MomSign(axis)*VecDot( JCS(x,axis),load_GCS(mx) )
      end do
      load_JCS(7)= VecMag( load_GCS(fx) )
      load_JCS(8)= VecMag( load_GCS(mx) )
      return                                        
      end
c 
c	CalcJntWrench                                
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Calculate the load wrench representing the joint
c				resultant load.
c
c	Routines called:
c
c	CalcInstantScrewAxis
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Joint			JrlPCS
c	Param			JNT_PRM.jnt_num
c	Param			JNT_PRM.num_jnt
c	Param			MP.nfrm
c	Param			SP.end
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Joint			JwreLCS
c
      subroutine CalcJntWrench( JF,SF,OF )
      implicit none

      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_prm.inc'
      include 'a_jnt.inc'      
      include 'a_names.inc'
                 
      record /joint_flag/     JF
      record /segment_flag/   SF
      record /option_flag/    OF

      integer*4 i,j,jnt,frm,seg,JntProxSegNum(15),NumLeftJnts,LeftJntNum(5),
     &          side,axis  
      real*4    TempLoad(6)

      data NumLeftJnts / 5 /
      data LeftJntNum  / 4,5,6,11,12 /
      data JntProxSegNum / 2,3,7,5,6,7,0,7,9,8,11,8,8,10,12 /

      JF.wrench= .false.
      if ( .not. JF.load ) goto 1000 ! error
      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_jnt
          jnt= GP.jnt_num(i)
          if ( jnt .ne. pvlb ) then
            seg= JntProxSegNum(jnt)
            do j= 1, 3
              TempLoad(j)= JrlPCS(j+3,frm,jnt)
              TempLoad(j+3)= JrlPCS(j,frm,jnt)
            end do
            call CalcInstantScrewAxis( TempLoad,
     &                                 SP(seg).end(x,distal),
     &                                 JwreLCS(1,frm,jnt) )
          end if
        end do ! i => jnt
      end do ! frm
      JF.wrench= .true.
      return                                                 

1000  write(6,1005)
      if ( OF.ToLog ) write(3,1005)
1005  format(' Error- Joint loads have not been calculated yet')
      return
      end        
c 
c	CalcJntPower                                
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Calculate the net joint power transfer.  This is done
c				in 3D so that the individual components can be examined.
c				Also, the summed joint power transfers for the leg joints
c				on each side are calculated if possible.  I'm not exactly
c				sure if this last quantity is going to prove useful but
c				I threw it in nonetheless...  NOTE that due to noise, 
c				imprecision in the motion system, etc. a joint velocity
c				component of signifcant magnitude can show up in a 
c				direction which should have minimal movement, for instance
c				the ab/adduction rotation of the knee which should
c				normally be nearly constant.  As a result, the moments
c				in this direction which should be purely structural may
c				generate a significant amount of apparent power.  At
c				least that is what I have found in preliminary testing.
c				Your mileage may vary....  In all the published
c				gait works that I know of only the sagittal plane power
c				transfer is reported so I don't really know what should
c				be observed at those joints which do have nonsagittal
c				function, such as the ankle and hip.  Lastly, the 4th
c				component of the joint power is the total magnitude of
c				the transfered power using w*m.
c
c	Routines called:
c
c	VecDot
c	VecSub
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Joint			JntCalcOK
c	Joint			JrlGCS
c	Joint			JrlJCS
c	Joint			JntSegNum
c	Param			JNT_PRM.jnt_num
c	Param			JNT_PRM.num_jnt
c	Joint			JvelJCS
c	Param			MP.nfrm
c	Segment			SvlGCS
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Joint			JntPower
c	Joint			JntLegPower
c
      subroutine CalcJntPower( CMND,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 /CmndLine/       CMND
      record /joint_flag/     JF
      record /segment_flag/   SF
      record /option_flag/    OF
        
      integer*4 LegJnt(3,2),i,j,k,jnt,frm,seg,side
      real*4    sign,VecDot,pi,RelVel(3)
      logical*4 BadSide
        
      data LegJnt / 1,2,3,4,5,6 / ! rtak,rtkn,rthp,lfak,lfkn,lfhp
                 
      JF.power= .false.
c      if ( .not. SF.position ) goto 1000
c      if ( .not. SF.param )    goto 1010
      if ( .not. JF.load )     goto 1020     
c      if ( .not. SF.vel_acc )  goto 1030
      if ( .not. JF.vel_acc_JCS ) goto 1040 ! error
      pi= 4.0*atan(1.0)
      do frm= 1, MP.nfrm
        do i= 1, GP.num_jnt
          jnt= GP.jnt_num(i)
          if ( JntCalcOK(frm,jnt) ) then
            if ( jnt .ne. pvlb ) then                                  
              do j= 1, 3
                JntPower(j,frm,jnt)= JrlJCS(j+3,frm,jnt)*        
     &                               JvelJCS(j,frm,jnt)*(pi/180.)
              end do
              call VecSub( SvlGCS(wx,frm,JntSegNum(distal,jnt)),
     &                     SvlGCS(wx,frm,JntSegNum(proximal,jnt)),
     &                     RelVel )
              JntPower(4,frm,jnt)= VecDot( RelVel,JrlGCS(mx,frm,jnt) )
            else
              do j= 1, 4
                JntPower(j,frm,jnt)= 0.0
              end do
            end if
          end if ! jnt loads have been calculated
        end do ! i= jnt
        if ( JF.legsummed ) then ! sum leg powers
          do side= rt, lf
            BadSide= .false.
            do j= 1, 3
              if ( .not. JntCalcOK(frm,LegJnt(j,side)) ) BadSide= .true.
            end do
            if ( .not. BadSide ) then
              do j= 1, 4                          
                do k= 1, 3
                  JntPowerLeg(j,frm,side)= JntPowerLeg(j,frm,side) + 
     &                                     JntPower(j,frm,LegJnt(k,side))
                end do
              end do
            else
              do j= 1, 4
                JntPowerLeg(j,frm,side)= 0.0
              end do
            end if ! side OK
          end do ! side
        end if ! legsummed true...
      end do ! frm
      JF.power= .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- Joint reaction load has not been calculated.')
      return                                  
      
1030  write(6,1035)                         
      if ( OF.ToLog ) write(3,1035)
1035  format('Error- Segment velocity has not been calculated.')
      return

1040  write(6,1045)                         
      if ( OF.ToLog ) write(3,1045)
1045  format('Error- JCS joint velocity has not been calculated.')
      return

1050  write(6,1055) CMND.OptText(1,2)
      if ( OF.ToLog ) write(3,1055) CMND.OptText(1,2)
1055  format('Error- ',a3,' is not a valid option for JNT POW'/
     &       '        The only valid option is CORrectmethod')
      return
      end                                
c                                        
c	FilterJnt
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Provide digitial filtering of joint 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 a 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 jcs angle, jcs vel/acc, joint load, and joint power
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_jnt
c	Param			GP.jnt_num
c	Param			MP.nfrm
c	Param			rMtnFrmRate
c	Names			jnt_names
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Joint			JRLegGCS
c	Joint			JRLegJCS
c	Joint			JntPower
c	Joint			JntPowerLeg
c	Joint			JaccEUL
c	Joint			JaccJCS
c	Joint			JangEUL
c	Joint			JangJCS
c	Joint			JrlPCS
c	Joint			JrlGCS
c	Joint			JrlJCS
c	Joint			JrlDCS
c	Joint			JvelEUL
c	Joint			JvelJCS
c
      subroutine FilterJnt( CMND,JF,OF )
      implicit none                       

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

      record /CmndLine/    CMND
      record /joint_flag/  JF
      record /option_flag/ OF         

      real*4    smplfreq,Filter(1024),TempData(MAX_MTN),sf,hfr,lfr
      integer*4 JntVarNum,i,j,k,n,NumFilPnts,NumBeginZero,NumEndZero,jnt,axis,
     &          side,load,ftype,tcvorder,optnum,ierr,NumBlank
      logical*4 JntVarFound,DoWindow,Error,UseTCV,BlankEnds
      character JntVarName(6)*3,AxisName(3)*4,LoadName(6)*3

      data JntVarName / 'JAN','JVA','EAN','EVA','LOA','POW' /
      data AxisName / 'flex','abad','inex' /
      data LoadName / 'fem','aam','iem','mlf','apf','cdf' /
c
c     check whether a proper joint variable has been specified to be filtered
c
      JntVarFound= .false.
      JntVarNum= 0
      Do while ( (JntVarNum .le. 5) .and. (.not. JntVarFound) )
        JntVarNum= JntVarNum + 1
        if ( CMND.CmndText(3) .eq. JntVarName(JntVarNum) ) JntVarFound= .true.
      end do
      if ( .not. JntVarFound ) goto 1000 ! error
      if ( (JntVarNum .eq. 1) .and. (.not. JF.angle_JCS) )   goto 1010 ! error
      if ( (JntVarNum .eq. 2) .and. (.not. JF.vel_acc_JCS) ) goto 1020 ! error
      if ( (JntVarNum .eq. 3) .and. (.not. JF.angle_EUL) )   goto 1030 ! error
      if ( (JntVarNum .eq. 4) .and. (.not. JF.vel_acc_EUL) ) goto 1040 ! error
      if ( (JntVarNum .eq. 5) .and. (.not. JF.load) )        goto 1080 ! error
      if ( (JntVarNum .eq. 6) .and. (.not. JF.power) )       goto 1090 ! error
      if ( rMtnFrmRate .lt. 1.0e-5 ) goto 1050 ! error  
      if ( MP.nfrm .lt. 2 )       goto 1060 ! error
c
c     Check whether time convolution filter will be used
c
      UseTCV= .false.
      do i= 1, CMND.NumOpt(2)
        if ( CMND.OptText(i,2) .eq. 'TCV' ) then
          UseTCV= .true.
          optnum= i
        end if                                                
      end do
      if ( UseTCV ) call RemoveCMNDOpt( CMND,2,optnum )
c
c     Setup digital filter if not using the convolution filter
c
      if ( .not. UseTCV ) then 
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 size by *2 just in case
        if ( i .gt. 11 ) goto 1070 ! error- too many samples, must be <=2048
        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,BlankEnds,NumBlank,Error,OF )
        if ( Error ) return ! error
c
c     Setup convolution filter if not using digital filter
c
      else
        sf= rMtnFrmRate
        lfr= 0.0
        hfr= 0.0                                             
        ftype= 0
        tcvorder= 1
        do i= 1, CMND.NumOpt(2)
          call CheckTcvOpt( CMND,2,i,lfr,hfr,tcvorder,ftype,
     &                      OF.ToLog,error )
          if ( error ) goto 2030
        end do
      end if
c                     
c     Do filtering of joint variable
c                                              
      do i= 1, GP.num_jnt                      
        jnt= GP.jnt_num(i)
        if ( JntVarNum .le. 4 ) then
          do axis= flex, inex               
            if ( OF.UpDate ) write(6,100) jnt,GP.num_jnt,Jnt_Names(jnt),AxisName(axis)
100         format('+ Filtering joint # ',i2,'/',i2,' ',a4,' ',a4)
c                                                               
c           Filter the proper joint variable quantity
c                                                      
            if ( JntVarNum .eq. 1 ) then ! filter JCS angles
              do j= 1, MP.nfrm              
                TempData(j)= JangJCS(axis,j,jnt)
              end do
              call FilterDataSet( UseTCV,TempData,n,NumFilPnts,
     &                            NumBeginZero,NumEndZero,Filter,DoWindow,
     &                            BlankEnds,NumBlank,sf,lfr,hfr,ftype,
     &                            tcvorder,ierr)
              if ( ierr .gt. 0 ) goto 3000
              do j= 1, MP.nfrm
                JangJCS(axis,j,jnt)= TempData(j)
              end do                            
              
            else if ( JntVarNum .eq. 2 ) then ! filter JCS vel and accel
              do j= 1, MP.nfrm
                TempData(j)= JvelJCS(axis,j,jnt)
              end do
              call FilterDataSet( UseTCV,TempData,n,NumFilPnts,
     &                            NumBeginZero,NumEndZero,Filter,DoWindow,
     &                            BlankEnds,NumBlank,sf,lfr,hfr,ftype,
     &                            tcvorder,ierr)
              if ( ierr .gt. 0 ) goto 3000
              do j= 1, MP.nfrm
                JvelJCS(axis,j,jnt)= TempData(j)
              end do
              do j= 1, MP.nfrm
                TempData(j)= JaccJCS(axis,j,jnt)
              end do
              call FilterDataSet( UseTCV,TempData,n,NumFilPnts,
     &                            NumBeginZero,NumEndZero,Filter,DoWindow,
     &                            BlankEnds,NumBlank,sf,lfr,hfr,ftype,
     &                            tcvorder,ierr)
              if ( ierr .gt. 0 ) goto 3000
              do j= 1, MP.nfrm
                JaccJCS(axis,j,jnt)= TempData(j)
              end do
              
            else if ( JntVarNum .eq. 3 ) then ! filter EUL angles
              do j= 1, MP.nfrm              
                TempData(j)= JangEUL(axis,j,jnt)
              end do
              call FilterDataSet( UseTCV,TempData,n,NumFilPnts,
     &                            NumBeginZero,NumEndZero,Filter,DoWindow,
     &                            BlankEnds,NumBlank,sf,lfr,hfr,ftype,
     &                            tcvorder,ierr)
              if ( ierr .gt. 0 ) goto 3000
              do j= 1, MP.nfrm
                JangEUL(axis,j,jnt)= TempData(j)
              end do
            else if ( JntVarNum .eq. 4 ) then ! filter JCS vel and accel
              do j= 1, MP.nfrm
                TempData(j)= JvelEUL(axis,j,jnt)
              end do
              call FilterDataSet( UseTCV,TempData,n,NumFilPnts,
     &                            NumBeginZero,NumEndZero,Filter,DoWindow,
     &                            BlankEnds,NumBlank,sf,lfr,hfr,ftype,
     &                            tcvorder,ierr)
              if ( ierr .gt. 0 ) goto 3000
              do j= 1, MP.nfrm
                JvelEUL(axis,j,jnt)= TempData(j)
              end do
              do j= 1, MP.nfrm
                TempData(j)= JaccEUL(axis,j,jnt)
              end do
              call FilterDataSet( UseTCV,TempData,n,NumFilPnts,
     &                            NumBeginZero,NumEndZero,Filter,DoWindow,
     &                            BlankEnds,NumBlank,sf,lfr,hfr,ftype,
     &                            tcvorder,ierr)
              if ( ierr .gt. 0 ) goto 3000
              do j= 1, MP.nfrm
                JaccEUL(axis,j,jnt)= TempData(j)
              end do                    
            end if
          end do ! axis    
                                   
        else if ( JntVarNum .eq. 5 ) then ! joint loads
          do load= fem, cdf
            if ( OF.UpDate ) write(6,103) jnt,GP.num_jnt,Jnt_Names(jnt),LoadName(load)
103         format('+ Filtering joint # ',i2,'/',i2,' ',a4,' ',a4)
            do j= 1, MP.nfrm
              TempData(j)= JrlJCS(load,j,jnt)
            end do
              call FilterDataSet( UseTCV,TempData,n,NumFilPnts,
     &                            NumBeginZero,NumEndZero,Filter,DoWindow,
     &                            BlankEnds,NumBlank,sf,lfr,hfr,ftype,
     &                            tcvorder,ierr)
              if ( ierr .gt. 0 ) goto 3000
            do j= 1, MP.nfrm
              JrlJCS(load,j,jnt)= TempData(j)
            end do
            do j= 1, MP.nfrm
              TempData(j)= JrlGCS(load,j,jnt)
            end do
              call FilterDataSet( UseTCV,TempData,n,NumFilPnts,
     &                            NumBeginZero,NumEndZero,Filter,DoWindow,
     &                            BlankEnds,NumBlank,sf,lfr,hfr,ftype,
     &                            tcvorder,ierr)
              if ( ierr .gt. 0 ) goto 3000
            do j= 1, MP.nfrm
              JrlGCS(load,j,jnt)= TempData(j)
            end do
            do j= 1, MP.nfrm
              TempData(j)= JrlPCS(load,j,jnt)
            end do
              call FilterDataSet( UseTCV,TempData,n,NumFilPnts,
     &                            NumBeginZero,NumEndZero,Filter,DoWindow,
     &                            BlankEnds,NumBlank,sf,lfr,hfr,ftype,
     &                            tcvorder,ierr)
              if ( ierr .gt. 0 ) goto 3000
            do j= 1, MP.nfrm
              JrlPCS(load,j,jnt)= TempData(j)
            end do
            do j= 1, MP.nfrm
              TempData(j)= JrlDCS(load,j,jnt)
            end do
              call FilterDataSet( UseTCV,TempData,n,NumFilPnts,
     &                            NumBeginZero,NumEndZero,Filter,DoWindow,
     &                            BlankEnds,NumBlank,sf,lfr,hfr,ftype,
     &                            tcvorder,ierr)
              if ( ierr .gt. 0 ) goto 3000
            do j= 1, MP.nfrm
              JrlDCS(load,j,jnt)= TempData(j)
            end do
          end do ! load
          
        else ! JntVarNum = 6 - joint power
          if ( OF.UpDate ) write(6,104) jnt,GP.num_jnt,Jnt_Names(jnt)
104       format('+ Filtering joint # ',i2,'/',i2,' ',a4)
          do k= 1, 4
            do j= 1, MP.nfrm
              TempData(j)= JntPower(k,j,jnt)
            end do
              call FilterDataSet( UseTCV,TempData,n,NumFilPnts,
     &                            NumBeginZero,NumEndZero,Filter,DoWindow,
     &                            BlankEnds,NumBlank,sf,lfr,hfr,ftype,
     &                            tcvorder,ierr)
              if ( ierr .gt. 0 ) goto 3000
            do j= 1, MP.nfrm
              JntPower(k,j,jnt)= TempData(j)
            end do
          end do ! k
        end if
      end do ! jnt     
                                            
      if ( (JntVarNum .eq. 5) .and. (JF.legsummed) ) then
        do side= rt, lf
          do load= fem, cdf
            do j= 1, MP.nfrm
              TempData(j)= JRLegJCS(load,j,side)
            end do
            call FilterDataSet( UseTCV,TempData,n,NumFilPnts,
     &                          NumBeginZero,NumEndZero,Filter,DoWindow,
     &                          BlankEnds,NumBlank,sf,lfr,hfr,ftype,
     &                          tcvorder,ierr)
            if ( ierr .gt. 0 ) goto 3000
            do j= 1, MP.nfrm
              JRLegJCS(load,j,side)= TempData(j)
            end do
            do j= 1, MP.nfrm
              TempData(j)= JRLegGCS(load,j,side)
            end do
            call FilterDataSet( UseTCV,TempData,n,NumFilPnts,
     &                          NumBeginZero,NumEndZero,Filter,DoWindow,
     &                          BlankEnds,NumBlank,sf,lfr,hfr,ftype,
     &                          tcvorder,ierr)
              if ( ierr .gt. 0 ) goto 3000
            do j= 1, MP.nfrm
              JRLegGCS(load,j,side)= TempData(j)
            end do
          end do ! load
        end do ! side
      end if
        
      if ( (JntVarNum .eq. 6) .and. (JF.legsummed) ) then
        do side= rt, lf
          do k= 1, 4
            do j= 1, MP.nfrm
              TempData(j)= JntPowerLeg(k,j,side)
            end do
            call FilterDataSet( UseTCV,TempData,n,NumFilPnts,
     &                          NumBeginZero,NumEndZero,Filter,DoWindow,
     &                          BlankEnds,NumBlank,sf,lfr,hfr,ftype,
     &                          tcvorder,ierr)
              if ( ierr .gt. 0 ) goto 3000
            do j= 1, MP.nfrm
              JntPowerLeg(k,j,side)= TempData(j)
            end do
          end do ! k 
        end do ! side
      end if
      if ( JF.NumProcOps .le. 4 ) then
        JF.NumProcOps= JF.NumProcOps + 1
        JF.ProcOp(JF.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 joint variable that can be filtered'/
     &       '         Valid variables are: JAN JVA EAN EVA')
      return
      
1010  write(6,1015) 
      if ( OF.ToLog ) write(3,1015) 
1015  format(' Error- JCS Angles cannot be filtered because they have not'/
     &       '        been calculated yet.')
      return
                           
1020  write(6,1025) 
      if ( OF.ToLog ) write(3,1025) 
1025  format(' Error- JCS vel/accel cannot be filtered because they have not'/
     &       '        been calculated yet.')
      return
      
1030  write(6,1035) 
      if ( OF.ToLog ) write(3,1035) 
1035  format(' Error- Euler Angles cannot be filtered because they have not'/
     &       '        been calculated yet.')
      return
      
1040  write(6,1045) 
      if ( OF.ToLog ) write(3,1045) 
1045  format(' Error- Euler vel/accel cannot be filtered because they have not'/
     &       '        been calculated yet.')
      return
      
1050  write(6,1055) rMtnFrmRate
      if ( OF.ToLog ) write(3,1055) rMtnFrmRate
1055  format(' Error- Sample rate is ',f10.6,' Hz which is too small.'/
     &       '        Something has gone wrong in reading the data file')
      return                
      
1060  write(6,1065) MP.nfrm
      if ( OF.ToLog ) write(3,1065) MP.nfrm
1065  format(' Error- There are only ',i4,' samples of data for each joint'/
     &       '        There must be at least 2 samples to perform filtering')
      return
                            
1070  write(6,1075) MP.nfrm
      if ( OF.ToLog ) write(3,1075) MP.nfrm
1075  format(' Error- There are ',i6,' samples per joint, but at most'/
     &       '        512 samples/joint can be filtered')
      return    

1080  write(6,1085) 
      if ( OF.ToLog ) write(3,1085) 
1085  format(' Error- Joint loads cannot be filtered because they have not'/
     &       '        been calculated yet.')
      return

1090  write(6,1095) 
      if ( OF.ToLog ) write(3,1095) 
1095  format(' Error- Joint powers cannot be filtered because they have not'/
     &       '        been calculated yet.')
      return

2030  write(6,2035) CMND.OptText(i,2)
      if ( OF.ToLog ) write(3,2035) CMND.OptText(i,2)
2035  format(' Error - ',a3,' is not a valid time convolution filter option'/
     &       '         Valid options are: /LOW=#  /HIGh=#  /ORDer=#'/
     &       '                            /BUTterworth /CHEbeyschev')
      return                        

3000  write(6,3005)
      if ( OF.ToLog ) write(3,3005) 
3005  format(' Error- Convolution filter failed')
      call conv_fltr_err( ierr,OF.ToLog )
      return
      end          
c                        
c
c                                         
c
      subroutine FilterDataSet( UseTCV,Data,n,NumFilPnts,
     &                          NumBeginZero,NumEndZero,Filter,DoWindow,
     &                          BlankEnds,NumBlank,
     &                          sf,lf,hf,ftype,tcvorder,ierr)
      implicit none
                   
      integer*4 n,NumFilPnts,NumBeginZero,NumEndZero,ftype,tcvorder,ierr,
     &          NumBlank
      real*4    Data(*),Filter(*),sf,lf,hf
      logical*4 DoWindow,UseTCV,BlankEnds

      integer*4 conv_fltr

      if ( UseTCV ) then
        ierr= conv_fltr( data,n,sf,lf,hf,ftype,tcvorder )
      else
        call DigFilterDataSet( Data,n,NumFilPnts,NumBeginZero,NumEndZero,
     &                         Filter,DoWindow,BlankEnds,NumBlank )        
        ierr= 0
      endif
      return
      end                
c                                        
c	CalcJntTranslation           
c                  
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Calculate the translation between the joint ends as
c				defined by the constant positin from the segment LCS's
c				on either side of the joint and the segment position/
c				orientation in the lab GCS.  If the joint were perfectly
c				revolute or spherical and the markers attached to the
c				segments were perfectly measured then the translation
c				would be zero.  Of course, this is planet Earth which 
c				once was and again will be, but for now is definitely 
c				not perfect.  As a result, this calculation functions
c				as a quick evaluation of the assumptions about joint
c				location and marker to segment position transformation.
c				I've found this to be most interesting...
c
c	Routines called:
c
c	MatTran
c	PntChangeCS
c	VecChangeCS
c	VecDot
c	VecMag
c	VecSub
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Param			GP.num_jnt
c	Param			GP.jnt_num
c	Joint			JaxesJCS
c	Names			jnt_names
c	Joint			JntSegNum
c	Param			MP.nfrm
c	Segment			SgpGCS
c	Param			SP.end
c	Joint			TransSignJCS
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Joint			JtrnDCS
c	Joint			JtrnGCS
c	Joint			JtrnJCS
c	Joint			JtrnPCS
c
      Subroutine CalcJntTranslation( 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 i,j,jnt,ProxSeg,DistSeg,frm,axis,ProxSegEndNum,DistSegEndNum
      real*4    ProxVecGCS(3),DistVecGCS(3),VecMag,GCStoLCS(3,3),VecDot

      if ( .not. SF.param )     goto 1000 ! error
      if ( .not. JF.angle_JCS ) goto 1010 ! error 
      if ( .not. SF.position )  goto 1020 ! error
      do i= 1, GP.num_jnt
        jnt= GP.jnt_num(i)
        if ( jnt .ne. pvlb ) then
          if ( OF.UpDate ) write(6,101) i,GP.num_jnt,Jnt_Names(jnt)
101       format('+Processing joint # ',i3,'/',i3,' : ',a4)
          do frm= 1, MP.nfrm
            ProxSeg= JntSegNum(proximal,jnt)
            DistSeg= JntSegNum(distal,jnt)
            ProxSegEndNum= LocalSegEndJntNum(proximal,jnt)
            DistSegEndNum= LocalSegEndJntNum(distal,jnt)
            call PntChangeCS( SgpGCS(x,x,frm,ProxSeg),
     &                        SP(ProxSeg).End(x,ProxSegEndNum),
     &                        ProxVecGCS )
            call PntChangeCS( SgpGCS(x,x,frm,DistSeg),
     &                        SP(DistSeg).End(x,DistSegEndNum),
     &                        DistVecGCS )
            call VecSub( DistVecGCS,ProxVecGCS,JtrnGCS(x,frm,jnt) )
            JtrnGCS(4,frm,jnt)= VecMag( JtrnGCS(x,frm,jnt) )
            call MatTran( SgpGCS(x,x,frm,DistSeg),GCStoLCS )
            call VecChangeCS( GCStoLCS,JtrnGCS(x,frm,jnt),
     &                        JtrnDCS(x,frm,jnt) )
            call MatTran( SgpGCS(x,x,frm,ProxSeg),GCStoLCS )
            call VecChangeCS( GCStoLCS,JtrnGCS(x,frm,jnt),
     &                        JtrnPCS(x,frm,jnt) )
            do axis= x, z
              JtrnJCS(axis,frm,jnt)= TransSignJCS(axis,jnt)*
     &                                   VecDot( JaxesJCS(x,axis,frm,jnt),
     &                                           JtrnGCS(x,frm,jnt) )
            end do                                       
            JtrnJCS(4,frm,jnt)= JtrnGCS(4,frm,jnt)
            JtrnPCS(4,frm,jnt)= JtrnGCS(4,frm,jnt)
            JtrnDCS(4,frm,jnt)= JtrnGCS(4,frm,jnt)
          end do ! frm 
        else ! jnt = pvlb
          do frm= 1, MP.nfrm
            do j= 1, 4
              JtrnGCS(j,frm,jnt)= 0.0
              JtrnJCS(j,frm,jnt)= 0.0
              JtrnPCS(j,frm,jnt)= 0.0
              JtrnDCS(j,frm,jnt)= 0.0
            end do ! j
          end do ! frm
        end if
      end do ! i => jnt
      JF.Translation= .true.                                              
      return           

1000  write(6,1005) 
      if ( OF.ToLog ) write(3,1005) 
1005  format(' Error- Joint translations can not be calculated because the'/
     &       '        segment parameters (SEG PRM) have not been calculated yet')
      return

1010  write(6,1005) 
      if ( OF.ToLog ) write(3,1015) 
1015  format(' Error- Joint translations can not be calculated because the'/
     &       '        joint JCS angles (JNT JAN) have not been calculated yet')
      return

1020  write(6,1025) 
      if ( OF.ToLog ) write(3,1025) 
1025  format(' Error- Joint translations can not be calculated because the'/
     &       '        segment positions (SEG POS) have not been calculated yet')
      return
      end                                          
c                                        
c	CalcJntScrewAngle
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Calculate the joint srew angle and express it relative
c				to several coordinate systems.  This is basically a
c				scaled vector along the joint finite screw axis having
c				a magnitude of the screw rotation and then just expressed
c				several different ways.  This was done to look at the
c				applicability of using this rather any of the different
c				euler angle expressions fro relative to segment orientation.
c				Near as I know, in the gait field Herman Woltring
c				originated the idea of using this technique.  It is
c				included here so that I could experiment with it. Do
c				with it what you may...   (P.S. it works but I don't
c				really see a reason to use it right now)
c
c	Routines called:
c
c	VecChangeCS
c	VecDot
c	VecLCS1toLCS2
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Joint			AngleSignJCS
c	Param			GP.num_jnt
c	Param			GP.jnt_num
c	Joint			JaxesJCS
c	Names			jnt_names
c	Joint			JntSegNum
c	Param			MP.nfrm
c	Joint			JFinScr
c	Segment			SgpGCS
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Joint			JfsAngDCS
c	Joint			JfsAngJCS
c	Joint			JfsAngPCS
c
      Subroutine CalcJntScrewAngle( 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 /joint_flag/    JF        
      record /option_flag/   OF

      integer*4 i,j,jnt,frm,ProxSeg,DistSeg
      real*4    TempVecGCS(3),VecDot,ProxDistSign(3,15)

      data ProxDistSign / 1,-1, 1,  1, 1, 1,  1,-1, 1,  ! rtak rtkn rthp
     &                   -1,-1,-1, -1, 1,-1, -1,-1,-1,  ! lfak lfkn lfhp
     &                    1, 1, 1,  1, 1, 1,            ! pvlb pvtk
     &                    1,-1, 1,  1,-1, 1,            ! rtsh rtel
     &                   -1,-1,-1, -1,-1,-1,            ! lfsh lfel
     &                    1, 1, 1,  1,-1, 1, -1,-1,-1 / ! neck rtwr lfwr

      if ( .not. JF.FiniteScrew )  goto 1000 ! error
      if ( .not. JF.angle_JCS )    goto 1010 ! error  
      do i= 1, GP.num_jnt
        jnt= GP.jnt_num(i)
        ProxSeg= JntSegNum(proximal,jnt)     
        DistSeg= JntSegNum(distal,jnt)
        if ( jnt .ne. pvlb ) then
          if ( OF.UpDate ) write(6,101) i,GP.num_jnt,Jnt_Names(jnt)
101       format('+Processing joint # ',i3,'/',i3,' : ',a4)
          do frm= 1, MP.nfrm
            do j= 1, 3
              JfsAngPCS(j,frm,jnt)= JFinScr(j,frm,jnt)*
     &                                     JFinScr(7,frm,jnt)
            end do
            call VecChangeCS( SgpGCS(x,x,frm,ProxSeg),
     &                        JfsAngPCS(x,frm,jnt),TempVecGCS )
            do j= 1, 3
              JfsAngJCS(j,frm,jnt)= AngleSignJCS(j,jnt)*
     &                                 VecDot( JaxesJCS(x,j,frm,jnt),
     &                                         TempVecGCS )
            end do
            call VecLCS1toLCS2( SgpGCS(x,x,frm,ProxSeg),
     &                          SgpGCS(x,x,frm,DistSeg),
     &                          JfsAngPCS(x,frm,jnt),
     &                          JfsAngDCS(x,frm,jnt)  )
            do j= 1, 3
              JfsAngPCS(j,frm,jnt)= ProxDistSign(j,jnt)*
     &                                     JfsAngPCS(j,frm,jnt)
              JfsAngDCS(j,frm,jnt)= ProxDistSign(j,jnt)*
     &                                     JfsAngDCS(j,frm,jnt)
            end do
          end do ! frm
        else
          do frm= 1, MP.nfrm
            do j= 1, 3
              JfsAngPCS(j,frm,jnt)= SgFinScrGCS(j,frm,pelv)*
     &                                     SgFinScrGCS(7,frm,pelv)
              JfsAngJCS(j,frm,jnt)= JfsAngPCS(j,frm,jnt)
              JfsAngPCS(j,frm,jnt)= JfsAngPCS(j,frm,jnt)
            end do
          end do
        end if ! jnt = pvlb
      end do ! i => jnt
      JF.ScrewAngle= .true.
      return

1000  write(6,1005)                                        
      if ( OF.ToLog ) write(3,1005)    
1005  format(' Error- Joint Screw Angles can not be calculated because the'/
     &       '        joint finite screws (SEG FSC) have not been calculated yet')
      return

1010  write(6,1005) 
      if ( OF.ToLog ) write(3,1015) 
1015  format(' Error- Joint Screw Angles can not be calculated because the'/
     &       '        joint JCS angles (JNT JAN) have not been calculated yet')
      return
      end
c                                        
c	CalcJntMomentArm
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Calculate the joint moment arms for the 3D joint
c				resultant loads.
c
c	Routines called:
c
c	VecMag
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Param			GP.num_jnt
c	Param			GP.jnt_num
c	Joint			JntCalcOK
c	Joint			JrlDCS
c	Joint			JrlGCS
c	Joint			JrlPCS
c	Names			jnt_names
c	Param			MP.nfrm
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Joint			JmaDCS
c	Joint			JmaGCS
c	Joint			JmaPCS
c
      subroutine CalcJntMomentArm( 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 /joint_flag/    JF        
      record /option_flag/   OF

      integer*4 i,j,frm,jnt,MomAxes(2,3)
      real*4    TempFrc,TempMom,VecMag

      data MomAxes / 2,3, 1,3, 1,2 /

      if ( .not. JF.load ) goto 1000
      do frm= 1, MP.nfrm
        do i= 1, GP.num_jnt
          jnt= GP.jnt_num(i)
          if ( JntCalcOK(frm,jnt) ) then
            do j= x, z
              TempFrc= sqrt( JrlGCS(MomAxes(1,j),frm,jnt)**2 +
     &                       JrlGCS(MomAxes(2,j),frm,jnt)**2   )
              if ( TempFrc .lt. 1.0e-5 ) then
                JmaGCS(j,frm,jnt)= 0.0
              else
                JmaGCS(j,frm,jnt)= abs(JrlGCS(j+3,frm,jnt))/TempFrc
              end if
              TempFrc= sqrt( JrlPCS(MomAxes(1,j),frm,jnt)**2 +
     &                       JrlPCS(MomAxes(2,j),frm,jnt)**2   )
              if ( TempFrc .lt. 1.0e-5 ) then
                JmaPCS(j,frm,jnt)= 0.0
              else
                JmaPCS(j,frm,jnt)= abs(JrlPCS(j+3,frm,jnt))/
     &                                       TempFrc
              end if
              TempFrc= sqrt( JrlDCS(MomAxes(1,j),frm,jnt)**2 +
     &                       JrlDCS(MomAxes(2,j),frm,jnt)**2   )
              if ( TempFrc .lt. 1.0e-5 ) then
                JmaDCS(j,frm,jnt)= 0.0
              else
                JmaDCS(j,frm,jnt)= abs(JrlDCS(j+3,frm,jnt))/
     &                                       TempFrc
              end if
              TempFrc= VecMag(JrlGCS(1,frm,jnt))
              TempMom= VecMag(JrlGCS(4,frm,jnt))
              if ( TempFrc .lt. 1.0e-5 ) then
                JmaGCS(4,frm,jnt)= 0.0
              else
                JmaGCS(4,frm,jnt)= TempMom/TempFrc
              end if
            end do ! j
          else
            do j= 1, 3
              JmaGCS(j,frm,jnt)= 0.0
              JmaPCS(j,frm,jnt)= 0.0
              JmaDCS(j,frm,jnt)= 0.0
            end do
            JmaGCS(4,frm,jnt)= 0.0
          end if ! JntCalcOK
        end do ! i
      end do ! frm
      JF.MomentArm= .true.
      return

1000  write(6,1005)
      if ( OF.ToLog ) write(3,1005)
1005  format( ' Error- The joint resultant loads have not been calculated yet')
      return
      end
c                                        
c	CalcJntWrenchArm
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Calculate the joint wrench  arms for the 3D joint
c				resultant loads.  This is essentially the perpendicular
c				distance from the joint to the line of action of the
c				wrench axis.
c
c	Routines called:
c
c	PerpenPtLine
c	VecChangeCS
c	VecLCS1toLCS2
c	VecMag
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Param			GP.num_jnt
c	Param			GP.jnt_num
c	Joint			JntCalcOK
c	Joint			JntSegNum
c	Joint			JwreLCS
c	Param			MP.nfrm
c	Segment			SgpGCS
c	Param			SP.End
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Joint			JwaDCS
c	Joint			JwaGCS
c	Joint			JwaPCS
c
      subroutine CalcJntWrenchArm( 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 /joint_flag/    JF        
      record /option_flag/   OF

      integer*4 i,j,frm,jnt,ProxSeg,DistSeg
      real*4    VecMag,ZeroVec(3)

      data ZeroVec / 0.0,0.0,0.0 /

      if ( .not. JF.wrench ) goto 1000
      do frm= 1, MP.nfrm
        do i= 1, GP.num_jnt
          jnt= GP.jnt_num(i)
          if ( JntCalcOK(frm,jnt) ) then
            ProxSeg= JntSegNum(proximal,jnt)
            DistSeg= JntSegNum(distal,jnt)
            call PerpenPtLine( JwreLCS(4,frm,jnt),
     &                         JwreLCS(1,frm,jnt),
     &                         SP(ProxSeg).End(x,Distal),
     &                         JwaPCS(1,frm,jnt) )
            call VecLCS1toLCS2( SgpGCS(1,1,frm,ProxSeg),
     &                          SgpGCS(1,1,frm,DistSeg),
     &                          JwaPCS(1,frm,jnt),
     &                          JwaDCS(1,frm,jnt) )
            call VecChangeCS( SgpGCS(1,1,frm,ProxSeg),
     &                        JwaPCS(1,frm,jnt),
     &                        JwaGCS(1,frm,jnt)      )
            JwaGCS(4,frm,jnt)= VecMag(JwaGCS(1,frm,jnt))
          else                                               
            do j= 1, 3
              JwaGCS(j,frm,jnt)= 0.0
              JwaPCS(j,frm,jnt)= 0.0
              JwaDCS(j,frm,jnt)= 0.0
            end do
            JwaGCS(4,frm,jnt)= 0.0
          end if
        end do ! i
      end do ! frm
      JF.WrenchArm= .true.
      return            

1000  write(6,1005)
      if ( OF.ToLog ) write(3,1005)
1005  format( ' Error- The joint resultant wrenchs have not been calculated yet')
      return
      end
c
c
c
      subroutine CalcJexment( JF,FF,OF )
      implicit none

      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_prm.inc'
      include 'a_jnt.inc'
      include 'a_gls2.inc'
      include 'a_frc.inc'
      include 'a_names.inc'

      record /joint_flag/    JF        
      record /force_flag/    FF        
      record /option_flag/   OF

      write(6,100)
100   format(' Sorry- This calculation is not available yet')
      return
      end
c
c
c
      subroutine CalcJntWork( JF,OF )
      implicit none

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

      record /joint_flag/    JF        
      record /option_flag/   OF

      write(6,100)
100   format(' Sorry- This calculation is not available yet')
      return
      end
c
c
c
      subroutine CalcJrlAng( JF,OF )
      implicit none

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

      record /joint_flag/    JF        
      record /option_flag/   OF

      write(6,100)
100   format(' Sorry- This calculation is not available yet')
      return                 
      end
c
c
c
      subroutine CalcDynamJcn( CMND,JF,OF )
      implicit none

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

      record /CmndLine/      CMND
      record /joint_flag/    JF        
      record /option_flag/   OF

      write(6,100)           
100   format(' Sorry- This calculation is not available yet')
      return
      end
c
c
c
      subroutine CalcAvgJcn( CMND,JF,OF )
      implicit none

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

      record /CmndLine/      CMND
      record /joint_flag/    JF        
      record /option_flag/   OF

      write(6,100)           
100   format(' Sorry- This calculation is not available yet')
      return
      end
