c                                        
c	CalcSegVelAcc
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Calculate the segment rotational and translational
c				velocity and acceleration relative the segment LCS
c				as well as relative to the lab GCS.  A number of
c				methods have been provided to do this.  The default
c				uses the location of the segment LCS centers as well
c				as the YXZ euler angle of the segment relative to 
c				the GCS.  These are differentiated and the vel/accel
c				calculated from these.  An alternative is to directly
c				differentiate the rotation matrices of the segment
c				relative to the GCS.  This requires 3x differentiations
c				and once one uses this program, you find out that this
c				operation is the slowest out all.  Thus, I don't
c				reccommed this approach.  However, since the two methods
c				were derived and programmed independently they function
c				as a check for one another.  I have found that they
c				yield the same vel and accel (yeah!!).  There is an older
c				method contained in here also (/ROT) which I don't
c				reccommend.  It's a leftover from previous attempts
c				and I just didn't have the heart to throw it out.
c				Lastly, there is an interesting alternative included
c				which uses the marker vel and accel to calculate the
c				segment vel and accel.  This has the interesting
c				property that it uses a least squares formulation.
c				Thus, the more markers on the segment the more 
c				accurate the estimate of vel and accel (supposedly).
c				This algorithm will be more useful later when I get
c				rigid body arrays incorporated into ANZ...  At present,
c				it is included as an experiment,  Priliminary tests
c				show that it makes a mess of things with only 3 markers.
c
c	Routines called:
c
c	filter
c	MatTran
c	MrkVAAtoSegVAA
c	OldEulAngDeriv
c	RemoveCMNDOpt
c	RotMatDeriv
c	SegFilterStatus1
c	VecChangeCS
c	YXZEulAngDeriv
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Param			GP.num_seg
c	Param			GP.seg_num
c	Param			rMtnFrmRate
c	Param			MP.nfrm
c	Segment			RMd
c	Segment			RMdd
c	Names			seg_names
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Segment			SacGCS
c	Segment			SacLCS
c	Segment			SgpGCS
c	Segment			SvlGCS
c	Segment			SvlLCS
c
      subroutine CalcSegVelAcc( CMND,SF,MF,OF )
      implicit none

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

      record /CmndLine/      CMND
      record /segment_flag/  SF
      record /marker_flag/   MF
      record /option_flag/   OF
      
      integer*4 seg,axis,frm,num,i,j,scale_factor,error,VecNorm,status,
     &          numoptrem,optnumrem(5)
      real*4    time_inc,temp(MAX_MTN),vel(MAX_MTN),acc(MAX_MTN),
     &          temp_vel(3),temp_acc(3),GCStoLCS(3,3)
      logical*4 Smooth,UseMatDeriv,UseRotAngDeriv,UseEulerDeriv,
     &          UseMrkDeriv,SmoothRM,UseEulerParam
      character axis_names(4)*4

      data axis_names / '   x','   y','   z','cntr' /
               
      if ( .not. SF.position ) goto 1000 ! error
      if ( rMtnFrmRate .lt. 0.0001 ) goto 1010 ! error
c
c     Check for option as to method of differentiating the rotation matrices
c
      Smooth= .false.  ! differentiation will take place
      UseEulerDeriv=  .true.
      UseMatDeriv=    .false.
      UseRotAngDeriv= .false.
      UseMrkDeriv=    .false.
      UseEulerParam=  .false.         
      SmoothRM=       .true.
      time_inc= 1.0/rMtnFrmRate
      if ( CMND.NumOpt(2) .gt. 0 ) then
        numoptrem= 0
        do i= 1, CMND.NumOpt(2)                   
          if ( CMND.OptText(i,2) .eq. 'EUL' ) then
            UseEulerDeriv=  .true.
c            UseMatDeriv=    .false.
c            UseRotAngDeriv= .false.
            UseMrkDeriv=    .false.
            UseEulerParam=  .false.
            numoptrem= numoptrem + 1
            optnumrem(numoptrem)= i
c          else if ( CMND.OptText(i,2) .eq. 'MAT' ) then
c            UseEulerDeriv=  .false.
c            UseMatDeriv=    .true.
c            UseRotAngDeriv= .false.
c            UseMrkDeriv=    .false.
c            UseEulerParam=  .false.
c            numoptrem= numoptrem + 1
c            optnumrem(numoptrem)= i
          else if ( CMND.OptText(i,2) .eq. 'SMO' ) then
          	SmoothRM= .true.
            numoptrem= numoptrem + 1
            optnumrem(numoptrem)= i
          else if ( CMND.OptText(i,2) .eq. 'NSM' ) then
          	SmoothRM= .false.
            numoptrem= numoptrem + 1
            optnumrem(numoptrem)= i
c          else if ( CMND.OptText(i,2) .eq. 'ROT' ) then
c            UseEulerDeriv=  .false.
c            UseMatDeriv=    .false.
c            UseRotAngDeriv= .true.
c            UseMrkDeriv=    .false.
c            UseEulerParam=  .false.
c            numoptrem= numoptrem + 1
c            optnumrem(numoptrem)= i
          else if ( CMND.OptText(i,2) .eq. 'MRK' ) then
            UseEulerDeriv=  .false.
            UseMatDeriv=    .false.
            UseRotAngDeriv= .false.
            UseMrkDeriv=    .true.
            UseEulerParam=  .false.
            numoptrem= numoptrem + 1
            optnumrem(numoptrem)= i
          else if ( CMND.OptText(i,2) .eq. 'EP ' ) then
            UseEulerDeriv=  .false.
            UseMatDeriv=    .false.
            UseRotAngDeriv= .false.
            UseMrkDeriv=    .false.
            UseEulerParam=  .true.
            numoptrem= numoptrem + 1
            optnumrem(numoptrem)= i
          end if
        end do
        do i= numoptrem, 1, -1
          call RemoveCMNDOpt( CMND,2,optnumrem(i) )
        end do
      end if ! options are present
      if ( (UseEulerDeriv .or. UseRotAngDeriv) .and. 
     &     (.not. SF.angle)                     ) goto 1060
      SF.vel_acc= .false.
c
c     Do the derivative calculation on each segment
c                          
      if ( UseMrkDeriv ) then
        call MrkVAAtoSegVAA( CMND,SF,MF,OF )
        return
      end if
      write(6,90)
90    format(' This will take a while...'/)      
      do i= 1, GP.num_seg
        seg= GP.seg_num(i)  
c        if ( UseMatDeriv ) then         
c          call RotMatDeriv( CMND,SmoothRM,i,seg,status,OF )
c        else if ( UseRotAngDeriv ) then
c          call OldEulAngDeriv( CMND,SmoothRM,i,seg,status,OF )
        if ( UseEulerParam ) then
          call EulerParamDeriv( CMND,SmoothRM,i,seg,status,OF )
        else ! New Euler angle derivative method
          call YXZEulAngDeriv( CMND,SmoothRM,i,seg,status,OF )
        end if
        if ( status .lt. 0 ) goto 1050
c                                                                       
c       The following formulas  compute the rotational velocities and
c       accelerations from the time derivative of the orientations of the
c       segment.  These velocities and accelerations are relative to the
c       LCS of the respective segment at each time frame.  These are only
c 
c        if ( UseRotAngDeriv .or. UseMatDeriv ) then                          
c          do frm= 1, MP.nfrm   
c            SvlLCS(rx,frm,seg)= SgpGCS(x,z,frm,seg)*RMd(x,y,frm)
c     &                             + SgpGCS(y,z,frm,seg)*RMd(y,y,frm)
c     &                             + SgpGCS(z,z,frm,seg)*RMd(z,y,frm)
c
c            SvlLCS(ry,frm,seg)= SgpGCS(x,x,frm,seg)*RMd(x,z,frm)
c     &                             + SgpGCS(y,x,frm,seg)*RMd(y,z,frm)
c     &                             + SgpGCS(z,x,frm,seg)*RMd(z,z,frm)
c 
c            SvlLCS(rz,frm,seg)= SgpGCS(x,y,frm,seg)*RMd(x,x,frm)
c     &                             + SgpGCS(y,y,frm,seg)*RMd(y,x,frm)
c     &                             + SgpGCS(z,y,frm,seg)*RMd(z,x,frm)
c
c            SacLCS(rx,frm,seg)= SgpGCS(x,z,frm,seg)*RMdd(x,y,frm)
c     &                             + SgpGCS(y,z,frm,seg)*RMdd(y,y,frm)
c     &                             + SgpGCS(z,z,frm,seg)*RMdd(z,y,frm)
c     &                             - SvlLCS(ry,frm,seg)*
c     &                               SvlLCS(rz,frm,seg)
c     
c            SacLCS(ry,frm,seg)= SgpGCS(x,z,frm,seg)*RMdd(x,y,frm)
c     &                             + SgpGCS(y,z,frm,seg)*RMdd(y,y,frm)
c     &                             + SgpGCS(z,z,frm,seg)*RMdd(z,y,frm)
c     &                             - SvlLCS(rx,frm,seg)*
c     &                               SvlLCS(rz,frm,seg)
c     
c            SacLCS(rz,frm,seg)= SgpGCS(x,z,frm,seg)*RMdd(x,y,frm)
c     &                             + SgpGCS(y,z,frm,seg)*RMdd(y,y,frm)
c     &                             + SgpGCS(z,z,frm,seg)*RMdd(z,y,frm)
c     &                             - SvlLCS(rx,frm,seg)*
c     &                               SvlLCS(ry,frm,seg)
c          end do ! frm
c
c       Convert rotational velocities and accelerations from local to global CS
c
c          do frm= 1, MP.nfrm
c            call VecChangeCS( SgpGCS(x,x,frm,seg),
c     &                        SvlLCS(rx,frm,seg),
c     &                        SvlGCS(rx,frm,seg)   )
c            call VecChangeCS( SgpGCS(x,x,frm,seg),         
c     &                        SacLCS(rx,frm,seg),
c     &                        SacGCS(rx,frm,seg)   )
c          end do ! frm
c        end if ! UseRotAngDeriv or UseMatDeriv
c
c       Now calculate the linear velocities and accelerations of the segment   
c       LCS's in terms of global coordinates.
c
        if ( .not. UseMrkDeriv ) then
          do axis= x, z
            if ( OF.UpDate ) 
     &         write(6,100) i,GP.num_seg,seg_names(seg),axis_names(axis),
     &                      axis_names(cntr)
100         format('+Differentiating segment # ',i2,'/',i2,': ',a4,a4,' ',a4)
            do frm= 1, MP.nfrm
              temp(frm)= SgpGCS(axis,cntr,frm,seg)
            end do ! frm
            call filter( CMND,Smooth,MP.nfrm,temp,time_inc,OF,
     &                   vel,acc,status )                                   
            if ( status .lt. 0 ) goto 1030
            call SegFilterStatus1( status,i,GP.num_seg,seg_names(seg),
     &                             axis_names(axis),axis_names(cntr),OF )
            do frm= 1, MP.nfrm
              SgpGCS(axis,cntr,frm,seg)= temp(frm)
              SvlGCS(axis,frm,seg)= vel(frm)
              SacGCS(axis,frm,seg)= acc(frm)
            end do ! frm
          end do ! axis
c                                       
c       Convert linear velocities and acclerations from global to local CS
c                            
          do frm= 1, MP.nfrm
            call MatTran( SgpGCS(x,x,frm,seg),GCStoLCS )
            do axis= x, z
              temp_vel(axis)= SvlGCS(axis,frm,seg)
              temp_acc(axis)= SacGCS(axis,frm,seg)
            end do ! axis
            call VecChangeCS( GCStoLCS,temp_vel,SvlLCS(x,frm,seg) )
            call VecChangeCS( GCStoLCS,temp_acc,SacLCS(x,frm,seg) )
          end do ! frm       
        end if ! method other than UseMrkDeriv
      end do ! seg
      if ( SF.NumProcOps .le. 4 ) then
        SF.NumProcOps= SF.NumProcOps + 1
        SF.ProcOp(SF.NumProcOps)= CMND.Text(1)(1:40)
      end if
      SF.vel_acc= .true.
      return                          
                 
1000  write(6,1005)
      if ( OF.ToLog ) write(3,1005)
1005  format(' Error- Segment position has not been calculated yet.'/
     &       '        This must be done before segment vel and acc can be calculated.')
      return
       
1010  write(6,1015)
      if ( OF.ToLog ) write(3,1015)
1015  format(' Error- The frame rate of the motion data is set to 0.'/
     &       '        This a serious problem, it probably occured when the'/
     &       '        marker or segment data was read in.')
      return
            
1030  write(6,1035) seg_names(seg),axis_names(axis),axis_names(cntr)
      if ( OF.ToLog ) write(3,1035) seg_names(seg),axis_names(axis),
     &                                      axis_names(cntr)
1035  format(' Error- Smoothing/differentiating filter has bombed out on'/
     &       '        ',a4,' ',a4,' ',a4,' while doing the calculations'/
     &       '        for segment translational velocity and acceleration.')
      return
                                      
1050  write(6,1055) 
      if ( OF.ToLog ) write(6,1055) 
1055  format(' Error- Filter operation unsuccessful.')
      return

1060  write(6,1065) 
      if ( OF.ToLog ) write(6,1065) 
1065  format(' Error- The segment angles must be calculated to be able to '/
     &       '        use the euler angle derivatives to find the segment'/
     &       '        velocity and acceleration')
      return
      end                                        
c
c	RotMatDeriv
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	DIfferentiate each of the 9 components of a segment's
c				rotation matrix giving its orientation relative to
c				the GCS.  NOTE that since the components are differentiated
c				independently of one another, it is possible that the
c				matrix may no longer be orthonormal.  Thus, code is
c				provided to do this but will probably make a mess 
c				since it will make the matrix orthonormal without
c				consideration as to how it is reorienting it in the
c				process.  THis smoothing of the components is optional
c				and is NOT turned on as a default.
c
c	Routines called:
c
c	filter
c	MakeOrthoNorm
c	SegFilterStatus1
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Param			GP.num_seg
c	Param			rMtnFrmRate
c	Param			MP.nfrm
c	Names			seg_names
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Segment			RMd
c	Segment			RMdd
c	Segment			SgpGCS
c
      subroutine RotMatDeriv( CMND,SmoothRM,Cnt,Seg,status,OF )
      implicit none
                          
      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_prm.inc'
      include 'a_seg.inc'
      include 'a_names.inc'
         
      record /CmndLine/ CMND   
      record /option_flag/ OF

      integer*4 Cnt,Seg,status
      logical*4 SmoothRM

      real*4    time_inc,temp(MAX_MTN),vel(MAX_MTN),acc(MAX_MTN)
      integer*4 axis1,axis2,frm,error
      character axis_names(4)*4
      logical*4 Smooth
                                          
      data axis_names / '   x','   y','   z','cntr' /

      time_inc= 1.0/rMtnFrmRate
      Smooth= .false. ! differentiation will take place
c
c     Before smoothing and differentiation make sure the rotation matrices are
c     orthonormal- they might not be if they have been digitally filtered
c
c      do frm= 1, MP.nfrm     
c        call MakeOrthoNorm( SgpGCS(1,1,frm,seg),error )
c        if ( error .ne. 0 ) goto 1000
c      end do
c                                    
c     Do the smoothing and differentiation
c
      do axis1= x, z                
        do axis2= x, z   
          if ( OF.UpDate ) 
     &      write(6,100) Cnt,GP.num_seg,seg_names(seg),axis_names(axis2),
     &                   axis_names(axis1)
100       format('+Differentiating segment # ',i2,'/',i2,': ',a4,a4,' ',a4)
          do frm= 1, MP.nfrm          
            temp(frm)= SgpGCS(axis2,axis1,frm,seg)
          end do ! frame
          call filter( CMND,Smooth,MP.nfrm,temp,time_inc,OF,
     &                 vel,acc,status )
          if ( status .lt. 0 ) return
          call SegFilterStatus1(status,Cnt,GP.num_seg,seg_names(seg),
     &                          axis_names(axis2),axis_names(axis1),OF )
          do frm= 1, MP.nfrm
            if ( SmoothRM ) 
     &        SgpGCS(axis2,axis1,frm,seg)= temp(frm)
            RMd(axis2,axis1,frm)=  vel(frm)
            RMdd(axis2,axis1,frm)= acc(frm)
          end do ! frm               
        end do ! axis2
      end do ! axis1
c
c     Now that the rotation matrices have been smoothed make them orthonormal
c     once again         
c
      if ( SmoothRM ) then
        do frm= 1, MP.nfrm
          call MakeOrthoNorm( SgpGCS(1,1,frm,seg),error )
          if ( error .ne. 0 ) goto 1010                     
        end do  
      end if                       
      return

1000  write(6,1005) seg_names(seg),frm
      if ( OF.ToLog ) write(6,1005) seg_names(seg),frm
1005  format(' Error- Normalization of segment rotation matrices previous'/
     &       '        to smoothing and differentiation bombed out at ',a4,
     &       ' frame # ',i3)
      status= -1                    
      return

1010  write(6,1015) seg_names(seg),frm
      if ( OF.ToLog ) write(6,1015) seg_names(seg),frm
1015  format(' Error- Normalization of segment rotation matrices after'/
     &       '        smoothing and differentiation bombed out at ',a4,
     &       ' frame # ',i3)
      status= -1                    
      return
      end
c
c	MakeOrthoNorm
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Make a 3x3 matrix orthonormal.  This is done using
c				simple normalization and cross products rather than
c				something more complex, but more general, such as
c				Gramm-Schmitt orthogonalization.
c
c	Routines called:
c
c	VecDot
c	VecMag
c	VecNorm
c	VecX
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	None
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	None
c
      subroutine MakeOrthoNorm( RotMat,error )
      implicit none

      real*4    RotMat(3,3)
      integer*4 error

      real*4    VecDot,VecMag,TempX(3),TempY(3),TempZ(3)
      integer*4 i,VecNorm

      error= 0
      if ( (abs(VecDot(RotMat(1,1),RotMat(1,2))) .gt. 1.0e-2) .or.
     &     (abs(VecDot(RotMat(1,1),RotMat(1,3))) .gt. 1.0e-2) .or.
     &     (abs(VecDot(RotMat(1,2),RotMat(1,3))) .gt. 1.0e-2)      ) then
        call VecX( RotMat(1,1),RotMat(1,2),TempZ )
        call VecX( TempZ,RotMat(1,1),TempY )
        call VecX( TempY,TempZ,TempX )
        error= VecNorm( TempX,RotMat(1,1) )
        if ( error .ne. 0 ) return
        error= VecNorm( TempY,RotMat(1,2) )
        if ( error .ne. 0 ) return
        error= VecNorm( TempZ,RotMat(1,3) )
        if ( error .ne. 0 ) return
      else if ( (abs(VecMag(RotMat(1,1))-1.0) .gt. 1.0e-2) .or.
     &          (abs(VecMag(RotMat(1,2))-1.0) .gt. 1.0e-2) .or.
     &          (abs(VecMag(RotMat(1,3))-1.0) .gt. 1.0e-2)      ) then
        do i= 1, 3 
          TempX(i)= RotMat(i,1)
          TempY(i)= RotMat(i,2)
          TempZ(i)= RotMat(i,3)
        end do
        error= VecNorm( TempX,RotMat(1,1) )
        if ( error .ne. 0 ) return
        error= VecNorm( TempY,RotMat(1,2) )
        if ( error .ne. 0 ) return
        error= VecNorm( TempZ,RotMat(1,3) )
        if ( error .ne. 0 ) return
      end if
      return
      end
c
c	OldEulAngDeriv
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Calculate the derivatives of the rotation matrix
c				components by differentiating the segment YXZ euler
c				angles relative to the lab GCS.  Then assemble these
c				into the matrix derivatives.  This is based upon 
c				having written out the each of the components of the
c				rotation matrix in terms of the euler angles and then
c				having differentiated each of these formulas by hand.
c				It was a real bitch to do and later, once I got my
c				head screwed on correctly, I figured out how to directly
c				calculate the segment rot vel & accel directly from
c				the euler derivatives rather than having to go through
c				these ridiculously complex formulas.  I don't use
c				this method anymore but I left the code in here since I
c				spent so much time deriving and programming it.  It's
c				a tribute to what can happen when you work hard but
c				don't think deeply enough about the problem before
c				diving in...
c
c	Routines called:
c
c	filter
c	JCSAngtoRotMat
c	SegFilterStatus2
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Param			GP.num_seg
c	Param			rMtnFrmRate
c	Param			MP.nfrm
c	Names			seg_names
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Segment			RMd
c	Segment			RMdd
c	Segment			SgpGCS
c
      subroutine OldEulAngDeriv( CMND,SmoothRM,Cnt,Seg,status,OF )
      implicit none          
                          
      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_prm.inc'
      include 'a_seg.inc'
      include 'a_names.inc'
                             
      record /CmndLine/ CMND   
      record /option_flag/ OF

      integer*4 Cnt,Seg,status
      logical*4 SmoothRM
                                 
      real*4    AngD(3,MAX_MTN),AngDD(3,MAX_MTN),cs(3,MAX_MTN),sn(3,MAX_MTN),
     &          Temp(MAX_MTN),TempD(MAX_MTN),TempDD(MAX_MTN),TimeInc,AngD1s,
     &          AngD2s,AngD3s,AngD12s,AngD13s,AngD23s,AngD12,AngD13,AngD23,dtor,
     &          Sign(3)
      integer*4 i,j,error,frm 
      logical*4 Smooth

      data Sign / 1.0,1.0,1.0 /
                                
c
c     First Differentiate the euler angles
c
      dtor= atan(1.0)/45.0
      Smooth= .false.       
      TimeInc= 1.0/rMtnFrmRate
      do i= 1, 3  
        if ( OF.UpDate ) write(6,100) cnt,GP.num_seg,seg_names(seg),i
100     format('+Differentiating segment # ',i2,'/',i2,' : ',a4,' Angle ',
     &         i1,'/3')
        do frm= 1, MP.nfrm           
          Temp(frm)= SangGCS(i,frm,seg)*dtor
        end do
        call filter( CMND,Smooth,MP.nfrm,temp,TimeInc,OF,
     &               TempD,TempDD,status )
        if ( status .lt. 0 ) return
        call SegFilterStatus2( status,seg,GP.num_seg,seg_names(seg),
     &                         i,OF )
        do frm= 1, MP.nfrm
          SangGCS(i,frm,seg)= Temp(frm)/dtor
          AngD(i,frm)=  TempD(frm)
          AngDD(i,frm)= TempDD(frm)
          cs(i,frm)=    cos(Temp(frm))
          sn(i,frm)=    sin(Temp(frm))
        end do
      end do ! i
      if ( SmoothRM ) then
        do frm= 1, MP.nfrm
          call JCSAngtoRotMat( SangGCS(1,frm,seg),Sign,
     &                         SgpGCS(1,1,frm,seg)       ) 
        end do
      end if      
c                                         
c     Now comes the fun part !!!  Assemble the rotation matrix and its
c     derivatives
c
      do j= 1, MP.nfrm                                 
        AngD1s=  AngD(1,j)**2    
        AngD2s=  AngD(2,j)**2
        AngD3s=  AngD(3,j)**2
        AngD12s= AngD(1,j)**2 + AngD(2,j)**2
        AngD13s= AngD(1,j)**2 + AngD(3,j)**2
        AngD23s= AngD(2,j)**2 + AngD(3,j)**2
        AngD12= AngD(1,j)*AngD(2,j)
        AngD23= AngD(2,j)*AngD(3,j)
        AngD13= AngD(1,j)*AngD(3,j)
c r11
c        SgpGCS(1,1,j,seg)= cs(1,j)*cs(3,j)-sn(1,j)*sn(2,j)*sn(3,j)
        RMd(1,1,j)= AngD(1,j)*(-sn(1,j)*cs(3,j)-cs(1,j)*sn(2,j)*sn(3,j))
     &              - AngD(2,j)*sn(1,j)*cs(2,j)*sn(3,j)
     &              + AngD(3,j)*(-cs(1,j)*sn(3,j)-sn(1,j)*sn(2,j)*cs(3,j))
     
        RMdD(1,1,j)= AngDD(1,j)*(-sn(1,j)*cs(3,j)-cs(1,j)*sn(2,j)*sn(3,j))
     &               - AngDD(2,j)*sn(1,j)*cs(2,j)*sn(3,j)
     &               + AngDD(3,j)*(-cs(1,j)*sn(3,j)-sn(1,j)*sn(2,j)*cs(3,j))
     &               + AngD13s*(-cs(1,j)*cs(3,j)+sn(1,j)*sn(2,j)*sn(3,j))
     &               + AngD2s*sn(1,j)*sn(2,j)*sn(3,j)
     &               + 2.0*AngD13*(sn(1,j)*sn(3,j)-cs(1,j)*sn(2,j)*cs(3,j))
     &               - 2.0*AngD12*cs(1,j)*cs(2,j)*sn(3,j)
     &               - 2.0*AngD23*sn(1,j)*cs(2,j)*cs(3,j)
c r21
c        SgpGCS(2,1,j,seg)= cs(1,j)*sn(3,j)+sn(1,j)*sn(2,j)*cs(3,j)
        RMd(2,1,j)= AngD(1,j)*(-sn(1,j)*sn(3,j)+cs(1,j)*sn(2,j)*cs(3,j))
     &              + AngD(2,j)*sn(1,j)*cs(2,j)*cs(3,j)
     &              + AngD(3,j)*(cs(1,j)*cs(3,j)-sn(1,j)*sn(2,j)*sn(3,j))
     
        RMdD(2,1,j)= AngDD(1,j)*(-sn(1,j)*sn(3,j)+cs(1,j)*sn(2,j)*cs(3,j))
     &               + AngDD(2,j)*sn(1,j)*cs(2,j)*cs(3,j)
     &               + AngDD(3,j)*(cs(1,j)*cs(3,j)-sn(1,j)*sn(2,j)*sn(3,j))
     &               + AngD13s*(-cs(1,j)*sn(3,j)-sn(1,j)*sn(2,j)*cs(3,j))
     &               - AngD2s*sn(1,j)*sn(2,j)*cs(3,j)
     &               + 2.0*AngD13*(-sn(1,j)*cs(3,j)-cs(1,j)*sn(2,j)*sn(3,j))
     &               + 2.0*AngD12*cs(1,j)*cs(2,j)*cs(3,j)
     &               - 2.0*AngD23*sn(1,j)*cs(2,j)*sn(3,j)
c r31
c        SgpGCS(3,1,j,seg)= -sn(1,j)*cs(2,j)                         
        RMd(3,1,j)= -AngD(1,j)*cs(1,j)*cs(2,j) + AngD(2,j)*sn(1,j)*sn(2,j)

        RMdD(3,1,j)= -AngDD(1,j)*cs(1,j)*cs(2,j)
     &               +  AngDD(2,j)*sn(1,j)*sn(2,j)
     &               +  AngD12s*sn(1,j)*cs(2,j)
     &               +  2.0*AngD12*cs(1,j)*sn(2,j)
c r12                           
c        SgpGCS(1,2,j,seg)= -cs(2,j)*sn(3,j)
        RMd(1,2,j)=  AngD(2,j)*sn(2,j)*sn(3,j) - AngD(3,j)*cs(2,j)*cs(3,j)

        RMdD(1,2,j)=  AngDD(2,j)*sn(2,j)*sn(3,j)
     &               -  AngDD(3,j)*cs(2,j)*cs(3,j)
     &               +  AngD23s*cs(2,j)*sn(3,j)
     &               +  2.0*AngD23*sn(2,j)*cs(3,j)
c r22
c        SgpGCS(2,2,j,seg)= cs(2,j)*cs(3,j)
        RMd(2,2,j)= -AngD(2,j)*sn(2,j)*cs(3,j) - AngD(3,j)*cs(2,j)*sn(3,j)

        RMdD(2,2,j)= -AngDD(2,j)*sn(2,j)*cs(3,j)
     &               -  AngDD(3,j)*cs(2,j)*sn(3,j)
     &               -  AngD23s*cs(2,j)*cs(3,j)
     &               +  2.0*AngD12*sn(2,j)*sn(3,j)
c r32
c        SgpGCS(3,1,j,seg)= sn(2,j)
        RMd(3,1,j)=  AngD(2,j)*cs(2,j)    

        RMdD(3,1,j)= AngDD(2,j)*cs(2,j) - AngD2s*sn(2,j)
c r13
c        SgpGCS(1,3,j,seg)= sn(1,j)*cs(3,j)+cs(1,j)*sn(2,j)*sn(3,j)
        RMd(1,3,j)= AngD(1,j)*(cs(1,j)*cs(3,j)-sn(1,j)*sn(2,j)*sn(3,j))
     &              + AngD(2,j)*cs(1,j)*cs(2,j)*sn(3,j)
     &              + AngD(3,j)*(-sn(1,j)*sn(3,j)+cs(1,j)*sn(2,j)*cs(3,j))

        RMdD(1,3,j)= AngDD(1,j)*(cs(1,j)*cs(3,j)-sn(1,j)*sn(2,j)*sn(3,j))
     &               + AngDD(2,j)*cs(1,j)*cs(2,j)*sn(3,j)
     &               + AngDD(3,j)*(-sn(1,j)*sn(3,j)+cs(1,j)*sn(2,j)*cs(3,j))
     &               + AngD13s*(-sn(1,j)*cs(3,j)-cs(1,j)*sn(2,j)*sn(3,j))
     &               - AngD2s*cs(1,j)*sn(2,j)*sn(3,j)
     &               + 2.0*AngD13*(-cs(1,j)*sn(3,j)-sn(1,j)*sn(2,j)*cs(3,j))
     &               - 2.0*AngD12*sn(1,j)*cs(2,j)*sn(3,j)
     &               + 2.0*AngD23*cs(1,j)*cs(2,j)*cs(3,j)
c r23
c        SgpGCS(2,3,j,seg)= sn(1,j)*sn(3,j)-cs(1,j)*sn(2,j)*cs(3,j)
        RMd(2,3,j)= AngD(1,j)*(cs(1,j)*sn(3,j)+sn(1,j)*sn(2,j)*cs(3,j))
     &              - AngD(2,j)*cs(1,j)*cs(2,j)*cs(3,j)
     &              + AngD(3,j)*(sn(1,j)*cs(3,j)+cs(1,j)*sn(2,j)*sn(3,j))

        RMdD(2,3,j)= AngDD(1,j)*(cs(1,j)*sn(3,j)+sn(1,j)*sn(2,j)*cs(3,j))
     &               - AngDD(2,j)*cs(1,j)*cs(2,j)*cs(3,j)
     &               + AngDD(3,j)*(sn(1,j)*cs(3,j)+cs(1,j)*sn(2,j)*sn(3,j))
     &               + AngD13s*(-sn(1,j)*sn(3,j)+cs(1,j)*sn(2,j)*cs(3,j))
     &               + AngD2s*cs(1,j)*sn(2,j)*cs(3,j)
     &               + 2.0*AngD13*(cs(1,j)*cs(3,j)-sn(1,j)*sn(2,j)*sn(3,j))
     &               + 2.0*AngD12*sn(1,j)*cs(2,j)*cs(3,j)
     &               + 2.0*AngD23*cs(1,j)*cs(2,j)*sn(3,j)
c r33
c        SgpGCS(3,3,j,seg)= cs(1,j)*cs(2,j)
        RMd(3,3,j)= -AngD(1,j)*sn(1,j)*cs(2,j) - AngD(2,j)*cs(1,j)*sn(2,j)

        RMdD(3,3,j)= -AngDD(1,j)*sn(1,j)*cs(2,j)
     &               -  AngDD(2,j)*cs(1,j)*sn(2,j)
     &               -  AngD12s*cs(1,j)*cs(2,j)
     &               +  2.0*AngD12*sn(1,j)*sn(2,j)
      end do ! j                                                      
      return
      end
c
c	YXZEulAngDeriv
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Calculate the segment rotational velocity and 
c				acceleration from the derivatives of the segment
c				YXZ euler angles relative to the lab GCS.  This is
c				the method which is the default of the four methods
c				available in ANZ.  The formulation is compact and I
c				believe correct (see comments for CalcSegVelAcc).
c				The formulas are similar to those found in any standard
c				3D dynamics text except for the actual angles used.
c				Most texts derive the formulas based upon the zxz
c				euler angles whereas here yxz is used.  YXZ is the
c				order used to calculate the joint angles and I just
c				decided to be consistent and use the same angle convention
c				throughout ANZ.  See the derivation notes for ANZ to
c				study how these formulas were obtained.
c
c	Routines called:
c
c	CalcSegRotAcc
c	CalcSegRotVel
c	filter
c	JCSAngtoRotMat
c	SegFilterStatus2
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Param			GP.num_seg
c	Param			rMtnFrmRate
c	Param			MP.nfrm
c	Names			seg_names
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Segment			SangGCS
c	Segment			SacGCS
c	Segment			SgpGCS
c	Segment			SvlGCS
c
      subroutine YXZEulAngDeriv( CMND,SmoothRM,Cnt,Seg,status,OF )
      implicit none          
                          
      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_prm.inc'
      include 'a_seg.inc'
      include 'a_names.inc'
                             
      record /CmndLine/ CMND   
      record /option_flag/ OF

      integer*4 Cnt,Seg,status
      logical*4 SmoothRM
                                 
      real*4    Ang(3,MAX_MTN),AngD(3,MAX_MTN),AngDD(3,MAX_MTN),dtor,
     &          Temp(MAX_MTN),TempD(MAX_MTN),TempDD(MAX_MTN),TimeInc,
     &          Sign(3)
      integer*4 i,j,error,frm 
      logical*4 Smooth
      
      data Sign / 1.0,1.0,1.0 /
                                
c
c     First Differentiate the euler angles
c
      dtor= atan(1.0)/45.0
      Smooth= .false.       
      TimeInc= 1.0/rMtnFrmRate
      do i= 1, 3  
        if ( OF.UpDate ) write(6,100) cnt,GP.num_seg,seg_names(seg),i
100     format('+Differentiating segment # ',i2,'/',i2,' : ',a4,' Angle ',
     &         i1,'/3')
        do frm= 1, MP.nfrm           
          Temp(frm)= SangGCS(i,frm,seg)*dtor !*angsing(i)
        end do
        call filter( CMND,Smooth,MP.nfrm,temp,TimeInc,OF,
     &               TempD,TempDD,status )
        if ( status .lt. 0 ) return
        call SegFilterStatus2( status,seg,GP.num_seg,seg_names(seg),
     &                         i,OF )
        do frm= 1, MP.nfrm
          SangGCS(i,frm,seg)= Temp(frm)/dtor
          Ang(i,frm)=   Temp(frm)
          AngD(i,frm)=  TempD(frm)
          AngDD(i,frm)= TempDD(frm)
        end do
      end do ! i
      if ( SmoothRM ) then
        do frm= 1, MP.nfrm
          call JCSAngtoRotMat( SangGCS(1,frm,seg),Sign,
     &                         SgpGCS(1,1,frm,seg)       ) 
        end do
      end if      
c                                         
c     Now compute segment rotational velocity and acceleration relative
c     to the GCS and segment LCS coord systems   
c
      do i= 1, MP.nfrm
        call CalcSegRotVel( Ang(1,i),AngD(1,i),
     &                      SvlGCS(rx,i,seg),
     &                      SvlLCS(rx,i,seg)  )   
        call CalcSegRotAcc( Ang(1,i),AngD(1,i),AngDD(1,i),
     &                      SacGCS(rx,i,seg),
     &                      SacLCS(rx,i,seg)  )
      end do ! i
      return
      end
c
c	CalcSegRotVel
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	
c		Calculate the segment rotational velocity
c		This routine computes the segment rotational velocity
c     in both the segment LCS and the lab GCS using 
c		the segment angles and its derivative(s).  NOTE:  These
c		formulas were specifically derived for the YXZ angle rotation
c		sequence used throughout ANZ.  Most standard texts present
c		formulas for the more common ZXZ rotation sequence.
c
c	Routines called:
c
c	None
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 CalcSegRotVel( Ang,AngD,svlGCS,svlLCS )
      implicit none
      
      real*4    Ang(3),AngD(3),svlGCS(3),svlLCS(3)
      
      integer*4 i
      real*4    sa(3),ca(3)
      
      do i= 1, 3
        sa(i)= sin(Ang(i))
        ca(i)= cos(Ang(i))
      end do
      svlGCS(1)=  AngD(2)*ca(1) + AngD(3)*sa(1)*ca(2)
      svlGCS(2)=  AngD(1) - AngD(3)*sa(2)
      svlGCS(3)= -AngD(2)*sa(1) + AngD(3)*ca(1)*ca(2)

      svlLCS(1)=  AngD(2)*ca(3) + AngD(1)*ca(2)*sa(3)
      svlLCS(2)=  AngD(2)*sa(3) + AngD(1)*ca(2)*ca(3)
      svlLCS(3)=  AngD(3) - AngD(1)*sa(2)
      return
      end
c
c	CalcSegRotAcc
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	
c		Calculate the segment rotational acceleration
c		This routine computes the segment rotational acceleration
c     in both the segment LCS and the lab GCS using 
c		the segment angles and its derivative(s).  NOTE:  These
c		formulas were specifically derived for the YXZ angle rotation
c		sequence used throughout ANZ.  Most standard texts present
c		formulas for the more common ZXZ rotation sequence.
c
c	Routines called:
c
c	None
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 CalcSegRotAcc( Ang,AngD,AngDD,sacGCS,sacLCS )
      implicit none
      
      real*4    Ang(3),AngD(3),AngDD(3),sacGCS(3),sacLCS(3)
      
      integer*4 i
      real*4    sa(3),ca(3)
      
      do i= 1, 3
        sa(i)= sin(Ang(i))
        ca(i)= cos(Ang(i))
      end do
                
      sacGCS(1)=  ca(1)*AngDD(2) + sa(1)*ca(2)*AngDD(3) 
     &          - sa(1)*AngD(1)*AngD(2) 
     &          + ca(1)*ca(2)*AngD(1)*AngD(3)
     &          - sa(1)*sa(2)*AngD(2)*AngD(3)                      
      sacGCS(2)=  AngDD(1) - sa(2)*AngDD(3)
     &          - ca(2)*AngD(2)*AngD(3)
      sacGCS(3)= -sa(1)*AngDD(2) + ca(1)*ca(2)*AngDD(3)
     &          - ca(1)*AngD(1)*AngD(2) 
     &          - sa(1)*ca(2)*AngD(1)*AngD(3)
     &          - ca(1)*sa(2)*AngD(2)*AngD(3)

      sacLCS(1)=  ca(2)*sa(3)*AngDD(1) + ca(3)*AngDD(2)
     &          - sa(2)*sa(3)*AngD(1)*AngD(2) 
     &          - ca(2)*ca(3)*AngD(1)*AngD(3)
     &          - sa(3)*AngD(2)*AngD(3)
      sacLCS(2)=  ca(2)*ca(3)*AngDD(1) + sa(3)*AngDD(2)
     &          - sa(2)*ca(3)*AngD(1)*AngD(2) 
     &          - ca(2)*sa(3)*AngD(1)*AngD(3)
     &          + ca(3)*AngD(2)*AngD(3)
      sacLCS(3)=  AngDD(3) - sa(2)*AngDD(1) - ca(2)*AngD(1)*AngD(2)
      return
      end        
c     
c	SegFilterStatus1
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Display the error or warning status of the GCV or
c				DPF filter/differentiation operations that have
c				occured while working with the segment's rotation
c				matrix
c
c	Routines called:
c
c	None
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 SegFilterStatus1( status,seg,NumSeg,SegName,AxisName1,
     &                            AxisName2,OF )
      implicit none
                                      
      include 'a_struct.inc'

      record /option_flag/ OF
         
      integer*4 status,seg,NumSeg     
      character SegName*4,AxisName1*4,AxisName2*4
          
      if ( status .eq. 2 ) then       
        write(6,100) seg,NumSeg,SegName,AxisName1,AxisName2
100     format('+Differentiating segment # 'i2,'/',i2,': ',a4,a4,' ',a4,
     &         '    Warning- Nearly Constant'/)
        if ( OF.ToLog ) write(3,100) seg,NumSeg,SegName,
     &                                       AxisName1,AxisName2
      else if ( status .eq. 3 ) then   
        write(6,110) seg,NumSeg,SegName,AxisName1,AxisName2
110     format('+Differentiating segment # 'i2,'/',i2,': ',a4,a4,' ',a4,
     &         '    Warning- Data very small'/)
        if ( OF.ToLog ) write(3,110) seg,NumSeg,SegName,
     &                                       AxisName1,AxisName2
      else if ( status .eq. 4 ) then   
        write(6,120) seg,NumSeg,SegName,AxisName1,AxisName2
120     format('+Differentiating segment # 'i2,'/',i2,': ',a4,a4,' ',a4,
     &         '    Filter error- Used Finite Dif'/)
        if ( OF.ToLog ) write(3,120) seg,NumSeg,SegName,
     &                                       AxisName1,AxisName2
      else if ( status .ge. 10 ) then   
        write(6,130) seg,NumSeg,SegName,AxisName1,AxisName2,status-10
130     format('+Differentiating segment # 'i2,'/',i2,': ',a4,a4,' ',a4,
     &         '    GCVSPL error #',i2,'- Used Finite Dif'/)
        if ( OF.ToLog ) write(3,120) seg,NumSeg,SegName,
     &                                       AxisName1,AxisName2,status-10
      end if
      return
      end
c
c	SegFilterStatus2
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Display the error or warning status of the GCV or
c				DPF filter/differentiation operations that have
c				occured while working with the segment's YXZ euler
c				angles relative to the lab GCS
c
c	Routines called:
c
c	None
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 SegFilterStatus2( status,seg,NumSeg,SegName,Axis,OF )
      implicit none
                                      
      include 'a_struct.inc'

      record /option_flag/ OF
         
      integer*4 status,seg,NumSeg,axis
      character SegName*4
                                                  
      if ( status .eq. 2 ) then       
        write(6,100) seg,NumSeg,SegName,Axis
100     format('+Differentiating segment # 'i2,'/',i2,': ',a4,' Angle ',
     &         i1,'/3    Warning- Nearly Constant'/)
        if ( OF.ToLog ) write(3,100) seg,NumSeg,SegName,Axis
      else if ( status .eq. 3 ) then   
        write(6,110) seg,NumSeg,SegName,Axis
110     format('+Differentiating segment # 'i2,'/',i2,': ',a4,' Angle ',
     &         i1,'/3    Warning- Data very small'/)
        if ( OF.ToLog ) write(3,110) seg,NumSeg,SegName,Axis
      else if ( status .eq. 4 ) then   
        write(6,120) seg,NumSeg,SegName,Axis
120     format('+Differentiating segment # 'i2,'/',i2,': ',a4,' Angle ',
     &         i1,'/3    Filter error- Used Finite Dif'/)
        if ( OF.ToLog ) write(3,120) seg,NumSeg,SegName,Axis
      else if ( status .ge. 10 ) then   
        write(6,130) seg,NumSeg,SegName,Axis,status-10
130     format('+Differentiating segment # 'i2,'/',i2,': ',a4,' Angle ',
     &         i1,'/3    GCVSPL error #',i2,'- Used Finite Dif'/)
        if ( OF.ToLog ) write(3,120) seg,NumSeg,SegName,
     &                                       Axis,status-10
      end if
      return
      end
c
c	MrkVAAtoSegVAA
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Use the velocity and acceleration of the marker
c				trajectories to calculate the rotational and 
c				translational velocities and accelerations of the
c				body segments.  This formulation is based upon a
c 				paper by Verstraete and Soutas-Little that was in
c				the Journal of Biomechanical Engineering in Spring
c				1990.  I have extended it slightly to calculate 
c				translational vel/accel- the original only was set
c				up to calculate rotational vel/accel.  This routine
c				should prove useful for calculations with rigid body
c				arrays with more than 3 markers attached to them which
c				have been afixed to the body segments rather than using
c				individual markers.  At present I do not yet have
c				support for rigid arrays in calculating the segment
c				position.  I'll include something such as the classic
c				Schutt algorithm in the future.  This routine has been
c				tested upon three markers/segment and I found that the
c				rotational vel/accel returned are terribly noisy while
c				the translational vel/accel are fine.  I'm not sure what
c				is up with this right now (Sept. 1990) but I plan to
c				look into this much further (probably do a paper on it).
c				For now this routine is primarily for experimentation...
c				P.S., the formulation for this is included in the
c				derivation notes for ANZ.
c
c     8/29/90
c     There is storage space in the arrays for up to 6 markers per
c     segment.  The requirement is given by:
c       M= # markers on a segment
c       N= Number of marker pairs/segment= (M-1)!
c       Storage of Least Squares arrays A,B,C,D= 3*N
c                          
c	Routines called:
c
c	FormSolveLSEqtn
c	MatTran
c	VecChangeCS
c	VecNorm
c	VecSub
c	VecX
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Param			GP.num_seg
c	Param			GP.seg_num
c	Param			MP.nfrm
c	Marker			MrkData
c	Marker			MrkVel
c	Marker			MrkAcc
c	Names			seg_names
c	Param			SP.dist_mrk_num
c	Param			SP.mrk_num
c	Param			SP.num_MrkDist
c	Param			SP.num_mrk
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Segment			SacGCS
c	Segment			SacLCS
c	Segment			SvlGCS
c	Segment			SvlLCS
c
      subroutine MrkVAAtoSegVAA( CMND,SF,MF,OF )
      implicit none

      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_prm.inc'
      include 'a_seg.inc'
      include 'a_mrk.inc'
      include 'a_names.inc'
         
      record /CmndLine/ CMND   
      record /segment_flag/ SF
      record /marker_flag/  MF
      record /option_flag/  OF

      integer*4 i,j,k,NumSet,Mrk1,Mrk2,frm,seg,base,mrk,ierr
      real*4    rd(3,MAX_MTN,15),vel(3,MAX_MTN,15),acc(3,MAX_MTN,15),
     &          vp,ap,nr(3),A(45,MAX_MTN),B(45,MAX_MTN),C(45,MAX_MTN),
     &          D(45),VelC(3,45),vecA(3),vecB(3),vecC(3),AccC(3,45),
     &          GCStoLCS(3,3),VecDot,SumVp(MAX_MTN),SumAp(MAX_MTN),
     &          AvgVelC(3),AvgAccC(3),SDVelC(3,MAX_MTN),SDAccC(3,MAX_MTN),
     &          AT(45),BT(45),CT(45)
      character filename*40
      logical*4 MakeRigid,error,VecNorm,Diagnostics

      if ( .not. MF.VelAcc )  goto 1000 ! error
      if ( .not. MF.Diagnostics ) goto 1010 ! error
c
c     Check for /RIGid option
c
      MakeRigid= .false.
      Diagnostics= .false.
      if ( CMND.NumOpt(2) .gt. 0 ) then
        do i= 1, CMND.NumOpt(2)
          if ( CMND.OptText(i,2) .eq. 'RIG' ) then
            MakeRigid= .true.
          else if ( CMND.OptText(i,2) .eq. 'DIA' ) then
            Diagnostics= .true.
          else
            goto 1030 ! error
          end if
        end do
      end if
      if ( Diagnostics .and. (.not. MakeRigid) ) goto 1020 ! error
c
c     DO one segment at a time
c
      do i= 1, GP.num_seg
        seg= GP.seg_num(i)
        if ( OF.UpDate ) 
     &    write(6,100) i,GP.num_seg,seg_names(seg)
100       format('+Processing segment # ',i2,'/',i2,': ',a4)
c
c       Form marker pairings on the segment which are needed for both 
c       the velocity and acceleration calculations
c
        NumSet= SP(seg).num_MrkDist
        if ( NumSet .lt. 3 ) then
          write(6,200) seg_names(seg)
          if ( OF.ToLog ) write(3,200) seg_names(seg)
200       format(' Warning- ',a4,' has less than 3 markers associated with it'/
     &           '          It''s segment vel/acc will be set to 0')
          do frm= 1, MP.nfrm
            do j= 1, 6
              SvlGCS(j,frm,seg)= 0.0
              SvlLCS(j,frm,seg)= 0.0
              SacGCS(j,frm,seg)= 0.0
              SacLCS(j,frm,seg)= 0.0
            end do
          end do
        else ! do the least squares computation of seg vel/acc
          if ( Diagnostics ) then
            do frm= 1, MP.nfrm
              SumVp(frm)= 0.0
              SumAp(frm)= 0.0
            end do
          end if
          do j= 1, NumSet
            Mrk1= SP(seg).dist_mrk_num(1,j)
            Mrk2= SP(seg).dist_mrk_num(2,j)
            do frm= 1, MP.nfrm
              call VecSub( MrkData(x,frm,mrk2),MrkData(x,frm,mrk1),rd(x,frm,j) )
              call VecSub( MrkVel(x,frm,mrk2),MrkVel(x,frm,mrk1),vel(x,frm,j) )
              call VecSub( MrkAcc(x,frm,mrk2),MrkAcc(x,frm,mrk1),acc(x,frm,j) )
              if ( MakeRigid ) then
                error= VecNorm( rd(x,frm,j),nr )
                if ( error ) goto 1040 ! error
                vp= VecDot( vel(x,frm,j),nr )
                SumVp(frm)= SumVp(frm) + abs(vp)
                ap= VecDot( acc(x,frm,j),nr )
                SumAp(frm)= SumAp(frm) + abs(ap)
                do k= 1, 3
                  vel(k,frm,j)= vel(k,frm,j) - vp*nr(k)
                  acc(k,frm,j)= acc(k,frm,j) - ap*nr(k)
                end do
              end if
              base= (j-1)*3
              A(base+1,frm)=  0.0
              A(base+2,frm)= -rd(z,frm,j)
              A(base+3,frm)=  rd(y,frm,j)
              B(base+1,frm)=  rd(z,frm,j)
              B(base+2,frm)=  0.0
              B(base+3,frm)= -rd(x,frm,j)
              C(base+1,frm)= -rd(y,frm,j)
              C(base+2,frm)=  rd(x,frm,j)
              C(base+3,frm)=  0.0
            end do ! frm
          end do ! j = marker set #
          if ( Diagnostics ) then
            do frm= 1, MP.nfrm
              SumVp(frm)= SumVp(frm)/float(NumSet)
              SumAp(frm)= SumAp(frm)/float(NumSet)
            end do
          end if
c
c     First do the computation of the segment rotational velocity
c
          do frm= 1, MP.nfrm
            do j= 1, NumSet
              do k= 1, 3
                D((j-1)*3+k)= vel(k,frm,j)
              end do
            end do
            do j= 1, 3*NumSet
              AT(j)= A(j,frm)
              BT(j)= B(j,frm)
              CT(j)= C(j,frm)
            end do
            call FormSolveLSEqtn( NumSet*3,AT,BT,CT,D,
     &                            SvlGCS(rx,frm,seg),error )
            if ( error ) goto 1050 ! Solution failed
c
c       Now calculate the translational velocity
c
            do j= 1, 3
              AvgVelC(j)= 0.0
            end do
            do j= 1, SP(seg).num_mrk 
              mrk= SP(seg).mrk_num(j)
              call VecSub( MrkData(1,frm,mrk),SgpGCS(1,4,frm,seg),vecA )
              call VecX( SvlGCS(rx,frm,seg),vecA,vecB )
              call VecSub( MrkVel(1,frm,mrk),vecB,vecA )
              do k= 1, 3
                VelC(k,j)= vecA(k)
                AvgVelC(k)= AvgVelC(k) + vecA(k)
              end do
            end do ! j
            do j= 1, 3
              AvgVelC(j)= AvgVelC(j)/float(SP(seg).num_mrk)
              SvlGCS(j,frm,seg)= AvgVelC(j)
            end do
            if ( Diagnostics ) then
              do k= 1, 3
                SDVelC(k,frm)= 0.0
                do j= 1, SP(seg).num_mrk 
                  SDVelC(k,frm)= SDVelC(k,frm) + VelC(k,j)**2.0 - AvgVelC(k)**2.0
                end do
                SDVelC(k,frm)= SDVelC(k,frm)/float(SP(seg).num_mrk-1)
              end do
            end if
          end do ! frm
c
c       Do computation of rotational acceleration now
c
          do frm= 1, MP.nfrm
            do j= 1, NumSet
              call VecX( SvlGCS(rx,frm,seg),rd(1,frm,j),vecA )
              call VecX( SvlGCS(rx,frm,seg),vecA,vecB )
              call VecSub( acc(1,frm,j),vecB,vecA )
              do k= 1, 3
                D((j-1)*3+k)= vecA(k)
              end do
            end do
            do j= 1, 3*NumSet
              AT(j)= A(j,frm)
              BT(j)= B(j,frm)
              CT(j)= C(j,frm)
            end do
            call FormSolveLSEqtn( NumSet*3,AT,BT,CT,D,
     &                            SacGCS(rx,frm,seg),error )
            if ( error ) goto 1060 ! Solution failed
c
c       Next calculate the translational acceleration
c
            do j= 1, 3
              AvgAccC(j)= 0.0
            end do
            do j= 1, SP(seg).num_mrk 
              mrk= SP(seg).mrk_num(j)
              call VecSub( MrkData(1,frm,mrk),SgpGCS(1,4,frm,seg),vecA )
              call VecX( SvlGCS(rx,frm,seg),vecA,vecB )
              call VecX( SvlGCS(rx,frm,seg),vecB,vecC )
              call VecX( SacGCS(rx,frm,seg),vecA,vecB )
              do k= 1, 3
                AccC(k,j)= MrkAcc(k,frm,mrk) - vecB(k) - vecC(k)
                AvgAccC(k)= AvgAccC(k) + AccC(k,j)
              end do
            end do ! j
            do j= 1, 3
              AvgAccC(j)= AvgAccC(j)/float(SP(seg).num_mrk)
              SacGCS(j,frm,seg)= AvgAccC(j)
            end do
            if ( Diagnostics ) then
              do k= 1, 3
                SDAccC(k,frm)= 0.0
                do j= 1, SP(seg).num_mrk 
                  SDAccC(k,frm)= SDAccC(k,frm) + AccC(k,j)**2.0 - AvgAccC(k)**2.0
                end do
                SDAccC(k,frm)= SDAccC(k,frm)/float(SP(seg).num_mrk-1)
              end do
            end if
          end do ! frm
c
c       Lastly transform vel/acc from GCS to LCS
c
          do frm= 1, MP.nfrm
            call MatTran( SgpGCS(1,1,frm,seg),GCStoLCS )
            call VecChangeCS( GCStoLCS,SvlGCS(1,frm,seg),
     &                                 SvlLCS(1,frm,seg) )
            call VecChangeCS( GCStoLCS,SvlGCS(4,frm,seg),
     &                                 SvlLCS(4,frm,seg) )
            call VecChangeCS( GCStoLCS,SacGCS(1,frm,seg),
     &                                 SacLCS(1,frm,seg) )
            call VecChangeCS( GCStoLCS,SacGCS(4,frm,seg),
     &                                 SacLCS(4,frm,seg) )
          end do ! frm
c
c         Now dump information to text file if diagnostics has been set
c      
          if ( Diagnostics ) then
            filename= seg_names(seg)//'_sva_mrk.dia'
            open(unit=2,file=filename,status='unknown',iostat=ierr,err=2000 )
            write(2,300) seg_names(seg),SP(seg).num_mrk,
     &                   (SP(seg).mrk_num(j),j=1,SP(seg).num_mrk)
300         format(1x,a4,' Segment vel/acc calculation from marker vel/acc diagnostics'/
     &             5x,i2,' markers attached to segment'/
     &             5x,'Marker numbers are: ',6i3 )
            write(2,309)
309         format(5x,'Marker Pairs:'/)
            do j= 1, SP(seg).num_MrkDist
              write(2,310) SP(seg).dist_mrk_num(1,j),
     &                     SP(seg).dist_mrk_num(2,j)
310           format(19x,2i4)
            end do
            write(2,320)
320         format(' Frame  NonRigid Vel',10x,'Trans Vel Std Dev',
     &                     'NonRigid Acc',10x,'Trans Acc Std Dev'//)
            do frm= 1, MP.nfrm
              write(2,330) frm,SumVp(frm),(SDVelC(j,frm),j=1,3),
     &                         SumAp(frm),(SDAccC(j,frm),j=1,3)
330           format(1x,i4,3x,8e13.5)
            end do
            close(unit=2)     
          end if ! Dump Diagnostics...
        end if ! are there enough markers on the segment ?
      end do ! seg  
      SF.vel_acc= .true.
      return

1000  write(6,1005)
      if ( OF.ToLog ) write(3,1005)
1005  format(' Error- Marker vel/accel has not been calculated yet'/
     &       '        This must be done before segment vel and acc can be calculated')
      return
       
1010  write(6,1015)
      if ( OF.ToLog ) write(3,1015)
1015  format(' Error- Segment/Marker diagnostics have not been calculated yet'/
     &       '        This must be done before segment vel and acc can be calculated'/
     &       '        The command to do this is SEG MRK')
      return

1020  write(6,1025)
      if ( OF.ToLog ) write(3,1025)
1025  format(' Error- The /DIAgnostics option is only valid when the /RIGid'/
     &       '        has also been set')
      return

1030  write(6,1035) CMND.OptText(i,2)
      if ( OF.ToLog ) write(3,1035) CMND.OptText(i,2)
1035  format(' Error- ',a3,' is not a valid option for SEG VAA/MRK'/
     &       '        Valid options are: /RIGid /DIAgnostics')
      return

1040  write(6,1045) seg_names(seg),frm,j
      if ( OF.ToLog ) write(3,1045) seg_names(seg),frm,j
1045  format(' Error- Direction normalization failed for ',a4,' at frame #',i3/
     &       '        marker pair # ,'i2)
      return

1050  write(6,1055) seg_names(seg),frm
      if ( OF.ToLog ) write(3,1055) seg_names(seg),frm
1055  format(' Error- Rot Velocity solution failed for ',a4,' at frame #',i3)
      return

1060  write(6,1065) seg_names(seg),frm
      if ( OF.ToLog ) write(3,1065) seg_names(seg),frm
1065  format(' Error- Rot Acceleration solution failed for ',a4,' at frame #',i3)
      return

2000  write(6,2005) ierr,filename
      if ( OF.ToLog ) write(3,2005) ierr,filename
2005  format(' Error- Error #',i4,' occured while opening the file : ',a40)
      return
      end
c
c	FormSolveLSEqtn
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Form the least squares formulation matrix components
c				and solve for the rot velocity or acceleration.  NOTE
c				that the formulation here is identical for both vel
c				and accel.  See the routine MrkVAAtoSegVAA for more
c				information.
c
c	Routines called:
c
c	Gauss_Elim
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 FormSolveLSEqtn( Num,A,B,C,D,RotVA,error )
      implicit none
      
      integer*4 Num
      real*4    A(1),B(1),C(1),D(1),RotVA(3)
      logical*4 error
      
      integer*4 i,j
      real*4    Mat(3,4)
      
c
c     Form matrix and column vector
c      
      do i= 1, 3
        do j= 1, 4
          Mat(i,j)= 0.0
        end do
      end do
      do i= 1, Num
        Mat(1,1)= Mat(1,1) + A(i)*A(i)
        Mat(1,2)= Mat(1,2) + A(i)*B(i)
        Mat(1,3)= Mat(1,3) + A(i)*C(i)
        Mat(2,2)= Mat(2,2) + B(i)*B(i)
        Mat(2,3)= Mat(2,3) + B(i)*C(i)
        Mat(3,3)= Mat(3,3) + C(i)*C(i)
        Mat(1,4)= Mat(1,4) + A(i)*D(i)
        Mat(2,4)= Mat(2,4) + B(i)*D(i)
        Mat(3,4)= Mat(3,4) + C(i)*D(i)
      end do
      Mat(2,1)= Mat(1,2)  ! Matrix is symmetric
      Mat(3,1)= Mat(1,3)
      Mat(3,2)= Mat(2,3)
c
c     Solve matrix and column vector to find rot vel/acc
c     [Mat]*v= bvec or [Mat]*a= bvec 
c     NOTE: Mat is in the form of an augmented matrix so that it is in 
c           the proper form for the gauss elimination solver routine
c      
      call Gauss_Elim( 3,Mat,RotVA,error )
      return
      end
c
c
c
      subroutine EulerParamDeriv( CMND,SmoothRM,Cnt,Seg,status,OF )
      implicit none          
                          
      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_prm.inc'
      include 'a_seg.inc'
      include 'a_names.inc'
                             
      record /CmndLine/ CMND   
      record /option_flag/ OF

      integer*4 Cnt,Seg,status
      logical*4 SmoothRM
                                 
      real*4    eprm(4,MAX_MTN),epd(4,MAX_MTN),epdd(4,MAX_MTN),dtor,
     &          Temp(MAX_MTN),TempD(MAX_MTN),TempDD(MAX_MTN),TimeInc
      integer*4 i,j,frm 
      logical*4 Smooth,error
                              
c
c     First compute the euler parameters from the rotation matrix
c
      do frm= 1, MP.nfrm
        call RMtoEP( SgpGCS(1,1,frm,seg),eprm(1,frm),error )
        if ( error ) goto 1000
      end do
c                              
c     Differentiate the euler parameters
c
      Smooth= .false.       
      TimeInc= 1.0/rMtnFrmRate
      do i= 1, 4
        if ( OF.UpDate ) write(6,100) cnt,GP.num_seg,seg_names(seg),i
100     format('+Differentiating segment # ',i2,'/',i2,' : ',a4,' Parameter ',
     &         i1,'/4')
        do frm= 1, MP.nfrm           
          Temp(frm)= eprm(i,frm)
        end do
        call filter( CMND,Smooth,MP.nfrm,temp,TimeInc,OF,
     &               TempD,TempDD,status )
        if ( status .lt. 0 ) return
        call SegFilterStatus2( status,seg,GP.num_seg,seg_names(seg),
     &                         i,OF )
        do frm= 1, MP.nfrm
          eprm(i,frm)=   Temp(frm)
          epd(i,frm)=  TempD(frm)
          epdd(i,frm)= TempDD(frm)
        end do
      end do ! i
      if ( SmoothRM ) then
        do frm= 1, MP.nfrm
          call EPtoRM( eprm(1,frm),SgpGCS(1,1,frm,seg) )
        end do
      end if      
c                                         
c     Now compute segment rotational velocity and acceleration relative
c     to the GCS and segment LCS coord systems   
c
      do frm= 1, MP.nfrm
        call SegVA_EP_GCS( eprm(1,frm),epd(1,frm),SvlGCS(rx,frm,seg) )
        call SegVA_EP_GCS( eprm(1,frm),epdd(1,frm),SacGCS(rx,frm,seg) )
        call SegVA_EP_LCS( eprm(1,frm),epd(1,frm),SvlLCS(rx,frm,seg) )
        call SegVA_EP_LCS( eprm(1,frm),epdd(1,frm),SacLCS(rx,frm,seg) )
      end do ! i
      return

1000  write(6,1005) frm
      if ( OF.ToLog ) write(3,1005) frm
1005  format(/' Error- Conversion of rotation matrix to euler parameter failed'/
     &       '        at frame # ',i3)
      return
      end
c
c
c
      subroutine SegVA_EP_GCS( ep,epd,wa )
      implicit none

      real*4    ep(4),epd(4),wa(3)

      wa(1)= 2.0*( -ep(2)*epd(1) + ep(1)*epd(2) - ep(4)*epd(3) + ep(3)*epd(4) )
      wa(2)= 2.0*( -ep(3)*epd(1) + ep(4)*epd(2) + ep(1)*epd(3) - ep(2)*epd(4) )
      wa(3)= 2.0*( -ep(4)*epd(1) - ep(3)*epd(2) + ep(2)*epd(3) + ep(1)*epd(4) )
      return
      end
c
c
c
      subroutine SegVA_EP_LCS( ep,epd,wa )
      implicit none

      real*4    ep(4),epd(4),wa(3)

      wa(1)= 2.0*( -ep(2)*epd(1) + ep(1)*epd(2) + ep(4)*epd(3) - ep(3)*epd(4) )
      wa(2)= 2.0*( -ep(3)*epd(1) - ep(4)*epd(2) + ep(1)*epd(3) + ep(2)*epd(4) )
      wa(3)= 2.0*( -ep(4)*epd(1) + ep(3)*epd(2) - ep(2)*epd(3) + ep(1)*epd(4) )
      return
      end
