c
c                                          
c
c
c     Copyright by Dwight Meglan 1990
c
c     Purpose: Initialize a upper extermity only marker set that incorporates the
c              trunk, head and right arm. This is only a partial implementation since
c              connectivity for doing joint resultant loads and powers is not present.
c              The necessay details are here for computing the segment position and
c              orientation and the joint angles/vel/accel. This routine was put together
c              in one afternoon so that we could resurrect some fouled up data for a 
c              study of ranges of upper extermity motion in daily living.
c
      subroutine InitUpperExtremAnalysis( HeadFlag,InitCycle )
      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_upper.inc'

      integer*4 i,j
      logical*4 HeadFlag,InitCycle

      UseHead= HeadFlag
      GP.analysis_type= UPR_ANL
      GP.num_seg= 4
      if ( UseHead ) GP.num_seg= 5
      GP.seg_num(1)= pelv  ! the order here is very critical for the
      GP.seg_num(2)= trnk  ! the compacting of the data just before
      GP.seg_num(3)= rtua  ! saving.  It must be in ascending numerical
      GP.seg_num(4)= rtla  ! sequence... 7 8 9 10 13
      GP.seg_num(5)= head
      GP.num_jnt= 4
      if ( UseHead ) GP.num_jnt= 5
      GP.jnt_num(1)= pvlb  ! same here 7 8 9 10 13
      GP.jnt_num(2)= pvtk
      GP.jnt_num(3)= rtel
      GP.jnt_num(4)= rtsh
      GP.jnt_num(5)= neck

      num_init_empty_mrk= 0      
                           
      MRK_RWR= 7                 ! right wrist- radial
      MRK_RWU= 6                 ! right wrist- ulnar
      MRK_REL= 5                 ! right elbow
      MRK_RSH= 4                 ! right shoulder
      MRK_RHP= 3                 ! right hip
      MRK_LSH= 2                 ! left shoulder
      MRK_LHP= 1                 ! left hip
      MRK_AHD= 8                 ! anterior head
      MRK_PHD= 9                 ! posterior head
                              
      MP.mrk_name(MRK_RWR)= 'rtwr'
      MP.mrk_name(MRK_RWU)= 'rtwu'
      MP.mrk_name(MRK_REL)= 'rtel'
      MP.mrk_name(MRK_RSH)= 'rtsh'
      MP.mrk_name(MRK_RHP)= 'rthp'
      MP.mrk_name(MRK_LSH)= 'lfsh'
      MP.mrk_name(MRK_LHP)= 'lfhp'
      MP.mrk_name(MRK_AHD)= 'anhd'
      MP.mrk_name(MRK_PHD)= 'pshd'

      do i= 1, MAX_SEG
        SP(i).num_mrk= 0
        SP(i).num_MrkDist= 0
        do j= 1, 4
          SP(i).mrk_num(j)= 0
        end do
      end do

      SP(pelv).num_mrk=2
      SP(trnk).num_mrk=4
      SP(head).num_mrk=2
      SP(rtua).num_mrk=2
      SP(rtla).num_mrk=3

      SP(pelv).mrk_num(1)= MRK_RHP
      SP(pelv).mrk_num(2)= MRK_LHP

      SP(trnk).mrk_num(1)= MRK_RHP
      SP(trnk).mrk_num(2)= MRK_LHP
      SP(trnk).mrk_num(3)= MRK_RSH
      SP(trnk).mrk_num(4)= MRK_LSH

      SP(rtua).mrk_num(1)= MRK_RSH
      SP(rtua).mrk_num(2)= MRK_REL

      SP(rtla).mrk_num(1)= MRK_REL
      SP(rtla).mrk_num(2)= MRK_RWR
      SP(rtla).mrk_num(3)= MRK_RWU

      SP(head).mrk_num(1)= MRK_AHD
      SP(head).mrk_num(2)= MRK_PHD

      SP(pelv).num_MrkDist=1
      SP(trnk).num_MrkDist=6
      SP(head).num_MrkDist=1     
      SP(rtua).num_MrkDist=1
      SP(rtla).num_MrkDist=3

      SP(pelv).num_end= 1
      SP(trnk).num_end= 4
      SP(rtua).num_end= 2
      SP(rtla).num_end= 2
      SP(head).num_end= 1

      if ( InitCycle ) MP.num_cycle= 0
      call SetUpFullBodyParam

      return
      end
c                     
c
c
c
c     Copyright by Dwight Meglan 1990
c
c     Purpose: Compute the position and orientation of the upper body segments 
c              from the meausred marker positions used in the OSU upper extremity
c              study. Only the Pelvis, Trunk, Head, right upper and lower arms were
c              measured. This was quickly thrown together to salvage a resident's
c              project!
c
c     Routines called:
c       CalcLCSAntMrk
c       CalcForeArmLCS
c       CalcLCSHead2
c       CalcLCSPelvis2
c       CalcLCSTrunk2
c       HandleSegPosError
c       InterpSegPos
c
      subroutine CalcUpperBodySegPos( 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_upper.inc'

      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    WrMid(3),MidShldr(3)
      logical*4 InterpFlag,InterpError,SkipLine

      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     Do the calculations frame by frame
c
      SkipLine=   .false.
      InterpFlag= .false.
      do frm= 1, MP.nfrm
        if ( OF.UpDate .and. (mod(frm,10) .eq. 0) )  then
          if ( SkipLine ) write(6,90)
90        format(' ')        
          write(6,100) frm,MP.nfrm
100       format('+Processing frame # ',i3,'/',i3)
          SkipLine= .false.
        end if             
c
c     Calculate the right forearm LCS
c                    
        call CalcForeArmLCS( MrkData(x,frm,MRK_RWR),
     &                       MrkData(x,frm,MRK_RWU),
     &                       MrkData(x,frm,MRK_REL),
     &                       SgpGCS(x,x,frm,rtla),
     &                       WrMid,error )
        if ( error .ne. 0 ) then
          call HandleSegPosError( error,frm,rtla,NumError(rtla),
     &                            ErrFrm(1,rtla),OF )
          InterpFlag= .true.
          SkipLine=   .true.   
        end if
c
c       Calculate upper arm LCS
c
        Call CalcLCSAntMrk( MrkData(x,frm,MRK_REL),
     &                      WrMid,MrkData(x,frm,MRK_RSH),
     &                      0.570,SgpGCS(x,x,frm,rtua),error )
        if ( error .ne. 0 ) then
          call HandleSegPosError( error,frm,rtua,NumError(rtua),
     &                            ErrFrm(1,rtua),OF )
          InterpFlag= .true.
          SkipLine=   .true.
        end if
c
c       Calculate Trunk LCS
c
        call CalcLCSTrunk2( MrkData(x,frm,MRK_RSH),
     &                      MrkData(x,frm,MRK_LSH),
     &                      MrkData(x,frm,MRK_RHP),
     &                      MrkData(x,frm,MRK_LHP),
     &                      SgpGCS(x,x,frm,trnk),MidShldr,error )
        if ( error .ne. 0 ) then                 
          call HandleSegPosError( error,frm,trnk,NumError(trnk),
     &                            ErrFrm(1,trnk),OF )
          InterpFlag= .true.
          SkipLine=   .true.
        end if        
c
c       Calculate Pelvis LCS - This is a very rough approximation!!!  The
c       person is assumed to be facing toward the back of the lab and sitting 
c       relatively upright
c
        call CalcLCSPelvis2( MrkData(x,frm,MRK_RHP),
     &                       MrkData(x,frm,MRK_LHP),
     &                       SgpGCS(x,x,frm,pelv),error )
        if ( error .ne. 0 ) then
          call HandleSegPosError( error,frm,pelv,NumError(pelv),
     &                            ErrFrm(1,pelv),OF )
          InterpFlag= .true.
          SkipLine=   .true.
        end if        
c
c       Calculate the Head LCS
c
        if ( UseHead ) then
          call CalcLCSHead2( MidShldr,
     &                       MrkData(x,frm,MRK_AHD),
     &                       MrkData(x,frm,MRK_PHD),
     &                       SgpGCS(x,x,frm,head),error )
          if ( error .ne. 0 ) then
            call HandleSegPosError( error,frm,head,NumError(head),
     &                              ErrFrm(1,head),OF )
            InterpFlag= .true.
            SkipLine=   .true.
          end if        
        end if ! UseHead
      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
c
c
c     Copyright by Dwight Meglan 1990
c
c     Purpose: Calculate the Pelvis position and orientation for the upper extremity
c              study. This assumes that the pelvis is always point vertically since
c              there were only two markers on the pelvis- Not a good idea, but the
c              resident did it, not me...
c
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      
      subroutine CalcLCSPelvis2( rasis,lasis,LCS,error )
      implicit none

      real*4    rasis(3),lasis(3),LCS(3,4)
      integer*4 error
                  
      real*4    vecH(3),vecV(3),vecW(3),HipWd,LabZ(3),VecMag
      integer*4 axis,x,y,z,cntr,NormError,VecNorm                            
                     
      data x,y,z,cntr / 1,2,3,4 /
      data LabZ / 0.0,0.0,1.0 /

      error= 0
      call VecSub( lasis, rasis,vecH )
      call VecX( vecH,LabZ,vecV )
      call VecX( vecV,vecH,vecW )
      NormError= VecNorm( vecV,LCS(x,x) )
      if ( NormError .ne. 0 ) error= error + 1
      NormError= VecNorm( vecH,LCS(x,y) )
      if ( NormError .ne. 0 ) error= error + 2
      NormError= VecNorm( vecW,LCS(x,z) )
      if ( NormError .ne. 0 ) error= error + 4
      HipWd= 0.5*VecMag( vecH ) 
      do axis= x, z
        LCS(axis,cntr)= ( lasis(axis) + rasis(axis) - HipWd*LCS(axis,x) )/2.0
      end do
      return         
      end                                  
c            
c      
c                      
c
c     Copyright by Dwight Meglan 1990
c
c     Purpose: Calculate the position and orientation of the trunk in the upper
c              extermity study.
c
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       
      subroutine CalcLCSTrunk2( rsh,lsh,ras,las,LCS,MidShldr,error )
      implicit none

      real*4    rsh(3),lsh(3),ras(3),las(3),LCS(3,4),MidShldr(3)
      integer*4 error
                                                
      real*4    MidPelv(3),VecG(3),VecF(3),VecT(3),VecU(3)
      integer*4 x,y,z,cntr,i,NormError,VecNorm,axis
             
      data x,y,z,cntr / 1,2,3,4 /
            
      error= 0
      call VecSub( lsh,rsh,VecG )
      do i= x, z     
        MidShldr(i)= ( rsh(i)+lsh(i) )/2.0
c        MidPelv(i)=  ( ras(i)+las(i) )/2.0
      end do
c      call VecSub( MidShldr,MidPelv,VecF )
      call VecSub( rsh,ras,VecF )
      call VecX( VecG,VecF,VecT )
      call VecX( VecF,VecT,VecU )
      NormError= VecNorm( VecT,LCS(x,x) )
      if ( NormError .ne. 0 ) error= error + 1
      NormError= VecNorm( VecU,LCS(x,y) )
      if ( NormError .ne. 0 ) error= error + 2
      NormError= VecNorm( VecF,LCS(x,z) )
      if ( NormError .ne. 0 ) error= error + 4
      do axis= x, z
c        LCS(axis,cntr)= ( MidShldr(axis) + MidPelv(axis) )/2.0
        LCS(axis,cntr)= ( lsh(axis) + ras(axis) )/2.0
      end do
      return 
      end
c 
c
c
c
c     Copyright by Dwight Meglan 1990
c
c     Purpose: Calculate the position and orientation of the forearm in the upper
c              extermity study.
c
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                                                     
      subroutine CalcForeArmLCS( WrRad,WrUln,Elbow,LCS,WrMid,error )
      implicit none

      real*4    WrRad(3),WrUln(3),Elbow(3),LCS(3,4),WrMid(3)
      integer*4 error

      real*4    VecA(3),VecB(3),VecP(3),VecQ(3)
      integer*4 axis,x,y,z,cntr,NormError,VecNorm

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

      error= 0
      call VecSub( WrUln,WrRad,vecA )                 
      do axis= x, z
        WrMid(axis)= ( WrRad(axis) + WrUln(axis) )/2.0
      end do
      call VecSub( Elbow,WrMid,VecB )          
      call VecX( vecB,vecA,vecQ )
      call VecX( vecB,VecQ,VecP )
      NormError= VecNorm( VecQ,LCS(x,x) )
      if ( NormError .ne. 0 ) error= error + 1
      NormError= VecNorm( VecP,LCS(x,y) )
      if ( NormError .ne. 0 ) error= error + 2
      NormError= VecNorm( VecB,LCS(x,z) )
      if ( NormError .ne. 0 ) error= error + 4
      do axis= x, z
        LCS(axis,cntr)= WrMid(axis) + 0.564*VecB(axis)
      end do
      return
      end                  
c            
c
c
c
c     Copyright by Dwight Meglan 1990
c
c
c     Purpose: Calculate the position and orientation of the head in the upper
c              extermity study. This routine cheats because there were only two 
c              markers on the head. It uses the mid shoulder marker computed from
c              the measured right and left shoulder markers as the third point on
c              the "head". Not a good idea, but when salvaging someone else's mess...
c
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                                                      
      subroutine CalcLCSHead2( ShMid,HdAnt,HdPost,LCS,error )
      implicit none

      real*4    ShMid(3),HdAnt(3),HdPost(3),LCS(3,4)
      integer*4 error
   
      real*4    HdMid(3),vecE(3),vecD(3),vecV(3),vecW(3)
      integer*4 x,y,z,cntr,i,j,axis,NormError,VecNorm
      
      data x,y,z,cntr / 1,2,3,4 /
                                
      error= 0
      call VecSub( HdAnt,HdPost,vecE )                 
      do axis= x, z
        HdMid(axis)= ( HdAnt(axis) + HdPost(axis) )/2.0
      end do
      call VecSub( HdMid,ShMid,VecD )
      call VecX( vecD,vecE,vecV )
      call VecX( vecV,VecD,VecW )
      NormError= VecNorm( VecW,LCS(x,x) )
      if ( NormError .ne. 0 ) error= error + 1
      NormError= VecNorm( VecV,LCS(x,y) )
      if ( NormError .ne. 0 ) error= error + 2
      NormError= VecNorm( VecD,LCS(x,z) )
      if ( NormError .ne. 0 ) error= error + 4
      do axis= x, z
        LCS(axis,cntr)= ( ShMid(axis) + HdMid(axis) )/2.0
      end do
      return
      end                 
c
c
c
c
c     Copyright by Dwight Meglan 1990
c
c     Purpose: Compute where the joints are located relative to the segment LCS
c              for the segments in the upper extermity study.
c
      subroutine CalcUpperBodySegEnds( 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_upper.inc'

      record /segment_flag/   SF

      integer*4 StartFrm,NumFrm
      logical*4 error

      real*4    temp_pnt(3),avg_pos(3),MidShldr(3),MidWrist(3)
      integer*4 i,j,k,axis,seg,frm

      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       Trunk-Pelvis - relative to pelvis and trunk.  NOTE that joint relative
c       to pelvis is at (0,0,0) by definition so calculation of pelvis LCS
c       center relative to trunk LCS is equivalent to finding the joint
c       relative to the trunk LCS.
c
        call PntGCStoLCS( SgpGCS(x,x,frm,trnk),
     &                    SgpGCS(x,cntr,frm,pelv), temp_pnt )
        do axis= x, z
          SP(trnk).end(axis,3)= SP(trnk).end(axis,3) +
     &                               temp_pnt(axis)
        end do
c
c       Neck - relative to trunk and head
c
        do axis= x, z
          MidShldr(axis)= ( MrkData(axis,frm,MRK_RSH)+
     &                      MrkData(axis,frm,MRK_LSH)  )/2.0
        end do
        call PntGCStoLCS( SgpGCS(x,x,frm,trnk),MidShldr,temp_pnt )
        do axis= x, z
          SP(trnk).end(axis,4)= SP(trnk).end(axis,4) +
     &                               temp_pnt(axis)
        end do                                        
        if ( UseHead ) then
          call PntGCStoLCS( SgpGCS(x,x,frm,head),MidShldr,temp_pnt )
          do axis= x, z
            SP(head).end(axis,proximal)= SP(head).end(axis,proximal) +
     &                                      temp_pnt(axis)
          end do                           
        end if
c                                        
c       Elbow - rt - relative to lower and upper arms
c
        call PntGCStoLCS( SgpGCS(x,x,frm,rtla),
     &                    MrkData(x,frm,MRK_REL),temp_pnt )
        do axis= x, z
          SP(rtla).end(axis,proximal)= SP(rtla).end(axis,proximal) +
     &                                      temp_pnt(axis)
        end do
        call PntGCStoLCS( SgpGCS(x,x,frm,rtua),
     &                    MrkData(x,frm,MRK_REL),temp_pnt )
        do axis= x, z                                     
          SP(rtua).end(axis,distal)= SP(rtua).end(axis,distal) +
     &                                    temp_pnt(axis)
        end do
c
c       Shoulder - rt  - relative to upper arm and trunk
c
        call PntGCStoLCS( SgpGCS(x,x,frm,rtua),
     &                    MrkData(x,frm,MRK_RSH),temp_pnt )
        do axis= x, z
          SP(rtua).end(axis,proximal)= SP(rtua).end(axis,proximal) +
     &                                      temp_pnt(axis)
        end do
        call PntGCStoLCS( SgpGCS(x,x,frm,trnk),
     &                    MrkData(x,frm,MRK_RSH), temp_pnt )
        do axis= x, z                                     
          SP(trnk).end(axis,rt)= SP(trnk).end(axis,rt) +
     &                                temp_pnt(axis)
        end do
c
c       Wrist - rt - relative to lower arms
c
        do axis= x, z
          MidWrist(axis)= ( MrkData(axis,frm,MRK_RWR) +
     &                      MrkData(axis,frm,MRK_RWU)   )/2.0
        end do
        call PntGCStoLCS( SgpGCS(x,x,frm,rtla),MidWrist,temp_pnt )
        do axis= x, z
          SP(rtla).end(axis,distal)= SP(rtla).end(axis,distal) +
     &                                    temp_pnt(axis)
        end do
      end do ! frm

      do i= 1, GP.num_seg
        seg= GP.seg_num(i)
        do j= 1, 4
          do axis= x, z
            SP(seg).end(axis,j)= SP(seg).end(axis,j)/
     &                                float(NumFrm)
          end do ! axis
        end do ! j
      end do ! i
      do axis= x, z
        SP(pelv).end(axis,3)= 0.0 ! pelvis-trunk joint
      end do ! axis
c
c     Calculate the average segment center position.  This is used as the
c     reference position for calculating segment potential energies later
c     on.  Note that it would be preferable to use standing position data
c     to find these positions but that motion can be used to get and average
c     height of the segment.  
c
      do i= 1, GP.num_seg
        seg= GP.seg_num(i)
        do axis= x, z
          avg_pos(axis)= 0.0
        end do ! axis
        do frm= StartFrm, StartFrm+NumFrm-1
          do axis= x, z
            avg_pos(axis)= avg_pos(axis) + SgpGCS(axis,cntr,frm,seg)
          end do ! axis
        end do ! frm
        do axis= x, z
          SP(seg).avg_cntr(axis)= avg_pos(axis)/float(NumFrm)
        end do ! axis
      end do ! i
      return                  
                
1000  error= .true.
      return
      end   
c
c
c
c
c     Copyright by Dwight Meglan 1990
c
c     Purpose: Compute the shape of the segments in the upper extremity study using
c              the same tricks used for the whole body. These shapes are used exclusively
c              to make pretty pictures 3D in SHOW3D and in the animation program on
c              the Vaxstation 8000. But hey, nobody looks at your graphs- they want
c              to see neat pictures they can relate to. Great for demos and conferences
c              mostly useless for research.
c
      subroutine CalcUpperBodySegShape( 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_upper.inc'

      integer*4 frm1,nfrm

      real*4    scale(5),VecLth,asis_width,shldr_width,HdLth,HdMid(3),
     &          mid_shldr(3),mid_asis(3),trnk_length,length
      integer*4 i,j,k,axis,frm,side,seg,uarm,larm
    
c                  foot   calf   thigh  uparm  lwarm
      data scale / 0.571, 0.567, 0.567, 0.564, 0.570 / ! from Dempster (1955)
c    
c     compute the average distance between markers which will be used to
c     calculate the location of the points defining the 3D parallelapipeds
c     which will represent the body segments.  These shapes are viewed using
c     SHOW- the companion results display program for ANALYZE.
c
      do i= 1, GP.num_seg
        seg= GP.seg_num(i)
        SP(seg).length= VecLth( SP(seg).end(x,distal),
     &                               SP(seg).end(x,proximal) )
      end do
      asis_width=  0.0   
      shldr_width= 0.0
      HdLth=       0.0
      do axis= x, z
        mid_shldr(axis)=  0.0
        mid_asis(axis)=   0.0
        HdMid(axis)=      0.0
      end do
      do frm= frm1, frm1+nfrm-1
        asis_width= asis_width + VecLth( MrkData(x,frm,MRK_RHP),
     &                                   MrkData(x,frm,MRK_LHP)  )
        shldr_width= shldr_width + VecLth( MrkData(x,frm,MRK_RSH),
     &                                     MrkData(x,frm,MRK_LSH) )
        do axis= x, z        
          mid_shldr(axis)= mid_shldr(axis) + 
     &                     ( MrkData(axis,frm,MRK_RSH) +
     &                       MrkData(axis,frm,MRK_LSH)  )/2.0
          mid_asis(axis)=  mid_asis(axis) + 
     &                     ( MrkData(axis,frm,MRK_RHP) +
     &                       MrkData(axis,frm,MRK_LHP)  )/2.0
          if ( UseHead ) 
     &      HdMid(axis)= HdMid(axis) + ( MrkData(axis,frm,MRK_AHD) +
     &                                   MrkData(axis,frm,MRK_PHD)  )/2.0
        end do               
      end do
      do axis= x, z
        mid_shldr(axis)= mid_shldr(axis)/float(nfrm)
        mid_asis(axis)=  mid_asis(axis)/float(nfrm)
        if ( UseHead ) HdMid(axis)=     HdMid(axis)/float(nfrm)
      end do
      trnk_length= VecLth( mid_shldr,mid_asis )
      asis_width=  asis_width/float(nfrm)
      shldr_width= shldr_width/float(nfrm)
      if ( UseHead ) HdLth= VecLth( HdMid,Mid_shldr )
c
c     Upper Arm
c                      
      uarm= rtua
      SP(uarm).shape(x,1)=  0.15*SP(uarm).length
      SP(uarm).shape(y,1)=  0.15*SP(uarm).length
      SP(uarm).shape(z,1)= -scale(4)*SP(uarm).length*0.9
      SP(uarm).shape(x,2)=  SP(uarm).shape(x,1)
      SP(uarm).shape(y,2)= -SP(uarm).shape(y,1)
      SP(uarm).shape(z,2)=  SP(uarm).shape(z,1)
      SP(uarm).shape(x,3)= -SP(uarm).shape(x,1)
      SP(uarm).shape(y,3)=  SP(uarm).shape(y,2)
      SP(uarm).shape(z,3)=  SP(uarm).shape(z,1)
      SP(uarm).shape(x,4)=  SP(uarm).shape(x,3)
      SP(uarm).shape(y,4)=  SP(uarm).shape(y,1)
      SP(uarm).shape(z,4)=  SP(uarm).shape(z,1)
      SP(uarm).shape(x,5)=  0.225*SP(uarm).length
      SP(uarm).shape(y,5)=  0.225*SP(uarm).length
      SP(uarm).shape(z,5)=  SP(uarm).shape(z,1) + SP(uarm).length*0.9
      SP(uarm).shape(x,6)=  SP(uarm).shape(x,5)
      SP(uarm).shape(y,6)= -SP(uarm).shape(y,5) 
      SP(uarm).shape(z,6)=  SP(uarm).shape(z,5)
      SP(uarm).shape(x,7)= -SP(uarm).shape(x,5)
      SP(uarm).shape(y,7)=  SP(uarm).shape(y,6)
      SP(uarm).shape(z,7)=  SP(uarm).shape(z,5)
      SP(uarm).shape(x,8)=  SP(uarm).shape(x,7)
      SP(uarm).shape(y,8)=  SP(uarm).shape(y,5)
      SP(uarm).shape(z,8)=  SP(uarm).shape(z,5)
c
c     Lower Arm
c
      larm= rtla
      SP(larm).shape(x,1)=  0.1*SP(larm).length
      SP(larm).shape(y,1)=  0.1*SP(larm).length
      SP(larm).shape(z,1)= -scale(5)*SP(larm).length 
      SP(larm).shape(x,2)=  SP(larm).shape(x,1)
      SP(larm).shape(y,2)= -SP(larm).shape(y,1)
      SP(larm).shape(z,2)=  SP(larm).shape(z,1)
      SP(larm).shape(x,3)= -SP(larm).shape(x,1)
      SP(larm).shape(y,3)=  SP(larm).shape(y,2)
      SP(larm).shape(z,3)=  SP(larm).shape(z,1)
      SP(larm).shape(x,4)=  SP(larm).shape(x,3)
      SP(larm).shape(y,4)=  SP(larm).shape(y,1)
      SP(larm).shape(z,4)=  SP(larm).shape(z,1)
      SP(larm).shape(x,5)=  0.165*SP(larm).length
      SP(larm).shape(y,5)=  0.165*SP(larm).length
      SP(larm).shape(z,5)=  SP(larm).shape(z,1) + SP(larm).length*0.95
      SP(larm).shape(x,6)=  SP(larm).shape(x,5)
      SP(larm).shape(y,6)= -SP(larm).shape(y,5) 
      SP(larm).shape(z,6)=  SP(larm).shape(z,5)
      SP(larm).shape(x,7)= -SP(larm).shape(x,5)
      SP(larm).shape(y,7)=  SP(larm).shape(y,6)
      SP(larm).shape(z,7)=  SP(larm).shape(z,5)
      SP(larm).shape(x,8)=  SP(larm).shape(x,7)
      SP(larm).shape(y,8)=  SP(larm).shape(y,5)
      SP(larm).shape(z,8)=  SP(larm).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)=  0.25*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)= -SP(pelv).shape(x,1)
      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)
c
c     calculate trunk segment points
c
c      SP(trnk).shape(x,1)=  SP(pelv).shape(x,1)
c      SP(trnk).shape(y,1)=  SP(pelv).shape(y,1)
      SP(trnk).shape(x,1)=  0.75*0.25*shldr_width
      SP(trnk).shape(y,1)=  0.75*0.45*shldr_width
      SP(trnk).shape(z,1)= -0.5*trnk_length
      SP(trnk).shape(x,2)=  SP(trnk).shape(x,1)
      SP(trnk).shape(y,2)= -SP(trnk).shape(y,1)                   
      SP(trnk).shape(z,2)=  SP(trnk).shape(z,1)
c      SP(trnk).shape(x,3)=  SP(pelv).shape(x,3)
c      SP(trnk).shape(y,3)=  SP(pelv).shape(y,3)
      SP(trnk).shape(x,3)= -SP(trnk).shape(x,1)
      SP(trnk).shape(y,3)=  SP(trnk).shape(y,2)
      SP(trnk).shape(z,3)=  SP(trnk).shape(z,1)
      SP(trnk).shape(x,4)=  SP(trnk).shape(x,3)
      SP(trnk).shape(y,4)=  SP(trnk).shape(y,1)
      SP(trnk).shape(z,4)=  SP(trnk).shape(z,1)
      SP(trnk).shape(x,5)=  0.25*shldr_width
      SP(trnk).shape(y,5)=  0.45*shldr_width
      SP(trnk).shape(z,5)=  0.5*trnk_length
      SP(trnk).shape(x,6)=  SP(trnk).shape(x,5)
      SP(trnk).shape(y,6)= -SP(trnk).shape(y,5)
      SP(trnk).shape(z,6)=  SP(trnk).shape(z,5)
      SP(trnk).shape(x,7)= -SP(trnk).shape(x,5)
      SP(trnk).shape(y,7)=  SP(trnk).shape(y,6)
      SP(trnk).shape(z,7)=  SP(trnk).shape(z,5)
      SP(trnk).shape(x,8)=  SP(trnk).shape(x,7)
      SP(trnk).shape(y,8)=  SP(trnk).shape(y,5)
      SP(trnk).shape(z,8)=  SP(trnk).shape(z,5)
c
c     calculate head segment points
c
      if ( UseHead ) then
        SP(head).shape(x,1)=  0.25*HdLth
        SP(head).shape(y,1)=  0.25*HdLth
        SP(head).shape(z,1)=  -0.5*HdLth
        SP(head).shape(x,2)=  SP(head).shape(x,1)
        SP(head).shape(y,2)= -SP(head).shape(y,1)
        SP(head).shape(z,2)=  SP(head).shape(z,1)
        SP(head).shape(x,3)= -SP(head).shape(x,1)
        SP(head).shape(y,3)=  SP(head).shape(y,2)
        SP(head).shape(z,3)=  SP(head).shape(z,1)
        SP(head).shape(x,4)=  SP(head).shape(x,3)
        SP(head).shape(y,4)=  SP(head).shape(y,1)
        SP(head).shape(z,4)=  SP(head).shape(z,1)
        SP(head).shape(x,5)=  SP(head).shape(x,1)
        SP(head).shape(y,5)=  SP(head).shape(y,1)
        SP(head).shape(z,5)=  0.5*HdLth
        SP(head).shape(x,6)=  SP(head).shape(x,2)
        SP(head).shape(y,6)=  SP(head).shape(y,2)
        SP(head).shape(z,6)=  SP(head).shape(z,5)
        SP(head).shape(x,7)=  SP(head).shape(x,3)
        SP(head).shape(y,7)=  SP(head).shape(y,3)
        SP(head).shape(z,7)=  SP(head).shape(z,5)
        SP(head).shape(x,8)=  SP(head).shape(x,4)
        SP(head).shape(y,8)=  SP(head).shape(y,4)
        SP(head).shape(z,8)=  SP(head).shape(z,5)
      end if 
      return
      end
             
