c
c
c
      subroutine InitMayoAnalysis(ReStart)
      implicit none
      include 'a_const.inc'

      include 'a_struct.inc'
      include 'a_prm.inc'
      include 'a_names.inc'
      include 'a_mrk.inc'
      include 'a_seg.inc'                       
      include 'a_jnt.inc'
      include 'a_gls2.inc'
      include 'a_frc.inc'
      include 'a_mayo.inc'

      integer*4 i          
      logical*4 ReStart
c
c     Set up marker info
c
c                          
c     initialize marker names
c
      MMA_RTSH= 1
      MMA_LFSH= 2
      MMA_RTEL= 3
      MMA_LFEL= 4 
      MMA_RTWR= 5
      MMA_LFWR= 6
      MMA_SACR= 7
      MMA_RTAS= 8
      MMA_LFAS= 9            
      MMA_RTTU= 10     
      MMA_LFTU= 11
      MMA_RTTF= 12
      MMA_LFTF= 13
      MMA_RTTR= 14
      MMA_LFTR= 15
      MMA_RTSU= 16
      MMA_LFSU= 17
      MMA_RTSF= 18
      MMA_LFSF= 19
      MMA_RTSR= 20
      MMA_LFSR= 21
      MMA_RTHL= 22
      MMA_LFHL= 23
      MMA_RTTO= 24
      MMA_LFTO= 25
      MMA_RTMF= 26      ! added so that foot has 3 markers
      MMA_LFMF= 27      ! added so that foot has 3 markers
      MMA_RTAK= 28
      MMA_LFAK= 29
      MMA_RTKN= 30
      MMA_LFKN= 31
      MMA_RTHP= 32
      MMA_LFHP= 33

      if ( ReStart ) then
        num_init_empty_mrk= 6
        init_empty_mrk(1)= MMA_RTAK
        init_empty_mrk(2)= MMA_LFAK
        init_empty_mrk(3)= MMA_RTKN
        init_empty_mrk(4)= MMA_LFKN
        init_empty_mrk(5)= MMA_RTHP
        init_empty_mrk(6)= MMA_LFHP
      end if                                              
c                                        
c     Setup segment data
c                             
      SP(rtft).num_mrk= 3   ! this info will NOT be used by rigid body
      SP(rtcf).num_mrk= 3   ! calculations. It is included for compatibility
      SP(rtth).num_mrk= 3   ! with older ANZ/SHOW3D code
      SP(pelv).num_mrk= 3
      SP(lfft).num_mrk= 3
      SP(lfcf).num_mrk= 3
      SP(lfth).num_mrk= 3
      SP(trnk).num_mrk=4
      SP(head).num_mrk=1
      SP(rtua).num_mrk=2
      SP(rtla).num_mrk=2
      SP(lfua).num_mrk=2
      SP(lfla).num_mrk=2

      SP(rtft).mrk_num(1)= MMA_RTTO
      SP(rtft).mrk_num(2)= MMA_RTHL
      SP(rtft).mrk_num(3)= MMA_RTMF

      SP(rtcf).mrk_num(1)= MMA_RTAK                  
      SP(rtcf).mrk_num(2)= MMA_RTSF
      SP(rtcf).mrk_num(3)= MMA_RTKN
                                      
      SP(rtth).mrk_num(1)= MMA_RTKN
      SP(rtth).mrk_num(2)= MMA_RTTF
      SP(rtth).mrk_num(3)= MMA_RTHP

      SP(lfft).mrk_num(1)= MMA_LFTO
      SP(lfft).mrk_num(2)= MMA_LFHL
      SP(lfft).mrk_num(3)= MMA_LFMF 

      SP(lfcf).mrk_num(1)= MMA_LFAK
      SP(lfcf).mrk_num(2)= MMA_LFSF
      SP(lfcf).mrk_num(3)= MMA_LFKN

      SP(lfth).mrk_num(1)= MMA_LFKN
      SP(lfth).mrk_num(2)= MMA_LFTF
      SP(lfth).mrk_num(3)= MMA_LFHP

      SP(pelv).mrk_num(1)= MMA_RTAS
      SP(pelv).mrk_num(2)= MMA_LFAS
      SP(pelv).mrk_num(3)= MMA_SACR
      
      SP(trnk).mrk_num(1)= MMA_RTAS
      SP(trnk).mrk_num(2)= MMA_LFAS
      SP(trnk).mrk_num(3)= MMA_RTSH
      SP(trnk).mrk_num(4)= MMA_LFSH
      
      SP(rtua).mrk_num(1)= MMA_RTSH
      SP(rtua).mrk_num(2)= MMA_RTEL
      
      SP(rtla).mrk_num(1)= MMA_RTEL
      SP(rtla).mrk_num(2)= MMA_RTWR
      
      SP(lfua).mrk_num(1)= MMA_LFSH
      SP(lfua).mrk_num(2)= MMA_LFEL
                                        
      SP(lfla).mrk_num(1)= MMA_LFEL
      SP(lfla).mrk_num(2)= MMA_LFWR
      
      SP(head).mrk_num(1)= MMA_RTSH
      SP(head).mrk_num(2)= MMA_LFSH

      SP(rtft).num_MrkDist=3
      SP(rtcf).num_MrkDist=3
      SP(rtth).num_MrkDist=3
      SP(lfft).num_MrkDist=3
      SP(lfcf).num_MrkDist=3
      SP(lfth).num_MrkDist=3
      SP(pelv).num_MrkDist=3
      SP(trnk).num_MrkDist=6
      SP(rtua).num_MrkDist=1
      SP(rtla).num_MrkDist=1
      SP(lfua).num_MrkDist=1
      SP(lfla).num_MrkDist=1
      SP(head).num_MrkDist=0     

      SP(rtft).num_end= 1
      SP(rtcf).num_end= 2
      SP(rtth).num_end= 2
      SP(lfft).num_end= 1 
      SP(lfcf).num_end= 2
      SP(lfth).num_end= 2
      SP(pelv).num_end= 3
      SP(trnk).num_end= 4
      SP(rtua).num_end= 2
      SP(rtla).num_end= 2
      SP(lfua).num_end= 2
      SP(lfla).num_end= 2
      SP(head).num_end= 1
c
c     Set up segment information needed for conversion from technical
c     coordinate system to segment local coordinate system
c
      SP(rtft).nmTCS= 3
      SP(rtft).mnTCS(1)= MMA_RTTO
      SP(rtft).mnTCS(2)= MMA_RTHL
      SP(rtft).mnTCS(3)= MMA_RTMF

      SP(rtcf).nmTCS= 3         
      SP(rtcf).mnTCS(1)= MMA_RTSF
      SP(rtcf).mnTCS(2)= MMA_RTSU
      SP(rtcf).mnTCS(3)= MMA_RTSR

      SP(rtth).nmTCS= 3
      SP(rtth).mnTCS(1)= MMA_RTTF
      SP(rtth).mnTCS(2)= MMA_RTTU
      SP(rtth).mnTCS(3)= MMA_RTTR

      SP(lfft).nmTCS= 3
      SP(lfft).mnTCS(1)= MMA_LFTO
      SP(lfft).mnTCS(2)= MMA_LFHL
      SP(lfft).mnTCS(3)= MMA_LFMF

      SP(lfcf).nmTCS= 3         
      SP(lfcf).mnTCS(1)= MMA_LFSF
      SP(lfcf).mnTCS(2)= MMA_LFSU
      SP(lfcf).mnTCS(3)= MMA_LFSR

      SP(lfth).nmTCS= 3
      SP(lfth).mnTCS(1)= MMA_LFTF
      SP(lfth).mnTCS(2)= MMA_LFTU
      SP(lfth).mnTCS(3)= MMA_LFTR

      SP(pelv).nmTCS= 3
      SP(pelv).mnTCS(1)= MMA_RTAS
      SP(pelv).mnTCS(2)= MMA_LFAS
      SP(pelv).mnTCS(3)= MMA_SACR

      SP(trnk).nmTCS= 4
      SP(trnk).mnTCS(1)= MMA_RTAS
      SP(trnk).mnTCS(2)= MMA_LFAS
      SP(trnk).mnTCS(3)= MMA_RTSH
      SP(trnk).mnTCS(4)= MMA_LFSH

      SP(rtua).nmTCS= 2
      SP(rtua).mnTCS(1)= MMA_RTSH
      SP(rtua).mnTCS(2)= MMA_RTEL

      SP(rtla).nmTCS= 2
      SP(rtla).mnTCS(1)= MMA_RTEL
      SP(rtla).mnTCS(2)= MMA_RTWR

      SP(lfua).nmTCS= 2
      SP(lfua).mnTCS(1)= MMA_LFSH
      SP(lfua).mnTCS(2)= MMA_LFEL

      SP(lfla).nmTCS= 2
      SP(lfla).mnTCS(1)= MMA_LFEL
      SP(lfla).mnTCS(2)= MMA_LFWR

      SP(head).nmTCS= 0
c
c     Set up segment & joint names
c
      call SetUpFullBodyParam
c
c     Set up joint connectivity information
c                                       
      call SetUpFullBodyKinetics
c
c     Setup General Parameters
c
      GP.analysis_type= MAYO_ANL   
      GP.num_seg= MAX_SEG
      GP.num_jnt= MAX_JNT
      do i= 1, MAX_SEG
        GP.seg_num(i)= i
        GP.jnt_num(i)= i
      end do

      if ( ReStart ) MP.num_cycle= 0

      return    
      end
c
c
c           
      subroutine InitMayoRtAnalysis
      implicit none
      include 'a_const.inc'

      include 'a_struct.inc'
      include 'a_prm.inc'
      include 'a_names.inc'
      include 'a_mrk.inc'
      include 'a_seg.inc'                       
      include 'a_jnt.inc'
      include 'a_gls2.inc'
      include 'a_frc.inc'
      include 'a_mayo.inc'
c                          
c     initialize marker names
c
      MIR_RTKU= 1  ! Knee Center Array marker # 1 - Upper
      MIR_RTKF= 2  ! Knee Center Array marker # 2 - Front
      MIR_RTKR= 3  ! Knee Center Array marker # 3 - Rear

      MIR_RTTU= 4  ! Upper thigh triad marker
      MIR_RTTF= 5  ! Front thigh triad marker
      MIR_RTTR= 6  ! Rear  thigh triad marker

      MIR_RTSU= 7  ! Upper shank triad marker
      MIR_RTSF= 8  ! Front shank triad marker
      MIR_RTSR= 9  ! Rear  shank triad marker

      MIR_RTAU= 10 ! Ankle Center Array marker # 1 - Upper
      MIR_RTAF= 11 ! Ankle Center Array marker # 2 - Front
      MIR_RTAR= 12 ! Ankle Center Array marker # 3 - Rear

      MIR_SACR= 13 ! Sacrum- Outer marker
      MIR_RTHP= 14 ! Right ASIS - not hip !
      MIR_LFHP= 15 ! Left  ASIS - not hip !
      MIR_SACI= 16 ! Sacral stick inner marker

      MIR_RTHL= 17 ! Centered heel marker
      MIR_RTTO= 18 ! Forefoot marker at 1/2 metatarsal head
      MIR_RTMF= 19 ! Lateral Midfoot

      num_init_empty_mrk= 0
      init_empty_mrk(1)= -1
c                                        
c     Setup segment data
c                             
      SP(rtft).num_mrk= 3   ! this info will NOT be used by rigid body
      SP(rtcf).num_mrk= 3   ! calculations. It is included for compatibility
      SP(rtth).num_mrk= 3   ! with older ANZ/SHOW3D code
      SP(pelv).num_mrk= 3
                                               
      SP(rtft).mrk_num(1)= MIR_RTTO
      SP(rtft).mrk_num(2)= MIR_RTHL
      SP(rtft).mrk_num(3)= MIR_RTMF

      SP(rtcf).mrk_num(1)= MIR_RTSU
      SP(rtcf).mrk_num(2)= MIR_RTSF
      SP(rtcf).mrk_num(3)= MIR_RTSR

      SP(rtth).mrk_num(1)= MIR_RTTU
      SP(rtth).mrk_num(2)= MIR_RTTF
      SP(rtth).mrk_num(3)= MIR_RTTR
                                        
      SP(pelv).mrk_num(1)= MMA_RTAS
      SP(pelv).mrk_num(2)= MMA_LFAS
      SP(pelv).mrk_num(3)= MMA_SACR

      SP(rtft).num_MrkDist=3
      SP(rtcf).num_MrkDist=3
      SP(rtth).num_MrkDist=3
      SP(pelv).num_MrkDist=3

      SP(rtft).num_end= 1
      SP(rtcf).num_end= 2
      SP(rtth).num_end= 2
      SP(pelv).num_end= 1
c
c     Set up segment information needed for conversion from technical
c     coordinate system to segment local coordinate system
c
      SP(rtft).nmTCS= 3
      SP(rtft).mnTCS(1)= MIR_RTTO
      SP(rtft).mnTCS(2)= MIR_RTHL
      SP(rtft).mnTCS(3)= MIR_RTMF

      SP(rtcf).nmTCS= 3            
      SP(rtcf).mnTCS(1)= MIR_RTSF
      SP(rtcf).mnTCS(2)= MIR_RTSU
      SP(rtcf).mnTCS(3)= MIR_RTSR

      SP(rtth).nmTCS= 3
      SP(rtth).mnTCS(1)= MIR_RTTF
      SP(rtth).mnTCS(2)= MIR_RTTU
      SP(rtth).mnTCS(3)= MIR_RTTR
                                      
      SP(pelv).nmTCS= 3
      SP(pelv).mnTCS(1)= MIR_RTHP   ! Actually right asis
      SP(pelv).mnTCS(2)= MIR_LFHP   ! Actually left asis
      SP(pelv).mnTCS(3)= MIR_SACR

c
c     Setup General Parameters
c
      call SetUpFullBodyParam
      GP.analysis_type= MIR_ANL

      GP.num_seg= 4
      GP.seg_num(1)= rtft          
      GP.seg_num(2)= rtcf
      GP.seg_num(3)= rtth
      GP.seg_num(4)= pelv    

      GP.num_jnt= 3
      GP.jnt_num(1)= rtak
      GP.jnt_num(2)= rtkn
      GP.jnt_num(3)= rthp
      return                          
      end
c                                       
c
c                              
      subroutine InitMayoLfAnalysis
      implicit none
      include 'a_const.inc'

      include 'a_struct.inc'
      include 'a_prm.inc'
      include 'a_names.inc'
      include 'a_mrk.inc'
      include 'a_seg.inc'                       
      include 'a_jnt.inc'
      include 'a_gls2.inc'
      include 'a_frc.inc'
      include 'a_mayo.inc'
c                          
c     initialize marker names
c
      MIL_LFKU= 1  ! Knee Center Array marker # 1 - Upper
      MIL_LFKF= 2  ! Knee Center Array marker # 2 - Front
      MIL_LFKR= 3  ! Knee Center Array marker # 3 - Rear

      MIL_LFTU= 4  ! Upper thigh triad marker
      MIL_LFTF= 5  ! Front thigh triad marker
      MIL_LFTR= 6  ! Rear  thigh triad marker

      MIL_LFSU= 7  ! Upper shank triad marker
      MIL_LFSF= 8  ! Front shank triad marker
      MIL_LFSR= 9  ! Rear  shank triad marker

      MIL_LFAU= 10 ! Ankle Center Array marker # 1 - Upper
      MIL_LFAF= 11 ! Ankle Center Array marker # 2 - Front
      MIL_LFAR= 12 ! Ankle Center Array marker # 3 - Rear

      MIL_SACR= 13 ! Sacrum- Outer marker
      MIL_RTHP= 14 ! Actually Right ASIS - not hip !
      MIL_LFHP= 15 ! Actually Left  ASIS - not hip !
      MIL_SACI= 16 ! Sacral stick inner marker

      MIL_LFHL= 17 ! Centered heel marker
      MIL_LFTO= 18 ! Forefoot marker at 1/2 metatarsal head
      MIL_LFMF= 19 ! Lateral Midfoot

      num_init_empty_mrk= 0           
      init_empty_mrk(1)= -1
c                                        
c     Setup segment data
c                             
      SP(lfft).num_mrk= 3   ! this info will NOT be used by rigid body
      SP(lfcf).num_mrk= 3   ! calculations. It is included for compatibility
      SP(lfth).num_mrk= 3   ! with older ANZ/SHOW3D code
      SP(pelv).num_mrk= 3
                                               
      SP(lfft).mrk_num(1)= MIL_LFTO
      SP(lfft).mrk_num(2)= MIL_LFHL
      SP(lfft).mrk_num(3)= MIL_LFMF

      SP(lfcf).mrk_num(1)= MIL_LFSU
      SP(lfcf).mrk_num(2)= MIL_LFSF                                 
      SP(lfcf).mrk_num(3)= MIL_LFSR

      SP(lfth).mrk_num(1)= MIL_LFTU
      SP(lfth).mrk_num(2)= MIL_LFTF
      SP(lfth).mrk_num(3)= MIL_LFTR

      SP(pelv).mrk_num(1)= MIR_RTHP    ! Actually right asis
      SP(pelv).mrk_num(2)= MIR_LFHP    ! Actually left asis
      SP(pelv).mrk_num(3)= MIR_SACR

      SP(lfft).num_MrkDist=3
      SP(lfcf).num_MrkDist=3
      SP(lfth).num_MrkDist=3
      SP(pelv).num_MrkDist=3

      SP(lfft).num_end= 1
      SP(lfcf).num_end= 2
      SP(lfth).num_end= 2
      SP(pelv).num_end= 1
c
c     Set up segment information needed for conversion from technical
c     coordinate system to segment local coordinate system
c
      SP(lfft).nmTCS= 3            
      SP(lfft).mnTCS(1)= MIL_LFTO
      SP(lfft).mnTCS(2)= MIL_LFHL
      SP(lfft).mnTCS(3)= MIL_LFMF

      SP(lfcf).nmTCS= 3         
      SP(lfcf).mnTCS(1)= MIL_LFSF  
      SP(lfcf).mnTCS(2)= MIL_LFSU
      SP(lfcf).mnTCS(3)= MIL_LFSR

      SP(lfth).nmTCS= 3  
      SP(lfth).mnTCS(1)= MIL_LFTF
      SP(lfth).mnTCS(2)= MIL_LFTU
      SP(lfth).mnTCS(3)= MIL_LFTR
                                      
      SP(pelv).nmTCS= 3
      SP(pelv).mnTCS(1)= MIL_RTHP    ! Actually right asis
      SP(pelv).mnTCS(2)= MIL_LFHP    ! Actually left asis
      SP(pelv).mnTCS(3)= MIL_SACR

c        
c     Setup General Parameters
c
      call SetUpFullBodyParam
      GP.analysis_type= MIL_ANL

      GP.num_seg= 4
      GP.seg_num(1)= lfft
      GP.seg_num(2)= lfcf
      GP.seg_num(3)= lfth          
      GP.seg_num(4)= pelv    

      GP.num_jnt= 3        
      GP.jnt_num(1)= lfak
      GP.jnt_num(2)= lfkn
      GP.jnt_num(3)= lfhp                              
      return    
      end
c
c                           
c
      subroutine CalcMayoAlignment( CMND,MF,SF,OF )
      implicit none

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

      record /CmndLine/      CMND
      record /marker_flag/   MF
      record /segment_flag/  SF                                         
      record /option_flag/   OF                       

      integer*4 mn,i,frm,ierr,side,cn,
     &          foot,calf,thgh,mshf,mshu,mshb,mthf,mthu,mthb,mtoe,mhel,mmft,
     &          mkne,mhip,mras,msac,mlas,seg
      real*4    ampn(3,MAX_MRK),aklcntr(3),ftht,veclth,scf(3),hc(3),
     &          tmpsgp(3,4),awd,kwd,kncntr

c               foot   calf   thigh
      data scf / 0.571, 0.567, 0.567 / ! from Dempster (1955)
c
c     Check whether ankle and knee widths were given in command line options
c
      if ( CMND.NumCmnd .ge. 4 ) then
        cn= 3
        call AssignWidth( CMND,cn,awd,ierr )
        if ( ierr .ne. 0 ) goto 4010 ! bad ankle width
        call AssignWidth( CMND,cn,kwd,ierr )
        if ( ierr .ne. 0 ) goto 4020 ! bad knee width
      else
        goto 4000 ! error- both widths must be present
      end if
c
c     Make sure markers have been read and checked
c     Then calculate marker average positions            
c                                                             
      if ( .not. MF.interpolated ) goto 1000
      do mn= 1, MP.num_mrk
        do i= x, z
          ampn(i,mn)= 0.0
          do frm= 1, MP.nfrm
            ampn(i,mn)=  ampn(i,mn) + MrkData(i,frm,mn)
          end do
          ampn(i,mn)= ampn(i,mn)/MP.nfrm
        end do                                                              
      end do
c                                                         
c     Foot Calculations
c     Determine position of the TCS markers relative to the LCS, the position
c     and orientation of LCS relative to TCS, and the location of the ankle
c     in terms of LCS   
c               
      if ( GP.Analysis_type .eq. MIR_ANL ) then                           
        call FootMayoInit( MIR_RTAF,MIR_RTAU,MIR_RTAR,
     &                     MIR_RTTO,MIR_RTHL,MIR_RTMF,
     &                     ampn,awd,rt,SP(rtft),aklcntr,ierr )
        if ( ierr .ne. 0 ) goto 1010                
        ftht= veclth(ampn(x,MIR_RTHL),aklcntr)
      else
        call FootMayoInit( MIL_LFAF,MIL_LFAU,MIL_LFAR,
     &                     MIL_LFTO,MIL_LFHL,MIL_LFMF,
     &                     ampn,awd,lf,SP(lfft),aklcntr,ierr )
        if ( ierr .ne. 0 ) goto 1020
        ftht= veclth(ampn(x,MIL_LFHL),aklcntr)
      end if            
c                                                     
c     Shank calculations                                 
c
      if ( GP.Analysis_type .eq. MIR_ANL ) then                           
        call CalfMayoInit( MIR_RTKF,MIR_RTKU,MIR_RTKR,
     &                     MIR_RTSF,MIR_RTSU,MIR_RTSR,
     &                     ampn,kwd,SP(rtcf),aklcntr,rt,kncntr,ierr )
        if ( ierr .ne. 0 ) goto 1030
      else                        
        call CalfMayoInit( MIL_LFKF,MIL_LFKU,MIL_LFKR,
     &                     MIL_LFSF,MIL_LFSU,MIL_LFSR,
     &                     ampn,kwd,SP(lfcf),aklcntr,lf,kncntr,ierr )
        if ( ierr .ne. 0 ) goto 1040
      end if                        
c                                 
c     Do Pelvis calculations                     
c
      if ( GP.Analysis_type .eq. MIR_ANL ) then                           
        call PelvisMayoInit( MIR_RTHP,MIR_LFHP,MIR_SACR,MIR_SACI,
     &                       MIR_RTKF,MIR_RTKU,MIR_RTKR,kwd,
     &                       ampn,SP(pelv),rt,hc,ierr )
        if ( ierr .ne. 0 ) goto 1050
      else                        
        call PelvisMayoInit( MIL_RTHP,MIL_LFHP,MIL_SACR,MIL_SACI,
     &                       MIL_LFKF,MIL_LFKU,MIL_LFKR,kwd,
     &                       ampn,SP(pelv),lf,hc,ierr )
        if ( ierr .ne. 0 ) goto 1060                     
      end if                                                   
c                                                             
c     Do Thigh calculations
c
      if ( GP.Analysis_type .eq. MIR_ANL ) then                           
        call ThighMayoInit( MIR_RTKF,MIR_RTKU,MIR_RTKR,
     &                      MIR_RTTF,MIR_RTTU,MIR_RTTR,
     &                      ampn,kwd,SP(rtth),hc,rt,ierr )
        if ( ierr .ne. 0 ) goto 1070
      else
        call ThighMayoInit( MIL_LFKF,MIL_LFKU,MIL_LFKR,
     &                      MIL_LFTF,MIL_LFTU,MIL_LFTR,
     &                      ampn,kwd,SP(lfth),hc,lf,ierr )
        if ( ierr .ne. 0 ) goto 1080                    
      end if                              
c
c      Do the segment shape calculations for the foot, calf, thigh 
c      and pelvis                        
c                                                                 
      if ( GP.analysis_type .eq. MIR_ANL ) then
        call CalcFootShape(scf(1),SP(rtcf).length,ftht,SP(rtft))    
        call CalcCalfShape(scf(2),SP(rtcf))
        call CalcThighShape(scf(3),SP(rtth))
        call CalcPelvisShape(SP(pelv).length,SP(pelv))
      else
        call CalcFootShape(scf(1),SP(lfcf).length,ftht,SP(lfft))    
        call CalcCalfShape(scf(2),SP(lfcf))
        call CalcThighShape(scf(3),SP(lfth))
      end if                     
c
c     Calculate the segment positions for diagnostic purposes
c
      if ( GP.analysis_type .eq. MIR_ANL ) then
        foot= rtft
        calf= rtcf
        thgh= rtth
        mshf=  MIR_RTSF
        mshu=  MIR_RTSU
        mshb=  MIR_RTSR                   
        mthf=  MIR_RTTF
        mthu=  MIR_RTTU
        mthb=  MIR_RTTR
        mtoe=  MIR_RTTO             
        mhel=  MIR_RTHL
        mmft=  MIR_RTMF  
        mhip=  MIR_RTHP
        mras=  MIR_RTHP
        msac=  MIR_SACR
        mlas=  MIR_LFHP
      else               
        foot= lfft
        calf= lfcf                   
        thgh= lfth
        mshf=  MIL_LFSF
        mshu=  MIL_LFSU
        mshb=  MIL_LFSR                   
        mthf=  MIL_LFTF
        mthu=  MIL_LFTU
        mthb=  MIL_LFTR
        mtoe=  MIL_LFTO             
        mhel=  MIL_LFHL
        mmft=  MIL_LFMF  
        mhip=  MIL_LFHP
        mras=  MIL_RTHP
        msac=  MIL_SACR
        mlas=  MIL_LFHP
      end if      
      do frm= 1, MP.nfrm
        seg= foot
        call CalcLCSFoot( MrkData(x,frm,mtoe),
     &                    MrkData(x,frm,mmft),
     &                    MrkData(x,frm,mhel),0.571,tmpsgp,ierr )
        if ( ierr .ne. 0 ) goto 1090
        call CalcTCStoLCS( tmpsgp,SP(foot).TCStoLCS,
     &                     SgpGCS(x,x,frm,foot) )

        seg= calf
        call CalcTCS( MrkData(x,frm,mshf),
     &                MrkData(x,frm,mshu),
     &                MrkData(x,frm,mshb),tmpsgp,ierr )
        if ( ierr .ne. 0 ) goto 1090
        call CalcTCStoLCS( tmpsgp,SP(calf).TCStoLCS,
     &                     SgpGCS(x,x,frm,calf) )

        seg= thgh
        call CalcTCS( MrkData(x,frm,mthf),
     &                MrkData(x,frm,mthu),
     &                MrkData(x,frm,mthb),tmpsgp,ierr )
        if ( ierr .ne. 0 ) goto 1090
        call CalcTCStoLCS( tmpsgp,SP(thgh).TCStoLCS,
     &                     SgpGCS(x,x,frm,thgh) )

        seg= pelv
        call CalcLCSPelvis( MrkData(x,frm,mras),
     &                      MrkData(x,frm,msac),
     &                      MrkData(x,frm,mlas),
     &                      8.0*0.0254,tmpsgp,ierr )
        if ( ierr .ne. 0 ) goto 1090
        call CalcTCStoLCS( tmpsgp,SP(pelv).TCStoLCS,
     &                     SgpGCS(x,x,frm,pelv) )

      end do            
      SF.position= .true.     
c                                              
c     Trick to properly update the segment parameter info so that it will
c     be properly saved and displayed- May cause problems though since the
c     segment parameters have not been fully calculated yet...
c
      if ( GP.analysis_type .eq. MIR_ANL ) then
        SF.RightSideAlign= .true.
        if ( SF.LeftSideAlign ) then
          GP.num_seg= 7
          do i= 1, 7
            GP.seg_num(i)= i 
          end do
        end if
      else
        SF.LeftSideAlign= .true.
        if ( SF.LeftSideAlign ) then
          GP.num_seg= 7
          do i= 1, 7
            GP.seg_num(i)= i 
          end do
        end if
      end if
      SF.Param= .true.
      return           
                 
1000  write(6,1005)                                       
      if ( OF.ToLog ) write(3,1005)               
1005  format(' Error- Markers must first be checked/interpolated')
      return                               

1010  write(6,1015)
      if ( OF.ToLog ) write(3,1015)
1015  format(' Error- Initialization of right foot failed')
      call CCInitErr(ierr,OF.ToLog)
      return

1020  write(6,1025)
      if ( OF.ToLog ) write(3,1025)
1025  format(' Error- Initialization of left foot failed')
      call CCInitErr(ierr,OF.ToLog)     
      return    

1030  write(6,1035)
      if ( OF.ToLog ) write(3,1035)
1035  format(' Error- Initialization of right calf failed')
      call CCInitErr(ierr,OF.ToLog)     
      return    

1040  write(6,1045)
      if ( OF.ToLog ) write(3,1045)
1045  format(' Error- Initialization of left calf failed')
      call CCInitErr(ierr,OF.ToLog)     
      return    

1050  write(6,1055)
      if ( OF.ToLog ) write(3,1055)
1055  format(' Error- Initialization of right side pelvis failed')
      call CCInitErr(ierr,OF.ToLog)     
      return    

1060  write(6,1065)
      if ( OF.ToLog ) write(3,1065)
1065  format(' Error- Initialization of left side pelvis failed')
      call CCInitErr(ierr,OF.ToLog)     
      return    

1070  write(6,1075)
      if ( OF.ToLog ) write(3,1075)
1075  format(' Error- Initialization of right thigh failed')
      call CCInitErr(ierr,OF.ToLog)     
      return    

1080  write(6,1085)
      if ( OF.ToLog ) write(3,1085)
1085  format(' Error- Initialization of left thigh failed')
      call CCInitErr(ierr,OF.ToLog)     
      return    

1090  write(6,1095) seg_names(seg),frm
      if ( OF.ToLog ) write(3,1095) seg_names(seg),frm
1095  format(' Error- Calculation of the TCS for ',a4,' failed at frame #',i4)
      call CCInitErr( ierr,OF.ToLog )
      return

4000  write(6,4005)
      if ( OF.ToLog ) write(3,4005)
4005  format(' Error- The ankle and knee widths must be given with MRK SLP'/
     &       '        Note that units are assumed to be meters but inches and'/
     &       '        centimeters can be used by including the units'/
     &       '        Ex: MRK SLP 4.0 IN 6.0 IN  or '/
     &       '            MRK SLP 0.05 0.08')
      return    

4010  write(6,4015) CMND.CmndText(cn)
      if ( OF.ToLog ) write(3,4015) CMND.CmndText(cn)
4015  format(' Error- ',a3,' is not a valid ankle width')
      return    

4020  write(6,4025) CMND.CmndText(cn)
      if ( OF.ToLog ) write(3,4025) CMND.CmndText(cn)
4025  format(' Error- ',a3,' is not a valid knee width')
      return    
      end
c
c
c
      subroutine AssignWidth( CMND,cn,wd,ierr )
      implicit none

      include 'a_struct.inc'
      
      record /CmndLine/ CMND

      integer*4 cn,ierr
      real*4    wd
                    
      ierr= 0
      if ( .not. CMND.CmndNumOK(cn) ) then
        ierr= 40
        return
      end if
      wd= CMND.CmndRealVal(cn)
      cn= cn+1
      if ( CMND.NumCmnd .ge. cn ) then
        if ( CMND.CmndText(cn) .eq. 'CM ' ) then
          wd= wd*.01
          cn= cn+1
        else if ( CMND.CmndText(cn) .eq. 'IN ' ) then
          wd= wd*.0254
          cn= cn+1
        end if
      end if
      return
      end            
c                                                  
c                                                   
c                              
      subroutine FootMayoInit( MRK_AF,MRK_AU,MRK_AB,MRK_TO,MRK_HL,MRK_MF,
     &                         ampn,aw,side,SP,ac,ierr )
      implicit none                  
      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_names.inc'                         

      record /seg_param/ SP
                                 
      integer*4 MRK_AF,MRK_AU,MRK_AB,MRK_TO,MRK_HL,MRK_MF,ierr,side
      real*4    ampn(3,MAX_MRK),ac(3),aw

      integer*4 i,mn,vecnorm,MinLineXPnts         
      real*4    ftLCS(3,4),ftTCS(3,4),veca(3),nrma(3),vecb(3),nrmb(3),sign,
     &          veclth,fp(3),stu,off,vecc(3),nrmc(3),vecd(3),nrmd(3),tmpac(3)

      data stu / 0.1016 / ! length of stick on upper marker (m)
      data off / 0.0413 / ! offset of joint center array (m)
c                                           
c     Determine ankle center         
c                                                                  
      ierr= 0
      call vecsub(ampn(x,MRK_AB),ampn(x,MRK_AU),veca)
      call vecsub(ampn(x,MRK_AF),ampn(x,MRK_AU),vecb)
      call vecX( veca,vecb,vecc )
      ierr= vecnorm( vecc,nrmc )
      if ( ierr .ne. 0 ) goto 1020 ! error
      do i= x, z
        vecd(i)= (ampn(x,MRK_AB)+ampn(x,MRK_AF))/2.0 - ampn(x,MRK_AU) 
      end do
      ierr= vecnorm( vecd,nrmd )
      if ( ierr .ne. 0 ) goto 1020 ! error
      sign= -1.0
      if ( side .eq. lf ) sign= 1.0
      do i= x, z
        tmpac(i)= ampn(i,MRK_AU) + stu*nrmd(i) + sign*(off+aw/2.0)*nrmc(i)
        ac(i)= tmpac(i)
      end do                        
c
c     The above ankle center (tmpac) can be used or the following line crossing
c     method algorithm may be used. I prefer the line crossing approach. If
c     the above simple approach to estimating the ankle center is used, comment
c     the following calculations (5 lines).
c
      call vecsub(ampn(x,MRK_TO),ampn(x,MRK_HL),vecb)
      ierr= vecnorm(vecb,nrmb)
      if ( ierr .ne. 0 ) goto 1030
      ierr= MinLineXPnts( nrmc,tmpac,nrmb,ampn(x,MRK_HL),ac,fp )
      if ( ierr .ne. 0 ) goto 1040      
c                                                 
c     Calculate standard foot LCS and position of TCS markers relative to it
c
      call CalcLCSFoot( ampn(x,MRK_TO),ac,ampn(x,MRK_HL),0.571,ftLCS,ierr )
      if ( ierr .ne. 0 ) goto 1000
      do i= 1, SP.nmTCS     
        mn= SP.mnTCS(i)
        call PntGCStoLCS( ftLCS,ampn(x,mn),SP.mpnLCS(x,i) )
      end do                        
c                                                
c      Calculate TCS and then the position and orientation of the LCS relative
c      to the TCS
c
      call CalcLCSFoot( ampn(x,MRK_TO),ampn(x,MRK_MF),ampn(x,MRK_HL),
     &                  0.571,ftTCS,ierr )
      if ( ierr .ne. 0 ) goto 1010
      call LCS2relLCS1( ftTCS,ftLCS,SP.TCStoLCS )
c
c     Calculate ankle center relative to LCS
c
      call PntGCStoLCS( ftLCS,ac,SP.end(x,proximal) )
c
c     Calculate an 'estimate' of the foot length
c
      SP.length= veclth( ampn(x,MRK_TO),ampn(x,MRK_HL) )
      return        
                                                 
1000  return            ! LCS calc error
1010  ierr= ierr + 10   ! TCS calc error
      return                        
1020  ierr= 20   ! normalization of ankle vector(s) error
      return 
1030  ierr= 30   ! normalization of foot vector error
      return                                                 
1040  ierr= 40   ! Line bisector calc error
      return                                   
      end   
c                                
c                                                   
c
      subroutine CalfMayoInit( MRK_KF,MRK_KU,MRK_KB,MRK_SF,MRK_SU,MRK_SB,
     &                         ampn,kw,SP,ac,side,kc,ierr )
      implicit none                           
      include 'a_const.inc'                            
      include 'a_struct.inc'
      include 'a_names.inc'                         

      record /seg_param/ SP
                                 
      integer*4 MRK_KF,MRK_KU,MRK_KB,MRK_SF,MRK_SU,MRK_SB,ierr,side
      real*4    ampn(3,MAX_MRK),ac(3),kw,kc(3)

      integer*4 i,mn,vecnorm
      real*4    cfLCS(3,4),cfTCS(3,4),veclth,mkl(3),mkm(3),veca(3),vecb(3),
     &          vecc(3),vecd(3),nrmc(3),nrmd(3),stu,off,sign

      data stu / 0.1016 / ! length of stick on upper marker (m)
      data off / 0.0413 / ! offset of joint center array (m)
c                                           
c     Determine knee center
c                                                                         
      ierr= 0                                              
      call vecsub(ampn(x,MRK_KB),ampn(x,MRK_KU),veca)
      call vecsub(ampn(x,MRK_KF),ampn(x,MRK_KU),vecb)
      call vecX( veca,vecb,vecc )
      ierr= vecnorm( vecc,nrmc )
      if ( ierr .ne. 0 ) goto 1020 ! error
      do i= x, z
        vecd(i)= (ampn(x,MRK_KB)+ampn(x,MRK_KF))/2.0 - ampn(x,MRK_KU) 
      end do
      ierr= vecnorm( vecd,nrmd )
      if ( ierr .ne. 0 ) goto 1020 ! error
      sign= -1.0
      if ( side .eq. lf ) sign= 1.0
      do i= x, z
        kc(i)= ampn(i,MRK_KU) + stu*nrmd(i) + sign*(off+kw/2.0)*nrmc(i)
      end do                        
c                                                 
c     Calculate calf LCS and position of TCS markers relative to it
c
      do i= x, z
        mkl(i)= kc(i) - nrmc(i)*sign*kw/2.0
        mkm(i)= kc(i) + nrmc(i)*sign*kw/2.0
      end do
      call CalcLCSCalfCC( mkl,mkm,ac,side,0.567,cfLCS,ierr )
      if ( ierr .ne. 0 ) goto 1000
      do i= 1, SP.nmTCS     
        mn= SP.mnTCS(i)
        call PntGCStoLCS( cfLCS,ampn(x,mn),SP.mpnLCS(x,i) )
      end do
c                                                
c      Calculate TCS and then the position and orientation of the LCS relative
c      to the TCS                                                    
c
      call CalcTCS( ampn(x,MRK_SF),ampn(x,MRK_SU),ampn(x,MRK_SB),cfTCS,ierr )
      if ( ierr .ne. 0 ) goto 1010
      call LCS2relLCS1( cfTCS,cfLCS,SP.TCStoLCS )
c
c     Calculate ankle and knee center relative to LCS
c
      call PntGCStoLCS( cfLCS,ac,SP.end(x,distal) )
      call PntGCStoLCS( cfLCS,kc,SP.end(x,proximal) )
c
c     Calculate an 'estimate' of the calf length
c
      SP.length= veclth( kc,ac )
      return        
                                                 
1000  return            ! LCS calc error
1010  ierr= ierr + 10   ! TCS calc error
      return                        
1020  ierr= ierr + 20   ! Vector normalization error
      return                        
      end   
c                                
c                                                   
c
      subroutine ThighMayoInit( MRK_KF,MRK_KU,MRK_KB,MRK_TF,MRK_TU,MRK_TB,
     &                          ampn,kw,SP,hc,side,ierr )
      implicit none                  
      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_names.inc'                         

      record /seg_param/ SP
                                 
      integer*4 MRK_KF,MRK_KU,MRK_KB,MRK_TF,MRK_TU,MRK_TB,ierr,side
      real*4    ampn(3,MAX_MRK),hc(3),kw
                            
      integer*4 i,mn,vecnorm
      real*4    thLCS(3,4),thTCS(3,4),veclth,kc(3),veca(3),vecb(3),
     &          vecc(3),vecd(3),nrmc(3),nrmd(3),stu,off,sign,mkl(3),mkm(3)
                                          
      data stu / 0.1016 / ! length of stick on upper marker (m)
      data off / 0.0413 / ! offset of joint center array (m)
c                                           
c     Determine knee center
c                                                                         
      ierr= 0                                              
      call vecsub(ampn(x,MRK_KB),ampn(x,MRK_KU),veca)
      call vecsub(ampn(x,MRK_KF),ampn(x,MRK_KU),vecb)
      call vecX( veca,vecb,vecc )
      ierr= vecnorm( vecc,nrmc )
      if ( ierr .ne. 0 ) goto 1020 ! error
      do i= x, z
        vecd(i)= (ampn(x,MRK_KB)+ampn(x,MRK_KF))/2.0 - ampn(x,MRK_KU) 
      end do
      ierr= vecnorm( vecd,nrmd )
      if ( ierr .ne. 0 ) goto 1020 ! error
      sign= -1.0
      if ( side .eq. lf ) sign= 1.0
      do i= x, z
        kc(i)= ampn(i,MRK_KU) + stu*nrmd(i) + sign*(off+kw/2.0)*nrmc(i)
      end do                        
      do i= x, z
        mkl(i)= kc(i) - nrmc(i)*sign*kw/2.0
        mkm(i)= kc(i) + nrmc(i)*sign*kw/2.0
      end do
c                                                 
c     Calculate thigh LCS and position of TCS markers relative to it
c
      call CalcLCSThighCC( mkl,mkm,hc,side,0.567,thLCS,ierr )
      if ( ierr .ne. 0 ) goto 1000
      do i= 1, SP.nmTCS     
        mn= SP.mnTCS(i)
        call PntGCStoLCS( thLCS,ampn(x,mn),SP.mpnLCS(x,i) )
      end do                   
c                                                
c      Calculate TCS and then the position and orientation of the LCS relative
c      to the TCS                                                    
c
      call CalcTCS( ampn(x,MRK_TF),ampn(x,MRK_TU),ampn(x,MRK_TB),thTCS,ierr )
      if ( ierr .ne. 0 ) goto 1010
      call LCS2relLCS1( thTCS,thLCS,SP.TCStoLCS )
c                           
c     Calculate knee and hip center relative to LCS
c
      call PntGCStoLCS( thLCS,kc,SP.end(x,distal) )
      call PntGCStoLCS( thLCS,hc,SP.end(x,proximal) )
c                         
c     Calculate an 'estimate' of the thigh length
c
      SP.length= veclth( hc,kc )
      return        
                                                 
1000  return            ! LCS calc error
1010  ierr= ierr + 10   ! TCS calc error
      return                        
1020  ierr= ierr + 20   ! Vector normalization error
      return                        
      end   
c                                
c                                                   
c                                
      subroutine PelvisMayoInit( MRK_RAS,MRK_LAS,MRK_SAC,MRK_SAI,           
     &                           MRK_KF,MRK_KU,MRK_KB,kw,
     &                           ampn,SP,side,hc,ierr )
      implicit none                  
      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_names.inc'                         

      record /seg_param/ SP
                                                         
      integer*4 MRK_RAS,MRK_LAS,MRK_SAC,MRK_SAI,MRK_KL,MRK_KM,side,ierr,
     &          MRK_KF,MRK_KU,MRK_KB
      real*4    ampn(3,MAX_MRK),hc(3),kw

      integer*4 i,mn,vecnorm                                              
      real*4    pvLCS(3,4),pvTCS(3,4),veclth,kc(3),stklen,mkl(3),mkm(3),
     &          vecc(3),vecd(3),nrmc(3),nrmd(3),stu,off,sign,veca(3),vecb(3)

      data stu / 0.1016 / ! length of stick on upper marker (m)
      data off / 0.0413 / ! offset of joint center array (m)

      ierr= 0                   
c                                                 
c     Calculate pelvis LCS and position of TCS markers relative to it
c                                            
      call CalcLCSPelvisCC( ampn(x,MRK_RAS),ampn(x,MRK_LAS),
     &                      ampn(x,MRK_SAC),ampn(x,MRK_SAI),pvLCS,ierr )
      if ( ierr .ne. 0 ) goto 1000
      do i= 1, SP.nmTCS     
        mn= SP.mnTCS(i)
        call PntGCStoLCS( pvLCS,ampn(x,mn),SP.mpnLCS(x,i) )
      end do
c                                                
c      Calculate TCS and then the position and orientation of the LCS relative
c      to the TCS                                                    
c
c      call CalcTCS( ampn(x,MRK_RAS),ampn(x,MRK_SAC),ampn(x,MRK_LAS),pvTCS,ierr )
      call CalcLCSPelvis( ampn(x,MRK_RAS),ampn(x,MRK_SAC),ampn(x,MRK_LAS),
     &                    8.0*0.0254,pvTCS,ierr )
      if ( ierr .ne. 0 ) goto 1010         
      call LCS2relLCS1( pvTCS,pvLCS,SP.TCStoLCS )
c                                           
c     Determine knee center in GCS as well as medial and lateral "markers"
c                                                                         
      call vecsub(ampn(x,MRK_KB),ampn(x,MRK_KU),veca)
      call vecsub(ampn(x,MRK_KF),ampn(x,MRK_KU),vecb)
      call vecX( veca,vecb,vecc )
      ierr= vecnorm( vecc,nrmc )
      if ( ierr .ne. 0 ) goto 1020 ! error
      do i= x, z
        vecd(i)= (ampn(x,MRK_KB)+ampn(x,MRK_KF))/2.0 - ampn(x,MRK_KU) 
      end do
      ierr= vecnorm( vecd,nrmd )
      if ( ierr .ne. 0 ) goto 1020 ! error
      sign= -1.0
      if ( side .eq. lf ) sign= 1.0
      do i= x, z
        kc(i)= ampn(i,MRK_KU) + stu*nrmd(i) + sign*(off+kw/2.0)*nrmc(i)
      end do                        
      do i= x, z
        mkl(i)= kc(i) - nrmc(i)*sign*kw/2.0
        mkm(i)= kc(i) + nrmc(i)*sign*kw/2.0
      end do
c                           
c     Calculate knee center relative to LCS                             
c                
      call PntGCStoLCS( pvLCS,kc,SP.end(x,distal) )
c                                                
c     Calculate hip center relative to LCS
c
      call CalcHipTyl( ampn(x,MRK_RAS),ampn(x,MRK_LAS),ampn(x,MRK_SAC),
     &                 ampn(x,MRK_SAI),mkl,mkm,side,hc,ierr )
      if ( ierr .ne. 0 ) goto 1020
      call PntGCStoLCS( pvLCS,hc,SP.end(x,side) )
c                                                                     
c     Calculate an 'estimate' of the pelvis width
c
      SP.length= veclth( ampn(x,MRK_RAS),ampn(x,MRK_LAS) )
      return        
                                                 
1000  return            ! LCS calc error        
1010  ierr= ierr + 10   ! TCS calc error
      return                        
1020  ierr= 50          ! Error calculating hip center
      return
      end   
