c
c
c
      subroutine InitF3DAnalysis(side)
      implicit none
      include 'a_const.inc'

      include 'a_struct.inc'
      include 'a_prm.inc'
      include 'a_names.inc'
      include 'a_jnt.inc'
      include 'a_ft3d.inc'
      include 'a_mrk.inc'

      integer*4 i,j,side
c
c     Setup segment & numbering and names
c
      tibia= 1 ! calf 
      calc= 2  ! calcaneus
      meta1= 3 ! first metatarsal
      meta5= 4 ! fifth metatarsal
      toe1= 5  ! big toe
      foot= 6  ! midfoot as a whole

      calc_tibia= 1  ! calcaneus to tibia joint
      m1_calc=    2  ! first metatarsal to calcaneus joint
      t1_m1=      3  ! big toe to first metatarsal joint
      m5_calc=    4  ! fifth metatarsal to calcaneus joint
      m1_m5=      5  ! first metatarsal to fifth metatarsal joint
      ft_t1=      6  ! midfoot to toe 1 joint
      ft_tb=      7  ! midfoot to tibia
 
      if ( side .eq. rt ) then
        seg_names(1)= 'RTIB'
        seg_names(2)= 'RCAL'
        seg_names(3)= 'RMT1'
        seg_names(4)= 'RMT5'
        seg_names(5)= 'RTO1'
        seg_names(6)= 'RTFT'

        jnt_names(1)= 'RCTB'
        jnt_names(2)= 'RM1C'  
        jnt_names(3)= 'RMT1'
        jnt_names(4)= 'RM5C'
        jnt_names(5)= 'RM15'
        jnt_names(6)= 'RFT1'
        jnt_names(7)= 'RFTB'
      else
        seg_names(1)= 'LTIB'
        seg_names(2)= 'LCAL'
        seg_names(3)= 'LMT1'
        seg_names(4)= 'LMT5'
        seg_names(5)= 'LTO1'
        seg_names(6)= 'LFFT'

        jnt_names(1)= 'LCTB'
        jnt_names(2)= 'LM1C'
        jnt_names(3)= 'LMT1'
        jnt_names(4)= 'LM5C'
        jnt_names(5)= 'LM15'
        jnt_names(6)= 'LFT1'
        jnt_names(7)= 'LFTB'
      end if
c
c     Set up marker info
c
      MF3D_TAS= 1
      MF3D_TAI= 2
      MF3D_TPS= 3
      MF3D_TPI= 4 
      MF3D_CM = 5
      MF3D_CP = 6
      MF3D_CL = 7
      MF3D_AM = 8
      MF3D_AL = 9            
      MF3D_M1P= 10
      MF3D_M1A= 11
      MF3D_M5P= 12
      MF3D_M5A= 13
      MF3D_T1A= 14
      MF3D_T1P= 15

      num_init_empty_mrk= 0
      init_empty_mrk(1)= -1
c                                        
c     Setup segment data      
c                

      SP(tibia).num_mrk= 4   ! this info will NOT be used by rigid body
      SP(calc).num_mrk=  3   ! calculations. It is used for the simple
      SP(meta1).num_mrk= 3   ! method of calculating the TCS of a segment
      SP(meta5).num_mrk= 3
      SP(toe1).num_mrk=  3
      SP(foot).num_mrk=  3
                                         
      SP(tibia).mrk_num(1)= MF3D_TAI
      SP(tibia).mrk_num(2)= MF3D_TAS
      SP(tibia).mrk_num(3)= MF3D_TPI
      SP(tibia).mrk_num(4)= MF3D_TPS

      SP(calc).mrk_num(1)= MF3D_CM
      SP(calc).mrk_num(2)= MF3D_CP
      SP(calc).mrk_num(3)= MF3D_CL
                                      
      SP(meta1).mrk_num(1)= MF3D_M1A
      SP(meta1).mrk_num(2)= MF3D_M5P 
      SP(meta1).mrk_num(3)= MF3D_M1P 
                                        
      SP(meta5).mrk_num(1)= MF3D_M5A
      SP(meta5).mrk_num(2)= MF3D_M1P 
      SP(meta5).mrk_num(3)= MF3D_M5P

      SP(toe1).mrk_num(1)= MF3D_T1A
      SP(toe1).mrk_num(2)= MF3D_M5A
      SP(toe1).mrk_num(3)= MF3D_T1P
                               
      SP(foot).mrk_num(1)= MF3D_M1A
      SP(foot).mrk_num(2)= MF3D_AL
      SP(foot).mrk_num(3)= MF3D_CP

      SP(tibia).num_MrkDist= 6
      SP(calc).num_MrkDist=  3
      SP(meta1).num_MrkDist= 3
      SP(meta5).num_MrkDist= 3
      SP(toe1).num_MrkDist=  3
      SP(foot).num_MrkDist=  3

      SP(tibia).num_end= 1 ! distal= calc_tibia
      SP(calc).num_end=  3 ! 1= m1_calc 2= m5_calc 3= calc_tibia
      SP(meta1).num_end= 3 ! dist= t1_m1   prox= m1_calc 3= m1_m5
      SP(meta5).num_end= 2 ! dist= m1_m5   prox= m5_calc
      SP(toe1).num_end=  1 ! proximal= t1_m1
      SP(foot).num_end=  2 ! distal= ft_t1 proximal= ft_tb
c
c     Set up segment information needed for conversion from technical
c     coordinate system to segment local coordinate system
c
      SP(tibia).nmTCS= 4
      SP(tibia).mnTCS(1)= MF3D_TAS
      SP(tibia).mnTCS(2)= MF3D_TAI
      SP(tibia).mnTCS(3)= MF3D_TPS
      SP(tibia).mnTCS(4)= MF3D_TPI

      SP(calc).nmTCS= 3         
      SP(calc).mnTCS(1)= MF3D_CM
      SP(calc).mnTCS(2)= MF3D_CP
      SP(calc).mnTCS(3)= MF3D_CL
                            
      SP(meta1).nmTCS= 3  
      SP(meta1).mnTCS(1)= MF3D_M1A
      SP(meta1).mnTCS(2)= MF3D_M1P
      SP(meta1).mnTCS(3)= MF3D_M5P
                            
      SP(meta5).nmTCS= 3
      SP(meta5).mnTCS(1)= MF3D_M5A
      SP(meta5).mnTCS(2)= MF3D_M5P
      SP(meta5).mnTCS(3)= MF3D_M1P

      SP(toe1).nmTCS= 3           
      SP(toe1).mnTCS(1)= MF3D_T1A
      SP(toe1).mnTCS(2)= MF3D_T1P
      SP(toe1).mnTCS(3)= MF3D_M5A

      SP(foot).nmTCS= 4  
      SP(foot).mnTCS(1)= MF3D_CP
      SP(foot).mnTCS(2)= MF3D_AM
      SP(foot).mnTCS(3)= MF3D_AL
      SP(foot).mnTCS(4)= MF3D_M1A

c
c     Set up joint connectivity information- This is not going to work for the
c     kinetics calculations since this forms a closed chain amongst the
c     metatarsals and the calcaneus, but I decided to include it as an academic
c     exercise anyway.
c
      JNT_TD(rt).num_jnt= 0
      JNT_TD(lf).num_jnt= 0
      if ( side .eq. rt ) then
        JNT_BU(rt).num_jnt= 7
        JNT_BU(lf).num_jnt= 0
      else         
        JNT_BU(rt).num_jnt= 7
        Jnt_BU(lf).num_jnt= 0
      end if
      JNT_BU(side).jnt_order(1)= t1_m1
      JNT_BU(side).jnt_order(2)= m1_m5
      JNT_BU(side).jnt_order(3)= m1_calc
      JNT_BU(side).jnt_order(4)= m5_calc
      JNT_BU(side).jnt_order(5)= calc_tibia
      JNT_BU(side).jnt_order(6)= ft_t1
      JNT_BU(side).jnt_order(6)= ft_tb

      JNT_BU(side).jnt_seg(1)= toe1
      JNT_BU(side).jnt_seg(2)= meta1
      JNT_BU(side).jnt_seg(3)= meta1
      JNT_BU(side).jnt_seg(4)= meta5
      JNT_BU(side).jnt_seg(5)= calc
      JNT_BU(side).jnt_seg(6)= foot
      JNT_BU(side).jnt_seg(7)= tibia
                                     
      JNT_BU(side).calc_jnt_num(1)= proximal
      JNT_BU(side).calc_jnt_num(2)= 3
      JNT_BU(side).calc_jnt_num(3)= proximal
      JNT_BU(side).calc_jnt_num(4)= proximal
      JNT_BU(side).calc_jnt_num(5)= proximal
      JNT_BU(side).calc_jnt_num(6)= proximal
      JNT_BU(side).calc_jnt_num(7)= proximal

      JNT_BU(side).num_conn_jnt(1)=  0
      JNT_BU(side).num_conn_jnt(2)=  2
      JNT_BU(side).num_conn_jnt(3)=  2
      JNT_BU(side).num_conn_jnt(4)=  1
      JNT_BU(side).num_conn_jnt(5)=  2
      JNT_BU(side).num_conn_jnt(6)=  0
      JNT_BU(side).num_conn_jnt(7)=  1
 
      JNT_BU(side).conn_jnt_num(1,1)=  0        ! t1_m1
      JNT_BU(side).conn_jnt_num(1,2)=  t1_m1    ! m1_m5
      JNT_BU(side).conn_jnt_num(2,2)=  m1_calc  ! t1_m1
      JNT_BU(side).conn_jnt_num(1,3)=  t1_m1    ! m1_calc
      JNT_BU(side).conn_jnt_num(2,3)=  m1_m5    ! m1_calc
      JNT_BU(side).conn_jnt_num(1,4)=  m1_m5    ! m5_calc
      JNT_BU(side).conn_jnt_num(1,5)=  m1_calc  ! calc_tibia
      JNT_BU(side).conn_jnt_num(2,5)=  m5_calc  ! calc_tibia
      JNT_BU(side).conn_jnt_num(1,6)=  0        ! ft_t1
      JNT_BU(side).conn_jnt_num(1,6)=  ft_t1    ! ft_tb
                                                   
      JNT_BU(side).conn_jnt_seg(1,1)=  0      ! toe1
      JNT_BU(side).conn_jnt_seg(1,2)=  0      ! meta1
      JNT_BU(side).conn_jnt_seg(1,3)=  0      ! meta1
      JNT_BU(side).conn_jnt_seg(1,4)=  0      ! meta5
      JNT_BU(side).conn_jnt_seg(1,5)=  0      ! calc
      JNT_BU(side).conn_jnt_seg(1,6)=  0      ! foot
      JNT_BU(side).conn_jnt_seg(1,7)=  0      ! tibia

      do i= 1, 4                                       
        JNT_BU(side).num_ext_load(i)= 1
        JNT_BU(side).ext_load_num(1,i)= 1
        JNT_BU(side).CalcOrder(i)= DisttoProx
      end do
c
c     Set up joint connectivity
c
      JntSegNum(distal,calc_tibia)=   calc
      JntSegNum(proximal,calc_tibia)= tibia
      JntSegNum(distal,m1_calc)=      meta1
      JntSegNum(proximal,m1_calc)=    calc
      JntSegNum(distal,t1_m1)=        toe1
      JntSegNum(proximal,t1_m1)=      meta1
      JntSegNum(distal,m5_calc)=      meta5
      JntSegNum(proximal,m5_calc)=    calc
      JntSegNum(distal,m1_m5)=        meta5
      JntSegNum(proximal,m1_m5)=      meta1
      JntSegNum(distal,ft_t1)=        toe1
      JntSegNum(proximal,ft_t1)=      foot
      JntSegNum(distal,ft_tb)=        foot
      JntSegNum(proximal,ft_tb)=      tibia

      do i= 1, MAX_SEG
	do j= 1, 4
	  SegJntNum(j,i)= 0
	end do
      end do	

      SegJntNum(1,toe1)=       t1_m1 ! this is a kludge since proximal=2
      SegJntNum(2,toe1)=       t1_m1 ! it makes this work for exporting
      SegJntNum(1,meta1)=      t1_m1
      SegJntNum(2,meta1)=      m1_calc
      SegJntNum(3,meta1)=      m1_m5
      SegJntNum(1,meta5)=      m1_m5
      SegJntNum(2,meta5)=      m5_calc
      SegJntNum(1,calc)=       m1_calc
      SegJntNum(2,calc)=       m5_calc
      SegJntNum(3,calc)=       calc_tibia
      SegJntNum(1,tibia)=      calc_tibia
      SegJntNum(1,foot)=       ft_t1
      SegJntNum(2,foot)=       ft_tb
c                                  
c     Setup General Parameters
c                  
      if ( side .eq. rt ) then                      
        GP.analysis_type= F3DR_ANL
      else
        GP.analysis_type= F3DL_ANL     
      end if
      GP.num_seg= 6
      GP.num_jnt= 7
      do i= 1, 7
        GP.seg_num(i)= i
        GP.jnt_num(i)= i
      end do            
      return    
      end                            
c
c                           
c
      subroutine CalcF3DAlignment( 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_ft3d.inc'

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

      integer*4 mn,i,frm,ierr,side
      real*4    ampn(3,MAX_MRK),ct(3),mp1(3),cm1(3),m1m5(3),cm5(3),
     &          InitSgp(3,4,6)

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     Tibia 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     ct is location in GCS of calcaneus-tibia "joint" 
c               
      call F3dTibiaInit( MF3D_TAS,MF3D_TAI,MF3D_TPS,MF3D_TPI,
     &                   MF3D_AM,MF3D_AL,ampn,SP(tibia),
     &                   InitSgp(1,1,tibia),ct,ierr )
      if ( ierr .ne. 0 ) goto 1010
c          
c     Toe calculations                          
c       mp1 is the location of the first metatarsalphalangeal "joint" in GCS
c
      side= rt                                  
      if ( GP.Analysis_type .eq. F3DL_ANL ) side= lf
      call F3dToe1Init( MF3D_T1A,MF3D_T1P,MF3D_M5A,
     &                  ampn,side,SP(toe1),InitSgp(1,1,toe1),mp1,ierr )
      if ( ierr .ne. 0 ) goto 1030
c                                 
c     First and fifth metarsal calculations
c       cm1 is the location of the calcaneus-1st metatarsal "joint" in GCS
c       cm5 is the location of the calcaneus-5th metatarsal "joint" in GCS
c       m1m5 is the location of the 1st metatarsal-5th metatarsal "joint" in GCS
c
      call F3dMeta1Init( MF3D_M1A,MF3D_M1P,MF3D_M5A,MF3D_M5P,MF3D_AM,MF3D_AL,
     &                   MF3D_CP,side,ampn,mp1,SP(meta1),
     &                   InitSgp(1,1,meta1),cm1,m1m5,ierr )
      if ( ierr .ne. 0 ) goto 1020
      call F3dMeta5Init( MF3D_M1A,MF3D_M1P,MF3D_M5A,MF3D_M5P,MF3D_AM,MF3D_AL,
     &                   MF3D_CP,side,ampn,m1m5,SP(meta5),
     &                   InitSgp(1,1,meta5),cm5,ierr )
      if ( ierr .ne. 0 ) goto 1040                  
c                                        
c     Calcaneus calculations              
c
      call F3dCalcInit( MF3D_CM,MF3D_CP,MF3D_CL,ct,cm1,cm5,
     &                  ampn,side,SP(calc),InitSgp(1,1,calc),ierr )
      if ( ierr .ne. 0 ) goto 1050
c                                        
c     Whole foot calculations
c
      call F3dFootInit( MF3D_CP,MF3D_AM,MF3D_AL,MF3D_M1A,
     &                  ampn,side,SP(foot),InitSgp(1,1,foot),ierr )
      if ( ierr .ne. 0 ) goto 1050
c
c      Do the segment shape calculations 
c
      call CalcF3dBodySegShape( 1,MP.nfrm,InitSgp )
      if ( GP.analysis_type .eq. F3DR_ANL ) then
        SF.RightSideAlign= .true.    
      else
        SF.LeftSideAlign= .true.
      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 tibia failed')
      call F3dInitErr(ierr,OF.ToLog)
      return
                
1020  write(6,1025)
      if ( OF.ToLog ) write(3,1025)
1025  format(' Error- Initialization of first metatarsal failed')
      call F3dInitErr(ierr,OF.ToLog)     
      return    

1030  write(6,1035)
      if ( OF.ToLog ) write(3,1035)
1035  format(' Error- Initialization of first toe failed')
      call F3dInitErr(ierr,OF.ToLog)     
      return    

1040  write(6,1045)
      if ( OF.ToLog ) write(3,1045)
1045  format(' Error- Initialization of fifth metatarsal failed')
      call F3dInitErr(ierr,OF.ToLog)     
      return    

1050  write(6,1055)
      if ( OF.ToLog ) write(3,1055)
1055  format(' Error- Initialization of calcaneus failed')
      call F3dInitErr(ierr,OF.ToLog)     
      return    
      end
c                                          
c                                                              
c
      subroutine F3dInitErr(ierr,logfile)
      implicit none                               

      integer*4 ierr                                          
      logical*4 logfile

      if ( ierr .eq. 1 ) then
         write(6,100) 
         if ( logfile ) write(3,100) 
100      format(' Error- LCS x axis calculation failure')

      else if ( ierr .eq. 2 ) then
         write(6,110)
         if ( logfile ) write(3,110) 
110      format(' Error- LCS y axis calculation failure')

      else if ( ierr .eq. 4 ) then
         write(6,120) 
         if ( logfile ) write(3,120) 
120      format(' Error- LCS z axis calculation failure')

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

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

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

      else if ( ierr .eq. 7 ) then
         write(6,170) 
         if ( logfile ) write(3,170) 
170      format(' Error- LCS x y and z axis calculation failure')
                            
      else if ( ierr .eq. 20 ) then
         write(6,180)
         if ( logfile ) write(3,180)
180      format(' Error- Normalization of metatarsal-calcaneus vector failed')

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

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

      else
        write(6,210) ierr
         if ( logfile ) write(3,210) ierr
210     format(' Error- Unknown error number # ',i2,' occured')
      end if                 
      return
      end                                
c                                
c                                                   
c
      subroutine F3dTibiaInit( TAS,TAI,TPS,TPI,AM,AL,ampn,SP,
     &                         InitSgp,ac,ierr )
      implicit none                  
      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_names.inc'                         

      record /seg_param/ SP
                                 
      integer*4 TAS,TAI,TPS,TPI,AM,AL,ierr
      real*4    ampn(3,MAX_MRK),ac(3),InitSgp(3,4)

      integer*4 i,mn,j
      real*4    tibLCS(3,4),veclth,tibTCS(3,4)
c                                           
c     Determine ankle center         
c                                         
      ierr= 0
      do i= x, z                   
        ac(i)= (ampn(i,AM)+ampn(i,AL))/2.0
      end do
c                                                 
c     Calculate tibia LCS and position of TCS markers relative to it
c           
      call CalcInitLCSTibia( ampn(x,TAS),ampn(x,TAI),ampn(x,TPS),
     &                   ampn(x,TPI),tibLCS,ierr )
      if ( ierr .ne. 0 ) goto 1000
      do i= 1, SP.nmTCS     
        mn= SP.mnTCS(i)
        call PntGCStoLCS( tibLCS,ampn(x,mn),SP.mpnLCS(x,i) )
      end do
c
c     Calculate the TCS and the position and orientation of the LCS relative
c     to the TCS
c
      call CalcTCS( ampn(x,TAI),ampn(x,TAS),ampn(x,TPI),tibTCS,ierr )
      if ( ierr .ne. 0 ) goto 1010
      call LCS2relLCS1( tibTCS,tibLCS,SP.TCStoLCS )
c
c     Store the position/orientation of the LCS for use in calculating the
c     segment shape
c
      do i= 1, 4
        do j= 1, 3
          InitSgp(j,i)= tibLCS(j,i)              
        end do 
      end do
c                                                 
c     Calculate ankle center relative to LCS
c
      call PntGCStoLCS( tibLCS,ac,SP.end(x,distal) )
c
c     Calculate an 'estimate' of the tibia length - not used
c
      SP.length= veclth( ampn(x,TAS),ampn(x,TAI) )
      return        
                                                 
1000  return            ! LCS calc error
1010  ierr= ierr + 10   ! TCS calc error
      return
      end   
c                                
c                                                   
c
      subroutine F3dCalcInit( CM,CP,CL,ct,cm1,cm5,ampn,side,SP,
     &                        InitSgp,ierr )
      implicit none                  
      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_names.inc'                         

      record /seg_param/ SP
                                 
      integer*4 CM,CL,CP,ierr,side
      real*4    ampn(3,MAX_MRK),ct(3),cm1(3),cm5(3),InitSgp(3,4)
                                  
      integer*4 i,mn,j
      real*4    calLCS(3,4),veclth,calTCS(3,4)
c                                                 
c     Calculate calcaneus LCS and position of TCS markers relative to it
c                                                   
      call CalcInitLCSCalc( ampn(x,CM),ampn(x,CP),ampn(x,CL),side,calLCS,ierr )
      if ( ierr .ne. 0 ) goto 1000
      do i= 1, SP.nmTCS     
        mn= SP.mnTCS(i)
        call PntGCStoLCS( calLCS,ampn(x,mn),SP.mpnLCS(x,i) )
      end do                      
c                           
c     Calculate the TCS and the position and orientation of the LCS relative
c     to the TCS
c
      call CalcTCS( ampn(x,CM),ampn(x,CP),ampn(x,CL),calTCS,ierr )
      if ( ierr .ne. 0 ) goto 1010
      call LCS2relLCS1( calTCS,calLCS,SP.TCStoLCS )
c
c     Store the position/orientation of the LCS for use in calculating the
c     segment shape
c
      do i= 1, 4
        do j= 1, 3
          InitSgp(j,i)= calLCS(j,i)              
        end do 
      end do
c                                 
c     Calculate ankle, 1st metatarsal-calcaneus and 
c     5th metatarsal-calcaneus joints relative to LCS
c           
      call PntGCStoLCS( calLCS,cm1,SP.end(x,1) )
      call PntGCStoLCS( calLCS,cm5,SP.end(x,2) )
      call PntGCStoLCS( calLCS,ct,SP.end(x,3) )
c
c     Calculate an 'estimate' of the calcaneus width - not used
c
      SP.length= veclth( ampn(x,CM),ampn(x,CL) )
      return        
                                                 
1000  return            ! LCS calc error
1010  ierr= ierr + 10   ! TCS calc error
      return
      end   
c                                
c                                                   
c
      subroutine F3dToe1Init( TA,TP,M5A,ampn,side,SP,InitSgp,mp,ierr )
      implicit none                  
      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_names.inc'                         
                                                                    
      record /seg_param/ SP
                                 
      integer*4 M5A,TA,TP,ierr,side
      real*4    ampn(3,MAX_MRK),mp(3),InitSgp(3,4)
                 
      integer*4 i,mn,j
      real*4    toeLCS(3,4),veclth,mrad,toeTCS(3,4)

      mrad= 0.0064 ! marker radius
c                                                 
c     Calculate calcaneus LCS and position of TCS markers relative to it
c                                                   
      call CalcInitLCSToe1( ampn(x,TA),ampn(x,TP),side,toeLCS,ierr )
      if ( ierr .ne. 0 ) goto 1000
      do i= 1, SP.nmTCS     
        mn= SP.mnTCS(i)
        call PntGCStoLCS( toeLCS,ampn(x,mn),SP.mpnLCS(x,i) )
      end do
c                
c     Calculate the TCS and the position and orientation of the LCS relative
c     to the TCS
c
      call CalcTCS( ampn(x,TA),ampn(x,M5A),ampn(x,TP),toeTCS,ierr )
      if ( ierr .ne. 0 ) goto 1010
      call LCS2relLCS1( toeTCS,toeLCS,SP.TCStoLCS )
c
c     Store the position/orientation of the LCS for use in calculating the
c     segment shape
c                           
      do i= 1, 4
        do j= 1, 3
          InitSgp(j,i)= toeLCS(j,i)
        end do 
      end do
c                                                    
c     Calculate an 'estimate' of the toe length
c
      SP.length= veclth( ampn(x,TA),ampn(x,TP) )
c
c     Calculate 1st metatarsal-toe joint relative to LCS. This is arbitrarily
c     placed below the posterior toe marker on the -x axis of the LCS
c
      do i= 1, 3
        mp(i)= ampn(i,TP)
      end do     
      mp(z)= (mp(z) - mrad)/2.0
      call PntGCStoLCS( toeLCS,mp,SP.end(x,proximal) )
      call PntGCStoLCS( toeLCS,mp,SP.end(x,1) ) ! kludge due to storage order
      return               
                                                 
1000  return            ! LCS calc error
1010  ierr= ierr + 10   ! TCS calc error
      return
      end   
c
c                                                   
c                          
      subroutine F3dMeta1Init( M1A,M1P,M5A,M5P,AM,AL,CP,
     &                         side,ampn,mp1,SP,InitSgp,cm1,m1m5,ierr )
      implicit none                  
      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_names.inc'                         

      record /seg_param/ SP
                                 
      integer*4 M1A,M1P,M5A,M5P,AM,AL,CP,ierr,side
      real*4    ampn(3,MAX_MRK),mp1(3),cm1(3),m1m5(3),InitSgp(3,4)

      integer*4 i,mn,MinLineXPnts,j,vecnorm       
      real*4    m1LCS(3,4),veclth,veca(3),nrma(3),vecb(3),nrmb(3),ac(3),mrad,
     &          m1TCS(3,4)

      mrad= 0.0064 ! marker radius
c                                                 
c     Calculate tibia LCS and position of TCS markers relative to it
c                                                                 
      call CalcInitLCSMeta( ampn(x,M1A),ampn(x,M1P),m1LCS,ierr )
      if ( ierr .ne. 0 ) goto 1000
      do i= 1, SP.nmTCS     
        mn= SP.mnTCS(i)
        call PntGCStoLCS( m1LCS,ampn(x,mn),SP.mpnLCS(x,i) )
      end do
c
c     Calculate the TCS and the position and orientation of the LCS relative
c     to the TCS
c
      call CalcTCS( ampn(x,M1A),ampn(x,M5P),ampn(x,M1P),m1TCS,ierr )
      if ( ierr .ne. 0 ) goto 1010
      call LCS2relLCS1( m1TCS,m1LCS,SP.TCStoLCS )
c
c     Store the position/orientation of the LCS for use in calculating the
c     segment shape
c
      do i= 1, 4
        do j= 1, 3                        
          InitSgp(j,i)= m1LCS(j,i)              
        end do 
      end do
c
c     Calculate m1-m5 joint in GCS
c                        
      do i= x, z
        m1m5(i)= (ampn(i,M1A)+ampn(i,M1P)+ampn(i,M5A)+ampn(i,M5P))/4.0
      end do
      m1m5(z)= (m1m5(z) - mrad)/2.0  ! marker radius subtracted
c
c     Calculate calcaneus-1st metatarsal joint in GCS
c
      call vecsub(ampn(x,M1P),ampn(x,CP),veca)
      ierr= vecnorm(veca,nrma)
      if ( ierr .ne. 0 ) goto 1020
      call vecsub(ampn(x,AM),ampn(x,AL),vecb)
      ierr= vecnorm(vecb,nrmb)
      if ( ierr .ne. 0 ) goto 1030
      ierr= MinLineXPnts( nrma,ampn(x,AL),nrmb,ampn(x,CP),ac,cm1 )
      if ( ierr .ne. 0 ) goto 1040
c
c     Calculate mp1, cm1, and m1m5 joints relative to LCS
c
      call PntGCStoLCS( m1LCS,mp1,SP.end(x,distal) )
      call PntGCStoLCS( m1LCS,cm1,SP.end(x,proximal) )
      call PntGCStoLCS( m1LCS,m1m5,SP.end(x,3) )
c   
c     Calculate an 'estimate' of the metatarsal length
c
      SP.length= veclth( ampn(x,M1P),ampn(x,M1A) )
      return        
                                                 
1000  return            ! LCS calc error
1010  ierr= ierr + 10   ! TCS calc error
      return
1020  ierr= 20   ! normalization of metatarsal-calcaneus vector error
      return 
1030  ierr= 30   ! normalization of ankle vector error
      return                                                 
1040  ierr= 40   ! Line bisector calc error
      return                                   
      end   
c
c                                                   
c
      subroutine F3dMeta5Init( M1A,M1P,M5A,M5P,AM,AL,CP,
     &                         side,ampn,m1m5,SP,InitSgp,cm5,ierr )
      implicit none                    
      include 'a_const.inc'
      include 'a_struct.inc'      
      include 'a_names.inc'                         

      record /seg_param/ SP
                                 
      integer*4 M1A,M1P,M5A,M5P,AM,AL,CP,ierr,side
      real*4    ampn(3,MAX_MRK),cm5(3),m1m5(3),InitSgp(3,4)
                                                  
      integer*4 i,mn,MinLineXPnts,j,vecnorm
      real*4    m5LCS(3,4),veclth,veca(3),nrma(3),vecb(3),nrmb(3),ac(3),mrad,
     &          m5TCS(3,4)
                                
      mrad= 0.0064 ! marker radius
c                                                 
c     Calculate tibia LCS and position of TCS markers relative to it
c
      call CalcInitLCSMeta( ampn(x,M5A),ampn(x,M5P),m5LCS,ierr )
      if ( ierr .ne. 0 ) goto 1000
      do i= 1, SP.nmTCS     
        mn= SP.mnTCS(i)
        call PntGCStoLCS( m5LCS,ampn(x,mn),SP.mpnLCS(x,i) )
      end do                    
c
c     Calculate the TCS and the position and orientation of the LCS relative
c     to the TCS
c
      call CalcTCS( ampn(x,M5A),ampn(x,M1P),ampn(x,M5P),m5TCS,ierr )
      if ( ierr .ne. 0 ) goto 1010
      call LCS2relLCS1( m5TCS,m5LCS,SP.TCStoLCS )
c                                                    
c     Store the position/orientation of the LCS for use in calculating the
c     segment shape
c
      do i= 1, 4
        do j= 1, 3
          InitSgp(j,i)= m5LCS(j,i)              
        end do             
      end do              
c
c     Calculate calcaneus-1st metatarsal joint in GCS
c                                 
      call vecsub(ampn(x,M5P),ampn(x,CP),veca)
      ierr= vecnorm(veca,nrma)
      if ( ierr .ne. 0 ) goto 1020
      call vecsub(ampn(x,AM),ampn(x,AL),vecb)
      ierr= vecnorm(vecb,nrmb)
      if ( ierr .ne. 0 ) goto 1030
      ierr= MinLineXPnts( nrma,ampn(x,AL),nrmb,ampn(x,CP),ac,cm5 )
      if ( ierr .ne. 0 ) goto 1040
c                          
c     Calculate cm5 and m1m5 joints relative to LCS
c
      call PntGCStoLCS( m5LCS,cm5,SP.end(x,proximal) )
      call PntGCStoLCS( m5LCS,m1m5,SP.end(x,distal) )
c   
c     Calculate an 'estimate' of the metatarsal length   
c
      SP.length= veclth( ampn(x,M5P),ampn(x,M5A) )
      return        
                                                 
1000  return            ! LCS calc error
1010  ierr= ierr + 10   ! TCS calc error
      return
1020  ierr= 20   ! normalization of metatarsal-calcaneus vector error
      return 
1030  ierr= 30   ! normalization of ankle vector error
      return                                                 
1040  ierr= 40   ! Line bisector calc error
      return                                   
      end   
c                                
c                                                   
c
      subroutine F3dFootInit( CP,AM,AL,M1A,ampn,side,SP,InitSgp,ierr )
      implicit none                  
      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_names.inc'                         
                                     
      record /seg_param/ SP
                                 
      integer*4 M1A,AM,AL,CP,ierr,side                               
      real*4    ampn(3,MAX_MRK),InitSgp(3,4)
                 
      integer*4 i,mn,j
      real*4    ftLCS(3,4),veclth,mrad,ac(3),ftTCS(3,4),mp1(3)
                              
      mrad= 0.0064 ! marker radius
c                                                 
c     Calculate calcaneus LCS and position of TCS markers relative to it
c                                                   
      call CalcInitLCSFtF3D( ampn(x,CP),ampn(x,AM),ampn(x,AL),ampn(x,M1A),
     &                       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 the TCS and the position and orientation of the LCS relative
c     to the TCS
c
      call CalcTCS( ampn(x,M1A),ampn(x,AL),ampn(x,CP),ftTCS,ierr )
      if ( ierr .ne. 0 ) goto 1010
      call LCS2relLCS1( ftTCS,ftLCS,SP.TCStoLCS )
c
c     Store the position/orientation of the LCS for use in calculating the
c     segment shape
c
      do i= 1, 4
        do j= 1, 3                        
          InitSgp(j,i)= ftLCS(j,i)              
        end do 
      end do
c                                                    
c     Calculate an 'estimate' of the foot length
c
      SP.length= veclth( ampn(x,CP),ampn(x,M1A) )
c
c     Calculate ankle center relative to LCS.
c
      do i= 1, 3            
        ac(i)= (ampn(i,AL)+ampn(i,AM))/2.0
      end do     
      call PntGCStoLCS( ftLCS,ac,SP.end(x,proximal) )
c
c     Calculate MP1 joint relative to LCS.
c
      do i= 1, 2 
        mp1(i)= ampn(i,M1A)
      end do    
      mp1(3)= (ampn(3,M1A)-mrad)/2.0
      call PntGCStoLCS( ftLCS,mp1,SP.end(x,distal) )
      return                   
                                                 
1000  return            ! LCS calc error
1010  ierr= ierr + 10   ! TCS calc error
      return
      end   
c        
c                     
c                                           
      subroutine CalcInitLCSTibia( tas,tai,tps,tpi,LCS,ierr )
      implicit none

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

      real*4    tas(3),tai(3),tps(3),tpi(3),LCS(3,4)
      integer*4 ierr

      real*4    vecY(3),vecZ(3),vecPA(3),veca(3),pta(3),ptb(3)
      integer*4 i,vecnorm,istat
                                 
      ierr= 0
c
c     Calculate the rotation matrix of the tibia LCS
c                
      do i= 1, 3
        pta(i)= (tas(i)+tps(i))/2.0
        ptb(i)= (tai(i)+tpi(i))/2.0
      end do
      call vecsub( pta,ptb,vecZ )
      istat= vecnorm( vecZ,vecZ )
      if ( istat .ne. 0 ) ierr= 4
      call vecsub( tas,tps,veca )
      istat= vecnorm( veca,veca )
      if ( istat .ne. 0 ) ierr= 8
      call vecx( vecZ,veca,vecY )
      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
      do i= 1, 3
        LCS(i,cntr)= (tas(i)+tai(i)+tps(i)+tpi(i))/4.0
      end do
      return
      end
c                     
c                     
c                                           
      subroutine CalcInitLCSCalc( cm,cp,cl,side,LCS,ierr )
      implicit none

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

      real*4    cm(3),cp(3),cl(3),LCS(3,4)
      integer*4 ierr,side
                                 
      real*4    vecY(3),vecZ(3),vecPA(3),veca(3),vecZ0(3)
      integer*4 i,vecnorm,istat

      data vecZ0 / 0.0,0.0,1.0 /
                                 
      ierr= 0
c
c     Calculate the rotation matrix of the LCS
c
      if ( side .eq. rt ) then
        call vecsub( cm,cl,vecY )
c        call vecsub( cp,cl,veca )
      else
        call vecsub( cl,cm,vecY )
c        call vecsub( cp,cm,veca )
      end if
      istat= vecnorm( vecY,vecY )
      if ( istat .ne. 0 ) ierr= 2
c      istat= vecnorm( veca,veca )
c      if ( istat .ne. 0 ) ierr= 8
      call vecx( vecY,vecZ0,vecPA )  ! modified version to make calc flat...
c      call vecx( vecY,veca,vecZ )
c      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
c      istat= vecnorm( vecZ,LCS(x,z) )
      istat= vecnorm( vecZ0,LCS(x,z) )
      if ( istat .ne. 0 ) ierr= ierr + 4
      do i= 1, 3
        LCS(i,cntr)= (cm(i)+cl(i))/2.0
      end do
      return
      end                      
c                     
c                                           
c
      subroutine CalcInitLCSToe1( ta,tp,side,LCS,ierr )
      implicit none             

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

      real*4    ta(3),tp(3),LCS(3,4)
      integer*4 ierr,side
                                 
      real*4    vecY(3),vecZ(3),vecPA(3),veca(3),mrad
      integer*4 i,vecnorm,istat
                                 
      ierr= 0
      mrad= 0.0064 ! marker radius
c
c     Coordinate system is aligned so that x axis goes from the posterior
c     to anterior markers, y axis is parallel to the floor (at calibration)
c     The lab GCS z axis (vertical) is used to generate the y axis while
c     the toe z is produced by crossing the toe x and y axes.
c     Note is ASSUMED that the person is standing on the floor when the 
c     calibration position is recorded.
c
      call vecsub( ta,tp,vecPA )
      vecZ(x)= 0.0             
      vecZ(y)= 0.0
      vecZ(z)= 1.0
      call vecx( vecZ,vecPA,vecY )
      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
c
c     Place the LCS at the midpoint of the ant/post markers - 
c     1/2 the remaining height from the floor (assumed to be z= 0.0)
c                                
      do i= x, z
        LCS(i,cntr)= (ta(i)+tp(i))/2.0
      end do
      LCS(z,cntr)= (LCS(z,cntr)-mrad)/2.0 ! set height here
      return
      end                      
c                     
c                                           
c
      subroutine CalcInitLCSMeta( ant,post,LCS,ierr )
      implicit none

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

      real*4    ant(3),post(3),LCS(3,4)
      integer*4 ierr
                                 
      real*4    vecY(3),vecZ(3),vecPA(3),veca(3),mrad
      integer*4 i,vecnorm,istat
                                 
      ierr= 0
      mrad= 0.0064 ! marker radius
c
c     Coordinate system is aligned so that x axis goes between the line
c     between the anterior and posterior metatarsal markers. The y is 
c     pependicular to this and in the floor plane because the vertical lab
c     axis (Z) is cross producted with the metatarsal x axis to generate the
c     y axis. The LCS z axis is the cross product of the LCS x and LCS y axes.
c     For this alignment to work a person must be standing with their foot
c     flat on the ground when the calibration data is taken, otherwise the LCS 
c     y axis is not correct and hence the LCS z also...
c                 
      call vecsub( ant,post,vecPA )   
      vecZ(x)= 0.0
      vecZ(y)= 0.0
      vecZ(z)= 1.0
      call vecx( vecZ,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
c                               
c     Place the LCS at the midpoint of the markers - 1/2 the remaining height 
c     from the floor (floor assumed to be z= 0.0)
c
      do i= x, z
        LCS(i,cntr)= (ant(i)+post(i))/2.0
      end do
      LCS(z,cntr)= (LCS(z,cntr)-mrad)/2.0 ! set marker height here
      return
      end                         
c        
c                     
c                                           
      subroutine CalcInitLCSFtF3D( cp,am,al,m1a,LCS,ierr )
      implicit none

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

      real*4    cp(3),am(3),al(3),m1a(3),LCS(3,4)
      integer*4 ierr

      real*4    vecY(3),vecZ(3),vecPA(3),veca(3),ac(3),vecZ0(3)
      integer*4 i,vecnorm,istat

      data vecZ0 / 0.0,0.0,1.0 /
                                 
      ierr= 0
c
c     Calculate the rotation matrix of the foot LCS
c                
      do i= 1, 3
        ac(i)= (am(i)+al(i))/2.0                      
      end do
      call vecsub( ac,cp,veca )
      call vecsub( m1a,cp,vecPA )
      call vecx( veca,vecPA,vecY )
      call vecx( vecY,vecZ0,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( vecZ0,LCS(x,z) )
      if ( istat .ne. 0 ) ierr= ierr + 4
      do i= 1, 3
        LCS(i,cntr)= (cp(i)+ac(i)+m1a(i))/3.0
      end do
      return
      end
c                                            
c
c                                                   
      subroutine CalcF3dSegPos( 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_ft3d.inc'

      record /CmndLine/      CMND
      record /marker_flag/   MF
      record /segment_flag/  SF                                         
      record /option_flag/   OF                       
                
      integer*4 num13(3),i,frm,j,side,seg,ierr,spvp,disp3ds,num14(4),
     &          np
      real*4    mpn(3,MAX_MRK),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),
     &          tmpsgp(3,4)
      logical*4 UseDisp3d,UseSpVp,DumpDiag,UseSimple
                         
      data num13 / 1,2,3 /
      data num14 / 1,2,3,4 /

      SF.position= .false.
      if ( (GP.analysis_type .eq. F3DR_ANL) .and.
     &     (.not. SF.RightSideAlign)              ) goto 1000 ! error
      if ( (GP.analysis_type .eq. F3DL_ANL) .and.
     &     (.not. SF.LeftSideAlign)              ) goto 1010 ! error
c                                         
c     Check for rigid body algorithm selection option
c                    
      UseSimple= .true.
      UseSpvp=   .false.
      UseDisp3d= .false.
      DumpDiag=  .false.
      if ( CMND.NumOpt(2) .gt. 0 ) then             
        do i= 1, CMND.NumOpt(2)                                
          if ( CMND.OptText(i,2) .eq. 'SPV' ) then
            UseDisp3d= .false.
            UseSpVp=   .true.
            UseSimple= .false.
          else if ( CMND.OptText(i,2) .eq. 'WOL' ) then
            UseSpVp=   .false.
            UseDisp3d= .true.
            UseSimple= .false.
          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 for rigid body algorithm
c       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       Compute position and orientation of the five foot segments using
c       the slected rigid body algorithm
c                                                    
        do i= 1, GP.num_seg
          seg= GP.seg_num(i)
          if ( UseSpVp ) then
            ierr= spvp( SP(seg).mpnLCS,mpn,SP(seg).nmTCS,
     &                  num14,SP(seg).mnTCS,SgpGCS(x,x,frm,seg),
     &                  max(frm,seg),min(frm,seg),avg(frm,seg),stdev(frm,seg) )
          else if ( UseDisp3d ) then ! disp3d algorithm
            ierr= disp3ds( SP(seg).mpnLCS,mpn,SP(seg).nmTCS,
     &                     num14,SP(seg).mnTCS,SgpGCS(x,x,frm,seg),
     &                     max(frm,seg),min(frm,seg),avg(frm,seg),
     &                     stdev(frm,seg),scf(frm,seg) )
          else 
            call CalcTCS( MrkData(x,frm,SP(seg).mrk_num(1)),
     &                    MrkData(x,frm,SP(seg).mrk_num(2)),
     &                    MrkData(x,frm,SP(seg).mrk_num(3)),tmpsgp,ierr )
            if ( ierr .ne. 0 ) goto 1030
            call CalcTCStoLCS( tmpsgp,SP(seg).TCStoLCS,
     &                         SgpGCS(x,x,frm,seg) )
          end if

          if ( ierr .ne. 0 ) then
            np= SP(seg).nmTCS
            goto 1050
          end if                    
        end do ! seg
      end do ! frm                               
c
c     Dump diagnostics if desired...
c              
      if ( DumpDiag ) 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.
      return
                 
1000  write(6,1005)       
      if ( OF.ToLog ) write(3,1005)
1005  format(' Error- Right side Foot3d 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 Foot3d 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 ',
     &       '        Foot3d marker set'/
     &       '        Valid options are: /SPVp /DISp3d /DIAgnostics')
      return

1030  write(6,1035) ierr
      if ( OF.ToLog ) write(3,1035) ierr
1035  format(' Error- Error # ',i4,' occurred trying to open sgp_rb.dia')
      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 CalcF3dBodySegShape( frm1,nfrm,isgp )
      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_ft3d.inc'

      integer*4 frm1,nfrm
      real*4    isgp(3,4,6)

      real*4    ampn(3,MAX_MRK)
      integer*4 i,side,mn,ierr,frm
c
c     calculate the average marker positions
c
      do mn= 1, MP.num_mrk
        do i= x, z
          ampn(i,mn)= 0.0
          do frm= frm1, frm1+nfrm-1
            ampn(i,mn)=  ampn(i,mn) + MrkData(i,frm,mn)
          end do
          ampn(i,mn)= ampn(i,mn)/float(nfrm)
        end do                                                              
      end do                                
c                   
c     Calculate the segment outlines
c      
      side= rt                    
      if ( GP.analysis_type .eq. F3DL_ANL) side= lf
      call CalcCalcaneusShape( ampn(x,MF3D_CM),ampn(x,MF3D_CP),
     &                         ampn(x,MF3D_CL),ampn(x,MF3D_M1P),
     &                         ampn(x,MF3D_M5P),isgp(1,1,calc),
     &                         SP(calc),side ) 
      call CalcTibiaShape( ampn(x,MF3D_TAS),ampn(x,MF3D_TAI),
     &                     ampn(x,MF3D_TPS),ampn(x,MF3D_TPI),
     &                     ampn(x,MF3D_AM),ampn(x,MF3D_AL),
     &                     isgp(1,1,tibia),SP(tibia),side ) 
      call CalcToe1Shape( ampn(x,MF3D_T1A),ampn(x,MF3D_T1P),isgp(1,1,toe1),
     &                    SP(toe1),side ) 
      call CalcMeta1Shape( ampn(x,MF3D_CM),ampn(x,MF3D_CP),        
     &                     ampn(x,MF3D_M1A),ampn(x,MF3D_M1P),
     &                     ampn(x,MF3D_M5A),ampn(x,MF3D_M5P),
     &                     isgp(1,1,meta1),SP(meta1),side,ierr ) 
      if ( ierr .ne. 0 ) write(6,100) ierr
100   format(' Error- Error # ',i2,' occurred during metatarsal 1 outline',
     &       ' calculation')
      call CalcMeta5Shape( ampn(x,MF3D_CL),ampn(x,MF3D_CP),
     &                     ampn(x,MF3D_M1A),ampn(x,MF3D_M1P),
     &                     ampn(x,MF3D_M5A),ampn(x,MF3D_M5P),
     &                     isgp(1,1,meta5),SP(meta5),side,ierr ) 
      if ( ierr .ne. 0 ) write(6,110) ierr
110   format(' Error- Error # ',i2,' occurred during metatarsal 5 outline',
     &       ' calculation')
      call CalcFtF3dShape( ampn(x,MF3D_CP),ampn(x,MF3D_AM),
     &                     ampn(x,MF3D_AL),ampn(x,MF3D_T1A),
     &                     isgp(1,1,foot),SP(foot),side ) 
      return    
      end                                  
c                                        
c
c
      subroutine CalcCalcaneusShape( cm,cp,cl,m1p,m5p,isgp,SP,side ) 
      implicit none
      include 'a_const.inc'             
      include 'a_struct.inc'
      include 'a_names.inc'                   

      record /seg_param/ SP

      integer*4 side                                       
      real*4    cm(3),cp(3),cl(3),m5p(3),m1p(3),isgp(3,4)

      integer*4 i
      real*4    ptaG(3),ptaL(3),ptbL(3),mrad

      mrad= 0.0064 ! marker radius
c
c     Calculate the x location (in LCS) of the front face
c     Note that isgp actually contains the position/orientation of
c     the calcaneus relative to the GCS at the calibration position
c
      do i= 1, 3
        ptaG(i)= cl(i) + 0.6*(m5p(i)-cl(i))
      end do
      call PntGCStoLCS( isgp,ptaG,ptaL)
      SP.shape(x,1)= ptaL(x)
      SP.shape(x,2)= SP.shape(x,1)                       
      SP.shape(x,5)= SP.shape(x,1)
      SP.shape(x,6)= SP.shape(x,1)
c
c     Calculate the x location (in LCS) of the back face
c
      call PntGCStoLCS( isgp,cp,ptaL )
      SP.shape(x,4)= ptaL(x) + mrad ! add marker radius
      SP.shape(x,3)= SP.shape(x,4)     
      SP.shape(x,8)= SP.shape(x,4)
      SP.shape(x,7)= SP.shape(x,4)
c                                 
c     Calculate the y location of the +y and -y faces
c
      call PntGCStoLCS( isgp,cl,ptaL )
      call PntGCStoLCS( isgp,cm,ptbL )
      if ( side .eq. rt ) then
        SP.Shape(y,1)= ptbL(y) - 3.0*mrad
        SP.Shape(y,2)= ptaL(y) + 3.0*mrad
      else                        
        SP.Shape(y,1)= ptaL(y) - 3.0*mrad
        SP.Shape(y,2)= ptbL(y) + 3.0*mrad
      end if
      SP.shape(y,4)= SP.shape(y,1)   
      SP.shape(y,5)= SP.shape(y,1)
      SP.shape(y,8)= SP.shape(y,1)
      SP.shape(y,3)= SP.shape(y,2)
      SP.shape(y,6)= SP.shape(y,2)
      SP.shape(y,7)= SP.shape(y,2)
c
c     Assign the z location of the bottom face
c
      do i= 1, 3
        ptaG(i)= isgp(i,cntr)
      end do
      ptaG(z)= 0.002
      call PntGCStoLCS( isgp,ptaG,ptaL )
      SP.shape(z,1)= ptaL(z)         
      SP.shape(z,2)= SP.shape(z,1)
      SP.shape(z,3)= SP.shape(z,1)
      SP.shape(z,4)= SP.shape(z,1)      
c                           
c     Assign the z location of the top face
c
      do i= x, z
        ptaG(i)= (m1p(i)+m5p(i))/2.0
      end do
      ptaG(z)= ptaG(z) - mrad
      call PntGCStoLCS( isgp,ptaG,ptaL )
      SP.shape(z,5)= ptaL(z)
      SP.shape(z,6)= SP.shape(z,5)   
      SP.shape(z,7)= SP.shape(z,5)
      SP.shape(z,8)= SP.shape(z,5)
      return                     
      end                          
c                                    
c                           
c
      subroutine CalcTibiaShape( tas,tai,tps,tpi,am,al,isgp,SP,side ) 
      implicit none
      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_names.inc'                   

      record /seg_param/ SP

      integer*4 side                                        
      real*4    tps(3),tpi(3),tai(3),tas(3),al(3),am(3),isgp(3,4)

      integer*4 i
      real*4    ptaG(3),ptaL(3),ptbL(3),mrad

      mrad= 0.0064 ! marker radius
c
c     Calculate the x location (in LCS) of the front face
c     Note that isgp actually contains the position/orientation of
c     the tibia relative to the GCS at the calibration position
c
      do i= 1, 3
        ptaG(i)= (tai(i)+tas(i))/2.0
      end do
      call PntGCStoLCS( isgp,ptaG,ptaL)
      SP.shape(x,1)= ptaL(x) - mrad
      SP.shape(x,2)= SP.shape(x,1)                
      SP.shape(x,5)= SP.shape(x,1)
      SP.shape(x,6)= SP.shape(x,1)
c
c     Calculate the x location (in LCS) of the back face
c
      do i= 1, 3
        ptaG(i)= (tpi(i)+tps(i))/2.0
      end do
      call PntGCStoLCS( isgp,ptaG,ptaL)
      SP.shape(x,4)= ptaL(x) + mrad ! add marker radius
      SP.shape(x,3)= SP.shape(x,4)     
      SP.shape(x,8)= SP.shape(x,4)
      SP.shape(x,7)= SP.shape(x,4)
c                                 
c     Calculate the y location of the +y face
c
      if ( side .eq. rt ) then
        do i= 1, 3
          ptaG(i)= am(i) + 0.3*(al(i)-am(i))
        end do     
      else
        do i= 1, 3
          ptaG(i)= al(i) + 0.3*(am(i)-al(i))
        end do
      end if
      call PntGCStoLCS( isgp,ptaG,ptaL )
      SP.Shape(y,1)= ptaL(y)
      SP.shape(y,4)= SP.shape(y,1)   
      SP.shape(y,5)= SP.shape(y,1)
      SP.shape(y,8)= SP.shape(y,1)
c                                 
c     Calculate the y location of the -y face
c
      if ( side .eq. rt ) then
        do i= 1, 3
          ptaG(i)= al(i) + 0.3*(am(i)-al(i))
        end do
      else
        do i= 1, 3 
          ptaG(i)= am(i) + 0.3*(al(i)-am(i))
        end do
      end if
      call PntGCStoLCS( isgp,ptaG,ptaL )
      SP.Shape(y,2)= ptaL(y)
      SP.shape(y,3)= SP.shape(y,2)
      SP.shape(y,6)= SP.shape(y,2)
      SP.shape(y,7)= SP.shape(y,2)
c
c     Assign the z location of the bottom face
c
      do i= 1, 3
        ptaG(i)= (am(i)+al(i))/2.0
      end do
      call PntGCStoLCS( isgp,ptaG,ptaL )
      SP.shape(z,1)= ptaL(z)
      SP.shape(z,2)= SP.shape(z,1)      
      SP.shape(z,3)= SP.shape(z,1)
      SP.shape(z,4)= SP.shape(z,1)
c
c     Assign the z location of the top face
c
      do i= 1, 3
        ptaG(i)= (tps(i)+tas(i))/2.0
      end do
      call PntGCStoLCS( isgp,ptaG,ptaL )
      SP.shape(z,5)= ptaL(z)
      SP.shape(z,6)= SP.shape(z,5)   
      SP.shape(z,7)= SP.shape(z,5)
      SP.shape(z,8)= SP.shape(z,5)
      return
      end                          
c
c
c
      subroutine CalcToe1Shape( ta,tp,isgp,SP,side ) 
      implicit none
      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_names.inc'                   

      record /seg_param/ SP

      integer*4 side
      real*4    ta(3),tp(3),isgp(3,4)

      integer*4 i            
      real*4    ptaG(3),ptaL(3),tlen,veclth,mrad,taL(3),tpL(3)

      mrad= 0.0064 ! marker radius
c                              
c     Calculate the x location (in LCS) of the front face
c                         
      call PntGCStoLCS( isgp,ta,taL )
      call PntGCStoLCS( isgp,tp,tpL )
      tlen= veclth(ta,tp)
c      SP.shape(x,1)= tlen
      SP.shape(x,1)= taL(x)
      SP.shape(x,2)= SP.shape(x,1)                
      SP.shape(x,5)= SP.shape(x,1)          
      SP.shape(x,6)= SP.shape(x,1)
c                
c     Calculate the x location (in LCS) of the back face
c
c      SP.shape(x,4)= -tlen
      SP.shape(x,4)= tpL(x)
      SP.shape(x,3)= SP.shape(x,4)     
      SP.shape(x,8)= SP.shape(x,4)
      SP.shape(x,7)= SP.shape(x,4)
c                                 
c     Calculate the y location of the +y face
c
      SP.Shape(y,1)= tlen/3.0
      SP.shape(y,4)= SP.shape(y,1)   
      SP.shape(y,5)= SP.shape(y,1)
      SP.shape(y,8)= SP.shape(y,1)
c                                 
c     Calculate the y location of the -y face
c
      SP.Shape(y,2)= -tlen/3.0
      SP.shape(y,3)= SP.shape(y,2)
      SP.shape(y,6)= SP.shape(y,2)
      SP.shape(y,7)= SP.shape(y,2)
c
c     Assign the z location of the bottom face
c
      do i= 1, 2
        ptaG(i)= isgp(i,cntr)
      end do
      ptaG(z)= 0.002
      call PntGCStoLCS( isgp,ptaG,ptaL )
      SP.shape(z,1)= ptaL(z)      
      SP.shape(z,2)= SP.shape(z,1)      
      SP.shape(z,3)= SP.shape(z,1)
      SP.shape(z,4)= SP.shape(z,1)
c
c     Assign the z location of the top face
c
      do i= 1, 3
        ptaG(i)= (ta(i)+tp(i))/2.0
      end do
      ptaG(z)= ptaG(z) - mrad
      call PntGCStoLCS( isgp,ptaG,ptaL )
      SP.shape(z,5)= ptaL(z)
      SP.shape(z,6)= SP.shape(z,5)   
      SP.shape(z,7)= SP.shape(z,5)
      SP.shape(z,8)= SP.shape(z,5)
      return                
      end                          
c
c
c
      subroutine CalcMeta1Shape( cm,cp,m1a,m1p,m5a,m5p,isgp,SP,side,ierr ) 
      implicit none
      include 'a_const.inc'
      include 'a_struct.inc'      
      include 'a_names.inc'                   

      record /seg_param/ SP
                                
      integer*4 side,ierr                                   
      real*4    cm(3),cp(3),m1a(3),m1p(3),m5a(3),m5p(3),isgp(3,4)

      integer*4 i,vecnorm,istat,MinLineXPnts
      real*4    ptaG(3),ptupL(3),ptupG(3),ptdnG(3),ptdnL(3),mrad,VecZ(3),
     &          mid(3),mnrm(3),ml(3),mlnrm(3),fwidth,veclth,m15m(3)

      data VecZ / 0.0,0.0,1.0 /       
                     
      mrad= 0.0064 ! marker radius
c
c     Calculate the location (in LCS) of the front face
c     Note that isgp actually contains the position/orientation of
c     the metatarsal relative to the GCS at the calibration position
c
      call CopyPnt( m1a,ptupG )
      ptupG(z)= ptupG(z) - mrad
      call CopyPnt( m1a,ptdnG )                                  
      ptdnG(z)= 0.002                   
      call PntGCStoLCS( isgp,ptupG,ptupL)
      call PntGCStoLCS( isgp,ptdnG,ptdnL)
      if ( side .eq. rt ) then
        call CopyPnt( ptupL,SP.shape(x,5) )
        call CopyPnt( ptdnL,SP.shape(x,1) )
      else                             
        call CopyPnt( ptupL,SP.shape(x,6) )
        call CopyPnt( ptdnL,SP.shape(x,2) )
      end if

c      do i= 1, 3
c        mid(i)= (m1a(i)+m5a(i))/2.0 - cp(i)
c      end do                           
c      istat= vecnorm(mid,mnrm)
c      if ( istat .ne. 0 ) goto 1000
c      call vecX( vecZ,mnrm,ml )
c      istat= vecnorm(ml,mlnrm)
c      if ( istat .ne. 0 ) goto 1000
c      fwidth= veclth( m1a,m5a )
c      if ( side .eq. lf ) fwidth= -fwidth
      do i= x, z
        ptupG(i)= m1a(i) + 0.375*(m5a(i)-m1a(i))
      end do
      ptupG(z)= ptupG(z) - mrad  
      call CopyPnt( ptupG,ptdnG )
      ptdnG(z)= 0.002
      call PntGCStoLCS( isgp,ptupG,ptupL )
      call PntGCStoLCS( isgp,ptdnG,ptdnL )
      if ( side .eq. rt ) then             
        call CopyPnt( ptupL,SP.shape(x,6) )
        call CopyPnt( ptdnL,SP.shape(x,2) )
      else                             
        call CopyPnt( ptupL,SP.shape(x,5) )
        call CopyPnt( ptdnL,SP.shape(x,1) )
      end if                      
c                    
c     Calculate the location (in LCS) of the back face
c                                      
c      do i= 1, 3                                       
c        ptupG(i)= m1p(i) + 0.375*(cm(i)-m1p(i))
c      end do             
      call CopyPnt( m1p,ptupG )
      ptupG(z)= ptupG(z) - mrad
      call CopyPnt( ptupG,ptdnG )         
      ptdnG(z)= 0.002
      call PntGCStoLCS( isgp,ptupG,ptupL )
      call PntGCStoLCS( isgp,ptdnG,ptdnL )
      if ( side .eq. rt ) then          
        call CopyPnt( ptupL,SP.shape(x,8) )
        call CopyPnt( ptdnL,SP.shape(x,4) )
      else                             
        call CopyPnt( ptupL,SP.shape(x,7) )
        call CopyPnt( ptdnL,SP.shape(x,3) )
      end if                               
      
c      do i= 1, 3
c        m15m(i)= (m1a(i)+m5a(i))/2.0
c        ptaG(i)= ptupG(i)
c      end do        
c      istat= MinLineXPnts( mnrm,m15m,mlnrm,ptaG,ptdnG,ptupG )
c      if ( istat .ne. 0 ) goto 1000
      do i= x, z
        ptupG(i)= m1p(i) + 0.375*(m5p(i)-m1p(i))
      end do
      ptupG(z)= ptupG(z) - mrad  
      call CopyPnt( ptupG,ptdnG )
c      ptupG(z)= ptaG(z)
      ptdnG(z)= 0.002      
      call PntGCStoLCS( isgp,ptupG,ptupL )
      call PntGCStoLCS( isgp,ptdnG,ptdnL )
      if ( side .eq. rt ) then          
        call CopyPnt( ptupL,SP.shape(x,7) )
        call CopyPnt( ptdnL,SP.shape(x,3) )
      else                             
        call CopyPnt( ptupL,SP.shape(x,8) )
        call CopyPnt( ptdnL,SP.shape(x,4) )
      end if                               
      ierr= 0
      return

1000  ierr= istat
      return
      end                          
c
c
c
      subroutine CalcMeta5Shape( cl,cp,m1a,m1p,m5a,m5p,isgp,SP,side,ierr ) 
      implicit none
      include 'a_const.inc'
      include 'a_struct.inc'      
      include 'a_names.inc'                   

      record /seg_param/ SP
                                
      integer*4 side,ierr
      real*4    cl(3),cp(3),m1a(3),m1p(3),m5a(3),m5p(3),isgp(3,4)

      integer*4 i,vecnorm,istat,MinLineXPnts           
      real*4    ptaG(3),ptupL(3),ptupG(3),ptdnG(3),ptdnL(3),mrad,VecZ(3),
     &          mid(3),mnrm(3),ml(3),mlnrm(3),fwidth,veclth,m15m(3)

      data VecZ / 0.0,0.0,1.0 /       
                    
      mrad= 0.0064 ! marker radius
c
c     Calculate the location (in LCS) of the front face
c     Note that isgp actually contains the position/orientation of
c     the metatarsal relative to the GCS at the calibration position
c
      call CopyPnt( m5a,ptupG )
      ptaG(z)= ptupG(z) - mrad
      call CopyPnt( m5a,ptdnG )                                  
      ptdnG(z)= 0.002                   
      call PntGCStoLCS( isgp,ptupG,ptupL)
      call PntGCStoLCS( isgp,ptdnG,ptdnL)
      if ( side .eq. rt ) then
        call CopyPnt( ptupL,SP.shape(x,6) )
        call CopyPnt( ptdnL,SP.shape(x,2) )
      else                             
        call CopyPnt( ptupL,SP.shape(x,5) )
        call CopyPnt( ptdnL,SP.shape(x,1) )
      end if

c      do i= 1, 3
c        mid(i)= (m1a(i)+m5a(i))/2.0 - cp(i)
c      end do                           
c      istat= vecnorm(mid,mnrm)
c      if ( istat .ne. 0 ) goto 1000
c      call vecX( vecZ,mnrm,ml )
c      istat= vecnorm(ml,mlnrm)           
c      if ( istat .ne. 0 ) goto 1000
c      fwidth= veclth( m1a,m5a )
c      if ( side .eq. lf ) fwidth= -fwidth
      do i= x, z
        ptupG(i)= m5a(i) + 0.375*(m1a(i)-m5a(i))
      end do
      ptupG(z)= ptupG(z) - mrad  
      call CopyPnt( ptupG,ptdnG )
      ptdnG(z)= 0.002
      call PntGCStoLCS( isgp,ptupG,ptupL )
      call PntGCStoLCS( isgp,ptdnG,ptdnL )
      if ( side .eq. rt ) then
        call CopyPnt( ptupL,SP.shape(x,5) )
        call CopyPnt( ptdnL,SP.shape(x,1) )
      else                             
        call CopyPnt( ptupL,SP.shape(x,6) )
        call CopyPnt( ptdnL,SP.shape(x,2) )
      end if
c                    
c     Calculate the location (in LCS) of the back face
c                                      
c      do i= 1, 3                                       
c        ptupG(i)= m5p(i) + 0.375*(cl(i)-m5p(i))
c      end do             
      call CopyPnt( m5p,ptupG )
      ptupG(z)= ptupG(z) - mrad
      call CopyPnt( ptupG,ptdnG )
      ptdnG(z)= 0.002
      call PntGCStoLCS( isgp,ptupG,ptupL )
      call PntGCStoLCS( isgp,ptdnG,ptdnL )
      if ( side .eq. rt ) then          
        call CopyPnt( ptupL,SP.shape(x,7) )
        call CopyPnt( ptdnL,SP.shape(x,3) )
      else                             
        call CopyPnt( ptupL,SP.shape(x,8) )
        call CopyPnt( ptdnL,SP.shape(x,4) )
      end if                               
      
c      do i= 1, 3
c        m15m(i)= (m1a(i)+m5a(i))/2.0
c        ptaG(i)= ptupG(i)
c      end do        
c      istat= MinLineXPnts( mnrm,m15m,mlnrm,ptaG,ptdnG,ptupG )
c      if ( istat .ne. 0 ) goto 1000
      do i= x, z
        ptupG(i)= m5p(i) + 0.375*(m1p(i)-m5p(i))
      end do
      ptupG(z)= ptupG(z) - mrad  
      call CopyPnt( ptupG,ptdnG )
c      ptupG(z)= ptaG(z)
      ptdnG(z)= 0.002      
      call PntGCStoLCS( isgp,ptupG,ptupL )
      call PntGCStoLCS( isgp,ptdnG,ptdnL )
      if ( side .eq. rt ) then          
        call CopyPnt( ptupL,SP.shape(x,8) )
        call CopyPnt( ptdnL,SP.shape(x,4) )
      else                             
        call CopyPnt( ptupL,SP.shape(x,7) )
        call CopyPnt( ptdnL,SP.shape(x,3) )
      end if                               
      ierr= 0
      return

1000  ierr= istat
      return
      end                          
c
c                           
c
      subroutine CalcFtF3dShape( cp,am,al,t1a,isgp,SP,side ) 
      implicit none
      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_names.inc'                   

      record /seg_param/ SP 

      integer*4 side                               
      real*4    cp(3),am(3),al(3),t1a(3),isgp(3,4)
                           
      integer*4 i            
      real*4    ptaG(3),ptaL(3),tlen,veclth,mrad

      mrad= 0.0064 ! marker radius
c                                  
c     Calculate the x location (in LCS) of the front face
c                                                 
      call PntGCStoLCS( isgp,t1a,ptaL )
      SP.shape(x,1)= ptaL(x)
      SP.shape(x,2)= SP.shape(x,1)                
      SP.shape(x,5)= SP.shape(x,1)         
      SP.shape(x,6)= SP.shape(x,1)
c                                  
c     Calculate the x location (in LCS) of the back face
c
      call PntGCStoLCS( isgp,cp,ptaL )
      SP.shape(x,4)= ptaL(x) + mrad
      SP.shape(x,3)= SP.shape(x,4)     
      SP.shape(x,8)= SP.shape(x,4)
      SP.shape(x,7)= SP.shape(x,4)
c                                 
c     Calculate the y location of the +y face
c
      if ( side .eq. rt ) then
        call PntGCStoLCS( isgp,am,ptaL )
      else
        call PntGCStoLCS( isgp,al,ptaL )
      end if
      SP.Shape(y,1)= ptaL(y) - 4.0*mrad
      SP.shape(y,4)= SP.shape(y,1)   
      SP.shape(y,5)= SP.shape(y,1)
      SP.shape(y,8)= SP.shape(y,1)
c                                  
c     Calculate the y location of the -y face
c
      if ( side .eq. rt ) then
        call PntGCStoLCS( isgp,al,ptaL )
      else
        call PntGCStoLCS( isgp,am,ptaL )
      end if                      
      SP.Shape(y,2)= ptaL(y) + 4.0*mrad
      SP.shape(y,3)= SP.shape(y,2)
      SP.shape(y,6)= SP.shape(y,2)
      SP.shape(y,7)= SP.shape(y,2)
c
c     Assign the z location of the bottom face
c                                 
      do i= 1, 2
        ptaG(i)= isgp(i,cntr)
      end do                      
      ptaG(z)= 0.002         
      call PntGCStoLCS( isgp,ptaG,ptaL )
      SP.shape(z,1)= ptaL(z)      
      SP.shape(z,2)= SP.shape(z,1)      
      SP.shape(z,3)= SP.shape(z,1)
      SP.shape(z,4)= SP.shape(z,1)
c                                  
c     Assign the z location of the top face at front
c
      do i= x, z
        ptaG(i)= t1a(i)
      end do                      
      ptaG(z)= ptaG(z) - mrad         
      call PntGCStoLCS( isgp,ptaG,ptaL )
      SP.shape(z,5)= ptaL(z)      
      SP.shape(z,6)= SP.shape(z,5)
c                                  
c     Assign the z location of the top face at back
c
      do i= 1, 3
        ptaG(i)= (am(i)+al(i))/2.0
      end do
      call PntGCStoLCS( isgp,ptaG,ptaL )
      SP.shape(z,7)= ptaL(z)
      SP.shape(z,8)= SP.shape(z,7)   
      return                
      end                          
c                   
c
c
      subroutine CopyPnt(pi,po)
      implicit none

      real*4    pi(3),po(3)
      integer*4 i

      do i= 1, 3
        po(i)= pi(i)
      end do
      return
      end
