c
c
c
      subroutine InitCKCAnalysis
      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_ckc.inc'

      integer*4 i          
c
c     Set up marker info
c
      num_init_empty_mrk= 3
      if ( MP.num_mrk .eq. 20 ) then
        init_empty_mrk(1)= 18  ! rtak
        init_empty_mrk(2)= 19  ! rtkn
        init_empty_mrk(3)= 20  ! rthp
      else
        init_empty_mrk(1)= 17  ! rtak
        init_empty_mrk(2)= 18  ! rtkn
        init_empty_mrk(3)= 19  ! rthp
      end if
c                          
c     initialize marker names
c
      MNC_SACR= 1
      MNC_APLV= 2
      MNC_RASI= 3
      MNC_LASI= 4 
      MNC_RTH1= 5
      MNC_RTH2= 6
      MNC_RTH3= 7
      MNC_LRKN= 8
      MNC_MRKN= 9
      MNC_RCF1= 10
      MNC_RCF2= 11
      MNC_RCF3= 12
      MNC_LRAK= 13                
      if ( MP.num_mrk .eq. 20 ) then ! medial ankle marker present
        MNC_MRAK= 14
        MNC_RTOE= 15
        MNC_RHEL= 16
        MNC_RGTR= 17
        MNC_RTAK= 18
        MNC_RTKN= 19
        MNC_RTHP= 20
      else ! medial ankle marker is missing
        MNC_RTOE= 14
        MNC_RHEL= 15
        MNC_RGTR= 16
        MNC_RTAK= 17
        MNC_RTKN= 18
        MNC_RTHP= 19
        MNC_MRAK= 17 ! just in case...
      end if    
c                                          
c     Setup segment data
c                             
      SP(rtft).num_mrk= 5
      SP(rtcf).num_mrk= 5
      SP(rtth).num_mrk= 5
      SP(pelv).num_mrk= 5

      SP(rtft).mrk_num(1)= MNC_RTOE
      SP(rtft).mrk_num(2)= MNC_RHEL
      SP(rtft).mrk_num(3)= MNC_RTAK
      SP(rtft).mrk_num(4)= MNC_MRAK
      SP(rtft).mrk_num(5)= MNC_LRAK

      SP(rtcf).mrk_num(1)= MNC_RTAK
      SP(rtcf).mrk_num(2)= MNC_RTKN
      SP(rtcf).mrk_num(3)= MNC_MRAK
      SP(rtcf).mrk_num(4)= MNC_LRAK
      SP(rtcf).mrk_num(5)= MNC_MRKN
c      SP(rtcf).mrk_num(6)= MNC_LRKN ! only 5 allowed right now
                              
      SP(rtth).mrk_num(1)= MNC_RTKN
      SP(rtth).mrk_num(2)= MNC_RTHP
      SP(rtth).mrk_num(3)= MNC_MRKN
      SP(rtth).mrk_num(4)= MNC_LRKN
      SP(rtth).mrk_num(5)= MNC_RGTR

      SP(pelv).mrk_num(1)= MNC_RASI
      SP(pelv).mrk_num(2)= MNC_LASI
      SP(pelv).mrk_num(3)= MNC_SACR
      SP(pelv).mrk_num(4)= MNC_APLV
      SP(pelv).mrk_num(5)= MNC_RTHP
c
c     Set up joint connectivity information - Only one scheme allowed here
c
      JNT_BU(rt).num_jnt= 3
      
      JNT_BU(rt).jnt_order(1)=  rtak
      JNT_BU(rt).jnt_order(2)=  rtkn
      JNT_BU(rt).jnt_order(3)=  rthp
      
      JNT_BU(rt).jnt_seg(1)= rtft
      JNT_BU(rt).jnt_seg(2)= rtcf
      JNT_BU(rt).jnt_seg(3)= rtth

      JNT_BU(rt).calc_jnt_num(1)=  proximal
      JNT_BU(rt).calc_jnt_num(2)=  proximal
      JNT_BU(rt).calc_jnt_num(3)=  proximal

      JNT_BU(rt).num_conn_jnt(1)= 0     
      JNT_BU(rt).num_conn_jnt(2)= 1
      JNT_BU(rt).num_conn_jnt(3)= 1 

      JNT_BU(rt).conn_jnt_num(1,1)= 0    ! rtak
      JNT_BU(rt).conn_jnt_num(1,2)= rtak ! rtkn
      JNT_BU(rt).conn_jnt_num(1,3)= rtkn ! rthp

      JNT_BU(rt).conn_jnt_seg(1,1)= 0      ! rtft
      JNT_BU(rt).conn_jnt_seg(1,2)= distal ! rtcf
      JNT_BU(rt).conn_jnt_seg(1,3)= distal ! rtth

      JNT_BU(rt).num_ext_load(1)= 1  ! rtft
      JNT_BU(rt).num_ext_load(2)= 0  ! rtcf
      JNT_BU(rt).num_ext_load(3)= 0  ! rtth

      JNT_BU(rt).ext_load_num(1,1)= 1    ! rtft

      do i= 1, 3
        JNT_BU(rt).CalcOrder(i)= DisttoProx
      end do
c
c     Setup General Parameters
c
      GP.analysis_type= CKC_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	AdjustCKCMrkData  
c
c	Copyright by Dwight Meglan 1992
c
c
c	Purpose:	This is a bit of a kludgey (sp?) routine to adjust the
c				position of some markers in the CKC study marker set
c				and create a couple of new positions from several of
c				marker's positions.  Specificially, the knee and ankle
c				markers are calculated from the medial and lateral markers
c				The hip positions
c				are created using the right and left ASIS markers and
c				the right knee center marker.  This method is documented
c				by Tylkowski (~1983) as well as Brand (1990).
c           
c	Routines called:
c
c	MinLineXPnts
c	Tricen
c	VecMag
c	VecNorm
c	VecSub
c	VecX
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Param			MRK_NUM
c	Param			MP.nfrm
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Marker			MrkData
c
      subroutine AdjustCKCMrkData( MF, OF )
      implicit none
      
      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_prm.inc'
      include 'a_mrk.inc'
      include 'a_names.inc'
      include 'a_ckc.inc'

      record /marker_flag/  MF
      record /option_flag/  OF

      integer*4 side,frm,axis,VecNorm,MinLineXPnts,error,calc_error
      real*4    sign,vecA(3),vecB(3),normA(3),normB(3),thigh_length,asis_width, 
     &          ankle_pnt(3),foot_pnt(3),x_axis(3),z_axis(3),asis_line(3),
     &          VecMag

      calc_error= 0
      MF.adjusted= .false.
      if ( .not. MF.interpolated ) goto 1000 ! error
      if ( MP.num_mrk .eq. 19 ) then
        write(6,120)
120     format(/' NOTE- Using missing medial ankle marker calculation method'/)
        if ( OF.ToLog ) write(3,120)
      end if
      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       First compute the ankle and knee centers from the medial and lateral
c       markers of the ankle and knee respectively. An alternate method has
c       to be used for the ankle when there are only 16 markers present 
c       because in the majority of the data taken the medial ankle marker was
c       not recorded. Thus, when 16 markers are present the method used to 
c       estimate the ankle center is similar to that used with the OSU full
c       body marker set.
c
c       Knee center
c
        do axis= x, z
          MrkData(axis,frm,MNC_RTAK)=
     &      ( MrkData(axis,frm,MNC_MRAK) + MrkData(axis,frm,MNC_LRAK) )/2.0
          MrkData(axis,frm,MNC_RTKN)=
     &      ( MrkData(axis,frm,MNC_MRKN) + MrkData(axis,frm,MNC_LRKN) )/2.0
        end do
c
c       Simple ankle center
c
        if ( MP.num_mrk .eq. 20 ) then
          do axis= x, z
            MrkData(axis,frm,MNC_RTAK)=
     &        ( MrkData(axis,frm,MNC_MRAK) + MrkData(axis,frm,MNC_LRAK) )/2.0
          end do
c
c       Fancy ankle center estimation
c
        else  
          call VecSub( MrkData(x,frm,MNC_MRKN),MrkData(x,frm,MNC_LRKN),vecA )
          calc_error= VecNorm( vecA, normA )
          if ( calc_error .ne. 0 ) goto 1010
          call VecSub( MrkData(x,frm,MNC_RTOE),MrkData(x,frm,MNC_RHEL),vecB )
          calc_error= VecNorm( vecB, normB )
          if ( calc_error .ne. 0 ) goto 1010
          calc_error= MinLineXPnts( normA,MrkData(x,frm,MNC_LRAK),
     &                              normB,MrkData(x,frm,MNC_RHEL),
     &                              ankle_pnt,foot_pnt  )
          if ( calc_error .ne. 0 ) goto 1010
          do axis= x, z
            MrkData(axis,frm,MNC_RTAK)= ankle_pnt(axis)
          end do ! axis
        end if
c
c       Calculate the approximate location of the hip using the method developed
c       by Tylkowski and Simon
c               
        call VecSub( MrkData(x,frm,MNC_LASI),MrkData(x,frm,MNC_RASI),vecA )
        calc_error= VecNorm( vecA, asis_line )
        if ( calc_error .ne. 0 ) goto 1010
        asis_width= VecMag( vecA )
        call VecSub( MrkData(x,frm,MNC_SACR),MrkData(x,frm,MNC_RASI),vecA )
        call VecX( asis_line, vecA, vecB )         
        calc_error= VecNorm( vecB, z_axis )
        if ( calc_error .ne. 0 ) goto 1010
        call VecX( asis_line, z_axis, vecB )   
        calc_error= VecNorm( vecB, x_axis )
        if ( calc_error .ne. 0 ) goto 1010
        call VecSub( MrkData(x,frm,MNC_RASI),MrkData(x,frm,MNC_RTKN),vecA )
        thigh_length= VecMag( vecA )
        do axis= x, z
          MrkData(axis,frm,MNC_RTHP)=
     &               MrkData(axis,frm,MNC_RASI)
     &             + 0.11*sign*asis_width*asis_line(axis)
     &             - 0.21*asis_width*x_axis(axis)
     &             - 0.12*thigh_length*z_axis(axis)
        end do ! axis
      end do ! frm                  
      MF.adjusted= .true.
c      MP.num_mrk= MP.num_mrk + 3
      return
       
1000  write(6,1005)
      if ( OF.ToLog ) write(3,1005)
1005  format(' Error- Marker data has not been checked and interpolated yet.'/
     &       '        Before adjustment can take place, this must be done- MRK INT.')
      return

1010  write(6,1015)
      if ( OF.ToLog ) write(3,1015)
1015  format(' Error- An error occured while calculating the ankle marker position.'/
     &       '        The marker positions were not successfully adjusted and as a'/
     &       '        result further analysis of the data is not possible.')
      return
      end      
c
c
c
      subroutine CalcCKCBodySegPos( 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_ckc.inc'

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

      integer*4 frm,seg,error,NumError(MAX_SEG),ErrFrm(MAX_MTN,MAX_SEG),i
      real*4    stklth
      logical*4 InterpFlag,InterpError,SkipLine,UseAntPlvMrk

      SF.position= .false.
      if ( .not. MF.adjusted ) goto 1000 ! error
c
c     Initialize the error counting array
c                                        
      do i= 1, MAX_SEG
        NumError(i)= 0
      end do ! i                 
c
c     Check for option to use anterior pelvis marker along the right and
c     left ASIS markers to calculate the orientation of the pelvis rather
c     than using the ASIS markers along with the sacral stick. THis is just
c     a check added to compare the two ways of calculating pelvis orientation
c
      UseAntPlvMrk= .false.
      if ( CMND.NumOpt(2) .ne. 0 ) then
        if ( CMND.OptText(1,2) .eq. 'APM' ) then
          UseAntPlvMrk= .true.
        else
          write(6,120) CMND.OptText(1,2)
          if ( OF.ToLog ) write(3,120) CMND.OptText(1,2)
120       format(' ERROR- ',a3,' is not a valid option of SEGment POSition'/
     &           '        calculation for the CKC marker set'/
     &           '        The only valid option is useAnteriorPelvisMarker')
        end if
      end if
c                                    
c     Do the calculations frame by frame
c                                       
      stklth= 8.0*.0254 ! Sacral Stick length
      SkipLine=   .false.
      InterpFlag= .false.
      do frm= 1, MP.nfrm
        if ( OF.UpDate .and. (mod(frm,10) .eq. 0) )  then
          if ( SkipLine ) write(6,90)
90        format(' ')        
          write(6,100) frm,MP.nfrm
100       format('+Processing frame # ',i3,'/',i3)
          SkipLine= .false.
        end if             
c
c     Calculate the right foot position and orientation
c                    
        call CalcLCSFoot( MrkData(x,frm,MNC_RTOE),
     &                    MrkData(x,frm,MNC_RTAK),
     &                    MrkData(x,frm,MNC_RHEL),
     &                    0.571,
     &                    SgpGCS(x,x,frm,rtft),error )
        if ( error .ne. 0 ) then
          call HandleSegPosError( error,frm,rtft,NumError(rtft),
     &                            ErrFrm(1,rtft),OF )
          InterpFlag= .true.
          SkipLine=   .true.   
        end if
c
c       Calculate the right calf position and orientation
c
          Call CalcLCS3( MrkData(x,frm,MNC_LRAK),
     &                   MrkData(x,frm,MNC_MRAK),
     &                   MrkData(x,frm,MNC_RTKN),
     &                   0.567,        
     &                   SgpGCS(x,x,frm,rtcf),error )
        if ( error .ne. 0 ) then
          call HandleSegPosError( error,frm,rtcf,NumError(rtcf),
     &                            ErrFrm(1,rtcf),OF )
          InterpFlag= .true.
          SkipLine=   .true.
        end if
c
c       Calculate the right thigh position and orientation
c
          Call CalcLCS3( MrkData(x,frm,MNC_LRKN),
     &                   MrkData(x,frm,MNC_MRKN),
     &                   MrkData(x,frm,MNC_RTHP),
     &                   0.567,        
     &                   SgpGCS(x,x,frm,rtth),error )
        if ( error .ne. 0 ) then
          call HandleSegPosError( error,frm,rtth,NumError(rtth),
     &                            ErrFrm(1,rtth),OF )
          InterpFlag= .true.
          SkipLine=   .true.
        end if
c
c       Calculate the position and orientation of the pelvis
c
        if ( UseAntPlvMrk ) then
          call CalcLCSPelvisAPM( MrkData(x,frm,MNC_RASI),
     &                           MrkData(x,frm,MNC_SACR),
     &                           MrkData(x,frm,MNC_LASI),stklth,
     &                           MrkData(x,frm,MNC_APLV),
     &                           SgpGCS(x,x,frm,pelv),error )
        else
          call CalcLCSPelvis( MrkData(x,frm,MNC_RASI),
     &                        MrkData(x,frm,MNC_SACR),
     &                        MrkData(x,frm,MNC_LASI),stklth,
     &                        SgpGCS(x,x,frm,pelv),error )
        end if
        if ( error .ne. 0 ) then
          call HandleSegPosError( error,frm,pelv,NumError(pelv),
     &                            ErrFrm(1,pelv),OF )
          InterpFlag= .true.
          SkipLine=   .true.
        end if
      end do ! frm                                     
c
c     Check for error in calculation of the segment positions and orientations
c     and if there are any interpolate the segment information for the missing
c     frames
c
      SF.position= .true.
      if ( InterpFlag ) then
        call InterpSegPos( NumError,ErrFrm,SgpGCS,OF,InterpError )
        if ( InterpError ) SF.position= .false.
      end if
      return

1000  write(6,1010)
      if ( OF.ToLog ) write(3,1010)
1010  format(' Error- MaRKer data has not been ADJusted.'/
     &       '        This must be done before segment position can be calculated.')
      return
      end
c                                        
c	CalcLCS3                            
c
c	Copyright by Dwight Meglan 1992
c
c	Purpose:	Calculate location/orientaiton of segment LCS from the
c				postions of three markers where the markers are on the lateral 
c           and medial sides of the distant joint and the "center" of the
c           proximal joint. EX: lateral and medial maleolous and center of
c           knee
c
c	Error Codes:	0 - no error
c					1 - X axis bad
c					2 - Y axis bad
c					4 - Z axis bad
c					3 - X and Y axes bad
c					5 - X and Z axes bad
c					6 - Y and Z axes bad
c					7 - X, Y, and Z axes bad
c
c	Routines called:
c		VecNorm
c		VecSub
c		VecX 
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	None
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	None         
c
      subroutine CalcLCS3( mrklat,mrkmed,mrkprx,pos_scale,local_coord_sys,error )
      implicit none     
 
      real*4    mrklat(3),mrkmed(3),mrkprx(3),pos_scale,local_coord_sys(3,4)
      integer*4 error

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

      error= 0
      do axis= x, z 
        mid(axis)= (mrklat(axis) + mrkmed(axis))/2.0
      end do
      Call VecSub( mrkmed,mrklat,vector1 )          
      Call VecSub( mrkprx,mid,vector2 )
      Call VecX( vector1, vector2, LCS_Xaxis )
      do axis= x,z
        LCS_Zaxis(axis)= vector2(axis)
      end do                                  
      Call VecX( LCS_Zaxis, LCS_Xaxis, LCS_Yaxis )
      NormError= VecNorm( LCS_Xaxis, local_coord_sys(x,x) )
      if ( NormError .ne. 0 ) error= error + 1
      NormError= VecNorm( LCS_Yaxis, local_coord_sys(x,y) )
      if ( NormError .ne. 0 ) error= error + 2
      NormError= VecNorm( LCS_Zaxis, local_coord_sys(x,z) )
      if ( NormError .ne. 0 ) error= error + 4
      do axis= x,z
        local_coord_sys(axis,cntr)= mid(axis) + pos_scale*vector2(axis)
      end do              
      return              
      end
c                                        
c	CalcLCSPelvisAPM
c
c	Copyright by Dwight Meglan 1992
c
c	Purpose:	Calculate location/orientaiton of pelvis LCS from right ASIS,
c				left ASIS, sacral stick, and anterior pelvis markers
c
c	Error Codes:	0 - no error
c					1 - X axis bad
c					2 - Y axis bad
c					4 - Z axis bad
c					3 - X and Y axes bad
c					5 - X and Z axes bad
c					6 - Y and Z axes bad
c					7 - X, Y, and Z axes bad
c
c	Routines called:
c		VecNorm
c		VecSub
c		VecX 
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	None
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c                    
c	None
c
      subroutine CalcLCSPelvisAPM( rasis, sacrum, lasis, antplv, stklth,
     &                             local_coord_sys,error )
      implicit none  

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

      error= 0
      Call VecSub( lasis, rasis,vector1 )
      Call VecSub( antplv,rasis,vector2 )
      Call VecX( vector1, vector2, LCS_Zaxis )
      do axis= x,z   
        LCS_Yaxis(axis)= vector1(axis)
      end do         
      Call VecX( LCS_Yaxis, LCS_Zaxis, LCS_Xaxis )
      NormError= VecNorm( LCS_Xaxis, local_coord_sys(x,x) )
      if ( NormError .ne. 0 ) error= error + 1
      NormError= VecNorm( LCS_Yaxis, local_coord_sys(x,y) )
      if ( NormError .ne. 0 ) error= error + 2
      NormError= VecNorm( LCS_Zaxis, local_coord_sys(x,z) )
      if ( NormError .ne. 0 ) error= error + 4
      do axis= x, z
        local_coord_sys(axis,cntr)=
     &           ( ( lasis(axis) + rasis(axis) )/2.0 +
     &             sacrum(axis) + StkLth*local_coord_sys(axis,x) )/ 2.0
      end do
      return         
      end                                  
c
c
c
c
c     Copyright by Dwight Meglan 1992
c
c     Purpose: Compute where the joints are located relative to the segment LCS
c              for the segments in the CKC study.
c
      subroutine CalcCKCBodySegEnds( StartFrm,NumFrm,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_ckc.inc'

      record /segment_flag/   SF

      integer*4 StartFrm,NumFrm
      logical*4 error

      real*4    tpnt(3),avg_pos(3)
      integer*4 i,j,k,seg,frm

      error= .true.
      if ( .not. SF.position ) return
      error= .false.
      do i= 1, GP.num_seg
        seg= GP.seg_num(i)
        do j= 1, 4
          do k= 1, 3
            SP(seg).end(k,j)= 0.0
          end do
        end do                 
      end do
c
c     Calculate the average position of the joint markers relative the
c     segment LCS's.  These positions will be the joint "ends".
c
      do frm= StartFrm, StartFrm+NumFrm-1
c                   
c       Right ankle relative to foot and relative to calf
c
        call PntGCStoLCS( SgpGCS(x,x,frm,rtft),MrkData(x,frm,MNC_RTAK),tpnt )
        do i= x, z
          SP(rtft).end(i,proximal)= SP(rtft).end(i,proximal) + tpnt(i)
        end do
        call PntGCStoLCS( SgpGCS(x,x,frm,rtcf),MrkData(x,frm,MNC_RTAK),tpnt )
        do i= x, z
          SP(rtcf).end(i,distal)= SP(rtcf).end(i,distal) + tpnt(i)
        end do
c
c       Right knee relative to both calf and thigh segments
c
        call PntGCStoLCS( SgpGCS(x,x,frm,rtcf),MrkData(x,frm,MNC_RTKN),tpnt )
        do i= x, z
          SP(rtcf).end(i,proximal)= SP(rtcf).end(i,proximal)+tpnt(i)
        end do      
        call PntGCStoLCS( SgpGCS(x,x,frm,rtth),MrkData(x,frm,MNC_RTKN),tpnt)
        do i= x, z 
          SP(rtth).end(i,distal)= SP(rtth).end(i,distal) + tpnt(i)
        end do
c
c       Right hip relative to thigh and pelvis
c
        call PntGCStoLCS( SgpGCS(x,x,frm,rtth),MrkData(x,frm,MNC_RTHP),tpnt)
        do i= x, z
          SP(rtth).end(i,proximal)= SP(rtth).end(i,proximal)+tpnt(i)
        end do
        call PntGCStoLCS( SgpGCS(x,x,frm,pelv),MrkData(x,frm,MNC_RTHP),tpnt)
        do i= x, z 
          SP(pelv).end(i,rt)= SP(pelv).end(i,rt) + tpnt(i)
        end do
      end do ! frm
c
c     Average the segment end positions
c
      do i= 1, GP.num_seg
        seg= GP.seg_num(i)
        do j= 1, 2
          do k= x, z
            SP(seg).end(k,j)= SP(seg).end(k,j)/float(NumFrm)
          end do ! axis
        end do ! j
      end do ! i
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 j= x, z
          avg_pos(j)= 0.0
        end do 
        do frm= StartFrm, StartFrm+NumFrm-1
          do j= x, z
            avg_pos(j)= avg_pos(j) + SgpGCS(j,cntr,frm,seg)
          end do 
        end do 
        do j= x, z
          SP(seg).avg_cntr(j)= avg_pos(j)/float(NumFrm)
        end do 
      end do 
      return
      end
c
c
c
c
c     Copyright by Dwight Meglan 1992
c
c     Purpose: Compute the shape of the segments in the ckc study using
c              the same tricks used for the whole body. These shapes are used 
c              exclusively to make pretty 3D pictures in SHOW3D.
c
      subroutine CalcCKCBodySegShape( 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_ckc.inc'

      integer*4 frm1,nfrm

      real*4    scale(5),VecLth,ftlth,ftht,asis_width
      integer*4 i,j,k,axis,frm,seg
                      
c                  foot   calf   thigh  uparm  lwarm
      data scale / 0.571, 0.567, 0.567, 0.564, 0.570 / ! from Dempster (1955)
c    
c     compute the average distance between markers which will be used to
c     calculate the location of the points defining the 3D parallelapipeds
c     which will represent the body segments.
c
      do i= 1, GP.num_seg
        seg= GP.seg_num(i)
        if ( seg .ne. rtft ) then
          SP(seg).length= VecLth( SP(seg).end(x,distal),
     &                                 SP(seg).end(x,proximal) )
        else
          ftlth= 0.0                             
          ftht=  0.0           
          do frm= frm1, frm1+nfrm-1
            ftlth= ftlth + VecLth(MrkData(x,frm,MNC_RTOE),
     &                            MrkData(x,frm,MNC_RHEL) )
            ftht= ftht + VecLth( MrkData(x,frm,MNC_RHEL),
     &                           MrkData(x,frm,MNC_RTAK) )
          end do
          ftlth= ftlth/float(nfrm)
          SP(rtft).length= ftlth
          ftht= ftht/float(nfrm)
        end if
      end do
      asis_width= 0.0
      do frm= frm1, frm1+nfrm-1
        asis_width= asis_width + VecLth( MrkData(x,frm,MNC_RASI),
     &                                   MrkData(x,frm,MNC_LASI)  )
      end do
      asis_width=  asis_width/float(nfrm)
c             
c     Compute the shapes of the segments
c                                   
c     The size of the segments is based upon ratios of segment width at
c     the proximal and distal ends relative to segment length.  These ratios
c     were calculated from a sample size of 1- namely myself.  They seem
c     to look OK, but obviously it will make fat people look slim and extremely
c     thin people look bulkier than they really are (I'm not real thin,
c     but at the same time not fat either...).  Besides, the segment shapes
c     are for illustration purposes only- they allow a better perspective
c     of the persons motion- no subsequent evaluations are based upon the
c     persons segment size/shape.
c
c     The order of the points describing the segment shape is: the first
c     four points describe the distal face of the shape while last four
c     define the proximal face.  The first point in each face is that in
c     the positive x, positive y quadrant of the LCS.  Subsequent points
c     are defined clockwise around the LCS positive z axis.
c
c     Right Foot        
c 
      SP(rtft).shape(x,1)=  scale(1)*SP(rtft).length
      SP(rtft).shape(y,1)=  0.1*SP(rtcf).length
      SP(rtft).shape(z,1)= -ftht/3.0
      SP(rtft).shape(x,2)=  SP(rtft).shape(x,1)
      SP(rtft).shape(y,2)= -SP(rtft).shape(y,1)
      SP(rtft).shape(z,2)=  SP(rtft).shape(z,1)
      SP(rtft).shape(x,3)=  SP(rtft).shape(x,1) - SP(rtft).length
      SP(rtft).shape(y,3)=  SP(rtft).shape(y,2)
      SP(rtft).shape(z,3)=  SP(rtft).shape(z,1)
      SP(rtft).shape(x,4)=  SP(rtft).shape(x,3)
      SP(rtft).shape(y,4)=  SP(rtft).shape(y,1)
      SP(rtft).shape(z,4)=  SP(rtft).shape(z,1)
      SP(rtft).shape(x,5)=  SP(rtft).shape(x,1)
      SP(rtft).shape(y,5)=  SP(rtft).shape(y,1)
      SP(rtft).shape(z,5)=  SP(rtft).shape(z,1) + 0.2*ftht
      SP(rtft).shape(x,6)=  SP(rtft).shape(x,2)
      SP(rtft).shape(y,6)=  SP(rtft).shape(y,2) 
      SP(rtft).shape(z,6)=  SP(rtft).shape(z,5)
      SP(rtft).shape(x,7)=  SP(rtft).shape(x,3)
      SP(rtft).shape(y,7)=  SP(rtft).shape(y,3)
      SP(rtft).shape(z,7)=  SP(rtft).shape(z,1) + ftht
      SP(rtft).shape(x,8)=  SP(rtft).shape(x,4)
      SP(rtft).shape(y,8)=  SP(rtft).shape(y,4)
      SP(rtft).shape(z,8)=  SP(rtft).shape(z,7)
c
c     Right Calf
c
      SP(rtcf).shape(x,1)=  0.1*SP(rtcf).length
      SP(rtcf).shape(y,1)=  0.1*SP(rtcf).length
      SP(rtcf).shape(z,1)= -scale(2)*SP(rtcf).length*0.9
      SP(rtcf).shape(x,2)=  SP(rtcf).shape(x,1)
      SP(rtcf).shape(y,2)= -SP(rtcf).shape(y,1)
      SP(rtcf).shape(z,2)=  SP(rtcf).shape(z,1)
      SP(rtcf).shape(x,3)= -SP(rtcf).shape(x,1)
      SP(rtcf).shape(y,3)=  SP(rtcf).shape(y,2)
      SP(rtcf).shape(z,3)=  SP(rtcf).shape(z,1)
      SP(rtcf).shape(x,4)=  SP(rtcf).shape(x,3)
      SP(rtcf).shape(y,4)=  SP(rtcf).shape(y,1)
      SP(rtcf).shape(z,4)=  SP(rtcf).shape(z,1)
      SP(rtcf).shape(x,5)=  0.15*SP(rtcf).length
      SP(rtcf).shape(y,5)=  0.15*SP(rtcf).length
      SP(rtcf).shape(z,5)=  SP(rtcf).shape(z,1) + SP(rtcf).length*0.9
      SP(rtcf).shape(x,6)=  SP(rtcf).shape(x,5)
      SP(rtcf).shape(y,6)= -SP(rtcf).shape(y,5) 
      SP(rtcf).shape(z,6)=  SP(rtcf).shape(z,5)
      SP(rtcf).shape(x,7)= -SP(rtcf).shape(x,5)
      SP(rtcf).shape(y,7)=  SP(rtcf).shape(y,6)
      SP(rtcf).shape(z,7)=  SP(rtcf).shape(z,5)
      SP(rtcf).shape(x,8)=  SP(rtcf).shape(x,7)
      SP(rtcf).shape(y,8)=  SP(rtcf).shape(y,5)
      SP(rtcf).shape(z,8)=  SP(rtcf).shape(z,5)
c
c     Right thigh
c
      SP(rtth).shape(x,1)=  0.138*SP(rtth).length
      SP(rtth).shape(y,1)=  0.138*SP(rtth).length
      SP(rtth).shape(z,1)= -scale(3)*SP(rtth).length*0.9
      SP(rtth).shape(x,2)=  SP(rtth).shape(x,1)
      SP(rtth).shape(y,2)= -SP(rtth).shape(y,1)
      SP(rtth).shape(z,2)=  SP(rtth).shape(z,1)
      SP(rtth).shape(x,3)= -SP(rtth).shape(x,1)
      SP(rtth).shape(y,3)=  SP(rtth).shape(y,2)
      SP(rtth).shape(z,3)=  SP(rtth).shape(z,1)
      SP(rtth).shape(x,4)=  SP(rtth).shape(x,3)
      SP(rtth).shape(y,4)=  SP(rtth).shape(y,1)
      SP(rtth).shape(z,4)=  SP(rtth).shape(z,1)
      SP(rtth).shape(x,5)=  0.2*SP(rtth).length
      SP(rtth).shape(y,5)=  0.2*SP(rtth).length
      SP(rtth).shape(z,5)=  SP(rtth).shape(z,1) + SP(rtth).length*0.9
      SP(rtth).shape(x,6)=  SP(rtth).shape(x,5)
      SP(rtth).shape(y,6)= -SP(rtth).shape(y,5) 
      SP(rtth).shape(z,6)=  SP(rtth).shape(z,5)
      SP(rtth).shape(x,7)= -SP(rtth).shape(x,5)
      SP(rtth).shape(y,7)=  SP(rtth).shape(y,6)
      SP(rtth).shape(z,7)=  SP(rtth).shape(z,5)
      SP(rtth).shape(x,8)=  SP(rtth).shape(x,7)
      SP(rtth).shape(y,8)=  SP(rtth).shape(y,5)
      SP(rtth).shape(z,8)=  SP(rtth).shape(z,5)
c
c     Now calculate the points to represent the pelvis segment.  This is
c     based upon the ratio of the lengths of the sides of a triangle formed
c     by the asis markers and the sacral marker.  The length of the sacral
c     stick was taken into account and the pelvis is considered to be starting
c     at the level of the asis markers and extending distally from there.
c     These measurments are based upon my pelvis dimensions.
c
c     calculate pelvis segment points
c
      SP(pelv).shape(x,1)=  (7./12.)*asis_width
      SP(pelv).shape(y,1)=  0.5*asis_width
      SP(pelv).shape(z,1)= -asis_width/3.0
      SP(pelv).shape(x,2)=  SP(pelv).shape(x,1)
      SP(pelv).shape(y,2)= -SP(pelv).shape(y,1)
      SP(pelv).shape(z,2)=  SP(pelv).shape(z,1)
      SP(pelv).shape(x,3)= -0.5*(7./12.)*asis_width
      SP(pelv).shape(y,3)=  SP(pelv).shape(y,2)
      SP(pelv).shape(z,3)=  SP(pelv).shape(z,1)
      SP(pelv).shape(x,4)=  SP(pelv).shape(x,3)
      SP(pelv).shape(y,4)=  SP(pelv).shape(y,1)
      SP(pelv).shape(z,4)=  SP(pelv).shape(z,1)
      SP(pelv).shape(x,5)=  SP(pelv).shape(x,1) 
      SP(pelv).shape(y,5)=  SP(pelv).shape(y,1)
      SP(pelv).shape(z,5)=  0.0
      SP(pelv).shape(x,6)=  SP(pelv).shape(x,2)
      SP(pelv).shape(y,6)=  SP(pelv).shape(y,2)
      SP(pelv).shape(z,6)=  SP(pelv).shape(z,5)
      SP(pelv).shape(x,7)=  SP(pelv).shape(x,3)
      SP(pelv).shape(y,7)=  SP(pelv).shape(y,3)
      SP(pelv).shape(z,7)=  SP(pelv).shape(z,5)
      SP(pelv).shape(x,8)=  SP(pelv).shape(x,4)
      SP(pelv).shape(y,8)=  SP(pelv).shape(y,4)
      SP(pelv).shape(z,8)=  SP(pelv).shape(z,5)

      return
      end
                                                      
c                                        
c	CalcCKCJntLoad  
c
c	Copyright by Dwight Meglan 1992
c
c	Purpose:	Calculate the joint loads.
c
c	Routines called:
c
c	CalcAllJntLoads
c	find_interp_frm
c	lagrange_interp
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Force			FlrCntct
c	Force			FtGrlOK
c	Param			GP.jnt_num
c	Param			GP.num_jnt
c	Param			JNT_BU
c	Param			JNT_DBL
c	Param			JNT_TD
c	Param			MP.cycle_length
c	Param		   rMtnFrmRate
c	Param			MP.lhs_frm
c	Param			MP.lto_frm
c	Param			MP.nfrm
c	Param			MP.num_lhs
c	Param			MP.num_lto
c	Param			MP.num_rhs
c	Param			MP.num_rto
c	Param			MP.rhs_frm
c	Param			MP.rto_frm
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Joint			event
c	Joint			JRLegGCS
c	Joint			JRLegJCS
c	Joint			JntCalcOK
c	Joint			jnt_calc_type
c	Joint			JrlDCS
c	Joint			JrlGCS
c	Joint			JrlJCS
c	Joint			JrlPCS
c
      subroutine CalcCKCJntLoad( CMND,SF,FF,JF,OF )
      implicit none                 

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

      record /CmndLine/      CMND
      record /segment_flag/  SF
      record /force_flag/    FF
      record /joint_flag/    JF
      record /option_flag/   OF
         
      integer*4 i,j,jnt,NumLegJnt,LegJnt(6),frm
 
      data LegJnt / 1,2,3,4,5,6 / ! rtak,rtkn,rthp,lfak,lfkn,lfhp      
c                
c     Check whether the required information has been read in and/or calculated
c

      if ( .not. SF.position )        goto 1000 ! errors...
      if ( .not. SF.vel_acc )         goto 1010
      if ( .not. SF.param )           goto 1020
      if ( .not. JF.angle_JCS )       goto 1070
      if ( .not. FF.cpr_LCS )         goto 1040
      if ( .not. FF.DataClipped )     goto 1050
c
c     Zero out the joint calc OK flags and the indicator of which type of
c     joint calculation order was used
c
      do frm= 1, MP.nfrm
        JrlCalcType(frm)= 0
        do i= 1, GP.num_jnt
          JntCalcOK(frm,GP.jnt_num(i))= .false.
        end do
      end do
c           
c     Check whether all 6 leg joints are present - check about this later
c     to see if it can be used for a single leg...
c   
      JF.legsummed= .false.     
      NumLegJnt= 0
      do i= 1, GP.num_jnt
        jnt= GP.jnt_num(i)
        do j= 1, 6
          if ( jnt .eq. LegJnt(j) ) NumLegJnt= NumLegJnt + 1
        end do ! j            
      end do ! i
      if ( NumLegJnt .ge. 6 ) JF.legsummed= .true.
      if ( JF.legsummed ) write(6,103)
103   format(' NOTE- Summed leg loads will be calculated in addition to'/
     &       '       individual leg joint loads'//)
c 
c     Do joint load calculation for each joint given in GP.jnt_num
c     at each frame of motion data.
c
      do frm= 1, MP.nfrm
        if ( OF.UpDate .and. (mod(frm,10) .eq. 0) ) 
     &    write(6,101) frm,MP.nfrm
101     format('+Processing frame # ',i3,'/',i3)
        call CalcAllJntLoads( frm,JNT_BU(rt),JF.legsummed )
        JrlCalcType(frm)= 12
      end do ! frm
      JF.load= .true.                            
      return
                                                                         
1000  write(6,1005)
      if ( OF.ToLog ) write(3,1005)
1005  format(' Error- Segment position has not been calculated yet.')
      return
      
1010  write(6,1015)
      if ( OF.ToLog ) write(3,1015)
1015  format(' Error- Segment acceleration has not been calculated yet.')
      return
      
1020  write(6,1025)
      if ( OF.ToLog ) write(3,1025)
1025  format(' Error- Segment parameters have not been calculated yet.')
      return
      
1030  write(6,1035)
      if ( OF.ToLog ) write(3,1035)
1035  format(' Error- Ground reaction load data has not been broken up between'/
     &       '        the individual feet yet.')
      return
      
1040  write(6,1045)
      if ( OF.ToLog ) write(3,1045)
1045  format(' Error- Individual foot centers of pressure in the foot LCS'/
     &       '        have not been calculated yet.')
      return

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

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