c
c
c
      subroutine InitCCAnalysis(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_clvlnd.inc'

      integer*4 i          
      logical*4 ReStart
c
c     Set up marker info
c
c                          
c     initialize marker names
c
      MCC_RTSH= 1
      MCC_LFSH= 2
      MCC_RTEL= 3
      MCC_LFEL= 4 
      MCC_RTWR= 5
      MCC_LFWR= 6
      MCC_SACR= 7
      MCC_RTAS= 8
      MCC_LFAS= 9            
      MCC_RTTU= 10
      MCC_LFTU= 11
      MCC_RTTF= 12
      MCC_LFTF= 13
      MCC_RTTR= 14
      MCC_LFTR= 15
      MCC_RTSU= 16
      MCC_LFSU= 17
      MCC_RTSF= 18
      MCC_LFSF= 19
      MCC_RTSR= 20
      MCC_LFSR= 21
      MCC_RTHL= 22
      MCC_LFHL= 23
      MCC_RTTO= 24
      MCC_LFTO= 25
      MCC_RTMF= 26      ! added so that foot has 3 markers
      MCC_LFMF= 27      ! added so that foot has 3 markers
      MCC_RTAK= 28
      MCC_LFAK= 29
      MCC_RTKN= 30
      MCC_LFKN= 31
      MCC_RTHP= 32
      MCC_LFHP= 33

      if ( ReStart ) then
        num_init_empty_mrk= 6
        init_empty_mrk(1)= MCC_RTAK
        init_empty_mrk(2)= MCC_LFAK
        init_empty_mrk(3)= MCC_RTKN
        init_empty_mrk(4)= MCC_LFKN
        init_empty_mrk(5)= MCC_RTHP
        init_empty_mrk(6)= MCC_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)= MCC_RTTO
      SP(rtft).mrk_num(2)= MCC_RTHL
      SP(rtft).mrk_num(3)= MCC_RTMF
c      SP(rtft).mrk_num(3)= MCC_RTTO  ! used for 2 marker on foot version

      SP(rtcf).mrk_num(1)= MCC_RTAK                  
      SP(rtcf).mrk_num(2)= MCC_RTSF
      SP(rtcf).mrk_num(3)= MCC_RTKN
                                      
      SP(rtth).mrk_num(1)= MCC_RTKN
      SP(rtth).mrk_num(2)= MCC_RTTF
      SP(rtth).mrk_num(3)= MCC_RTHP

      SP(lfft).mrk_num(1)= MCC_LFTO
      SP(lfft).mrk_num(2)= MCC_LFHL
      SP(lfft).mrk_num(3)= MCC_LFMF 
c      SP(rtft).mrk_num(3)= MCC_LFTO  ! used for 2 marker on foot version

      SP(lfcf).mrk_num(1)= MCC_LFAK
      SP(lfcf).mrk_num(2)= MCC_LFSF
      SP(lfcf).mrk_num(3)= MCC_LFKN

      SP(lfth).mrk_num(1)= MCC_LFKN
      SP(lfth).mrk_num(2)= MCC_LFTF
      SP(lfth).mrk_num(3)= MCC_LFHP

      SP(pelv).mrk_num(1)= MCC_RTAS
      SP(pelv).mrk_num(2)= MCC_LFAS
      SP(pelv).mrk_num(3)= MCC_SACR
      
      SP(trnk).mrk_num(1)= MCC_RTAS
      SP(trnk).mrk_num(2)= MCC_LFAS
      SP(trnk).mrk_num(3)= MCC_RTSH
      SP(trnk).mrk_num(4)= MCC_LFSH
      
      SP(rtua).mrk_num(1)= MCC_RTSH
      SP(rtua).mrk_num(2)= MCC_RTEL
      
      SP(rtla).mrk_num(1)= MCC_RTEL
      SP(rtla).mrk_num(2)= MCC_RTWR
      
      SP(lfua).mrk_num(1)= MCC_LFSH
      SP(lfua).mrk_num(2)= MCC_LFEL
                                        
      SP(lfla).mrk_num(1)= MCC_LFEL
      SP(lfla).mrk_num(2)= MCC_LFWR
      
      SP(head).mrk_num(1)= MCC_RTSH
      SP(head).mrk_num(2)= MCC_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)= MCC_RTTO
      SP(rtft).mnTCS(2)= MCC_RTHL
      SP(rtft).mnTCS(3)= MCC_RTMF
c      SP(rtft).mnTCS(3)= MCC_RTAK  ! used for 2 marker on foot version

      SP(rtcf).nmTCS= 3         
      SP(rtcf).mnTCS(1)= MCC_RTSF
      SP(rtcf).mnTCS(2)= MCC_RTSU
      SP(rtcf).mnTCS(3)= MCC_RTSR

      SP(rtth).nmTCS= 3
      SP(rtth).mnTCS(1)= MCC_RTTF
      SP(rtth).mnTCS(2)= MCC_RTTU
      SP(rtth).mnTCS(3)= MCC_RTTR

      SP(lfft).nmTCS= 3
      SP(lfft).mnTCS(1)= MCC_LFTO
      SP(lfft).mnTCS(2)= MCC_LFHL
      SP(lfft).mnTCS(3)= MCC_LFMF
c      SP(lfft).mnTCS(3)= MCC_LFAK  ! used for 2 marker on foot version

      SP(lfcf).nmTCS= 3         
      SP(lfcf).mnTCS(1)= MCC_LFSF
      SP(lfcf).mnTCS(2)= MCC_LFSU
      SP(lfcf).mnTCS(3)= MCC_LFSR

      SP(lfth).nmTCS= 3
      SP(lfth).mnTCS(1)= MCC_LFTF
      SP(lfth).mnTCS(2)= MCC_LFTU
      SP(lfth).mnTCS(3)= MCC_LFTR

      SP(pelv).nmTCS= 3
      SP(pelv).mnTCS(1)= MCC_RTAS
      SP(pelv).mnTCS(2)= MCC_LFAS
      SP(pelv).mnTCS(3)= MCC_SACR

      SP(trnk).nmTCS= 4
      SP(trnk).mnTCS(1)= MCC_RTAS
      SP(trnk).mnTCS(2)= MCC_LFAS
      SP(trnk).mnTCS(3)= MCC_RTSH
      SP(trnk).mnTCS(4)= MCC_LFSH

      SP(rtua).nmTCS= 2
      SP(rtua).mnTCS(1)= MCC_RTSH
      SP(rtua).mnTCS(2)= MCC_RTEL

      SP(rtla).nmTCS= 2
      SP(rtla).mnTCS(1)= MCC_RTEL
      SP(rtla).mnTCS(2)= MCC_RTWR

      SP(lfua).nmTCS= 2
      SP(lfua).mnTCS(1)= MCC_LFSH
      SP(lfua).mnTCS(2)= MCC_LFEL

      SP(lfla).nmTCS= 2
      SP(lfla).mnTCS(1)= MCC_LFEL
      SP(lfla).mnTCS(2)= MCC_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= CC_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 InitCCRAnalysis
      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_clvlnd.inc'
c                          
c     initialize marker names
c
      MCR_RTKM= 1  ! Medial knee
      MCR_RTKL= 2  ! Lateral knee
      MCR_RTTU= 3  ! Upper thigh triad marker
      MCR_RTTF= 4  ! Front thigh triad marker
      MCR_RTTR= 5  ! Rear  thigh triad marker
      MCR_RTSU= 6  ! Upper shank triad marker
      MCR_RTSF= 7  ! Front shank triad marker
      MCR_RTSR= 8  ! Rear  shank triad marker
      MCR_RTAM= 9  ! Medial ankle
      MCR_RTAL= 10 ! Lateral ankle
      MCR_SACR= 11 ! Sacrum- Outer marker
      MCR_RTHP= 12 ! Right ASIS - not hip !
      MCR_LFHP= 13 ! Left  ASIS - not hip !
      MCR_SACI= 14 ! Sacral stick inner marker
      MCR_RTHL= 15 ! Centered heel marker
      MCR_RTTO= 16 ! Forefoot marker at 1/2 metatarsal head
      MCR_RTMF= 17 ! Lateral Midfoot
c      MCR_RTFR= 17 ! Lateral heel marker - used in 2 marker on foot version
c      MCR_RTFF= 18 ! Lateral Forefoot - used in 2 marker on foot version

      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)= MCR_RTTO
      SP(rtft).mrk_num(2)= MCR_RTHL
      SP(rtft).mrk_num(3)= MCR_RTMF

      SP(rtcf).mrk_num(1)= MCR_RTAL
      SP(rtcf).mrk_num(2)= MCR_RTSF
      SP(rtcf).mrk_num(3)= MCR_RTKL

      SP(rtth).mrk_num(1)= MCR_RTKL
      SP(rtth).mrk_num(2)= MCR_RTTF
      SP(rtth).mrk_num(3)= MCR_RTHP

      SP(pelv).mrk_num(1)= MCC_RTAS
      SP(pelv).mrk_num(2)= MCC_LFAS
      SP(pelv).mrk_num(3)= MCC_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)= MCR_RTTO
      SP(rtft).mnTCS(2)= MCR_RTHL
      SP(rtft).mnTCS(3)= MCR_RTMF

      SP(rtcf).nmTCS= 3            
      SP(rtcf).mnTCS(1)= MCR_RTSF
      SP(rtcf).mnTCS(2)= MCR_RTSU
      SP(rtcf).mnTCS(3)= MCR_RTSR

      SP(rtth).nmTCS= 3
      SP(rtth).mnTCS(1)= MCR_RTTF
      SP(rtth).mnTCS(2)= MCR_RTTU
      SP(rtth).mnTCS(3)= MCR_RTTR
                                      
      SP(pelv).nmTCS= 3
      SP(pelv).mnTCS(1)= MCR_RTHP   ! Actually right asis
      SP(pelv).mnTCS(2)= MCR_LFHP   ! Actually left asis
      SP(pelv).mnTCS(3)= MCR_SACR

c
c     Setup General Parameters
c
      call SetUpFullBodyParam
      GP.analysis_type= CCR_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 InitCCLAnalysis
      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_clvlnd.inc'
c                          
c     initialize marker names
c
      MCL_LFKM= 1  ! Medial knee
      MCL_LFKL= 2  ! Lateral knee
      MCL_LFTU= 3  ! Upper thigh triad marker
      MCL_LFTF= 4  ! Front thigh triad marker
      MCL_LFTR= 5  ! Rear  thigh triad marker
      MCL_LFSU= 6  ! Upper shank triad marker
      MCL_LFSF= 7  ! Front shank triad marker
      MCL_LFSR= 8  ! Rear  shank triad marker
      MCL_LFAM= 9  ! Medial ankle
      MCL_LFAL= 10 ! Lateral ankle
      MCL_SACR= 11 ! Sacrum- Outer marker
      MCL_RTHP= 12 ! Actually Right ASIS - not hip !
      MCL_LFHP= 13 ! Actually Left  ASIS - not hip !
      MCL_SACI= 14 ! Sacral stick inner marker
      MCL_LFHL= 15 ! Centered heel marker
      MCL_LFTO= 16 ! Forefoot marker at 1/2 metatarsal head
      MCL_LFMF= 17 ! Lateral Midfoot
c      MCL_LFFR= 17 ! Lateral heel marker - used in 2 marker on foot version
c      MCL_LFFF= 18 ! Lateral Forefoot - used in 2 marker on foot version

      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)= MCL_LFTO
      SP(lfft).mrk_num(2)= MCL_LFHL
      SP(lfft).mrk_num(3)= MCL_LFMF

      SP(lfcf).mrk_num(1)= MCL_LFAL
      SP(lfcf).mrk_num(2)= MCL_LFSF                                 
      SP(lfcf).mrk_num(3)= MCL_LFKL

      SP(lfth).mrk_num(1)= MCL_LFKL
      SP(lfth).mrk_num(2)= MCL_LFTF
      SP(lfth).mrk_num(3)= MCL_LFHP

      SP(pelv).mrk_num(1)= MCR_RTHP    ! Actually right asis
      SP(pelv).mrk_num(2)= MCR_LFHP    ! Actually left asis
      SP(pelv).mrk_num(3)= MCR_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)= MCL_LFTO
      SP(lfft).mnTCS(2)= MCL_LFHL
      SP(lfft).mnTCS(3)= MCL_LFMF

      SP(lfcf).nmTCS= 3         
      SP(lfcf).mnTCS(1)= MCL_LFSF  
      SP(lfcf).mnTCS(2)= MCL_LFSU
      SP(lfcf).mnTCS(3)= MCL_LFSR

      SP(lfth).nmTCS= 3  
      SP(lfth).mnTCS(1)= MCL_LFTF
      SP(lfth).mnTCS(2)= MCL_LFTU
      SP(lfth).mnTCS(3)= MCL_LFTR
                                      
      SP(pelv).nmTCS= 3
      SP(pelv).mnTCS(1)= MCL_RTHP    ! Actually right asis
      SP(pelv).mnTCS(2)= MCL_LFHP    ! Actually left asis
      SP(pelv).mnTCS(3)= MCL_SACR

c        
c     Setup General Parameters
c
      call SetUpFullBodyParam
      GP.analysis_type= CCL_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 CalcCCAlignment( 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_clvlnd.inc'

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

      integer*4 mn,i,frm,ierr,side,
     &          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)

c               foot   calf   thigh
      data scf / 0.571, 0.567, 0.567 / ! from Dempster (1955)
c
c     First 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. CCR_ANL ) then                           
c        call FootCCInit2( MCR_RTAM,MCR_RTAL,MCR_RTFF,MCR_RTFR,MCR_RTTO,MCR_RTHL,
c     &                   ampn,SP(rtft),aklcntr,ierr )
        call FootCCInit3( MCR_RTAM,MCR_RTAL,MCR_RTTO,MCR_RTHL,MCR_RTMF,
     &                    ampn,SP(rtft),aklcntr,ierr )
        if ( ierr .ne. 0 ) goto 1010                
        ftht= veclth(ampn(x,MCR_RTHL),aklcntr)
      else
c        call FootCCInit2( MCL_LFAM,MCL_LFAL,MCL_LFFF,MCL_LFFR,MCL_LFTO,MCL_LFHL,
c     &                   ampn,SP(lfft),aklcntr,ierr )
        call FootCCInit3( MCL_LFAM,MCL_LFAL,MCL_LFTO,MCL_LFHL,MCL_LFMF,
     &                    ampn,SP(lfft),aklcntr,ierr )
        if ( ierr .ne. 0 ) goto 1020
        ftht= veclth(ampn(x,MCL_LFHL),aklcntr)
      end if            
c                                                     
c     Shank calculations                                 
c
      if ( GP.Analysis_type .eq. CCR_ANL ) then                           
        call CalfCCInit( MCR_RTAM,MCR_RTAL,MCR_RTKM,MCR_RTKL,MCR_RTSF,MCR_RTSU,
     &                   MCR_RTSR,ampn,SP(rtcf),aklcntr,rt,ierr )
        if ( ierr .ne. 0 ) goto 1030
      else
        call CalfCCInit( MCL_LFAM,MCL_LFAL,MCL_LFKM,MCL_LFKL,MCL_LFSF,MCL_LFSU,
     &                   MCL_LFSR,ampn,SP(lfcf),aklcntr,lf,ierr )
        if ( ierr .ne. 0 ) goto 1040
      end if                        
c                                 
c     Do Pelvis calculations                     
c
      if ( GP.Analysis_type .eq. CCR_ANL ) then                           
        call PelvisCCInit( MCR_RTHP,MCR_LFHP,MCR_SACR,MCR_SACI,MCR_RTKM,
     &                     MCR_RTKL,ampn,SP(pelv),rt,hc,ierr )
        if ( ierr .ne. 0 ) goto 1050
      else
        call PelvisCCInit( MCL_RTHP,MCL_LFHP,MCL_SACR,MCL_SACI,MCL_LFKM,
     &                     MCL_LFKL,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. CCR_ANL ) then                           
        call ThighCCInit( MCR_RTKM,MCR_RTKL,MCR_RTTF,MCR_RTTU,
     &                    MCR_RTTR,ampn,SP(rtth),hc,rt,ierr )
        if ( ierr .ne. 0 ) goto 1070
      else
        call ThighCCInit( MCL_LFKM,MCL_LFKL,MCL_LFTF,MCL_LFTU,
     &                    MCL_LFTR,ampn,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. CCR_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. CCR_ANL ) then
        foot= rtft
        calf= rtcf
        thgh= rtth
        mshf=  MCR_RTSF
        mshu=  MCR_RTSU
        mshb=  MCR_RTSR                   
        mthf=  MCR_RTTF
        mthu=  MCR_RTTU
        mthb=  MCR_RTTR
        mtoe=  MCR_RTTO             
        mhel=  MCR_RTHL
        mmft=  MCR_RTMF  
        mhip=  MCR_RTHP
        mras=  MCR_RTHP
        msac=  MCR_SACR
        mlas=  MCR_LFHP
      else               
        foot= lfft
        calf= lfcf                   
        thgh= lfth
        mshf=  MCL_LFSF
        mshu=  MCL_LFSU
        mshb=  MCL_LFSR                   
        mthf=  MCL_LFTF
        mthu=  MCL_LFTU
        mthb=  MCL_LFTR
        mtoe=  MCL_LFTO             
        mhel=  MCL_LFHL
        mmft=  MCL_LFMF  
        mhip=  MCL_LFHP
        mras=  MCL_RTHP
        msac=  MCL_SACR
        mlas=  MCL_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. CCR_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
      end
c                                          
c                                                              
c
      subroutine CCInitErr(ierr,logfile)
      implicit none                               

      integer*4 ierr
      logical*4 logfile

      integer*4 CSNum
      character CSName(2)*3
      data CSName /'LCS','TCS'/

      CSNum= 1                 
      if ( (ierr .gt. 10) .and. (ierr .lt. 20) ) CSNum= 2
                                                 
      if ( (ierr .eq. 1) .or. (ierr .eq. 11) ) then
         write(6,100) CSName(CSNum)
         if ( logfile ) write(3,100) CSName(CSNum)
100      format(' Error- ',a3,' x axis calculation failure')

      else if ( (ierr .eq. 2) .or. (ierr .eq. 12) ) then
         write(6,110) CSName(CSNum)
         if ( logfile ) write(3,110) CSName(CSNum)
110      format(' Error- ',a3,' y axis calculation failure')

      else if ( (ierr .eq. 4) .or. (ierr .eq. 14) ) then
         write(6,120) CSName(CSNum)
         if ( logfile ) write(3,120) CSName(CSNum)
120      format(' Error- ',a3,' z axis calculation failure')

      else if ( (ierr .eq. 3) .or. (ierr .eq. 13) ) then
         write(6,130) CSName(CSNum)
         if ( logfile ) write(3,130) CSName(CSNum) 
130      format(' Error- ',a3,' x and y axis calculation failure')

      else if ( (ierr .eq. 5) .or. (ierr .eq. 15) ) then
         write(6,150) CSName(CSNum)
         if ( logfile ) write(3,150) CSName(CSNum) 
150      format(' Error- ',a3,' x and z axis calculation failure')

      else if ( (ierr .eq. 6) .or. (ierr .eq. 16) ) then
         write(6,160) CSName(CSNum)
         if ( logfile ) write(3,160) CSName(CSNum) 
160      format(' Error- ',a3,' x and y axis calculation failure')

      else if ( (ierr .eq. 7) .or. (ierr .eq. 17) ) then
         write(6,170) CSName(CSNum)
         if ( logfile ) write(3,170) CSName(CSNum) 
170      format(' Error- ',a3,' x y and z axis calculation failure')

      else if ( ierr .eq. 8 ) then
         write(6,173)
         if ( logfile ) write(3,173)
173      format(' Error- Normalization of sacral stick vector failed')

      else if ( ierr .eq. 9 ) then
         write(6,175)
         if ( logfile ) write(3,175)
175      format(' Error- Normalization of ASIS vector failed')

      else if ( ierr .eq. 10 ) then
         write(6,177)
         if ( logfile ) write(3,177)
177      format(' Error- Calculation of pelvic triangle center failed')


      else if ( ierr .eq. 20 ) then
         write(6,180)
         if ( logfile ) write(3,180)
180      format(' Error- Normalization of ankle vector failed')

      else if ( ierr .eq. 30 ) then
         write(6,190)
         if ( logfile ) write(3,190)
190      format(' Error- Normalization of foot vector failed')

      else if ( ierr .eq. 40 ) then
         write(6,200)
         if ( logfile ) write(3,200)
200      format(' Error- Line bisector calculation failed')

      else if ( ierr .eq. 50 ) then
         write(6,210)
         if ( logfile ) write(3,210)
210      format(' Error- Hip center calculation failed')
      end if                 
      return
      end
c                                
c                                                   
c
      subroutine FootCCInit2( MRK_AM,MRK_AL,MRK_FF,MRK_FR,MRK_TO,MRK_HL,
     &                        ampn,SP,ac,ierr )
      implicit none                  
      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_names.inc'                         

      record /seg_param/ SP
                                 
      integer*4 MRK_AM,MRK_AL,MRK_FF,MRK_FR,MRK_TO,MRK_HL,ierr
      real*4    ampn(3,MAX_MRK),ac(3)

      integer*4 i,mn,vecnorm,MinLineXPnts         
      real*4    ftLCS(3,4),ftTCS(3,4),veca(3),nrma(3),vecb(3),nrmb(3),
     &          veclth,fp(3)
c                                           
c     Determine ankle center         
c
      ierr= 0
      call vecsub(ampn(x,MRK_AM),ampn(x,MRK_AL),veca)
      ierr= vecnorm(veca,nrma)
      if ( ierr .ne. 0 ) goto 1020
      call vecsub(ampn(x,MRK_TO),ampn(x,MRK_HL),vecb)
      ierr= vecnorm(vecb,nrmb)
      if ( ierr .ne. 0 ) goto 1030
      ierr= MinLineXPnts( nrma,ampn(x,MRK_AL),nrmb,ampn(x,MRK_HL),ac,fp )
      if ( ierr .ne. 0 ) goto 1040
c      do i= x, z
c        ac(i)= (ampn(i,MRK_AM)+ampn(i,MRK_AL))/2.0
c      end do
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_FF),ac,ampn(x,MRK_FR),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 error
      return 
1030  ierr= 30   ! normalization of foot vector error
      return                                                 
1040  ierr= 40   ! Line bisector calc error
      return                                   
      end   
c                                
c                                                   
c
      subroutine FootCCInit3( MRK_AM,MRK_AL,MRK_TO,MRK_HL,MRK_MF,
     &                        ampn,SP,ac,ierr )
      implicit none                  
      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_names.inc'                         

      record /seg_param/ SP
                                 
      integer*4 MRK_AM,MRK_AL,MRK_TO,MRK_HL,MRK_MF,ierr
      real*4    ampn(3,MAX_MRK),ac(3)                         

      integer*4 i,mn,vecnorm,MinLineXPnts         
      real*4    ftLCS(3,4),ftTCS(3,4),veca(3),nrma(3),vecb(3),nrmb(3),
     &          veclth,fp(3)
c                                           
c     Determine ankle center         
c
      ierr= 0
      call vecsub(ampn(x,MRK_AM),ampn(x,MRK_AL),veca)
      ierr= vecnorm(veca,nrma)
      if ( ierr .ne. 0 ) goto 1020
      call vecsub(ampn(x,MRK_TO),ampn(x,MRK_HL),vecb)
      ierr= vecnorm(vecb,nrmb)
      if ( ierr .ne. 0 ) goto 1030
      ierr= MinLineXPnts( nrma,ampn(x,MRK_AL),nrmb,ampn(x,MRK_HL),ac,fp )
      if ( ierr .ne. 0 ) goto 1040
c      do i= x, z
c        ac(i)= (ampn(i,MRK_AM)+ampn(i,MRK_AL))/2.0
c      end do
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 error
      return 
1030  ierr= 30   ! normalization of foot vector error
      return                                                 
1040  ierr= 40   ! Line bisector calc error
      return                                   
      end   
c                                
c                                                   
c
      subroutine CalfCCInit( MRK_AM,MRK_AL,MRK_KM,MRK_KL,MRK_SF,MRK_SU,MRK_SB,
     &                       ampn,SP,ac,side,ierr )
      implicit none                  
      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_names.inc'                         

      record /seg_param/ SP
                                 
      integer*4 MRK_AM,MRK_AL,MRK_KM,MRK_KL,MRK_SF,MRK_SU,MRK_SB,ierr,side
      real*4    ampn(3,MAX_MRK),ac(3)

      integer*4 i,mn,vecnorm
      real*4    cfLCS(3,4),cfTCS(3,4),veclth,kc(3)

      ierr= 0
c                                           
c     Determine knee center
c                                                                         
      do i= x, z
        kc(i)= (ampn(i,MRK_KL) + ampn(i,MRK_KM))/2.0
      end do
c                                                 
c     Calculate calf LCS and position of TCS markers relative to it
c
      call CalcLCSCalfCC( ampn(x,MRK_KL),ampn(x,MRK_KM),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                        
      end   
c                                
c                                                   
c
      subroutine ThighCCInit( MRK_KM,MRK_KL,MRK_TF,MRK_TU,MRK_TB,
     &                       ampn,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_KM,MRK_KL,MRK_TF,MRK_TU,MRK_TB,ierr,side
      real*4    ampn(3,MAX_MRK),hc(3)
                            
      integer*4 i,mn,vecnorm
      real*4    thLCS(3,4),thTCS(3,4),veclth,kc(3)

      ierr= 0
c                                           
c     Determine knee center
c                                                                         
      do i= x, z
        kc(i)= (ampn(i,MRK_KL) + ampn(i,MRK_KM))/2.0
      end do                     
c                                                 
c     Calculate thigh LCS and position of TCS markers relative to it
c
      call CalcLCSThighCC( ampn(x,MRK_KL),ampn(x,MRK_KM),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                        
      end   
c                                
c                                                   
c                                
      subroutine PelvisCCInit( MRK_RAS,MRK_LAS,MRK_SAC,MRK_SAI,MRK_KM,MRK_KL,
     &                         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
      real*4    ampn(3,MAX_MRK),hc(3)

      integer*4 i,mn,vecnorm                                  
      real*4    pvLCS(3,4),pvTCS(3,4),veclth,kc(3),stklen

      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     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),ampn(x,MRK_KL),ampn(x,MRK_KM),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   
c
c
c
      subroutine CalcHipTyl( ras,las,sac,saci,knl,knm,side,hc,ierr )
      implicit none   

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

      real*4    ras(3),las(3),sac(3),saci(3),knm(3),knl(3),hc(3)
      integer*4 side,ierr                             

      real*4    stklen,kc(3),veca(3),nrma(3),sacpnt(3),nrmRL(3),width,
     &          vecb(3),vecIS(3),nrmIS(3),vecPA(3),nrmPA(3),vecc(3),tlen,
     &          vecmag
      integer*4 i,vecnorm        

      ierr= 0
      stklen= 8.0*.0254 ! sacral stick length
c
c     Determine knee center
c                                                                         
      do i= x, z
        kc(i)= (knl(i) + knm(i))/2.0
      end do
c                                 
c     Calculate point at base of sacral stick
c        
      call vecsub( saci,sac,veca )
      ierr= vecnorm( veca,nrma )
      if ( ierr .gt. 0 ) goto 1000                    
      do i= x, z
        sacpnt(i)= sac(i) + stklen*nrma(i)
      end do
c                         
c      Calculate ASIS vector (from right to left) and pelvis width              
c
      call vecsub( las,ras,veca )
      ierr= vecnorm( veca,nrmRL )
      if ( ierr .ne. 0 ) goto 1000
      width= vecmag( veca )      
c                         
c     Calculate anterior direction and distance from correct ASIS
c     marker (right or left) to corresponding knee center
c
      call vecsub( sacpnt,ras,vecb )
      call vecx( veca,vecb,vecIS )
      ierr= vecnorm( vecIS,nrmIS )
      if ( ierr .ne. 0 ) goto 1000          
      call vecx( veca,vecIS,vecPA )
      ierr= vecnorm( vecPA,nrmPA )             
      if ( ierr .ne. 0 ) goto 1000
      if ( side .eq. rt ) then    
        call vecsub( ras,kc,vecc )
      else
        call vecsub( las,kc,vecc )
      end if
      tlen= vecmag(vecc)        
c
c     Do the actual hip center calculation
c
      if ( side .eq. rt ) then
        do i= x, z
          hc(i)= ras(i) + 0.11*width*nrmRL(i) - 0.21*width*nrmPA(i) 
     &                  - 0.12*tlen*nrmIS(i)
        end do    
      else
        do i= x, z
          hc(i)= las(i) - 0.11*width*nrmRL(i) - 0.21*width*nrmPA(i) 
     &                  - 0.12*tlen*nrmIS(i)
        end do    
      end if
      return 
                   
1000  ierr= 1
      return
      end         
c        
c                     
c                                           
      subroutine CalcLCSPelvisCC( ras,las,sac,saci,LCS,ierr )
      implicit none

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

      real*4    ras(3),las(3),sac(3),saci(3),LCS(3,4)
      integer*4 side,ierr                             

      real*4    vecY(3),vecZ(3),vecPA(3),stklen,veca(3),nrma(3),sacpnt(3),
     &          nrmY(3)
      integer*4 i,vecnorm,istat
                                 
      ierr= 0
      stklen= 8.0*.0254 ! sacral stick length
c
c     Calculate point at base of sacral stick          
c
      call vecsub( saci,sac,veca )
      istat= vecnorm( veca,nrma )
      if ( istat .gt. 0 ) goto 1000
      do i= x, z
        sacpnt(i)= sac(i) + stklen*nrma(i)
      end do
c
c     Move ASIS points medially by 1/2 the marker diameter
c                      
      call vecsub( las,ras,vecY )
      istat= vecnorm( vecY,nrmY )
      if ( istat .ne. 0 ) goto 1010
      do i= x, z
        las(i)= las(i) - 0.75*.0254*nrmY(i)
        ras(i)= ras(i) + 0.75*.0254*nrmY(i)
      end do
c
c     Calculate the rotation matrix of the pelvis LCS
c
      call vecsub( sacpnt,ras,veca )
      call vecx( vecY,veca,vecZ )
      call vecx( vecY,vecZ,vecPA )
      istat= vecnorm( vecPA,LCS(x,x) )
      if ( istat .ne. 0 ) ierr= ierr + 1
      istat= vecnorm( vecY,LCS(x,y) )
      if ( istat .ne. 0 ) ierr= ierr + 2
      istat= vecnorm( vecZ,LCS(x,z) )
      if ( istat .ne. 0 ) ierr= ierr + 4
      call tricen( ras,las,sacpnt,LCS(x,cntr),istat )
      if ( istat .ne. 0 ) ierr= 10
      return

1000  ierr= 8
      return
1010  ierr= 9
      return                 
      end
c
c
c
      subroutine CalcLCSThighCC( mkn1,mkn2,hc,side,sf,LCS,ierr )
      implicit none

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

      real*4    mkn1(3),mkn2(3),hc(3),sf,LCS(3,4)
      integer*4 side,ierr

      real*4    kc(3),vecY(3),vecZ(3),vecPA(3)
      integer*4 i,vecnorm,istat

      ierr= 0                 
      do i= x, z
        kc(i)= (mkn1(i)+mkn2(i))/2.0
      end do                    
      call vecsub( mkn2,mkn1,vecY )
      if ( side .eq. lf ) then
        do i= x, z
          vecY(i)= -vecY(i)  
        end do
      end if
      call vecsub( hc,kc,vecZ )
      call vecx( vecY,vecZ,vecPA )
      call vecx( vecZ,vecPA,vecY )
      istat= vecnorm( vecPA,LCS(x,x) )
      if ( istat .ne. 0 ) ierr= ierr + 1
      istat= vecnorm( vecY,LCS(x,y) )
      if ( istat .ne. 0 ) ierr= ierr + 2
      istat= vecnorm( vecZ,LCS(x,z) )
      if ( istat .ne. 0 ) ierr= ierr + 4
      do i= x, z
        LCS(i,cntr)= kc(i) + sf*(hc(i)-kc(i))
      end do
      return           
      end
c
c
c        
      subroutine CalcLCSCalfCC( mkn1,mkn2,ac,side,sf,LCS,ierr )
      implicit none

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

      real*4    mkn1(3),mkn2(3),ac(3),sf,LCS(3,4)
      integer*4 side,ierr                                    

      real*4    kc(3),vecY(3),vecZ(3),vecPA(3)
      integer*4 i,vecnorm,istat

      ierr= 0
      do i= x, z
        kc(i)= (mkn1(i)+mkn2(i))/2.0
      end do                    
      call vecsub( mkn2,mkn1,vecY )
      if ( side .eq. lf ) then
        do i= x, z
          vecY(i)= -vecY(i)  
        end do
      end if
      call vecsub( kc,ac,vecZ )
      call vecx( vecY,vecZ,vecPA )
      call vecx( vecZ,vecPA,vecY )
      istat= vecnorm( vecPA,LCS(x,x) )
      if ( istat .ne. 0 ) ierr= ierr + 1
      istat= vecnorm( vecY,LCS(x,y) )
      if ( istat .ne. 0 ) ierr= ierr + 2
      istat= vecnorm( vecZ,LCS(x,z) )
      if ( istat .ne. 0 ) ierr= ierr + 4
      do i= x, z
        LCS(i,cntr)= ac(i) + sf*(kc(i)-ac(i))
      end do
      return
      end
c
c
c
      subroutine CalcTCS( mf,mu,mb,LCS,ierr )
      implicit none

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

      real*4    mf(3),mu(3),mb(3),LCS(3,4)
      integer*4 ierr

      real*4    vecY(3),vecZ(3),vecPA(3),vecA(3)
      integer*4 i,vecnorm,istat

      ierr= 0
      call vecsub( mf,mb,vecPA )
      istat= vecnorm( vecPA,vecPA )
      if ( istat .ne. 0 ) then
        ierr= 8
        return
      end if
      call vecsub( mu,mb,vecA )
      istat= vecnorm( vecA,vecA )
      if ( istat .ne. 0 ) then
        ierr= 9
        return
      end if
      call vecx( vecA,vecPA,vecY )
      call vecx( vecPA,vecY,vecZ )
      istat= vecnorm( vecPA,LCS(x,x) )
      if ( istat .ne. 0 ) ierr= ierr + 1
      istat= vecnorm( vecY,LCS(x,y) )
      if ( istat .ne. 0 ) ierr= ierr + 2
      istat= vecnorm( vecZ,LCS(x,z) )
      if ( istat .ne. 0 ) ierr= ierr + 4
      do i= x, z
        LCS(i,cntr)= (mb(i)+mf(i))/2.0
      end do                    
      return
      end           
c                                            
c
c                                                   
      subroutine CalcCCBodySegPos( 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_clvlnd.inc'

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

      integer*4 num13(3),i,frm,j,side,foot,calf,thgh,uarm,larm,mshf,mshu,mshb,
     &          mthf,mthu,mthb,mtoe,mhel,makl,mkne,mhip,seg,ierr,spvp,disp3ds,
     &          np,melb,mwri,mshl,mmft
      real*4    mpn(3,MAX_MRK),tmpsgp(3,4),mnec(3),
     &          max(MAX_MTN,MAX_SEG),min(MAX_MTN,MAX_SEG),
     &          avg(MAX_MTN,MAX_SEG),stdev(MAX_MTN,MAX_SEG),scf(MAX_MTN,MAX_SEG)
      logical*4 UseSimple,UseDisp3d,UseSpVp,DumpDiag

      data num13 / 1,2,3 /

      SF.position= .false.
      if ( .not. SF.RightSideAlign ) goto 1000 ! error
      if ( .not. SF.LeftSideAlign )  goto 1010 ! error
c
c     Check for rigid body algorithm selection option
c                    
      UseSimple= .true.
      UseSpvp=   .false.
      UseDisp3d= .false.
      if ( CMND.NumOpt(2) .gt. 0 ) then
        do i= 1, CMND.NumOpt(2)
          if ( CMND.OptText(i,2) .eq. 'SPV' ) then
            UseSimple= .false.
            UseDisp3d= .false.
            UseSpVp=   .true.
          else if ( CMND.OptText(i,2) .eq. 'WOL' ) then
            UseSimple= .false.
            UseSpVp=   .false.
            UseDisp3d= .true.
          else if ( CMND.OptText(i,2) .eq. 'DIA' ) then
            DumpDiag=  .true.
          else
            goto 1020 ! error
          end if
        end do     
      end if               
c
c     Do the calculations frame by frame                                 
c
      do frm= 1, MP.nfrm
        if ( OF.UpDate .and. (mod(frm,10) .eq. 0) )
     &     write(6,100) frm,MP.nfrm
100     format('+Processing frame # ',i3,'/',i3,'.')
c
c       Move a slice of marker position data if a rigid body algorithm
c       will be used. This is needed so that the data is in memory in the
c       proper arrangement for how I wrote the rigid body routines...
c        
        if ( UseSpVp .or. UseDisp3d ) then
          do i= 1, MP.num_mrk
            do j= x, z
              mpn(j,i)= MrkData(j,frm,i)
            end do
          end do
        end if
c
c       Do each side of the body
c
        do side= rt, lf
          if ( side .eq. lf )  then
            foot= lfft
            calf= lfcf                   
            thgh= lfth
            uarm= lfua
            larm= lfla
            mshf=  MCC_LFSF
            mshu=  MCC_LFSU
            mshb=  MCC_LFSR                    
            mthf=  MCC_LFTF
            mthu=  MCC_LFTU
            mthb=  MCC_LFTR
            mtoe=  MCC_LFTO             
            mhel=  MCC_LFHL
            makl=  MCC_LFAK
            mmft=  MCC_LFMF  
            mkne=  MCC_LFKN
            mhip=  MCC_LFHP
            melb=  MCC_LFEL
            mwri=  MCC_LFWR
            mshl=  MCC_LFSH
          else
            foot= rtft
            calf= rtcf
            thgh= rtth
            uarm= rtua
            larm= rtla
            mshf=  MCC_RTSF
            mshu=  MCC_RTSU
            mshb=  MCC_RTSR
            mthf=  MCC_RTTF
            mthu=  MCC_RTTU
            mthb=  MCC_RTTR
            mtoe=  MCC_RTTO
            mhel=  MCC_RTHL
            makl=  MCC_RTAK
            mmft=  MCC_RTMF
            mkne=  MCC_RTKN
            mhip=  MCC_RTHP
            melb=  MCC_RTEL
            mwri=  MCC_RTWR
            mshl=  MCC_RTSH
          end if
c                                                          
c       First do the calves and reconstruct the ankle and knee points from
c       the calf motion also
c                                                    
          seg= calf
          if ( UseSimple ) then
            call CalcTCS( MrkData(x,frm,mshf),
     &                    MrkData(x,frm,mshu),
     &                    MrkData(x,frm,mshb),tmpsgp,ierr )
            if ( ierr .ne. 0 ) goto 1030
            call CalcTCStoLCS( tmpsgp,SP(calf).TCStoLCS,
     &                         SgpGCS(x,x,frm,calf) )

          else if ( UseSpVp ) then
            ierr= spvp( SP(calf).mpnLCS,mpn,SP(calf).nmTCS,
     &                  num13,SP(calf).mnTCS,SgpGCS(x,x,frm,calf),
     &                  max(frm,calf),min(frm,calf),avg(frm,calf),stdev(frm,calf) )
            if ( ierr .ne. 0 ) then
              np= SP(calf).nmTCS
              goto 1050
            end if

          else if ( UseDisp3d ) then
            ierr= disp3ds( SP(calf).mpnLCS,mpn,SP(calf).nmTCS,
     &                     num13,SP(calf).mnTCS,SgpGCS(x,x,frm,calf),
     &                     max(frm,calf),min(frm,calf),avg(frm,calf),
     &                     stdev(frm,calf),scf(frm,calf) )
            if ( ierr .ne. 0 ) then
              np= SP(calf).nmTCS
              goto 1050
            end if     
          end if           
                  
          call PntChangeCS( SgpGCS(x,x,frm,calf),
     &                      SP(calf).end(x,distal),
     &                      MrkData(x,frm,makl) )
          call PntChangeCS( SgpGCS(x,x,frm,calf),
     &                      SP(calf).end(x,proximal),
     &                      MrkData(x,frm,mkne) )
c                                                      
c         Now calculate the foot LCS
c                                          
          seg= foot
          if ( UseSimple ) then
c            call CalcLCSFoot( MrkData(x,frm,mtoe),  ! used with 2 marker
c     &                        MrkData(x,frm,makl),  ! cleveland clinic version
c     &                        MrkData(x,frm,mhel),0.571,tmpsgp,ierr )
            call CalcLCSFoot( MrkData(x,frm,mtoe),
     &                        MrkData(x,frm,mmft),
     &                        MrkData(x,frm,mhel),0.571,tmpsgp,ierr )
            if ( ierr .ne. 0 ) goto 1030
            call CalcTCStoLCS( tmpsgp,SP(foot).TCStoLCS,
     &                         SgpGCS(x,x,frm,foot) )

          else if ( UseSpVp ) then
            ierr= spvp( SP(foot).mpnLCS,mpn,SP(foot).nmTCS,
     &                  num13,SP(foot).mnTCS,SgpGCS(x,x,frm,foot),
     &                  max(frm,foot),min(frm,foot),avg(frm,foot),stdev(frm,foot) )
            if ( ierr .ne. 0 ) then
              np= SP(foot).nmTCS
              goto 1050
            end if

          else if ( UseDisp3d ) then
            ierr= disp3ds( SP(foot).mpnLCS,mpn,SP(foot).nmTCS,
     &                     num13,SP(foot).mnTCS,SgpGCS(x,x,frm,foot),
     &                     max(frm,foot),min(frm,foot),avg(frm,foot),
     &                     stdev(frm,foot),scf(frm,foot) )
            if ( ierr .ne. 0 ) then
              np= SP(foot).nmTCS
              goto 1050
            end if                            
          end if
c                                                          
c       Reconstruct the thigh LCS
c                                                    
          seg= thgh           
          if ( UseSimple ) then
            call CalcTCS( MrkData(x,frm,mthf),
     &                    MrkData(x,frm,mthu),
     &                    MrkData(x,frm,mthb),tmpsgp,ierr )
            if ( ierr .ne. 0 ) goto 1030
            call CalcTCStoLCS( tmpsgp,SP(thgh).TCStoLCS,
     &                         SgpGCS(x,x,frm,thgh) )

          else if ( UseSpVp ) then 
            ierr= spvp( SP(thgh).mpnLCS,mpn,SP(thgh).nmTCS,
     &                  num13,SP(thgh).mnTCS,SgpGCS(x,x,frm,thgh),
     &                  max(frm,thgh),min(frm,thgh),avg(frm,thgh),stdev(frm,thgh) )
            if ( ierr .ne. 0 ) then
              np= SP(thgh).nmTCS
              goto 1050
            end if

          else if ( UseDisp3d ) then
            ierr= disp3ds( SP(thgh).mpnLCS,mpn,SP(thgh).nmTCS,
     &                     num13,SP(thgh).mnTCS,SgpGCS(x,x,frm,thgh),
     &                     max(frm,thgh),min(frm,thgh),avg(frm,thgh),
     &                     stdev(frm,thgh),scf(frm,thgh) )
            if ( ierr .ne. 0 ) then
              np= SP(thgh).nmTCS
              goto 1050
            end if
          end if                     
c                  
c     Reconstruct the upper arm LCS  
c
          seg= uarm
          Call CalcLCSAntMrk( MrkData(x,frm,melb),
     &                        MrkData(x,frm,mwri),
     &                        MrkData(x,frm,mshl),
     &                        0.570,SgpGCS(x,x,frm,uarm),ierr )
          if ( ierr .ne. 0 ) goto 1030
c                                
c     Reconstruct the lower arm LCS  
c
          seg= larm
          Call CalcLCSAntMrk( MrkData(x,frm,mwri),
     &                        MrkData(x,frm,mshl),
     &                        MrkData(x,frm,melb),            
     &                        0.570,SgpGCS(x,x,frm,larm),ierr )
          if ( ierr .ne. 0 ) goto 1030

        end do ! side
c
c       Reconstruct the pelvis and calculate the position of the hips
c
        seg= pelv
        if ( UseSimple ) then
          call CalcLCSPelvis( MrkData(x,frm,MCC_RTAS),
     &                        MrkData(x,frm,MCC_SACR),
     &                        MrkData(x,frm,MCC_LFAS),
     &                        8.0*0.0254,tmpsgp,ierr )
          if ( ierr .ne. 0 ) goto 1030
          call CalcTCStoLCS( tmpsgp,SP(pelv).TCStoLCS,
     &                       SgpGCS(x,x,frm,pelv) )

        else if ( UseSpVp ) then                     
          ierr= spvp( SP(pelv).mpnLCS,mpn,SP(pelv).nmTCS,
     &                num13,SP(pelv).mnTCS,SgpGCS(x,x,frm,pelv),
     &                max(frm,pelv),min(frm,pelv),avg(frm,pelv),stdev(frm,pelv) )
          if ( ierr .ne. 0 ) then
            np= SP(pelv).nmTCS
            goto 1050
          end if

        else if ( UseDisp3d ) then
          ierr= disp3ds( SP(pelv).mpnLCS,mpn,SP(pelv).nmTCS,
     &                   num13,SP(pelv).mnTCS,SgpGCS(x,x,frm,pelv),
     &                   max(frm,pelv),min(frm,pelv),avg(frm,pelv),
     &                   stdev(frm,pelv),scf(frm,pelv) )
          if ( ierr .ne. 0 ) then
            np= SP(pelv).nmTCS
            goto 1050
          end if
        end if                     

        call PntChangeCS( SgpGCS(x,x,frm,pelv),
     &                    SP(pelv).end(x,rt),
     &                    MrkData(x,frm,MCC_RTHP) )
        call PntChangeCS( SgpGCS(x,x,frm,pelv), 
     &                    SP(pelv).end(x,lf),
     &                    MrkData(x,frm,MCC_LFHP) )
c
c       Reconstruct the trunk
c
        seg= trnk
        call CalcLCSTrunk( MrkData(x,frm,MCC_RTSH),
     &                     MrkData(x,frm,MCC_LFSH),
     &                     MrkData(x,frm,MCC_RTAS),
     &                     MrkData(x,frm,MCC_LFAS),
     &                     MrkData(x,frm,MCC_SACR),
     &                     SgpGCS(x,x,frm,trnk),ierr )
        if ( ierr .ne. 0 ) goto 1030
c                                    
c       Reconstruct the head- fake it actually for aesthetic reasons :)
c
        do i= 1, 3
          mnec(i)= (MrkData(i,frm,MCC_RTSH)+MrkData(i,frm,MCC_LFSH))/2.0
        end do
        call CalcLCSHead( mnec,SgpGCS(x,x,frm,trnk),
     &                    SgpGCS(x,x,frm,head) )

      end do ! frm                  
c
c     Dump diagnostics if desired...
c              
      if ( DumpDiag .and. (UseSpVp .or. UseDisp3d) ) then
        open(unit=1,file='sgp_rb.dia',status='unknown',iostat=ierr,err=1030)
        do i= 1, GP.num_seg
          seg= GP.seg_num(i)
          if ( UseSpVp ) write(1,500) seg_names(seg)
500       format(' Spoor-Veldpaus algorithm for ',a4/
     &           1x,'Frame Average',5x,'Std Dev',7x,'Min',7x,'Max'/)
          if ( UseDisp3d ) write(1,505) seg_names(seg)
505       format(' Woltring Disp3d algorithm for ',a4/
     &           1x,'Frame Average',5x,'Std Dev',7x,'Min',7x,'Max',
     &           7x,'Scale Factor'/)
          do frm= 1, MP.nfrm
            if ( UseSpVp ) then
              write(1,510) frm,avg(frm,seg),stdev(frm,seg),min(frm,seg),
     &                     max(frm,seg)
510           format(1x,i4,4(1x,g12.5))
            else
              write(1,520) frm,avg(frm,seg),stdev(frm,seg),min(frm,seg),
     &                     max(frm,seg),scf(frm,seg)
520           format(1x,i4,5(1x,g12.5))
            end if
          end do
        end do
        close(unit=1)
      end if                                       

      SF.position= .true.
      num_init_empty_mrk= 0
      return

1000  write(6,1005)       
      if ( OF.ToLog ) write(3,1005)
1005  format(' Error- Right side of Cleveland Clinic marker set has not been',
     &       ' aligned'/'        This can be done with the MRK SLP command')
      return

1010  write(6,1015)
      if ( OF.ToLog ) write(3,1015)
1015  format(' Error- Left side of Cleveland Clinic marker set has not been',
     &       ' aligned'/'        This can be done with the MRK SLP command')
      return

1020  write(6,1025) CMND.OptText(i,2)
      if ( OF.ToLog ) write(3,1025) CMND.OptText(i,2)
1025  format(' Error- ',a3,' is not a valid option for SEG POS for the Cleveland'/
     &       '        Clinic marker set'/
     &       '        Valid options are: /SPVp /WOLtring /DIAgnostics')
      return

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

1050  write(6,1055) seg_names(seg),frm            
      if ( OF.ToLog ) write(3,1055) seg_names(seg),frm
1055  format(' Error- Rigid body calculation failure on ',a4,' at frame # ',i4)
      call handle_rb_err( ierr,np,seg,frm )
      return            
      end                                              
c                               
c 
c
      subroutine CalcCCBodySegEnds( frm1,nfrm,SF,error )
      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_clvlnd.inc'

      record /segment_flag/   SF

      integer*4 frm1,nfrm
      logical*4 error

      integer*4 frm,i,axis,seg
      real*4    PlvPnt(3,MAX_MTN),NeckPnt(3,MAX_MTN),avg_pos(3)

      error= .true.
      if ( .not. SF.position ) return
      error= .false.
c           
c       Trunk-Pelvis - relative to pelvis and trunk.  NOTE that joint relative
c       to pelvis is at (0,0,0) by definition so calculation of pelvis LCS
c       center relative to trunk LCS is equivalent to finding the joint
c       relative to the trunk LCS.
c
      do frm= 1, MP.nfrm
        do i= x, z                                       
          PlvPnt(i,frm)= SgpGCS(i,cntr,frm,pelv)
        end do
      end do
      call AvgSegJntPos( SgpGCS(x,x,1,trnk),PlvPnt,
     &                   frm1,nfrm,3,SP(trnk) )
      do i= 1, 3
        SP(pelv).end(i,3)= 0.0
      end do
c                                         
c       Neck - relative to trunk and head
c
      do frm= 1, MP.nfrm 
        do i= x, z                                           
          NeckPnt(i,frm)= (MrkData(i,frm,MCC_RTSH) +
     &                     MrkData(i,frm,MCC_LFSH) )/2.0
        end do
      end do
      call AvgSegJntPos( SgpGCS(x,x,1,trnk),NeckPnt,
     &                   frm1,nfrm,4,SP(trnk) )
      call AvgSegJntPos( SgpGCS(x,x,1,head),NeckPnt,
     &                   frm1,nfrm,proximal,SP(head) )
c
c     Elbow - rt & lf - relative to lower and upper arms
c
      call AvgSegJntPos( SgpGCS(x,x,1,rtla),MrkData(x,1,MCC_RTEL),
     &                   frm1,nfrm,proximal,SP(rtla) )
      call AvgSegJntPos( SgpGCS(x,x,1,lfla),MrkData(x,1,MCC_LFEL),
     &                   frm1,nfrm,proximal,SP(lfla) )

      call AvgSegJntPos( SgpGCS(x,x,1,rtua),MrkData(x,1,MCC_RTEL),
     &                   frm1,nfrm,distal,SP(rtua) )
      call AvgSegJntPos( SgpGCS(x,x,1,lfua),MrkData(x,1,MCC_LFEL),
     &                   frm1,nfrm,distal,SP(lfua) )
c             
c     Shoulder - rt & lf - relative to upper arm and trunk
c
      call AvgSegJntPos( SgpGCS(x,x,1,rtua),MrkData(x,1,MCC_RTSH),
     &                   frm1,nfrm,proximal,SP(rtua) )
      call AvgSegJntPos( SgpGCS(x,x,1,lfua),MrkData(x,1,MCC_LFSH),
     &                   frm1,nfrm,proximal,SP(lfua) )

      call AvgSegJntPos( SgpGCS(x,x,1,trnk),MrkData(x,1,MCC_RTSH),
     &                   frm1,nfrm,rt,SP(trnk) )
      call AvgSegJntPos( SgpGCS(x,x,1,trnk),MrkData(x,1,MCC_LFSH),
     &                   frm1,nfrm,lf,SP(trnk) )
c
c     Wrist - rt & lf - relative to lower arms
c
      call AvgSegJntPos( SgpGCS(x,x,1,rtla),MrkData(x,1,MCC_RTWR),
     &                   frm1,nfrm,distal,SP(rtla) )
      call AvgSegJntPos( SgpGCS(x,x,1,lfla),MrkData(x,1,MCC_LFWR),
     &                   frm1,nfrm,distal,SP(lfla) )
c
c     Calculate the average segment center position.  This is used as the
c     reference position for calculating segment potential energies later
c     on.  Note that it would be preferable to use standing position data
c     to find these positions but that motion can be used to get and average
c     height of the segment.  
c
      do i= 1, GP.num_seg
        seg= GP.seg_num(i)
        do axis= x, z
          avg_pos(axis)= 0.0
        end do ! axis
        do frm= frm1, frm1+nfrm-1
          do axis= x, z       
            avg_pos(axis)= avg_pos(axis) + SgpGCS(axis,cntr,frm,seg)
          end do ! axis
        end do ! frm   
        do axis= x, z
          SP(seg).avg_cntr(axis)= avg_pos(axis)/float(nfrm)
        end do ! axis
      end do ! i

      return    
      end           
c
c
c
      subroutine AvgSegJntPos( sgp,mpn,frm1,nfrm,nend,SP )
      implicit none

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

      record /seg_param/ SP

      real*4    sgp(3,4,MAX_MTN),mpn(3,MAX_MTN)
      integer*4 frm1,nfrm,nend
 
      integer*4 i,j
      real*4    pnt(3),tmp(3)

      do i= 1, 3
        pnt(i)= 0.0
      end do
      do j= frm1, frm1+nfrm-1
        call PntGCStoLCS( sgp(1,1,j),mpn(1,j),tmp )
        do i= 1, 3
          pnt(i)= pnt(i) + tmp(i)
        end do
      end do
      do i= 1, 3
        SP.end(i,nend)= pnt(i)/float(nfrm)
      end do
      return
      end
c
c
c
      subroutine CalcCCBodySegShape( frm1,nfrm )
      implicit none
                                                            
      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_prm.inc'
      include 'a_mrk.inc'
      include 'a_seg.inc'
      include 'a_names.inc'                   
      include 'a_clvlnd.inc'

      integer*4 frm1,nfrm

      real*4    scale(5),veclth,svec(3),msh(3),mas(3),tlen,shwd,vecmag
      integer*4 uarm(2),larm(2),i

c                  foot   calf   thigh  uparm  lwarm
      data scale / 0.571, 0.567, 0.567, 0.564, 0.570 / ! from Dempster (1955)
c
c     First calculate the arm segment shapes
c
      uarm(rt)= rtua
      uarm(lf)= lfua
      larm(rt)= rtla
      larm(lf)= lfla  
      do i= rt, lf
        SP(uarm(i)).length= veclth( SP(uarm(i)).end(x,distal),
     &                                   SP(uarm(i)).end(x,proximal) )
        call CalcUarmShape( scale(4),SP(uarm(i)) )
        SP(larm(i)).length= veclth( SP(larm(i)).end(x,distal),
     &                                   SP(larm(i)).end(x,proximal) )
        call CalcLarmShape( scale(4),SP(larm(i)) )
      end do
c
c     Calculate the segment shape point locations for the trunk.  
c                         
      call avgvec( nfrm,MrkData(1,frm1,MCC_RTSH),
     &             MrkData(1,frm1,MCC_LFSH),svec )
      call avgmidpnt( nfrm,MrkData(1,frm1,MCC_RTSH),
     &                MrkData(1,frm1,MCC_LFSH),msh )
      call avgmidpnt( nfrm,MrkData(1,frm1,MCC_RTAS),
     &                MrkData(1,frm1,MCC_LFAS),mas )
      tlen= veclth( msh,mas ) 
      shwd= vecmag( svec )                  
      SP(trnk).length= shwd
c                   
c     calculate trunk segment points
c           
      call CalcTrunkShape( tlen,shwd,SP(trnk),SP(pelv) )
c
c     calculate head segment points
c
      call CalcHeadShape( shwd,SP(head) )      
      return    
      end
 
