c                                        
c	do_FRC_calc  
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Controlling routine that calls the force plate data calculation
c				routines
c
c	Routines called:
c		ConditionFrcData 
c		AlignFrcMtnData
c		ClipFrcMtnData
c		CalcCntrPres
c		CalcCntrPresLCS
c		CalcFrcWrench
c		CalcFrcIntegral
c		ReconstructGRL
c		CalcPitchMomentArm
c		CalcPitchWrenchArm
c		CalcCprVel
c		FilterFRC
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	None
c
c	Common Block Variable MODIFIED by routine:
c	Block Name		Variable Name
c
c	None
c
      subroutine do_FRC_calc( CMND,FF,SF,JF,CF,OF )
      implicit none

      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_prm.inc'
                          
      record /CmndLine/     CMND
      record /force_flag/   FF
      record /segment_flag/ SF
      record /joint_flag/   JF
      record /cycle_flag/   CF
      record /option_flag/  OF

      real*4 MinFz(MAX_PLT),MaxFz(MAX_PLT)
                                                     
      if ( CMND.CmndText(2) .eq. 'CON' ) then 
        call ConditionFrcData( CMND,FF,OF )

      else if ( CMND.CmndText(2) .eq. 'ALI' ) then 
        call AlignFrcMtnData( CMND,FF,SF,
     &                        JF,CF,OF )

      else if ( CMND.CmndText(2) .eq. 'CLI' ) then
        call ClipFrcMtnData( FF,SF,JF,OF )

      else if ( CMND.CmndText(2) .eq. 'CPR' ) then
        call CalcCntrPres( FF,OF )

      else if ( CMND.CmndText(2) .eq. 'LCP' ) then
        call CalcCntrPresLCS( FF,SF,OF )
                                   
      else if ( CMND.CmndText(2) .eq. 'WRE' ) then
        call CalcFrcWrench( FF,OF )
                  
      else if ( CMND.CmndText(2) .eq. 'INT' ) then
        call CalcFrcIntegral( CMND,FF,SF,OF )

      else if ( CMND.CmndText(2) .eq. 'REC' ) then
        call ReconstructGRL( FF,JF,OF )

      else if ( CMND.CmndText(2) .eq. 'PMA' ) then
        call CalcPitchMomentArm( FF,SF,OF )

      else if ( CMND.CmndText(2) .eq. 'PWA' ) then
        call CalcPitchWrenchArm( FF,SF,OF )

      else if ( CMND.CmndText(2) .eq. 'CPV' ) then
        call CalcCprVel( FF,SF,OF )

      else if ( CMND.CmndText(2) .eq. 'FIL' ) then
         call FilterFRC( CMND,FF,OF )

      else
        write(6,100) CMND.CmndText(2)              
        if ( OF.ToLog ) write(3,100) CMND.CmndText(2)
100     format(' ',a3,' is an invalid FRC command.'/
     &         ' Valid commands are: CONdition      ALIgn  '/
     &         '                     CLIpdata       CenterofPRessure'/
     &         '                     WREnch         LcsCenterofPressure'/
     &         '                     INTegral       REConstructGRL'/
     &         '                     PitchMomentArm PitchWrenchArm'/
     &         '                     CPVelocity     FILter')
      end if
      return
      end
c         
c   ConditionFrcData                          
c
c	Copyright by Dwight Meglan 1990
c
c                       
c	Purpose:	Evaluate raw force plate data.  Fix GRL's so that they are all
c				relative to the lab GCS.  Determine the no load applied signal
c				level of each plate and remove this if desired.  Create a set
c				of GRL's which contain the combined force plate data.  Lastly,
c				using a definable threshold determine at each time frame whether
c				the plate is loaded or not.
c
c	Routines called:
c		VecChangeCS
c		VecX
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Force			FRC_COND.idle_percent
c	Force			FRC_COND.num_idle_frm
c	Param			FP.nfrm
c	Param			FP.num_plt
c	Param			FP.plt_loc.LCS
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Force_Plate		CmbGrl
c	Force_Plate		FcDataOK
c	Force_Plate		FpDataOK
c	Force_Plate		FpData
c
      subroutine ConditionFrcData( CMND,FF,OF )
      implicit none                           

      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_prm.inc'
      include 'a_gls2.inc'
      include 'a_frc.inc'
      include 'a_names.inc'
                            
      record /CmndLine/     CMND
      record /force_flag/   FF
      record /cycle_flag/   CF
      record /option_flag/  OF

      real*4    MinLd(6,MAX_PLT),MaxLd(6,MAX_PLT)       

      integer*4 MAX_REGION
      parameter (MAX_REGION=50)
                    
      real*4    base_value,force(3),moment(3),
     &          added_moment(3),base(6),IdlePct,FzMinThreshold,
     &          IdleMaxThreshold,LdRange(6),DelLd,DelLdMax(6),
     &          RegionMin(6,MAX_REGION,MAX_PLT),
     &          RegionMax(6,MAX_REGION,MAX_PLT),
     &          RegionAvg(6,MAX_REGION,MAX_PLT),
     &          RegionRange(6,MAX_REGION,MAX_PLT),minval,sum,VecMag
      integer*4 frm,i,j,channel,start,end,IdleWidth,plt,IdleFrm,load,ld,
     &          LogtoInt,RegionStart(MAX_REGION),RegionEnd(MAX_REGION),
     &          MinRegion(MAX_PLT),NumRegion
      logical*4 ZeroPlt,Diagnostics,ReStart,UseAllLoads,UseRegionMethod
      character YesNo(2)*3,ldname(6)*2
      
      data YesNo /'yes','no '/ 
      data ldname /'fx','fy','fz','mx','my','mz'/
                                                          
      if ( .not. FF.read ) goto 1000
c                              
c     Check for options- NOZ,IDL,FRM,FZM,IDM,NOD,ALL,VER,REG,PCT
c                
      ZeroPlt= .true.
      IdlePct= FRC_COND.idle_percent
      IdleFrm= FRC_COND.num_idle_frm
      FzMinThreshold=   10.0 ! Newtons
      IdleMaxThreshold= 100.0 ! Newtons
      Diagnostics= .false.
      UseAllLoads= .false.
      UseRegionMethod= .true.
      NumRegion= 40
      if ( CMND.NumOpt(2) .gt. 0 ) then
        do i= 1, CMND.NumOpt(2)         
          if ( CMND.OptText(i,2) .eq. 'NOZ' ) then
            ZeroPlt= .false.
          else if ( CMND.OptText(i,2) .eq. 'IDL' ) then ! set idle level
            if ( .not. CMND.OptNumOK(i,2) .or.
     &           ( CMND.OptRealVal(i,2) .lt. 0.0 ) .or.
     &           ( CMND.OptRealVal(i,2) .gt. 100.0 ) ) goto 1010 ! error
            IdlePct= CMND.OptRealVal(i,2)
          else if ( CMND.OptText(i,2) .eq. 'IDM' ) then ! set maximum idle level
            if ( .not. CMND.OptNumOK(i,2) .or.
     &           ( CMND.OptIntVal(i,2) .lt. 0 ) ) goto 1050 ! error
            IdleMaxThreshold= CMND.OptIntVal(i,2)
          else if ( CMND.OptText(i,2) .eq. 'FRM' ) then ! set idle frames
            if ( .not. CMND.OptNumOK(i,2) .or.
     &           ( CMND.OptIntVal(i,2) .lt. 0 ) .or.           
     &           ( CMND.OptIntVal(i,2) .gt. FP.nfrm ) ) goto 1020 ! error
            IdleFrm= CMND.OptIntVal(i,2)
          else if ( CMND.OptText(i,2) .eq. 'FZM' ) then ! set minimum fz level
            if ( .not. CMND.OptNumOK(i,2) .or.
     &           ( CMND.OptIntVal(i,2) .lt. 0 ) ) goto 1040 ! error
            FzMinThreshold= CMND.OptIntVal(i,2)
          else if ( CMND.OptText(i,2) .eq. 'DIA' ) then ! show diagnostics
            Diagnostics= .true.
          else if ( CMND.OptText(i,2) .eq. 'ALL' ) then ! use all plate loads
            UseAllLoads= .true.
          else if ( CMND.OptText(i,2) .eq. 'VER' ) then ! use plate vertical
            UseAllLoads= .false.
          else if ( CMND.OptText(i,2) .eq. 'PCT' ) then ! use percent method
            UseRegionMethod= .false.
          else if ( CMND.OptText(i,2) .eq. 'REG' ) then ! use regions to zero
            UseRegionMethod= .true.
            if ( CMND.OptNumOK(i,2) ) then
               if ( (CMND.OptIntVal(i,2) .lt. 1 ) .or.
     &              ( CMND.OptIntVal(i,2) .gt. MAX_REGION ) ) goto 1060 ! error
               NumRegion= CMND.OptIntVal(i,2)
            end if
          else                                                 
            goto 1030 ! bad option
          end if         
        end do ! i                                             
      end if ! options are present 
c
c     Display settings being used
c                                           
      if ( Diagnostics ) write(6,500) YesNo(LogtoInt(ZeroPlt)+1),IdlePct,IdleFrm,
     &                                IdleMaxThreshold,FzMinThreshold 
500   format(' Force plate condition settings'//
     &       ' Remove zero level from plate signals (NOZ): ',7x,a3/
     &       ' Variation allowed in idle region     (IDL): ',4x,f6.2,' % fz change'/
     &       ' Minimum # of frames in idle region   (FRM): ',7x,i3/
     &       ' Maximum level of Fz in idle region   (IDM): ',f10.2,' N'/
     &       ' Minimum Fz plate active threshold    (FZM): ',f10.2,' N '/)
c     
c     find vertical force minimums and maximums     
c                         
      do plt= 1, FP.num_plt    
        do ld= fx, mz
          MinLd(ld,plt)= FpData(ld,1,plt)
          MaxLd(ld,plt)= FpData(ld,1,plt)           
        end do
      end do      
      do frm= 2, FP.nfrm
        do plt= 1, FP.num_plt 
          do ld= fx, mz
          if ( FpData(ld,frm,plt) .lt. MinLd(ld,plt) ) 
     &      MinLd(ld,plt)= FpData(ld,frm,plt)
          if ( FpData(ld,frm,plt) .gt. MaxLd(ld,plt) ) 
     &      MaxLd(ld,plt)= FpData(ld,frm,plt)
          end do                               
        end do
      end do                            
      if ( Diagnostics ) then
         write(6,810) 
         if ( OF.ToLog) write(3,810) 
 810     format('Plate minimums and maximums'/
     &          'Plt Load     Min       Max')
         do plt= 1, FP.num_plt    
            do ld= fx, mz
               write(6,811) plt,ldname(ld),MinLd(ld,plt),MaxLd(ld,plt)
               if ( OF.ToLog ) write(3,811) plt,ldname(ld),
     &                                      MinLd(ld,plt),MaxLd(ld,plt)
 811           format(1x,i2,1x,a2,1x,f12.6,1x,f12.6)
            end do
         end do                            
      end if
c
c     if region method then setup the regions and find their mins,maxs,
c     averages, and ranges. Identify region which the smallest change 
c     over it and call this the zero region. It is assumed that this is
c     the region where there is no load on the force plate
c
      if ( UseRegionMethod ) then
         frm= 1
         do i= 1, NumRegion
            RegionStart(i)= frm
            RegionEnd(i)= frm + float(FP.nfrm)/float(NumRegion)
            frm= RegionEnd(i) + 1
         end do
         RegionEnd(NumRegion)= FP.nfrm
         if ( RegionStart(NumRegion) .gt. RegionEnd(NumRegion) )
     &     RegionStart(NumRegion)= RegionEnd(NumRegion)-1
         do i= 1, NumRegion
            do plt= 1, FP.num_plt
               do ld= fx, mz
                  RegionMin(ld,i,plt)= FpData(ld,RegionStart(i),plt)
                  RegionMax(ld,i,plt)= FpData(ld,RegionStart(i),plt)
                  sum= 0.0
                  do frm= RegionStart(i), RegionEnd(i)
                     if ( RegionMin(ld,i,plt) .gt. FpData(ld,frm,plt) )
     &                  RegionMin(ld,i,plt)= FpData(ld,frm,plt)
                     if ( RegionMax(ld,i,plt) .lt. FpData(ld,frm,plt) )
     &                  RegionMax(ld,i,plt)= FpData(ld,frm,plt)
                     sum= sum + FpData(ld,frm,plt)
                  end do
                  RegionAvg(ld,i,plt)= sum/(RegionEnd(i)-RegionStart(i)+1)
                  RegionRange(ld,i,plt)= RegionMax(ld,i,plt) - 
     &                                   RegionMin(ld,i,plt)
               end do
            end do
         end do
         do plt= 1, FP.num_plt
            MinRegion(plt)= 1
            minval= 0.0
            if ( UseAllLoads ) then
               do ld= fx, mz
c                  minval= minval + abs(RegionAvg(ld,1,plt))
                  minval= minval + abs(RegionRange(ld,1,plt))
               end do
            else
c               minval= abs(RegionAvg(fz,1,plt))
               minval= abs(RegionRange(fz,1,plt))
            end if
            do i= 2, NumRegion
               if ( UseAllLoads ) then
                  sum= 0.0
                  do ld= fx, mz
c                     sum= sum + abs(RegionAvg(ld,i,plt))
                     sum= sum + abs(RegionRange(ld,i,plt))
                  end do
               else
c                  sum= abs(RegionAvg(fz,i,plt))
                  sum= abs(RegionRange(fz,i,plt))
               end if
               if ( sum .lt. minval ) then
                  MinRegion(plt)= i
                  minval= sum
               end if
            end do
         end do
      end if

c                               
c       Find base values of all 6                           
c       signal channels and subtract them so that loads start at zero.       
c       This corrects for zero shifting of the force plate amplifiers.
c       The region used for zeroing the channels is scanned for by looking for
c       a zone in all load signals which is IdleFrm wide and varies in magnitude
c       by less than IdlePct of the maximum range of the signal.  IdleFrm
c       and IdlePct have default values (set using OPT FRC FRM #
c       or OPT FRC IDL #) which are overridden using the options 
c       /FRM= or /IDL= .
c
c       Optionally (/ALL rather than the default /VER)
c       Changed from looking at Fz for region to looking at all 6 signals to
c       find region because of faulty A/D at Mayo which allowed some of the
c       channels to have a first order decay at the beginning of sample time.
c       This resulted in large values being subtracted from some channels and
c       subsequent screwing up of the GRLs !!!
c       dam  Feb. 11, 1993
c                                  
      if ( ZeroPlt ) then
        write(6,103)                                     
103     format(' Calculating base line values of force plate signals',
     &         ' and then zeroing signals...'/)
        do plt= 1, FP.num_plt  
          if ( UseRegionMethod ) then
             start= RegionStart(MinRegion(plt))
             end= RegionEnd(MinRegion(plt))
          else
             start= 1
             end= 1
             IdleWidth= 1
             do ld= fx, mz
                DelLdMax(ld)= abs( MaxLd(ld,plt) - MinLd(ld,plt) )*(IdlePct/100.)
             end do
             do while ( (IdleWidth .lt. IdleFrm) .and. (end .lt. FP.nfrm) )
                end= start + IdleWidth
                ReStart= .false.
                if ( UseAllLoads ) then
                   do ld= fx, mz
                      DelLd= abs( FpData(ld,end,plt) - FpData(ld,start,plt) )
                      if ( DelLd .gt. DelLdMax(ld) ) ReStart= .true.
                   end do
                else
                   DelLd= abs( FpData(fz,end,plt) - FpData(fz,start,plt) )
                   if ( DelLd .gt. DelLdMax(fz) ) ReStart= .true.
                end if
                if ( (.not. ReStart) .and. 
     &               ( abs(FpData(fz,end,plt)) .le. IdleMaxThreshold) ) then
                   IdleWidth= IdleWidth + 1
                else                                     
                   start= start + IdleWidth
                   IdleWidth= 1                                       
                end if    
             end do             ! while
          end if
          if ( end .lt. FP.nfrm .or. UseRegionMethod ) then ! true means that there is an 
            do i= fx, mz                  ! idle region in the force plate data
              base(i)= 0.0     
              do frm= start, end
                base(i)= base(i) + FpData(i,frm,plt)
              end do
              base(i)= base(i)/float( end-start+1 )
              do frm= 1, FP.nfrm
                FpData(i,frm,plt)= FpData(i,frm,plt)-base(i)
              end do                   
            end do ! i
            if ( Diagnostics ) then
               write(6,119) plt,start,end
               if ( OF.ToLog ) write(3,119) plt,start,end
 119           format(' Plate ',i1,' idle region, frames ',i5,' to ',i5)
               write(6,120) base(1),base(4),
     &                      base(2),base(5),base(3),base(6)
               if ( OF.ToLog ) write(3,120) base(1),base(4),base(2),
     &                                      base(5),base(3),base(6)
120            format( ' Zeroing out force/moment values of:'/
     &                 '   fx - ',f11.4,' N',10x,'mx - ',f11.4,' Nm'/
     &                 '   fy - ',f11.4,' N',10x,'my - ',f11.4,' Nm'/
     &                 '   fz - ',f11.4,' N',10x,'mz - ',f11.4,' Nm'/)
            end if	
          else
            write(6,100) IdleFrm,DelLdMax(1),DelLdMax(4),
     &                   DelLdMax(2),DelLdMax(5),DelLdMax(3),DelLdMax(6),plt
            if ( OF.ToLog ) write(3,100) IdleFrm,DelLdMax(1),DelLdMax(4),
     &                      DelLdMax(2),DelLdMax(5),DelLdMax(3),DelLdMax(6),plt
100         format(' ERROR- Could not find idle region with width of ',i3,' frames'/
     &             '        and maximum load changes of:'//
     &             '   fx - ',f11.4,' N',10x,'mx - ',f11.4,' Nm'/
     &             '   fy - ',f11.4,' N',10x,'my - ',f11.4,' Nm'/
     &             '   fz - ',f11.4,' N',10x,'mz - ',f11.4,' Nm'//
     &             ' Raw data is being used for plate # ',i2,' rather than removing'/
     &             ' the base line force/moment values.'/)
          end if                                                            
        end do ! plt
      end if ! zero plates
c                              
c     convert force plate data from local coordinate systems to global (lab)
c     coordinate system 
c
      if ( .not. FF.adjusted ) then ! change Coord System...
        write(6,101)                
101     format(' Converting force plate data to lab coordinate system...'/)
        do plt= 1, FP.num_plt                                   
          do frm= 1, FP.nfrm
            if ( OF.UpDate .and. (mod(frm,100) .eq. 0) ) 
     &        write(6,102) frm,FP.nfrm,plt,FP.num_plt
102         format('+Processing frame # ',i5,'/',i5,' Plate # ',i2,'/',i2)
            call VecChangeCS( FP.plt_loc(plt).LCS(x,x),
     &                        FpData(fx,frm,plt),force )
            call VecChangeCS( FP.plt_loc(plt).LCS(x,x),
     &                        FpData(mx,frm,plt),moment )
            call VecX( FP.plt_loc(plt).LCS(x,cntr), force,
     &                 added_moment )
            do i= x, z           
              FpData(i,frm,plt)=   force(i)  
              FpData(i+3,frm,plt)= moment(i) + added_moment(i)
            end do
            if ( VecMag(FpData(fx,frm,plt)) .gt. FzMinThreshold ) then
              FpDataOK(frm,plt)= .true.
            else
              FpDataOK(frm,plt)= .false.
            end if
          end do   ! frm
        end do  ! plt
        FF.adjusted= .true.
        do i= 1, 4
          FP.plt_loc(i).LCS(z,cntr)= 0.0
        end do
      end if
c
c     Combine force plate data for use in combined data calculations
c
      write(6,130)
130   format(' Combining force data...')
      do frm= 1, FP.nfrm
        do load= fx, mz
          CmbGrl(load,frm)= 0.0
          do plt= 1, FP.num_plt
            CmbGrl(load,frm)= CmbGrl(load,frm) + FpData(load,frm,plt)
          end do ! plt
        end do ! load
        if ( VecMag(CmbGrl(fx,frm)) .gt. FzMinThreshold ) then
          FcDataOK(frm)= .true.
        else
          FcDataOK(frm)= .false.
        end if
*$ifdef DEBUG
        write(4,551) frm,FpData(fz,frm,1),FpData(fz,frm,2),FpData(fz,frm,3),
     &               CmbGrl(fz,frm),FcDataOK(frm)
 551    format(1x,i5,1x,4(f10.4,1x),l1)
*$endif
      end do ! frm
      write(6,131)
131   format('+Combining force data...   Done')
      return  
                          
1000  write(6,1005)                
      if ( OF.ToLog ) write(3,1005)
1005  format(' Error- Force plate data has not been read in yet.')
      return                         
      
1010  write(6,1015) CMND.OptRealVal(i,2)
      if ( OF.ToLog ) write(3,1015) CMND.OptRealVal(i,2)
1015  format(' Error- ',f9.3,' is an invalid percentage for plate idle region '/
     &       '        level.  Idle level must be >0 and <100 %.'/
     &       '        (Of magnitude of plate vertical force)') 
      return
                                                           
1020  write(6,1025) CMND.OptIntVal(i,2),FP.nfrm
      if ( OF.ToLog ) write(3,1025) CMND.OptIntVal(i,2),FP.nfrm
1025  format(' Error- ',i5,' is an invalid number of frames for the idle'/
     &       '        region.  The number must >0 and <',i5,'.')
      return     
            
1030  write(6,1035) CMND.OptText(i,2)
      if ( OF.ToLog ) write(3,1035) CMND.OptText(i,2)
1035  format(' Error- ',a3,' is an invalid option for FoRCe ANAlyze'/
     &       '        Valid Options are: NOZeroplates IDLepercent idleFRaMes'/
     &       '                           FZMinimumthreshold IDleMaxthreshold'/
     &       '                           NODiagnostics useALLloads useVERtical'/
     &       '                           REGionmethod=# PerCenTmethod')
      return
 
1040  write(6,1045) CMND.OptText(i,2)
      if ( OF.ToLog ) write(3,1045) CMND.OptText(i,2)
1045  format(' Error- ',a3,' is an invalid fz minimum force threshold'/
     &       '        The threshold must be a number >= 0.0')
      return

1050  write(6,1055) CMND.OptText(i,2)
      if ( OF.ToLog ) write(3,1055) CMND.OptText(i,2)
1055  format(' Error- ',a3,' is an invalid idle maximum force threshold'/
     &       '        The threshold must be a number >= 0.0')
      return

1060  write(6,1065) CMND.OptText(i,2),MAX_REGION
      if ( OF.ToLog ) write(3,1065) CMND.OptText(i,2),MAX_REGION
1065  format(' Error- ',a3,' is an invalid number of regions'/
     &       '        The number of regions must be between 1 and ',i3)
      return
      end                                                                 
c         
c   ClipFrcMtnData                          
c
c	Copyright by Dwight Meglan 1990
c
c                       
c	Purpose:	Clip the motion data down to a single cycle defined by the
c				gait events and do the same with the force plate data using
c				frame offset to time align the data and the frame ratio to
c				extract those frames of data which correspond to the motion
c				data.
c
c           NOTE that for non gait marker sets, such as the CKC study, that
c           only the force/EMG data needs to be trimmed.
c
c	Routines called:
c		None
c
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	None
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Segment			BodyCM
c	Energy			BodyKE
c	Energy			BodyPE
c	Energy			BodyTE
c	Segment			BdMom
c	Force_Plate		CombIntegral
c	Force_Plate		CombIntegral2
c	Force_Plate		CmbCp
c	Force_Plate		CmbGrl
c	Force_Plate		CmbGrlWre
c	EMG				EMGData
c	Param			EP.NumSamples
c	Joint			event
c	Force_Plate		FcDataOK
c	Force_Plate		FpDataOK
c	Force_Plate		FlrCntct
c	Force_Plate		FtGrlOK
c	Param			FP.av_frm_ratio
c	Param			FP.first_frm
c	Param			FP.frm_offset
c	Param			FP.frm_rate
c	Param			FP.last_frm
c	Param			FP.nfrm
c	Force_Plate		FtIntegral
c	Force_Plate		FtIntegral2
c	Force_Plate		FtGrl
c	Force_Plate		ind_ft_cp
c	Force_Plate		FtGRLWre
c	Joint			JfsAngDCS
c	Joint			JfsAngJCS
c	Joint			JfsAngPCS
c	Joint			JtrnDCS
c	Joint			JtrnGCS
c	Joint			JtrnJCS
c	Joint			JtrnPCS
c	Joint			joint_acc_EUL
c	Joint			joint_acc_JCS
c	Joint			joint_angle_JCS
c	Joint			joint_axes_EUL
c	Joint			joint_axes_JCS
c	Joint			joint_angle_EUL
c	Joint			joint_vel_JCS
c	Joint			joint_vel_EUL
c	Force_Plate		FpData
c	Marker			MrkAcc
c	Marker			MrkVel
c	Marker			MrkData
c	Marker			MrkDist
c	Marker			MpnLCS
c	Param			MTN_PTM.cycle_first_frm
c	Param			MTN_PTM.cycle_last_frm
c	Param			MTN_PTM.first_frm
c	Param			MTN_PTM.lhs_frm
c	Param			MTN_PTM.lto_frm
c	Param			MTN_PTM.num_cycle
c	Param			MTN_PTM.nfrm
c	Param			MTN_PTM.rhs_frm
c	Param			MTN_PTM.rto_frm
c	Force_Plate		PltIntegral
c	Force_Plate		PltIntegral2
c	Force_Plate		FpCp
c	Force_Plate		FpWre
c	Segment			SgFinScrGCS
c	Joint			JFinScr
c	Segment			SgInsScrGCS
c	Joint			JInsScr
c	Energy			SegKE
c	Energy			SegPE
c	Energy			SegTE
c	Segment			SacGCS
c	Segment			SacLCS
c	Segment			SangGCS
c	Segment			SgMom
c	Segment			SgpGCS
c	Segment			SvlGCS
c	Segment			SvlLCS
c
      subroutine ClipFrcMtnData( FF,SF,JF,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_enr.inc'
      include 'a_jnt.inc'
      include 'a_gls2.inc'
      include 'a_frc.inc'
      include 'a_emg.inc'
      include 'a_names.inc'
                                      
      record /force_flag/   FF
      record /segment_flag/ SF
      record /joint_flag/   JF
      record /option_flag/  OF
                 
      integer*4 frm,FrcFrm,plt,load,MtnFrm,mrk,i,j,k,seg,jnt,FirstHSFrm,ft,
     &          axis,chnl,EMGFrm,NumSamples

      if ( .not. FF.aligned ) goto 1000
c
c     Clip raw load data to the length of the first gait cycle and only
c     keep those frames which correspond to the motion frames
c
      do frm= 1, MP.cycle_length(1)
        FrcFrm= 1 + FP.frm_Offset + float(frm-1)*FP.av_frm_ratio
        if ( FrcFrm .gt. 0 ) then
          do load= fx, mz
            do plt= 1, FP.num_plt                                      
              FpData(load,frm,plt)= FpData(load,FrcFrm,plt)
           end do ! plt
            do ft= rt, lf
              FtGrl(load,frm,ft)= FtGrl(load,FrcFrm,ft)
            end do
            CmbGrl(load,frm)= CmbGrl(load,FrcFrm)
          end do ! load 
          do ft= rt, lf
            FtGrlOK(frm,ft)= FtGrlOK(FrcFrm,ft)
          end do
          do plt= 1, FP.num_plt
            FpDataOK(frm,plt)= FpDataOK(FrcFrm,plt)
          end do
          FcDataOK(frm)= FcDataOK(FrcFrm)
          do axis= x, z
            do plt= 1, FP.num_plt                                      
              FpCp(axis,frm,plt)= FpCp(axis,FrcFrm,plt)
            end do ! plt
            do ft= rt, lf
              FtCpGCS(axis,frm,ft)= FtCpGCS(axis,FrcFrm,ft)
            end do
            CmbCp(axis,frm)= CmbCp(axis,FrcFrm)
          end do ! axis
          do i= 1, 8
            do plt= 1, FP.num_plt                                      
              FpWre(i,frm,plt)= FpWre(i,FrcFrm,plt)
            end do ! plt
            do ft= rt, lf                              
              FtGRLWre(i,frm,ft)= FtGRLWre(i,FrcFrm,ft)
            end do
            CmbGrlWre(i,frm)= CmbGrlWre(i,FrcFrm)
          end do ! i
          do i= 1, 4
            do plt= 1, FP.num_plt                                      
              PltIntegral(i,frm,plt)=  PltIntegral(i,FrcFrm,plt)
              PltIntegral2(i,frm,plt)= PltIntegral2(i,FrcFrm,plt)
            end do ! plt
            do ft= rt, lf
              FtIntegral(i,frm,ft)=  FtIntegral(i,FrcFrm,ft)
              FtIntegral2(i,frm,ft)= FtIntegral2(i,FrcFrm,ft)
            end do
            CombIntegral(i,frm)=  CombIntegral(i,FrcFrm)
            CombIntegral2(i,frm)= CombIntegral2(i,FrcFrm)
          end do ! i
        else ! zero out the force information for this frame...
          do load= fx, mz
            do plt= 1, FP.num_plt                                      
              FpData(load,frm,plt)= 0.0
           end do ! plt
            do ft= rt, lf
              FtGrl(load,frm,ft)= 0.0
            end do
            CmbGrl(load,frm)= 0.0
          end do ! load 
          do ft= rt, lf
            FtGrlOK(frm,ft)= .false.
          end do
          do plt= 1, FP.num_plt
            FpDataOK(frm,plt)= .false.
          end do
          FcDataOK(frm)= .false.
          do axis= x, z
            do plt= 1, FP.num_plt                                      
              FpCp(axis,frm,plt)= 0.0
            end do ! plt
            do ft= rt, lf
              FtCpGCS(axis,frm,ft)= 0.0
            end do
            CmbCp(axis,frm)= 0.0
          end do ! axis 
          do i= 1, 8
            do plt= 1, FP.num_plt                                      
              FpWre(i,frm,plt)= 0.0
            end do ! plt
            do ft= rt, lf                              
              FtGRLWre(i,frm,ft)= 0.0
            end do                   
            CmbGrlWre(i,frm)= 0.0
          end do ! i
          do i= 1, 4                      
            do plt= 1, FP.num_plt                                      
              PltIntegral(i,frm,plt)=  0.0
              PltIntegral2(i,frm,plt)= 0.0
            end do ! plt
            do ft= rt, lf               
              FtIntegral(i,frm,ft)=  0.0
              FtIntegral2(i,frm,ft)= 0.0
            end do
            CombIntegral(i,frm)=  0.0
            CombIntegral2(i,frm)= 0.0
          end do ! i
        end if      
      end do ! frm  
      NumSamples= MP.cycle_length(1)*(float(EP.SampleRate)/
     &                                     float(MP.frm_rate))
      if ( FP.frm_offset .ge. 0 ) then 
        do frm= 1, NumSamples-FP.first_frm
          do chnl= 1, EP.NumChnl
            EMGData(frm,chnl)= EMGData(frm+FP.first_frm,chnl)
          end do
        end do
      else
        do frm= NumSamples+FP.frm_offset, 1, -1
          do chnl= 1, EP.NumChnl
            EMGData(frm-FP.frm_offset,chnl)= EMGData(frm,chnl)
          end do
        end do
        do frm= 1, -FP.frm_offset
          do chnl= 1, EP.NumChnl
            EMGData(frm,chnl)= 0.0
          end do
        end do
      end if
      EP.NumSamples= NumSamples
      FP.first_frm= 1
      FP.last_frm= MP.cycle_length(1)
      FP.nfrm=  MP.cycle_length(1)
      FP.frm_rate= nint(rMtnFrmRate)
      FP.av_frm_ratio= 1.0
      FP.frm_offset= 0
c
c     This section only applies to gait based marker sets. At present all
c     other analyses assume that the entire time series of motion data is
c     of interest and thus there is no clipping of motion based data...
c
c     Clip the motion data to the length of the first cycle.  Also, adjust
c     the frame #'s which describe the gait cycle characteristics- Mainly
c     stuff in the MP structure
c
      if ( (GP.analysis_type .eq. FULL_ANL) .or.
     &     (GP.analysis_type .eq. HH_ANL)        ) then
        FirstHSFrm= MP.lhs_frm(1)
        if ( MP.first_ft .eq. rt ) FirstHSFrm= MP.rhs_frm(1)
        do frm= 1, MP.cycle_length(1)
          MtnFrm= FirstHSFrm + frm - 1       
          do mrk= 1, MP.num_mrk
            do j= x, z
              MrkData(j,frm,mrk)= MrkData(j,MtnFrm,mrk)
              MrkVel(j,frm,mrk)=   MrkVel(j,MtnFrm,mrk)
              MrkAcc(j,frm,mrk)=   MrkAcc(j,MtnFrm,mrk)
            end do ! j
          end do ! mrk
        end do ! frm
      
        if ( SF.position ) then ! reassign segment data
          do frm= 1, MP.cycle_length(1)
            MtnFrm= FirstHSFrm + frm - 1
            do ft= rt, lf
              FlrCntct(frm,ft)= FlrCntct(MtnFrm,ft)
            end do ! ft
            GaitEvent(frm)= GaitEvent(MtnFrm)
            do i= 1, GP.num_seg
              seg= GP.seg_num(i)
              do j= x, z
                do k= x, cntr
                  SgpGCS(j,k,frm,seg)= SgpGCS(j,k,MtnFrm,seg)
                end do ! k
                SangGCS(j,frm,seg)= SangGCS(j,MtnFrm,seg)
                SegKE(j,frm,seg)= SegKE(j,MtnFrm,seg)
              end do ! j
              SegPE(frm,seg)= SegPE(MtnFrm,seg)
              SegTE(frm,seg)= SegTE(MtnFrm,seg)
              do j= x, rz
                SvlGCS(j,frm,seg)= SvlGCS(j,MtnFrm,seg)
                SvlLCS(j,frm,seg)= SvlLCS(j,MtnFrm,seg)
                SacGCS(j,frm,seg)= SacGCS(j,MtnFrm,seg)
                SacLCS(j,frm,seg)= SacLCS(j,MtnFrm,seg)
              end do ! j
              do j= 1, 8
                SgFinScrGCS(j,frm,seg)= SgFinScrGCS(j,MtnFrm,seg)
                SgInsScrGCS(j,frm,seg)= SgInsScrGCS(j,MtnFrm,seg)
                SgMom(j,frm,seg)= SgMom(j,frm,seg)
              end do ! j
              do j= 1, SP(seg).num_mrk
                do k= x, z
                  MpnLCS(k,j,frm,seg)= MpnLCS(k,j,MtnFrm,seg)
                end do ! k
              end do ! j
              do j= 1, SP(seg).num_MrkDist
                MrkDist(j,frm,seg)= MrkDist(j,MtnFrm,seg)
              end do ! j
            end do ! i => seg
            do i= x, z
              BodyCM(i,frm)= BodyCM(i,MtnFrm)
            end do ! i
            do i= 1, 8
              BdMom(i,frm)= BdMom(i,MtnFrm)
            end do ! i
            do i= 1, 3
              BodyKE(i,frm)= BodyKE(i,MtnFrm)
            end do
            BodyPE(frm)= BodyPE(MtnFrm)
            BodyTE(frm)= BodyTE(MtnFrm)
          end do ! frm
        end if
      
        if ( JF.angle_JCS .or. JF.angle_EUL .or. JF.FiniteScrew ) then
          do frm= 1, MP.cycle_length(1)
            MtnFrm= FirstHSFrm + frm - 1
            do i= 1, GP.num_jnt
              jnt= GP.jnt_num(i)
              do j= x, z
                JangJCS(j,frm,jnt)= JangJCS(j,MtnFrm,jnt)
                JvelJCS(j,frm,jnt)=   JvelJCS(j,MtnFrm,jnt)
                JaccJCS(j,frm,jnt)=   JaccJCS(j,MtnFrm,jnt)
                JangEUL(j,frm,jnt)= JangEUL(j,MtnFrm,jnt)
                JvelEUL(j,frm,jnt)=   JvelEUL(j,MtnFrm,jnt)
                JaccEUL(j,frm,jnt)=   JaccEUL(j,MtnFrm,jnt)
                JfsAngJCS(j,frm,jnt)=  JfsAngJCS(j,MtnFrm,jnt)
                JfsAngPCS(j,frm,jnt)=  JfsAngPCS(j,MtnFrm,jnt)
                JfsAngDCS(j,frm,jnt)=  JfsAngDCS(j,MtnFrm,jnt)
                do k= x, z
                  JaxesJCS(k,j,frm,jnt)= JaxesJCS(k,j,MtnFrm,jnt)
                  JaxesEUL(k,j,frm,jnt)= JaxesEUL(k,j,MtnFrm,jnt)
                end do ! k            
              end do ! j
              do j= 1, 4
                JtrnGCS(j,frm,jnt)= JtrnGCS(j,MtnFrm,jnt)
                JtrnJCS(j,frm,jnt)= JtrnJCS(j,MtnFrm,jnt)
                JtrnPCS(j,frm,jnt)= JtrnPCS(j,MtnFrm,jnt)
                JtrnDCS(j,frm,jnt)= JtrnDCS(j,MtnFrm,jnt)
              end do
              do j= 1, 8
                JFinScr(j,frm,jnt)= JFinScr(j,MtnFrm,jnt)
                JInsScr(j,frm,jnt)= JInsScr(j,MtnFrm,jnt)
              end do ! j
            end do ! i => jnt
          end do ! frm
        end if
      
        if ( MP.first_ft .eq. rt ) then
          MP.rhs_frm(1)= 1
          MP.rhs_frm(2)= MP.rhs_frm(2) - FirstHSFrm + 1
          MP.lhs_frm(1)= MP.lhs_frm(1) - FirstHSFrm + 1
          MP.rto_frm(1)= MP.rto_frm(1) - FirstHSFrm + 1
          MP.lto_frm(1)= MP.lto_frm(1) - FirstHSFrm + 1
        else
          MP.lhs_frm(1)= 1
          MP.lhs_frm(2)= MP.lhs_frm(2) - FirstHSFrm + 1
          MP.rhs_frm(1)= MP.rhs_frm(1) - FirstHSFrm + 1
          MP.lto_frm(1)= MP.lto_frm(1) - FirstHSFrm + 1
          MP.rto_frm(1)= MP.rto_frm(1) - FirstHSFrm + 1
        end if
      end if ! is the motion data a gait marker set...

      MP.first_frm= 1
      MP.nfrm= MP.cycle_length(1)
      MP.num_cycle= 1
      MP.cycle_first_frm(1)= 1
      MP.cycle_last_frm(1)=  MP.nfrm
      FF.DataClipped= .true.
      return
        
1000  write(6,1005)
      if ( OF.ToLog ) write(3,1005)
1005  format(' Error- Force and motion data must aligned before the data'/
     &       '        can clipped to a single gait cycle.')
      return
      end
c                    
c                                             
c	Swap                                                           
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	swap to integer*4 numbers
c
      subroutine swap( num1, num2 )
      implicit none

      integer*4 num1, num2, temp

      temp= num1
      num1= num2
      num2= temp
      return
      end
c        
c	CalcCntrPres
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Calculate the center of pressure of the GRL on the force
c				plates, combined GRL as well as individual feet.
c
c	Routines called:
c		None
c
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Force			CmbGrl
c	Force			FcDataOK
c	Force			FpDataOK
c	Force			FtGrlOK
c	Param			FP.nfrm
c	Param			FP.num_plt
c	Force			FtGrl
c	Force			FpData
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Force			CmbCp
c	Force			ind_ft_cp
c	Force			FpCp
c
      subroutine CalcCntrPres( FF,OF )
      implicit none              
c
c     This subroutine calculates the center of pressure under each individual
c     foot and also the combined center of pressure when both feet are in
c     contact with the floor during double stance

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

      record /force_flag/   FF
      record /option_flag/  OF

      integer*4 foot,frm,plt
      logical*4 CpBad,CpOK
                             
      FF.cpr_GCS= .false.   
      if ( .not. FF.adjusted ) goto 1000 ! error       
c
c     Calculate the centers of pressure under each individual foot.
c  
      if ( FF.individual_feet ) then
        do foot= rt, lf    
          do frm= 1, FP.nfrm
            if ( .not. FtGrlOK(frm,foot) ) then 
              FtCpGCS(x,frm,foot)= 0.0
              FtCpGCS(y,frm,foot)= 0.0
              FtCpGCS(z,frm,foot)= 0.0
            else
              if ( abs(FtGrl(fz,frm,foot)) .gt. 1.0 ) then
                FtCpGCS(x,frm,foot)= -FtGrl(my,frm,foot)/
     &                                  FtGrl(fz,frm,foot)          
                FtCpGCS(y,frm,foot)=  FtGrl(mx,frm,foot)/
     &                                  FtGrl(fz,frm,foot)
                FtCpGCS(z,frm,foot)= FP.plt_loc(1).LCS(z,cntr)
                CpBad= .true.
                do plt= 1, FP.num_plt
                   if ( CpOK(FtCpGCS(x,frm,foot),FP.plt_loc(plt).LCS,
     &                       FP.plt_size(1,plt)) ) CpBad= .false.
                end do
                if ( CpBad ) then
                   FtCpGCS(x,frm,foot)= 0.0
                   FtCpGCS(y,frm,foot)= 0.0
                   FtCpGCS(z,frm,foot)= 0.0
                   FtGrlOK(frm,foot)= .false.
                end if
              else
                FtCpGCS(x,frm,foot)= 0.0
                FtCpGCS(y,frm,foot)= 0.0
                FtCpGCS(z,frm,foot)= 0.0
              end if
            end if ! fz valid
          end do ! frm 
        end do ! foot
      else
        write(6,500)
        if ( OF.ToLog ) write(3,500)
500     format(' NOTE- The individual foot centers of pressure are NOT being'/
     &         '       calculated because FoRCe ALIgn has NOT been carried out yet')
      end if ! force data has been broken down to individual foot components
c
c     Calculate the combined center of pressure for the resultant of both
c     feet.  This should differ from the individual foot centers of pressure only
c     during double stance.  If one of the feet has missed the force plates
c     then the combined center pressure is the same as the individual force
c     plate center of pressures     
c
      do frm= 1, FP.nfrm
        if ( .not. FcDataOK(frm) ) then
          CmbCp(x,frm)= 0.0                   
          CmbCp(y,frm)= 0.0
          CmbCp(z,frm)= 0.0
        else
          if ( abs(CmbGrl(fz,frm)) .gt. 1.0 ) then
            CmbCp(x,frm)= -CmbGrl(my,frm)/CmbGrl(fz,frm)
            CmbCp(y,frm)=  CmbGrl(mx,frm)/CmbGrl(fz,frm)
            CmbCp(z,frm)=  FP.plt_loc(1).LCS(z,cntr)
            CpBad= .true.
            do plt= 1, FP.num_plt
               if ( CpOK(CmbCp(x,frm),FP.plt_loc(plt).LCS,
     &                   FP.plt_size(1,plt)) ) CpBad= .false.
            end do
            if ( CpBad ) then
               CmbCp(x,frm)=  0.0                               
               CmbCp(y,frm)=  0.0
               CmbCp(z,frm)=  0.0
               FcDataOK(frm)= .false.
            end if
          else
            CmbCp(x,frm)=  0.0                               
            CmbCp(y,frm)=  0.0
            CmbCp(z,frm)=  0.0
          end if
        end if ! fz valid
      end do ! frm
c
c     Do the plate data individually
c
      do plt= 1, FP.num_plt
        do frm= 1, FP.nfrm
          if ( .not. FpDataOK(frm,plt) ) then
            FpCp(x,frm,plt)= 0.0
            FpCp(y,frm,plt)= 0.0
            FpCp(z,frm,plt)= 0.0        
          else
            if ( abs(FpData(fz,frm,plt)) .gt. 1.0 ) then
              FpCp(x,frm,plt)= -FpData(my,frm,plt)/FpData(fz,frm,plt)
              FpCp(y,frm,plt)=  FpData(mx,frm,plt)/FpData(fz,frm,plt)
              FpCp(z,frm,plt)=  FP.plt_loc(plt).LCS(z,cntr)
              if ( .not. CpOK(FpCp(x,frm,plt),FP.plt_loc(plt).LCS,
     &                        FP.plt_size(1,plt)) ) then
                 FpCp(x,frm,plt)= 0.0
                 FpCp(y,frm,plt)= 0.0
                 FpCp(z,frm,plt)= 0.0
                 FpDataOK(frm,plt)= .false.
              end if
            else
              FpCp(x,frm,plt)= 0.0
              FpCp(y,frm,plt)= 0.0
              FpCp(z,frm,plt)= 0.0
            end if
          end if ! fz valid
        end do ! frm
      end do ! plt
      FF.cpr_GCS= .true.
      return                            
                                                            
1000  write(6,1005)
      if ( OF.ToLog ) write(3,1005)
1005  format(' Error- The force plate data has not been analyzed yet')
      return
      end
c
c
c
      function CpOK(cp,lcs,size)
      implicit none

      real*4    cp(3),size(2),lcs(3,4)
      logical*4 CpOK

      real*4 cpl(3)

      CpOK= .true.
      call PntGCStoLCS(lcs,cp,cpl)
      if ( (abs(cpl(1)) .gt. size(1)/2.0) .or. 
     &     (abs(cpl(2)) .gt. size(2)/2.0)      ) CpOK= .false.
      return
      end

c        
c	CalcCntrPresLCS
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Calculate the center of pressure of the GRL on the feet in
c				the LCS of the feet.  Assign FtGRLLCS which is the GRL
c				of the ground upon the feet in the foot LCS.  Also, calculate
c				the strike index upon the feet.
c
c           NOTE: I modified this on Feb. 7, 1993 to handle the data from the
c                 foot3d marker set. The center of pressure and strike index
c                 are calculated relative to the foot and the big toe LCS and
c                 stored in the right and left foot arrays, respectively.
c                 Right foot has foot LCP and left foot has toe LCP. This is
c                 the case no matter if the right or left foot is being 
c                 evaluated with the foot3d marker set...
c
c	Routines called:
c		PntGCStoLCS
c		LoadGCStoLCS
c
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Force			FlrCntct
c	Force			FtGrlOK
c	Force			ind_ft_cp
c	Param			MP.cycle_length
c	Segment			SgpGCS
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Force			FtGRLLCS
c	Force			FtGrl
c	Force			FtCpLCS
c	Force			StrIdx
c
      subroutine CalcCntrPresLCS( FF,SF,OF )
      implicit none

      include 'a_const.inc'
      include 'a_struct.inc' 
      include 'a_prm.inc'
      include 'a_gls2.inc'
      include 'a_frc.inc'
      include 'a_seg.inc'       
      include 'a_ft3d.inc'
      include 'a_names.inc'

      record /force_flag/   FF
      record /segment_flag/ SF
      record /option_flag/  OF

      integer*4 i,MtnFrm,ft,seg,j,seg1,seg2,side
      real*4    sglen(2)         
                                                     
      if ( .not. FF.individual_feet ) goto 1000 ! error
      if ( .not. FF.cpr_GCS ) goto 1010  ! error
      if ( .not. SF.position ) goto 1020 ! error
      if ( .not. SF.param )    goto 1030 ! error
      if ( .not. FF.DataClipped ) goto 1050 ! error
      if ( (GP.Analysis_Type .eq. F3DR_ANL) .or. 
     &     (GP.Analysis_Type .eq. F3DL_ANL)      ) then
        seg1= foot
        seg2= toe1
      else
        seg1= rtft
        seg2= lfft
      end if       
      if ( (SP(seg1).length .lt. 0.0001) .or.
     &     (SP(seg2).length .lt. 0.0001)      ) goto 1040 ! error
      sglen(rt)= SP(seg1).length
      sglen(lf)= SP(seg2).length              
      do i= 1, MP.cycle_length(1) 
        do side= rt, lf    
          seg= seg1
          if ( side .eq. lf ) seg= seg2
          if ( GP.Analysis_Type .eq. F3DR_ANL ) then
            ft= rt 
          else if ( GP.Analysis_Type .eq. F3DL_ANL ) then
            ft= lf
          else
            ft= side
          end if       
          if ( FtGrlOK(i,ft) .and. FlrCntct(i,ft) ) then
            call PntGCStoLCS( SgpGCS(x,x,i,seg),
     &                        FtCpGCS(x,i,ft),
     &                        FtCpLCS(x,i,side) )
            StrIdx(i,side)= FtCpLCS(x,i,side)/sglen(side)
     &                            + .429 ! from Dempster
            FtGrl(mx,i,side)= 0.0
            FtGrl(my,i,side)= 0.0                 
            call LoadGCStoLCS( SgpGCS(x,x,i,seg),
     &                         FtGrl(fx,i,ft),FtGRLLCS(fx,i,side) )
            do j= fx, mz
              FtGRLLCS(j,i,side)= -FtGRLLCS(j,i,side)
            end do   
          else
            do j= 1, 3                               
              FtCpLCS(j,i,side)= 0.0
            end do
            do j= 1, 6
              FtGRLLCS(j,i,side)= 0.0
            end do             
            StrIdx(i,side)= 0.0
          end if            
        end do ! side   
      end do ! i                     
      FF.cpr_LCS= .true.
      FF.StrIdx= .true.
      return    
                                                          
1000  write(6,1005)                 
      if ( OF.ToLog ) write(3,1005)
1005  format(' Error- Force plate data has not been separated between the'/
     &       '        individual feet.')
      return
      
1010  write(6,1015)
      if ( OF.ToLog ) write(3,1015)
1015  format(' Error- GCS centers of pressure have not been calculated for'/
     &       '        the individual feet.')
      return
                                       
1020  write(6,1025)
      if ( OF.ToLog ) write(3,1025)
1025  format(' Error- Segment position has not been calculated.')
      return

1030  write(6,1035)
      if ( OF.ToLog ) write(3,1035)
1035  format(' Error- Foot length has not been calculated')
      return

1040  write(6,1045)
      if ( OF.ToLog ) write(3,1045)
1045  format(' Error- Foot length is 0.')
      return

1050  write(6,1055)
      if ( OF.ToLog ) write(3,1055)
1055  format(' Error- Force/motion data has not been clipped yet')
      return
      end
c        
c	CalcFrcWrench
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Calculate the load wrench due to the GRL upon the force 
c				plates, combined GRL and foot GRL's.  
c
c	Routines called:
c		CalcInstantScrewAxis
c
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Force			CmbGrl
c	Force			CmbCp
c	Force			FcDataOK
c	Force			FpDataOK
c	Force			FtGrlOK
c	Param			FP.nfrm
c	Param			FP.num_plt
c	Force			FtGrl
c	Force			ind_ft_cp
c	Force			FpData
c	Force			FpCp
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Force			FtGRLWre
c						wrench(1-3) - direction cosines of wrench axis
c						wrench(4-6) - vector from CS origin to wrench axis
c						wrench(7)   - moment about wrench axis ( Nm )
c						wrench(8)   - force along wrench axis (N)
c	Force			CmbGrlWre
c	Force			FpWre
c
      subroutine CalcFrcWrench( FF,OF )
      implicit none
      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_prm.inc'
      include 'a_gls2.inc'
      include 'a_frc.inc'
      include 'a_names.inc'
                 
      record /force_flag/  FF
      record /option_flag/ OF

      integer*4 frm,plt,ft,i
      real*4    TempLoad(6)

      FF.wrench= .false.
      if ( .not. FF.read ) goto 1000 ! error
      if ( .not. FF.cpr_GCS ) goto 1010 ! error
c                            
c     Calculate the load wrench for the force plates, combined load set,
c     and individual foot ground reaction loads depending upon what is 
c     available
c
      do frm= 1, FP.nfrm
        if ( OF.UpDate .and. (mod(frm,10) .eq. 0) )  
     &    write(6,101) frm,FP.nfrm
101     format('+Processing frame # ',i5,'/',i5)
        do plt= 1, FP.num_plt
          if ( FpDataOK(frm,plt) ) then
            do i= 1, 3
              TempLoad(i)= FpData(i+3,frm,plt)
              TempLoad(i+3)= FpData(i,frm,plt)
            end do
            call CalcInstantScrewAxis( TempLoad,
     &                                 FpCp(x,frm,plt),
     &                                 FpWre(1,frm,plt) )
          else
            do i= 1, 8
              FpWre(i,frm,plt)= 0.0
            end do
          end if
        end do ! plt
        if ( FF.adjusted ) then ! calculate combined load wrench
          if ( FcDataOK(frm) ) then
            do i= 1, 3
              TempLoad(i)= CmbGrl(i+3,frm)
              TempLoad(i+3)= CmbGrl(i,frm)
            end do
            call CalcInstantScrewAxis( TempLoad,
     &                                 CmbCp(x,frm),
     &                                 CmbGrlWre(1,frm) )
          else
            do i= 1, 8
              CmbGrlWre(i,frm)= 0.0
            end do
          end if
        end if                                 
        if ( FF.individual_feet ) then ! calculate foot load wrench
          do ft= rt, lf
            if ( FtGrlOK(frm,ft) ) then
              do i= 1, 3
                TempLoad(i)= FtGrl(i+3,frm,ft)
                TempLoad(i+3)= FtGrl(i,frm,ft)
              end do
              call CalcInstantScrewAxis( TempLoad,
     &                                   FtCpGCS(x,frm,ft),
     &                                   FtGRLWre(1,frm,ft) )
            else
              do i= 1, 8
                FtGRLWre(i,frm,ft)= 0.0
              end do
            end if
          end do ! ft
        end if
      end do ! frm
      FF.wrench= .true.
      return
      
1000  write(6,1005)
      if ( OF.ToLog ) write(3,1005)
1005  format(' Error- Force plate data has not been read in yet')
      return
      
1010  write(6,1015)
      if ( OF.ToLog ) write(3,1015)
1015  format(' Error- Force plate center of pressure has not been calculated yet')
      return
      end
c        
c	CalcFrcIntegral
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Calculate the time integral due of the GRL upon the force 
c				plates, combined GRL and foot GRL's.  The first integral
c				is useful because the x and components should have total
c				value of 0 over the gait cycle if it is steady state while
c				the second integral gives the path of the body center of 
c				mass provided that both feet struck force plates during
c				the measurement interval.  This means that at first heel
c				strike the trailing foot must also be on a force plate...
c				NOTE:	The above comments apply only to the combined force
c						plate GRL data integrals.
c
c	Routines called:
c		IntegrateData
c		VecMag
c
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Segment			BCMVel
c	Segment			BodyCM
c	Force			CmbGrl
c	Force			FcDataOK
c	Force			FpDataOK
c	Force			FtGrlOK
c	Param			FP.first_frm
c	Param			FP.frm_rate
c	Param			FP.nfrm
c	Param			FP.num_plt
c	Force			FtGrl
c	Param			GP.weight
c	Force			FpData
c  Param       rMtnFrmRate
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Force			CombIntegral
c	Force			CombIntegral2
c	Force			FtIntegral
c	Force			FtIntegral2
c	Force			PltIntegral
c	Force			PltIntegral2
c
      subroutine CalcFrcIntegral( CMND,FF,SF,OF )
      implicit none
                                        
      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_prm.inc'
      include 'a_gls2.inc'
      include 'a_frc.inc'
      include 'a_seg.inc'
      include 'a_names.inc'

      record /CmndLine/     CMND
      record /force_flag/   FF
      record /segment_flag/ SF
      record /option_flag/  OF

      integer*4 i,j,k,foot,axis,frm,plt,IntAxis(4)
      real*4    VecMag,TimeInc,Data(MAX_ELC),IntData(MAX_ELC),
     &          IntData2(MAX_ELC),t0,vel0(3),delt
      logical*4 AlignCM,ZeroVel
      
      data IntAxis / 1,2,3,6 /

      if ( .not. FF.adjusted ) goto 1000
      if ( FP.frm_rate .lt. 0.0001 ) goto 1010
      TimeInc= 1.0/float(FP.frm_rate)
c
c     check for option to align 2nd integral with body center of mass
c     if it has been calculated....
c
      AlignCM= .false.
      ZeroVel= .false.
      if ( CMND.NumOpt(2) .gt. 0 ) then
        do i= 1, CMND.NumOpt(2)
          if ( CMND.OptText(i,2) .eq. 'BCM' ) then
            if ( .not. SF.cntr_mass ) goto 1020
            if ( .not. FF.aligned ) goto 1030
            AlignCM= .true.
          else if ( CMND.OptText(i,2) .eq. 'VLZ' ) then
            ZeroVel= .true.
          else
            goto 1040
          end if
        end do
      end if
c
c     Do the individual feet if they are present
c
      if ( FF.individual_feet ) then
        do foot= rt, lf           
          do i= 1, 4
            do frm= 1, FP.nfrm
              if ( FtGrlOK(frm,foot) ) then
                Data(frm)= -FtGrl(IntAxis(i),frm,foot)
              else
                Data(frm)= 0.0
              end if
            end do
            call IntegrateData(Data,FP.nfrm,TimeInc,IntData)
            call IntegrateData(IntData,FP.nfrm,TimeInc,IntData2)
            do frm= 1, FP.nfrm
              FtIntegral(i,frm,foot)=  IntData(frm)
              FtIntegral2(i,frm,foot)= IntData2(frm)
            end do
          end do ! i
        end do ! foot
      end if
c
c     Do the combined force plate data
c
      do i= 1, 4
        do frm= 1, FP.nfrm
          if ( FcDataOK(frm) ) then
            Data(frm)= -CmbGrl(IntAxis(i),frm)
          else
            Data(frm)= 0.0
          end if
        end do
        call IntegrateData(Data,FP.nfrm,TimeInc,IntData)
        call IntegrateData(IntData,FP.nfrm,TimeInc,IntData2)
        do frm= 1, FP.nfrm
          CombIntegral(i,frm)=  IntData(frm)
          CombIntegral2(i,frm)= IntData2(frm)
        end do
      end do ! i
c
c     Adjust Combined GRL integrals using position of body center of
c     mass and its estimated velocity at the point where the force
c     plate and motion data first coincide
c      
      if ( AlignCM ) then
        t0= float(FP.first_frm)/float(FP.frm_rate)
        do i= 1, 3 
          if ( .not. ZeroVel ) then 
            if ( SF.BCMVel ) then
              vel0(i)= BCMVel(i,1)
            else ! cubic estimate of velocity of body CM at t0
              vel0(i)= rMtnFrmRate*
     &                 ( 2.0*BodyCM(i,4) -  9.0*BodyCM(i,3) +
     &                  18.0*BodyCM(i,2) - 11.0*BodyCM(i,1) )/6.0
            end if
          else
            vel0(i)= 0.0
          end if
          if ( i .ne. 3 ) then
            do frm= 1, FP.nfrm
              CombIntegral(i,frm)=  
     &          CombIntegral(i,frm)/GP.weight + vel0(i)
              CombIntegral2(i,frm)= 
     &          CombIntegral2(i,frm)/GP.weight + 
     &          vel0(i)*(float(frm-1)*TimeInc-t0) + BodyCM(1,frm)
            end do ! frm
          else ! z component
            do frm= 1, FP.nfrm
              delt= float(frm-1)*TimeInc-t0
              CombIntegral(i,frm)=  
     &          CombIntegral(i,frm)/GP.weight - 9.871*delt + vel0(i)
              CombIntegral2(i,frm)= 
     &          CombIntegral2(i,frm)/GP.weight - 4.936*(delt**2.0) +
     &          vel0(i)*delt + BodyCM(1,frm)
            end do ! frm
          end if ! z component or not
        end do ! i
        if ( .not. SF.BCMVel ) write(6,500) (vel0(i),i=1,3),t0
500     format(' NOTE Initial velocity of CM estimated at:'/
     &         '   x: ',f10.4,' m/s'/
     &         '   y: ',f10.4,' m/s'/
     &         '   z: ',f10.4,' m/s'//
     &         ' Force data t0: ',f10.4,' sec')
      end if ! AlignCM
c
c     Do the individual force plates
c
      do plt= 1, FP.num_plt
        do i= 1, 4
          do frm= 1, FP.nfrm
            if ( FpDataOK(frm,plt) ) then
              Data(frm)= -FpData(IntAxis(i),frm,plt)
            else
              Data(frm)= 0.0
            end if
          end do
          call IntegrateData(Data,FP.nfrm,TimeInc,IntData)
          call IntegrateData(IntData,FP.nfrm,TimeInc,IntData2)
          do frm= 1, FP.nfrm
            PltIntegral(i,frm,plt)=  IntData(frm)
            PltIntegral2(i,frm,plt)= IntData2(frm)
          end do
        end do ! i
      end do ! plt
      FF.Integral= .true.
      return

1000  write(6,1005)
      if ( OF.ToLog ) write(3,1005)
1005  format('Error- Force plate data has not been combined yet')
      return

1010  write(6,1015)
      if ( OF.ToLog ) write(3,1015)
1015  format('Error- Force data frame rate is less than 0.')
      return
      
1020  write(6,1025)
      if ( OF.ToLog ) write(3,1025)
1025  format('Error- Body center of mass has not been calculated yet')
      return

1030  write(6,1035)
      if ( OF.ToLog ) write(3,1035)
1035  format('Error- Force & motion data has not been aligned yet')
      return

1040  write(6,1045) CMND.OptText(1,2)
      if ( OF.ToLog ) write(3,1045) CMND.OptText(1,2)
1045  format('Error- ',a3,' is not a valid option for FRC INT'/
     &       '       Valid options are: /BCM /VLZ')
      return
      end
c        
c	IntegrateData
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Perform numerical integration of data
c				Using extended trapezoidal rule integration
c				ref: Numerical Recipes, p. 107,111
c
c	Routines called:
c		None
c
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	None
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	None
c
      subroutine IntegrateData( Data,NumPnts,Inc,IntData )
      implicit none
      
      real*4    Data(1),IntData(1),Inc
      integer*4 NumPnts,i
      
      IntData(1)= 0.0
      do i= 2, NumPnts
        IntData(i)= IntData(i-1) + 0.5*Inc*(Data(i-1) + Data(i))
      end do
      return
      end
c        
c	ReconstructGRL
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Rebuild the GRL and it's point of application from the
c				calculated ankle loads.  This functions as an independent
c				check of the Joint Resultant Load calculations and also as
c				a means to estimate the GRL for the those situations in
c				which a calculation scheme was used to find the JRL when no
c				force plate data was available.  This last case should
c				provide a very poor estimate of the GRL since the inaccuracies
c				of the determination of the segmental kinematics as well as
c				segment mass/inertia parameters will have a large effect on
c				the results.
c
c	Routines called:
c		PntChangeCS
c		PntGCStoLCS
c		VecChangeCS
c
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Force			FlrCntct
c	Param			GP.num_seg
c	Param			GP.seg_num
c	Joint			JntCalcOK
c	Joint			JrlDCS
c	Marker			MrkData
c	Param			MRK_NUM.toe
c	Param			MRK_NUM.heel
c	Param			MP.nfrm
c	Segment			SacLCS
c	Segment			SgpGCS
c	Param			SP.end
c	Param			SP.inertia
c	Param			SP.length
c	Param			SP.mass
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Force			rCmbCp
c	Force			rCmbGrl
c	Force			rFtCpGCS
c	Force			rFtCpLCS
c	Force			rFtGrlGCS
c	Force			rFtGrlLCS
c	Force			rStrIdx
c
      subroutine ReconstructGRL( FF,JF,OF )
      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'

      record /force_flag/   FF
      record /joint_flag/   JF
      record /option_flag/  OF

      integer*4 i,foot,side,axis,load,frm,jnt
      real*4    gravity,toe(3,2),heel(3,2),temp1(3),temp2(3),CPz(2)
      logical*4 rtOK,lfOK

      data gravity / 9.816 /
c
c     If both feet are present, then reconstruct the ground reaction loads
c     from the calculated joint reaction loads at the ankles along with the
c     segment mass,inerita, and acceleration parameters.
c
      FF.reconstructed= .false.
      if ( .not. JF.load ) goto 1000 ! error
      rtOK= .false.
      lfOK= .false.
      do i= 1, GP.num_seg
        if ( GP.seg_num(i) .eq. rtft ) rtOK= .true.
        if ( GP.seg_num(i) .eq. lfft ) lfOK= .true.
      end do ! i
      if ( (.not. rtOK) .or. (.not. lfOK) ) goto 1010 ! error
      if ( MP.nfrm .le. 0 ) goto 1020 ! error
      if ( (SP(rtft).length .le. 0.00001) .or.
     &     (SP(lfft).length .le. 0.00001)      ) goto 1030 ! error
c
c     First calculate the z position of the ground reaction load
c     relative to the foot LCS.  Note that this position is assumed,
c     whereas the x and y locations are calculated
c
      foot= rtft
      do side= rt, lf
        do axis= x, z
          toe(axis,side)=  0.0
!          heel(axis,side)= 0.0
        end do ! axis
        if ( side .eq. lf ) foot= lfft
        do frm= 1, MP.nfrm
          call PntGCStoLCS( MrkData(x,frm,MRK_NUM(side).toe),
     &                      SgpGCS(x,x,frm,foot),temp1 )       
!          call PntGCStoLCS( MrkData(x,frm,MRK_NUM(side).heel),
!     &                      SgpGCS(x,x,frm,foot),temp2 )
          do axis= x, z 
            toe(axis,side)=  toe(axis,side) + temp1(axis)
!            heel(axis,side)= heel(axis,side) + temp2(axis)
          end do ! axis
        end do ! frm
        do axis= x, z
          toe(axis,side)=  toe(axis,side)/float(MP.nfrm)
!          heel(axis,side)= heel(axis,side)/float(MP.nfrm)
        end do ! axis
        CPz(side)= toe(z,side) - .030 !m
      end do ! side           
c
c     Now find the ground reaction load.  Note that JrlDCS is
c     calculated as the reaction of the foot upon the calf, but since here
c     the reaction of the calf on the foot is needed, the sign of the joint
c     load has been reversed.
c
      foot= rtft          
      jnt=  rtak
      do side= rt, lf    
        if ( side .eq. lf ) foot= lfft
        if ( side .eq. lf ) jnt=  lfak                
        do frm= 1, MP.nfrm
          if ( JntCalcOK(frm,jnt) .and. FlrCntct(frm,side) ) then
            do load= fx, fz
              rFtGrlLCS(load,frm,side)= SP(foot).mass*
     &                                        SacLCS(x,frm,foot)
     &                                      + JrlDCS(load,frm,jnt)
            end do ! load
            rFtGrlLCS(fz,frm,side)= rFtGrlLCS(fz,frm,side) -
     &                                    SP(foot).mass*gravity
            call VecChangeCS( SgpGCS(x,x,frm,foot),
     &                        rFtGrlLCS(fx,frm,side),
     &                        rFtGrlGCS(fx,frm,side)  )
            rFtCpLCS(z,frm,side)= CPz(side)
            rFtCpLCS(y,frm,side)= ( SP(foot).inertia(x)*
     &                                    SacLCS(rx,frm,foot)
     &                                 + JrlDCS(mx,frm,jnt)
     &                                 + SP(foot).end(y,proximal)*
     &                                    JrlDCS(fz,frm,jnt)
     &                                 - SP(foot).end(z,proximal)*
     &                                    JrlDCS(fy,frm,jnt)
     &                                 + CPz(side)*rFtGrlLCS(fy,frm,side) )/
     &                                   rFtGrlLCS(fz,frm,side)
            rFtCpLCS(x,frm,side)= ( -SP(foot).inertia(y)*
     &                                    SacLCS(ry,frm,foot)
     &                                 - JrlDCS(my,frm,jnt)
     &                                 + SP(foot).end(x,proximal)*
     &                                    JrlDCS(fz,frm,jnt)
     &                                 - SP(foot).end(z,proximal)*
     &                                    JrlDCS(fx,frm,jnt)
     &                                 + CPz(side)*rFtGrlLCS(fx,frm,side) )/
     &                                   rFtGrlLCS(fz,frm,side)
            call PntChangeCS( SgpGCS(x,x,frm,foot),          
     &                        rFtCpLCS(x,frm,side),     
     &                        rFtCpGCS(x,frm,side)  )
            rFtGrlLCS(mx,frm,side)= 0.0
            rFtGrlLCS(my,frm,side)= 0.0
            rFtGrlLCS(mz,frm,side)= SP(foot).inertia(z)*
     &                                     SacLCS(rz,frm,foot)
     &                                  + JrlDCS(mz,frm,jnt)
     &                                  + SP(foot).end(x,proximal)*
     &                                     JrlDCS(fy,frm,jnt)
     &                                  - SP(foot).end(y,proximal)*
     &                                     JrlDCS(fx,frm,jnt)
     &                                  - rFtCpLCS(x,frm,side)*
     &                                     rFtGrlLCS(fy,frm,side)
     &                                  + rFtCpLCS(y,frm,side)*
     &                                     rFtGrlLCS(fx,frm,side)
            call VecChangeCS( SgpGCS(x,x,frm,foot),
     &                        rFtGrlLCS(mx,frm,side),
     &                        rFtGrlGCS(mx,frm,side)  )
          else ! zero out, since joint load does not exist for this frame
            do axis= x, z  
              rFtCpLCS(axis,frm,side)=   0.0
              rFtCpGCS(axis,frm,side)=   0.0
            end do ! axis                                           
            do load= fx, mz
              rFtGrlLCS(load,frm,side)= 0.0
              rFtGrlGCS(load,frm,side)= 0.0
            end do ! load
          end if ! JntCalcOK
        end do ! frm
      end do ! side
c     
c     Calculate the Strike Index using the reconstructed GRL data
c
      foot= rtft
      do side= rt, lf
        if ( side .eq. lf ) foot= lfft
        do frm= 1, MP.nfrm
          if ( FlrCntct(frm,side) ) 
     &      rStrIdx(frm,side)= rFtCpLCS(x,frm,side)/
     &                                SP(foot).length + .429 ! from Dempster
        end do ! frm
      end do ! side               
c
c     Now that the individual foot quantities are known, calculate the combined
c     foot quantities. 
c                                  
      do frm= 1, MP.nfrm
        if ( FlrCntct(frm,rt) .and. FlrCntct(frm,lf) ) then
          do load= fx, mz
            rCmbGrl(load,frm)= rFtGrlGCS(load,frm,rt) +
     &                               rFtGrlGCS(load,frm,lf)
          end do ! axis
          if ( abs(rCmbGrl(fz,frm)) .lt. 0.0001 ) then
            do axis= x, z
              rCmbCp(axis,frm)= 0.0
            end do ! axis
          else
            rCmbCp(x,frm)= ( rFtGrlGCS(fz,frm,rt)*
     &                             rFtCpGCS(x,frm,rt)
     &                          + rFtGrlGCS(fz,frm,lf)*
     &                             rFtCpGCS(x,frm,lf)    )/
     &                          rCmbGrl(fz,frm)
            rCmbCp(y,frm)= ( rFtGrlGCS(fz,frm,rt)*
     &                             rFtCpGCS(y,frm,rt)
     &                          + rFtGrlGCS(fz,frm,lf)*
     &                             rFtCpGCS(y,frm,lf)    )/
     &                          rCmbGrl(fz,frm)
            rCmbCp(z,frm)= 0.0
          end if ! fz too small
        else if ( FlrCntct(frm,rt) ) then
          do load= fx, fz
            rCmbGrl(load,frm)= rFtGrlGCS(load,frm,rt)
          end do ! load
          do axis= x, z
            rCmbCp(axis,frm)= rFtCpGCS(axis,frm,rt)
          end do ! axis
        else if ( FlrCntct(frm,lf) ) then
          do load= fx, fz
            rCmbGrl(load,frm)= rFtGrlGCS(load,frm,lf)
          end do ! load
          do axis= x, z
            rCmbCp(axis,frm)= rFtCpGCS(axis,frm,lf)
          end do ! axis
        else
          do load= fx, fz
            rCmbGrl(load,frm)= 0.0
          end do ! load
          do axis= x, z
            rCmbCp(axis,frm)= 0.0
          end do ! axis
        end if ! which foot contact case
      end do ! frm
      FF.reconstructed= .true.
      return
                       
1000  write(6,1005)
      if ( OF.ToLog ) write(3,1005)
1005  format(' Error- Joint loads have not been calculated yet.')
      return

1010  write(6,1015)
      if ( OF.ToLog ) write(3,1015)
1015  format(' Error- Both feet are not present in segments being analyzed.')
      return
      
1020  write(6,1025)
      if ( OF.ToLog ) write(3,1025)
1025  format(' Error- Number of motion data frames is <= zero.')
      return

1030  write(6,1035)
      if ( OF.ToLog ) write(3,1035)
1035  format(' Error- Length of feet is <= zero.')
      return                   
      end
c        
c	CalcPitchMomentArm
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Calculate the perpendicular distance from the center of mass
c				to the line of action of the GRF vector passing through the
c				center of pressure for the combined force plate GRL.  This
c				provides a 'measure' of the disturbing moment causing the
c				body oscillate.  This quantity is an idea of mine and I'm
c				not yet sure whether it provides useful information...
c
c	Routines called:
c		NormVec
c		PerpenPtLine
c		VecGCStoLCS
c		VecMag
c
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Segment			BodyCM
c	Force			CmbCp
c	Force			CmbGrl
c	Force			FcDataOK
c	Param			MP.nfrm
c	Segment			SgpGCS
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Force			PmaPnt
c	Force			PmaGCS
c	Force			PmaPlvLCS
c	Force			PmaTrkLCS
c
      subroutine CalcPitchMomentArm( FF,SF,OF )
      implicit none

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

      record /force_flag/   FF
      record /segment_flag/ SF
      record /option_flag/  OF

      integer*4 frm,i 
      real*4    TempVec(3),VecMag
      logical*4 error,VecNorm


      if ( .not. FF.DataClipped ) goto 1000 ! error
      if ( .not. SF.cntr_mass )   goto 1010 ! error
      if ( .not. FF.cpr_GCS )     goto 1020 ! error
      do frm= 1, MP.nfrm
        if ( FcDataOK(frm) ) then
          do i= x, z
            PmaPnt(i,frm)= 0.0                           
            PmaGCS(i,frm)= 0.0
          end do
          PmaGCS(4,frm)= 0.0
          error= VecNorm( CmbGrl(fx,frm),tempVec )
          if ( .not. error ) then
            call PerpenPtLine( CmbCp(x,frm),tempVec,
     &                         BodyCM(1,frm),PmaPnt(1,frm) )
            do i= x, z
              PmaGCS(i,frm)= PmaPnt(i,frm) - BodyCM(i,frm)
            end do
            PmaGCS(4,frm)= VecMag(PmaGCS(1,frm))
          end if
          call VecGCStoLCS( SgpGCS(1,1,frm,pelv),PmaGCS(1,frm),
     &                      PmaPlvLCS(1,frm) )
          call VecGCStoLCS( SgpGCS(1,1,frm,trnk),PmaGCS(1,frm),
     &                      PmaTrkLCS(1,frm) )
        end if ! FcDataOK ?
      end do ! frm                                 
      FF.PitchMomArm= .true.
      return

1000  write(6,1005) 
      if ( OF.ToLog ) write(3,1005)
1005  format(' Error- Force plate data has not been clipped yet, thus the'/
     &       '        ground reaction loads and the center of mass data are'/
     &       '        aligned properly')
      return
 
1010  write(6,1015) 
      if ( OF.ToLog ) write(3,1015)
1015  format(' Error- The body center of mass has not been calculated yet')
      return
                                                        
1020  write(6,1025) 
      if ( OF.ToLog ) write(3,1025)
1025  format(' Error- The combined ground reaction force center of pressures'/
     &       '        have not been calculated yet')
      return
      end
c        
c	CalcPitchWrenchArm
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Calculate the perpendicular distance from the center of mass
c				to the line of action of the GRL wrench for the combined force 
c				plate GRL.  This provides a 'measure' of the disturbing wrench
c				causing the body oscillate.  This quantity is an idea of mine 
c				and I'm not yet sure whether it provides useful information...
c				This might more general and hence useful than the pitching
c				moment arm since the wrench will also reflect the vertical
c				twisting moment (mz) applied to the ground whereas this will
c				not be apparent in the moment arm.
c
c	Routines called:
c		NormVec
c		PerpenPtLine
c		VecGCStoLCS
c		VecMag
c
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Segment			BodyCM
c	Force			CmbGrlWre
c	Force			FcDataOK
c	Param			MP.nfrm
c	Segment			SgpGCS
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Force			PwaPnt
c	Force			PwaGCS
c	Force			PwaPlvLCS
c	Force			PwaTrkLCS
c
      subroutine CalcPitchWrenchArm( FF,SF,OF )
      implicit none

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

      record /force_flag/   FF
      record /segment_flag/ SF
      record /option_flag/  OF

      integer*4 frm,i 
      real*4    TempVec(3),VecMag
      logical*4 error,VecNorm

                  
      if ( .not. FF.DataClipped ) goto 1000 ! error
      if ( .not. SF.cntr_mass )   goto 1010 ! error
      if ( .not. FF.wrench )      goto 1020 ! error
      do frm= 1, MP.nfrm
        if ( FcDataOK(frm) ) then
          do i= x, z
            PwaPnt(i,frm)= 0.0                           
            PwaGCS(i,frm)= 0.0
          end do
          PwaGCS(4,frm)= 0.0
          if ( .not. error ) then
            call PerpenPtLine( CmbGrlWre(4,frm),CmbGrlWre(1,frm),
     &                         BodyCM(1,frm),PwaPnt(1,frm) )             
            do i= x, z
              PwaGCS(i,frm)= PwaPnt(i,frm) - BodyCM(i,frm)
            end do
            PwaGCS(4,frm)= VecMag(PwaGCS(1,frm))
          end if
          call VecGCStoLCS( SgpGCS(1,1,frm,pelv),PwaGCS(1,frm),
     &                      PwaPlvLCS(1,frm) )
          call VecGCStoLCS( SgpGCS(1,1,frm,trnk),PwaGCS(1,frm),
     &                      PwaTrkLCS(1,frm) )
        end if ! FcDataOK ?
      end do ! frm
      FF.PitchWreArm= .true.
      return

1000  write(6,1005) 
      if ( OF.ToLog ) write(3,1005)
1005  format(' Error- Force plate data has not been clipped yet, thus the'/
     &       '        ground reaction loads and the center of mass data are'/
     &       '        aligned properly')
      return
 
1010  write(6,1015) 
      if ( OF.ToLog ) write(3,1015)
1015  format(' Error- The body center of mass has not been calculated yet')
      return
                                                        
1020  write(6,1025) 
      if ( OF.ToLog ) write(3,1025)
1025  format(' Error- The combined ground reaction force wrench'/
     &       '        has not been calculated yet')
      return
      end
c        
c	CalcCprVel
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Calculate the velocity of the center of pressure under the
c				individual feet.  This is not the change in the CP but rather
c				the velocity of the point on the foot corresponding at a time
c				point, being the data frame, at which the CP is located 
c				relative to the lab GCS.  One might except this to be 0 except
c				at the initial heelstrike and later toeoff where there might be
c				some sliding of the foot relative to ground.  However, my 
c				initial tests have this not to be the case and in fact a velocity
c				is present, though small, during most of the ground contact
c				phase.  I believe this is not real but rather a result of
c				cumulative error due to the accuracy of the force plates, the
c				motion measurement system as well as the numerical routines
c				utilized to estimate the foot segment position and velocity
c				relative to the lab GCS.  By the way, this quantity was 
c				originally programmed to provide data for my foot impact
c				simulations which are showing some curious results in this
c				area, but it turned out to be more interesting than I 
c				thought...
c
c	Routines called:
c		VecChangeCS
c		VecX
c
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Force			FtGrlOK
c	Force			FtCpLCS
c	Param			MP.nfrm
c	Segment			SvlGCS
c	Segment			SvlLCS
c	Segment			SgpGCS
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Force			FtCprVelGCS
c
      subroutine CalcCprVel( FF,SF,OF )
      implicit none
      
      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_prm.inc'
      include 'a_names.inc'
      include 'a_seg.inc'
      include 'a_gls2.inc'
      include 'a_frc.inc'

      record /force_flag/   FF
      record /segment_flag/ SF
      record /option_flag/  OF

      integer*4 frm,i,side,ft
      real*4    VelLCS(3),VelGCS(3) 

      if ( .not. FF.cpr_LCS ) goto 1000 ! error
      if ( .not. SF.vel_acc ) goto 1010 ! error
      do side= rt, lf
        ft= rtft
        if ( side .eq. lf ) ft= lfft
        do frm= 1, MP.nfrm
          if ( FtGrlOK(frm,side) ) then
            call VecX( SvlLCS(4,frm,ft),
     &                 FtCpLCS(1,frm,side),VelLCS )
            call VecChangeCS( SgpGCS(1,1,frm,ft),VelLCS,VelGCS )
            do i= 1, 3
              FtCprVelGCS(i,frm,side)= VelGCS(i) + SvlGCS(i,frm,ft)
            end do
          else
            do i= 1, 3
              FtCprVelGCS(i,frm,side)= 0.0
            end do
          end if
        end do ! frm
      end do ! side
      FF.CprVelGCS= .true.
      return
      
1000  write(6,1005) 
      if ( OF.ToLog ) write(3,1005)
1005  format(' Error- Foot LCS center of pressure has not been calculated yet')
      return
 
1010  write(6,1015) 
      if ( OF.ToLog ) write(3,1015)
1015  format(' Error- Segment velocity has not been calculated yet')
      return
      end
