c         
c
c   
c
c     Copyright by Dwight Meglan 1990
c
c     Purpose: A general gauss elimination routine with partial pivoting for
c              the solution of simultaneous equations.
c     Input:
c       AB   : Augmented matrix where last column in the RHS of eqtn. Ax = b
c       size : Number of equations/variables
c       out  : Solution vector, x
c       error: Matrix A was singular error flag
c
        subroutine Gauss_Elim( size,AB,out,error )
        implicit none  

        integer*4 size
        real*4    AB(size,size+1),out(size)
        logical*4 error
                             
        integer*4 i,j,k,row,col,pivot,vrbl_num
        real*4    temp,ratio,value

        error= .false.
        do i= 1, size-1
          pivot= i
          do j= pivot+1, size
            if ( abs(AB(pivot,i)) .lt. abs(AB(j,i)) ) pivot= j
          end do
          if ( abs(AB(pivot,i)) .lt. 1.0e-10 ) goto 999
          if ( pivot .ne. i ) then
            do col= i, size
              temp= AB(i,col)
              AB(i,col)= AB(pivot,col)
              AB(pivot,col)= temp
            end do
          end if
          do row= i+1,size
            if ( AB(row,i) .ne. 0.0 ) then
              ratio= AB(row,i)/AB(i,i)
              do col= i+1,size
                AB(row,col)= AB(row,col) - ratio*AB(i,col)
              end do
            end if
          end do ! row
        end do ! i
        if ( abs(AB(size,size)) .lt. 1.0e-10 ) goto 999
        AB(size,size+1)= AB(size,size+1)/AB(size,size)
        do j= 2, size
          vrbl_num= size+1-j
          value= AB(vrbl_num,size+1)
          do k= vrbl_num+1, size
            value= value - AB(vrbl_num,k)*AB(k,size+1)
          end do ! k
          AB(vrbl_num,size+1)= value/AB(vrbl_num,vrbl_num)
        end do
        do i= 1, 3
          out(i)= AB(i,size+1)
        end do
        return
999     error= .true.
        return
        end
c
c
c
c
c     Copyright by Dwight Meglan 1990
c
c
c    Purpose:
c    Invert a general 3x3 matrix.  
c
c    Note!!!  This routine is not at present being used because the transpose
c    of a rotation matrix is the same as the inverse since a rotation
c    matrix is orthogonal.
c
      function MatInv3x3( matrix, inverse )
      implicit none

      real*4    matrix(3,3),inverse(3,3)
      integer*4 MatInv3x3

      real*4    cofactor(3,3),det,det1,det2,det3
      integer*4 i,j

c
c     form determinant of matrix and check for det=0 meaning that matrix
c     is singular and as a result will not invert
c
      det1= matrix(2,2)*matrix(3,3)-matrix(2,3)*matrix(3,2)
      det2= matrix(2,1)*matrix(3,3)-matrix(3,1)*matrix(2,3)
      det3= matrix(2,1)*matrix(3,2)-matrix(3,1)*matrix(2,2)
      det=  matrix(1,1)*det1-matrix(1,2)*det2+matrix(1,3)*det3
      if ( abs(det) .lt. 1.0e-10 ) then
        MatInv3x3= 1
        return
      else
        MatInv3x3= 0
      endif
c
c     form cofactor matrix
c
      cofactor(1,1)=  det1
      cofactor(1,2)= -det2
      cofactor(1,3)=  det3
      cofactor(2,1)= -(matrix(1,2)*matrix(3,3)-matrix(1,3)*matrix(3,2))
      cofactor(2,2)=   matrix(1,1)*matrix(3,3)-matrix(1,3)*matrix(3,1)
      cofactor(2,3)= -(matrix(1,1)*matrix(3,2)-matrix(1,2)*matrix(3,1))
      cofactor(3,1)=   matrix(1,2)*matrix(2,3)-matrix(1,3)*matrix(2,2)
      cofactor(3,2)= -(matrix(1,1)*matrix(2,3)-matrix(1,3)*matrix(2,1))
      cofactor(3,3)=   matrix(1,1)*matrix(2,2)-matrix(1,2)*matrix(2,1)
c
c     transpose cofactor matrix and load into inverse
c
      do i= 1,3
        do j= 1,3
          inverse(i,j)= cofactor(j,i)/det
        end do
      end do           
      return
      end
c                     
c
c                  
c
c     Copyright by Dwight Meglan 1990
c                
c
c  Subroutine Description:
c     This subroutine changes a vector's representation from one coordiante
c     system to another.  The transformation matrix is passed into the routine.
c     Since the quantity being transformed is a vector and thus does not
c     have a fixed location but only direction, only the rotation matrix
c     is required to transform the vector.
c
c  Data passed into subroutine
c   - trans_matrix -  transformation matrix ( actually a rotation matrix )
c   - vec_old_cs   -  vector expressed in the original CS
c
c  Data returned from subroutine
c   - vec_new_cs   -  vector expressed in the new CS
c
      subroutine VecChangeCS( trans_matrix, vec_old_cs, vec_new_cs )
      implicit none

      real*4    trans_matrix(3,3), vec_old_cs(3), vec_new_cs(3)

      real*4    sum
      integer*4 new_axis,old_axis,x,z
      data x,z / 1,3 /
      
      do new_axis= x, z
        sum= 0.0
        do old_axis= x, z
          sum= sum + trans_matrix(new_axis,old_axis)*vec_old_cs(old_axis)
        end do ! old_axis
        vec_new_cs(new_axis)= sum
      end do ! new_axis                                      
      return
      end                                                  
c                     
c                
c                  
c
c     Copyright by Dwight Meglan 1990
c
c     Purpose: transform a point from the local coordinate system to the global
c              coordinate system
c
c
      subroutine PntChangeCS( trans_matrix, pnt_old_cs, pnt_new_cs )
      implicit none
      
      real*4    trans_matrix(3,4), pnt_old_cs(3), pnt_new_cs(3)
      
      real*4    sum
      integer*4 new_axis,old_axis,x,z,cntr
      data x,z,cntr / 1,3,4 /
                 
      do new_axis= x, z
        sum= 0.0
        do old_axis= x, z
          sum= sum + trans_matrix(new_axis,old_axis)*pnt_old_cs(old_axis)
        end do ! old_axis
        pnt_new_cs(new_axis)= sum + trans_matrix(new_axis,cntr)
      end do ! new_axis                                      
      return
      end                                                  

c
c      
c
c
c     Copyright by Dwight Meglan 1990
c
c                     
c  Function Description:
c    Calculate the minimum distance from a point to a plane
c
c  Data passed into function
c   - normal - direction cosines of normal to the plane
c   - plane_pt - location of one point in the plane
c   - point -  location of point whose distance from plane is being calculated
c
c  Data returned from function
c   - distance from point to plane
c
      function DistPtPlane( normal, plane_pt, point, dist )
      implicit none
         
      real*4    normal(3),plane_pt(3),point(3),dist
      integer*4 DistPtPlane
                
      real*4    sum, mag
      integer*4 i

      DistPtPlane= 0
      sum= 0.0
      mag= 0.0
      do i= 1,3
        sum= sum + normal(i)*( point(i)-plane_pt(i) )
        mag= mag + normal(i)**2
      end do
      if ( mag .lt. 1.0e-10 ) then
        DistPtPlane= 1
        return
      end if
      dist= abs(sum)/sqrt(mag)
      return
      end
c
c
c       
c
c     Copyright by Dwight Meglan 1990
c
c
c  Function Description:    
c    Calculate the minimum distance between two lines
c
c  Data passed into function
c   - dir1 - direction cosines of line1
c   - dir2 - direction cosines of line2
c   - pt1  - location of a point on line1
c   - pt2  - location of a point on line2
c
c  Data returned from function
c   - minimum distance between two lines
c
      function DistLineLine( dir1, pt1, dir2, pt2, dist )
      implicit none
                
      real*4    dir1(3),pt1(3),dir2(3),pt2(3),dist
      integer*4 DistLineLine

      real*4    det,det1,det2,det3,mag,dir1temp(3),dir2temp(3)
      integer*4 i,x,y,z,VecNorm

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

      DistLineLine= 0
      DistLineLine= VecNorm( dir1, dir1temp )
      if ( DistLineLine .ne. 0 ) return
      DistLineLine= VecNorm( dir2, dir2temp )
      if ( DistLineLine .ne. 0 ) return
      det1= dir1temp(y)*dir2temp(z)-dir1temp(z)*dir2temp(y) 
      det2= dir1temp(z)*dir2temp(x)-dir1temp(x)*dir2temp(z) 
      det3= dir1temp(x)*dir2temp(y)-dir1temp(y)*dir2temp(x) 
      det= ( pt2(x)-pt1(x) )*det1 + ( pt2(y)-pt1(y) )*det2 + 
     &     ( pt2(z)-pt1(z) )*det3
      mag= sqrt( det1**2 + det2**2 + det3**2 )
      if ( mag .lt. 1.0e-10 ) then
        DistLineLine= 1
        return
      end if
      dist= abs(det)/mag
      return
      end

c                
c
c
c
c     Copyright by Dwight Meglan 1990
c
c
c  Subroutine Description:
c    Calculate the minimum distance between a line and a point in 3D space
c
c  Data passed into subroutine
c   - dir   - direction cosines of line
c   - ln_pt - location of a point on the line
c   - pt    - loaction of the point whose distance from the line is calculated
c                        
c  Data returned from subroutine
c   - distance from point to line
c
      function DistPtLine( dir, ln_pt, pt )
      implicit none

      real*4    dir(3),ln_pt(3),pt(3)
      integer*4 DistPtLine

      real*4    det1,det2,det3
      integer*4 x,y,z

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

      det1= ( pt(x)-ln_pt(x) )*dir(y) - ( pt(y)-ln_pt(y) )*dir(x)
      det2= ( pt(y)-ln_pt(y) )*dir(z) - ( pt(z)-ln_pt(z) )*dir(y)
      det3= ( pt(z)-ln_pt(z) )*dir(x) - ( pt(x)-ln_pt(x) )*dir(z)
      DistPtLine= sqrt( det1**2 + det2**2 + det3**2 )
      return
      end
c
c
c                              
c
c     Copyright by Dwight Meglan 1990
c
c
c  Function Description:
c    Calculate vector dot product
c
c  Data passed into function
c   - VecA - first vector
c   - VecB - second vector
c
c  Data returned from function
c   - magnitude of vector dot product
c          
      function VecDot( VecA, VecB )
      implicit none
  
      real*4    VecA(3),VecB(3),VecDot
 
      integer*4 i

      VecDot= 0.0
      do i= 1,3
        VecDot= VecDot + VecA(i)*VecB(i)
      end do
      return
      end       
c
c
c
c
c     Copyright by Dwight Meglan 1990
c
c
c  Function Description:
c    Calculate vector magnitude
c
c  Data passed into function
c   - Vector
c
c  Data returned from function
c   - magnitude of vector
c
      function VecMag( Vector )
      implicit none
 
      real*4    Vector(3),VecMag

      integer*4 i

      VecMag= 0.0
      do i= 1,3
        VecMag= VecMag + Vector(i)**2
      end do
      VecMag= sqrt( VecMag )
      return
      end
c
c
c
c
c     Copyright by Dwight Meglan 1990
c
c
c  Subroutine Description:
c    Subtract vector B from Vector A
c
c  Data passed into subroutine
c   - VecA - first vector
c   - VecB - second vector
c
c  Data returned from subroutine
c   - VecOut - resultant vector
c
      subroutine VecSub( VecA, VecB, VecOut )
      implicit none

      real*4    VecA(3),VecB(3),VecOut(3)

      integer*4 i

      do i= 1,3
        VecOut(i)= VecA(i) - VecB(i)
      end do
      return
      end
c
c
c
c
c     Copyright by Dwight Meglan 1990
c
c
c  Subroutine Description:
c    Calculate vector cross product
c                
c  Data passed into subroutine
c   - VecA - first vector
c   - VecB - second vector
c
c  Data returned from subroutine
c   - VecOut - result of cross product
c
      subroutine VecX( VecA, VecB, VecOut )
      implicit none

      real*4    VecA(3),VecB(3),VecOut(3)

      integer*4 x,y,z
      data    x,y,z / 1,2,3 /

      VecOut(x)= VecA(y)*VecB(z) - VecA(z)*VecB(y)
      VecOut(y)= VecA(z)*VecB(x) - VecA(x)*VecB(z)
      VecOut(z)= VecA(x)*VecB(y) - VecA(y)*VecB(x)
      return
      end
                 
c                 
c                 
c                 
c
c     Copyright by Dwight Meglan 1990
c
c
c  Subroutine Description:
c    Normalize vector: make it a unit vector
c
c  Data passed into subroutine
c   - VecIn - original vector
c
c  Data returned from subroutine
c   - VecOut - normalized vector

      function VecNorm( VecIn, VecOut )
      implicit none

      real*4    VecIn(3), VecOut(3)
      integer*4 VecNorm

      real*4    sum
      integer*4 i

      VecNorm= 0
      sum= 0.0
      do i= 1, 3
        sum= sum + VecIn(i)*VecIn(i)
      end do
      sum= sqrt(sum)
      if ( sum .lt. 1.0e-10 ) then
        VecNorm= 1       
        return
      end if
      do i= 1,3
        VecOut(i)= VecIn(i)/sum
      end do
      return
      end

                         

c                 
c                
c      
c
c     Copyright by Dwight Meglan 1990
c
c
c  Function Description:
c    Calculate distance between two points        
c
c  Data passed into subroutine
c   - point1 - first point
c   - point2 - second point
c
c  Data returned from subroutine
c   - distance between points

      function VecLth( point1, point2 )
      implicit none
      
      real*4    point1(3),point2(3),VecLth
      
      integer*4 i
      
      VecLth= 0.0
      do i= 1, 3
        VecLth= VecLth + (point1(i)-point2(i))**2
      end do
      VecLth= sqrt(VecLth)
      return
      end

                                      
c
c
c
c
c     Copyright by Dwight Meglan 1990
c
c
c  Subroutine Description:
c    Transform a point's coordinates from the GCS to a LCS of a body segment
c
c  Data passed into subroutine
c   - LCS_matrix  - orientation and location of LCS relative to GCS
c   - GCS_pnt_loc - location of point in GCS
c
c  Data returned from subroutine
c   - LCS_pnt_loc - location of point in LCS
c
c  User defined subroutines called
c    MAT_TRANSPOSE - transpose a matrix
c    VEC_CHANGE_CS - transform vector from one CS to another   
c    VEC_SUB       - subtract two vectors
c
c                   
      subroutine PntGCStoLCS( LCS_matrix, GCS_pnt_loc, LCS_pnt_loc )
      implicit none

      real*4    LCS_matrix(3,4),GCS_pnt_loc(3),LCS_pnt_loc(3)

      real*4    temp_pnt(3),GCS_to_LCS(3,3)
      integer*4 x,center
                 
      data x,center / 1,4 /

      call VecSub( GCS_pnt_loc, LCS_matrix(x,center), temp_pnt )
      call MatTran( LCS_matrix, GCS_to_LCS )
      call VecChangeCS( GCS_to_LCS, temp_pnt, LCS_pnt_loc )
      return
      end
c
c
c
c
c     Copyright by Dwight Meglan 1990
c
c     Purpose: Transform a vector from the global coordinate system to the local
c              coordinate system
c
      subroutine VecGCStoLCS( LCS, VecGCS, VecLCS )
      implicit none

      real*4 LCS(3,3),VecGCS(3),VecLCS(3),GCStoLCS(3,3)

      call MatTran( LCS, GCStoLCS )
      call VecChangeCS( GCStoLCS,VecGCS,VecLCS )
      return
      end
c
c
c
c     
c     Copyright by Dwight Meglan 1990
c
c 
c  Subroutine Description:
c    Transform a vector from one body segment LCS to another
c
c  Data passed into subroutine
c   - LCS1_to_GCS - location and orientation of original LCS relative to GCS 
c   - LCS2_to_GCS - location and orientation of new LCS relative to GCS
c   - vec_LCS1    - original vector
c
c  Data returned from subroutine   
c   - vec_LCS2    - vector in new LCS

      subroutine VecLCS1toLCS2( LCS1_to_GCS, LCS2_to_GCS, 
     &                          vec_LCS1, vec_LCS2        )
      implicit none

      real*4  LCS1_to_GCS(3,3),LCS2_to_GCS(3,3),vec_LCS1(3),vec_LCS2(3)

      real*4  vec_GCS(3),GCS_to_LCS2(3,3)

      call VecChangeCS( LCS1_to_GCS,Vec_LCS1,Vec_GCS )
      call MatTran( LCS2_to_GCS,GCS_to_LCS2 )
      call VecChangeCS( GCS_to_LCS2,Vec_GCS,Vec_LCS2 )
      return
      end
c
c
c
c     
c     Copyright by Dwight Meglan 1990
c
c 
c  Subroutine Description:
c    Transform a point from one body segment LCS to another
c
c  Data passed into subroutine
c   - LCS1_to_GCS - location and orientation of original LCS relative to GCS 
c   - LCS2_to_GCS - location and orientation of new LCS relative to GCS
c   - pnt_LCS1    - original point
c
c  Data returned from subroutine   
c   - pnt_LCS2    - vpoint in new LCS

      subroutine PntLCS1toLCS2( LCS1_to_GCS, LCS2_to_GCS, 
     &                          pnt_LCS1, pnt_LCS2        )
      implicit none
                                       
      real*4  LCS1_to_GCS(3,3),LCS2_to_GCS(3,3),pnt_LCS1(3),pnt_LCS2(3)

      real*4    pnt_GCS(3),GCS_to_LCS2(3,3)
                                  
      call PntChangeCS( LCS1_to_GCS,pnt_LCS1,pnt_GCS )
      call PntGCStoLCS( LCS2_to_GCS,pnt_GCS,pnt_LCS2 )
      return    
      end         
c
c          
c     Subroutine description
c       Compute the position and orientation of local coordinate system 2
c       in terms of LCS1 starting with knowing both CS in terms of a common
c       coordinate system
c
      subroutine LCS2relLCS1( LCS1,LCS2,LCS21 )
      implicit none

      real*4   LCS1(3,4),LCS2(3,4),LCS21(3,4)

      real*4   tLCS1(3,3),tmp(3)

      call MatTran(LCS1,tLCS1)
      call MatMult(tLCS1,LCS2,LCS21)
      call VecSub(LCS2(1,4),LCS1(1,4),tmp)
      call VecChangeCS(tLCS1,tmp,LCS21(1,4))
      return
      end
c
c
c     Convert from the technical coordinate system (known relative to the
c     global coordinate system) to the Local coordinate system. The LCS 
c     position and orientation is known relative to the TCS and this normally
c     fixed conversion is applied to the TCS to produce the LCS known relative
c     to the GCS.
c
      subroutine CalcTCStoLCS( TCS,TCStoLCS,LCS )
      implicit none
      
      real*4    TCS(3,4),TCStoLCS(3,4),LCS(3,4)

      call PntChangeCS( TCS,TCStoLCS(1,4),LCS(1,4) )
      call MatMult( TCS,TCStoLCS,LCS )
      return
      end
c
c     Copyright by Dwight Meglan 1990
c
c
c  Subroutine Description:
c    Transpose a 3x3 matrix
c                     
c  Data passed into subroutine
c   - matrix1 - matrix to be transposed
c
c  Data returned from subroutine
c   - matrix2 - transposed matrix

      subroutine MatTran( matrix1, matrix2 )
      implicit none
      
      real*4    matrix1(3,3),matrix2(3,3)

      integer*4 i,j

      do i= 1, 3
        do j= 1, 3
          matrix2(j,i)= matrix1(i,j)
        end do
      end do
      return
      end

c
c
c
c
c     Copyright by Dwight Meglan 1990
c
c
c  Subroutine Description:
c    Multiply two 3x3 matrices together.  Matrix2 is postmultiplied with
c    matrix1.
c
c  Data passed into subroutine
c   - matrix1 - first matrix to be multiplied
c   - matrix2 - second of the matrices to be multiplied
c
c  Data returned from subroutine
c   - matrix3 - result of matrix multiplication

      subroutine MatMult( matrix1,matrix2,matrix3 )
      implicit none

      real*4    matrix1(3,3),matrix2(3,3),matrix3(3,3)

      real*4    sum
      integer*4 i,j,k

      do i= 1, 3
        do j= 1, 3
          sum= 0.0
          do k= 1, 3
            sum= sum + matrix1(j,k)*matrix2(k,i)
          end do
          matrix3(j,i)= sum
        end do
      end do
      return
      end
c
c                     
c
c
c     Copyright by Dwight Meglan 1990
c
c     Purpose: For two lines whose directions (vecA,vecB) and a point on each that
c              the line passes through (pntA,pntB) is known, compute the point along 
c              each line at which a line through which is the mimimum distance between
c              A and B passes (pnt1 along A and pnt2 along B).
c
c      The value of MinLineXpnts is 0 if the points are found and 1 if they are not
c      as in the case of parallel lines.
c
      function MinLineXPnts( vecA,pntA,vecB,pntB,pnt1,pnt2 )
      implicit none

      real*4    vecA(3),pntA(3),vecB(3),pntB(3),pnt1(3),pnt2(3)
      integer*4 MinLineXPnts

      real*4    VecDot,dpAab,dpBab,dpAB,s,t,vecAB(3),normA(3),normB(3)
      integer*4 axis,x,z,VecNorm
      data x,z / 1,3 /

      MinLineXPnts= 0
      call VecSub( pntA,pntB,vecAB )
      MinLineXPnts= VecNorm( vecA, normA )
      if ( MinLineXPnts .ne. 0 ) return
      MinLineXPnts= VecNorm( vecB, normB )
      if ( MinLineXPnts .ne. 0 ) return
      dpAB= VecDot( normA,normB )
      if ( abs(dpAB-1.0) .lt. 1.0e-10 ) then
        MinLineXPnts= 1
        return
      endif
      dpAab= VecDot( normA,vecAB )
      dpBab= VecDot( normB,vecAB )
      t= (dpBab-dpAab*dpAB)/(1.0-dpAB**2)
      s= dpAB*t - dpAab
      do axis= x, z
        pnt1(axis)= pntA(axis) + s*normA(axis)
        pnt2(axis)= pntB(axis) + t*normB(axis)
      end do          
      return
      end                   
c
c     This subroutine calculates the x,y,z euler angles of local coordinate
c     system 1 relative to local coordinate 2.  The axes of the LCS's are
c     assumed to be expressed relative to a common global coordinate system
c     (GCS).  The algorithm used here comes from a book by R.P. Paul of
c     MIT (1981) p.70-71.  It is supposed to be less susceptible to noise
c     in the kinematic data then other formulations.
c
c     This algorithm is from the old Boston/OSU postprcessor (OUT) and has not had
c     its axes properly rotated for the new orientation of the CS's.  Thus,
c     it returns incorrect angle values.  It is not used at present- 5/24/89
c
c     These angles are the fixed axis angles as opposed to the floating axis angles
c     that the accepted standard in gait these days. See the discussion in J. Craig's
c     book "Introduction to Robotics" in chapter 2 for a good review of fixed versus
c     floating axis euler angles.
c
c
c     Error Codes: 0 - no error
c                  1 - Angle 1 (flex/extension) bad
c                  2 - Angle 2 (ab/adduction) bad
c                  4 - Angle 3 (in/external) bad
c                  3 - Angles 1 and 2 bad
c                  5 - Angles 1 and 3 bad  
c                  6 - Angles 2 and 3 bad
c                  7 - Angles 1, 2, and 3 bad
c
c
c     Copyright by Dwight Meglan 1990
c
      subroutine EulerAngles2( LCS1, LCS2, angles, error )
      implicit none

      real*4    LCS1(3,3),LCS2(3,3),angles(3)
      integer*4 error

      real*4    temp_mat(3,3),rot_mat(3,3),temp,temp2,rtod,c2,s2,angsign(3)  
      integer*4 i

      data angsign / 1,-1,-1 /
                                           
      error= 0
      rtod= 45.0/atan(1.0)                 
      call MatTran( LCS2, temp_mat ) ! Invert matrix
      call MatMult( temp_mat, LCS1, rot_mat )    ! now calc lcs1 relative to
      if ( (abs(rot_mat(2,1)) .lt. 1.0e-10) .and. ! lcs2
     &     (abs(rot_mat(1,1)) .lt. 1.0e-10)       ) then ! error
        do i= 1, 3
          angles(i)= 0
        end do
        error= 7
        return
      else
        angles(3)= atan2( rot_mat(2,1),rot_mat(1,1) )
      end if  
      c2= cos(angles(3))
      s2= sin(angles(3))                   
      temp= rot_mat(1,1)*c2 + rot_mat(2,1)*s2
      if ( (abs(temp) .lt. 1.0e-10) .and.
     &     (abs(rot_mat(3,1)) .lt. 1.0e-10)  ) then
        angles(1)= 0.0                     
        error= error + 1
      else
        angles(1)= atan2( -rot_mat(3,1),temp )
      end if
      temp=  -rot_mat(1,2)*s2 + rot_mat(2,2)*c2
      temp2=  rot_mat(1,3)*s2 - rot_mat(2,3)*c2
      if ( (abs(temp)  .lt. 1.0e-10) .and.  
     &     (abs(temp2) .lt. 1.0e-10)       ) then
        angles(2)= 0.0
        error= error + 4
      else
        angles(2)= atan2( temp2,temp )
      end if
      do i= 1, 3                            
        angles(i)= angles(i)*rtod !*angsign(i)
      end do
      return  
      end
c
c                            
c
c
c     Copyright by Dwight Meglan 1990
c
      subroutine EulerAngles( LCS1, LCS2, angles, error )
      implicit none
c
c     Purpose: This routine computes the euler angles describing the orientation of 
c              CS 2 relative to CS 1. These angles are about the fixed axes of LCS1 
c              in the order of rotation about y, then x, and then z. Since this is 
c              a fixed axis sequence, the angles correspond to PITCH, ROLL, and YAW
c              respectively.
c
c     These angles are not the same as the commonly accepted floating axis y, x, and z
c     sequence that corresponds to the so called Joint Coordinate System of Chao as well 
c     as Grood and Suntay. These angles have been included for completeness sake 
c     because this was the sequence previously unused at Boston Children's Hospital
c     before the Gait Lab was moved to OSU. These angles ARE NOT used nowadays. 
c
c
c     See the discussion in J. Craig's book "Introduction to Robotics" in chapter 2 
c     for a good review of fixed versus floating axis euler angles.
c
c     Error Codes: 0 - no error  
c                  1 - Angle 1 (flexion/extension) bad
c                  2 - Angle 2 (ab/adduction) bad
c                  4 - Angle 3 (in/external) bad
c                  3 - Angles 1 and 2 bad
c                  5 - Angles 1 and 3 bad
c                  6 - Angles 2 and 3 bad
c                  7 - Angles 1, 2, and 3 bad
c
      real*4    LCS1(3,3),LCS2(3,3),angles(3)
      integer*4 error

      real*4    temp_mat(3,3),rot_mat(3,3),cos2,rtod,angsign(3)
      integer*4 i
                                           
      data angsign / 1,-1,-1 /
                                           
      error= 0
      rtod= 45.0/atan(1.0)                 
      call MatTran( LCS2, temp_mat ) ! Invert matrix
      call MatMult( temp_mat, LCS1, rot_mat )    ! now calc lcs1 relative to
      cos2= sqrt( rot_mat(1,2)**2 + rot_mat(2,2)**2 )
      if ( cos2 .lt. 1.0e-10 ) then ! error- ill defined rotation
        if ( rot_mat(3,2) .gt. 0.0 ) then
          angles(2)=  atan(1.0)*2.0
        else
          angles(2)= -atan(1.0)*2.0
        end if
        angles(1)= atan2( rot_mat(1,3),rot_mat(1,1) )
        angles(3)= 0.0
        error= 7
        return
      else
        angles(2)= atan2( rot_mat(3,2),cos2 )
      end if  
      if ( (abs(rot_mat(3,1)) .lt. 1.0e-10) .and.
     &     (abs(rot_mat(3,3)) .lt. 1.0e-10)  ) then
        angles(1)= 0.0
        error= error + 1
      else
        angles(1)= atan2( -rot_mat(3,1)/cos2,rot_mat(3,3)/cos2 )
      end if
      if ( (abs(rot_mat(1,2)) .lt. 1.0e-10) .and.
     &     (abs(rot_mat(2,2)) .lt. 1.0e-10)       ) then
        angles(3)= 0.0              
        error= error + 4
      else      
        angles(3)= atan2( -rot_mat(1,2)/cos2,rot_mat(2,2)/cos2 )
      end if
      do i= 1, 3                            
        angles(i)= angles(i)*rtod !*angsign(i)
      end do
      return
      end   
c
c                            
c
c
c     Copyright by Dwight Meglan 1990
c
      subroutine EulerAnglesXZY( LCS1, LCS2, angles, error )
      implicit none
c
c     Purpose: This routine computes the euler angles describing the orientation of 
c              CS 2 relative to CS 1. These angles are about the floating axes of LCS1 
c              in the order of rotation about x, then z, and then y.
c
c     This angle sequence is used to export the relative segment orientations
c     (joint angles) for the SIMM software package by Musculographics.
c
c     Error Codes: 0 - no error  
c                  1 - Angle 1 bad
c                  2 - Angle 2 bad
c                  4 - Angle 3 bad
c                  3 - Angles 1 and 2 bad
c                  5 - Angles 1 and 3 bad
c                  6 - Angles 2 and 3 bad
c                  7 - Angles 1, 2, and 3 bad
c
      real*4    LCS1(3,3),LCS2(3,3),angles(3)
      integer*4 error
                              
      real*4    temp_mat(3,3),rot_mat(3,3),cos2,rtod
      integer*4 i
                                           
      error= 0
      rtod= 45.0/atan(1.0)                 
      call MatTran( LCS2, temp_mat ) ! Invert matrix
      call MatMult( temp_mat, LCS1, rot_mat )    ! now calc lcs1 relative to
      cos2= sqrt( rot_mat(1,1)**2 + rot_mat(1,3)**2 )
      if ( cos2 .lt. 1.0e-10 ) then ! error- ill defined rotation
        angles(2)= 90.0
        if ( rot_mat(1,2) .gt. 0.0 ) angles(2)= -angles(2)
        angles(1)= atan2( rot_mat(3,1),rot_mat(2,1) )
        angles(3)= 0.0
        error= 7
        return               
      else
        angles(2)= atan2( -rot_mat(1,2),cos2 )
      end if  
      if ( (abs(rot_mat(3,2)) .lt. 1.0e-10) .and.
     &     (abs(rot_mat(2,2)) .lt. 1.0e-10)  ) then
        angles(1)= 0.0
        error= error + 1
      else
        angles(1)= atan2( rot_mat(3,2)/cos2,rot_mat(2,2)/cos2 )
      end if
      if ( (abs(rot_mat(1,3)) .lt. 1.0e-10) .and.
     &     (abs(rot_mat(1,1)) .lt. 1.0e-10)       ) then
        angles(3)= 0.0              
        error= error + 4
      else      
        angles(3)= atan2( rot_mat(1,3)/cos2,rot_mat(1,1)/cos2 )
      end if
      do i= 1, 3                            
        angles(i)= angles(i)*rtod
      end do                
      return
      end   
c
c                            
c
c
c     Copyright by Dwight Meglan 1990
c
      subroutine EulerAnglesZXY( LCS1, LCS2, angles, error )
      implicit none
c
c     Purpose: This routine computes the euler angles describing the orientation of 
c              CS 2 relative to CS 1. These angles are about the floating axes of LCS1 
c              in the order of rotation about z, then x, and then y.
c
c     This angle sequence is used to export the relative segment orientations
c     for the SIMM software package by Musculographics.
c
c     Error Codes: 0 - no error  
c                  1 - Angle 1 bad
c                  2 - Angle 2 bad
c                  4 - Angle 3 bad
c                  3 - Angles 1 and 2 bad
c                  5 - Angles 1 and 3 bad
c                  6 - Angles 2 and 3 bad
c                  7 - Angles 1, 2, and 3 bad
c
      real*4    LCS1(3,3),LCS2(3,3),angles(3)
      integer*4 error
                              
      real*4    temp_mat(3,3),rot_mat(3,3),cos2,rtod
      integer*4 i
                                           
      error= 0
      rtod= 45.0/atan(1.0)                 
      call MatTran( LCS2, temp_mat ) ! Invert matrix
      call MatMult( temp_mat, LCS1, rot_mat )    ! now calc lcs1 relative to
      cos2= sqrt( rot_mat(3,1)**2 + rot_mat(3,3)**2 )
      if ( cos2 .lt. 1.0e-10 ) then ! error- ill defined rotation
        angles(2)= 90.0
        if ( rot_mat(2,3) .gt. 0.0 ) angles(2)= -angles(2)
        angles(1)= atan2( -rot_mat(1,2),rot_mat(2,2) )
        angles(3)= 0.0
        error= 7
        return               
      else
        angles(2)= atan2( rot_mat(3,2),cos2 )
      end if  
      if ( (abs(rot_mat(1,2)) .lt. 1.0e-10) .and.
     &     (abs(rot_mat(2,2)) .lt. 1.0e-10)  ) then
        angles(1)= 0.0
        error= error + 1
      else
        angles(1)= atan2( -rot_mat(1,2)/cos2,rot_mat(2,2)/cos2 )
      end if
      if ( (abs(rot_mat(3,1)) .lt. 1.0e-10) .and.
     &     (abs(rot_mat(3,3)) .lt. 1.0e-10)       ) then
        angles(3)= 0.0              
        error= error + 4
      else      
        angles(3)= atan2( -rot_mat(3,1)/cos2,rot_mat(3,3)/cos2 )
      end if
      do i= 1, 3                            
        angles(i)= angles(i)*rtod
      end do                
      return
      end   
c                                                   
c                            
c
c
c     Copyright by Dwight Meglan 1990
c
      subroutine ShldrAngles( LCS1, LCS2, angles, error )
      implicit none
c
c     Purpose: Compute the angles describing the orientation of CS 2 relative to CS 1
c              in terms of a polar system- actually z-y-z euler angles. This is 
c              logical and convenient system for describing shoulder motion and was
c              used in the OSU upper extremity study. It avoids the singularity 
c              problem when the shoulder is abducted to 90 degrees.
c
c     Error Codes: 0 - no error  
c                  1 - Angle 1 (azimuth) bad
c                  2 - Angle 2 (inclination) bad
c                  4 - Angle 3 (twist) bad
c                  3 - Angles 1 and 2 bad
c                  5 - Angles 1 and 3 bad
c                  6 - Angles 2 and 3 bad
c                  7 - Angles 1, 2, and 3 bad
c
      real*4    LCS1(3,3),LCS2(3,3),angles(3)
      integer*4 error

      real*4    temp_mat(3,3),rot_mat(3,3),sin2,rtod,angsign(3)
      integer*4 i
                                           
      data angsign / -1,1,-1 /
                                           
      error= 0                                  
      rtod= 45.0/atan(1.0)                 
      call MatTran( LCS2, temp_mat ) ! Invert matrix
      call MatMult( temp_mat, LCS1, rot_mat )    ! now calc lcs1 relative to
      sin2= sqrt( rot_mat(1,3)**2 + rot_mat(2,3)**2 )
      if ( sin2 .lt. 1.0e-10 ) then ! First and Third rotation axes
        error= 7                                ! are colinear
        if ( rot_mat(3,3) .gt. 0.0 ) then
          angles(2)= 0.0
        else
          angles(2)= 180.0
        endif
        angles(1)= atan2( -rot_mat(1,2),rot_mat(1,1) )
        angles(3)= 0.0
        return
      else
        angles(2)= atan2( sin2,rot_mat(3,3) )
      end if  
      if ( (abs(rot_mat(2,3)) .lt. 1.0e-10) .and.
     &     (abs(rot_mat(1,3)) .lt. 1.0e-10)       ) then
        angles(1)= 0.0
        error= error + 1
      else
        angles(1)= atan2( rot_mat(2,3)/sin2,-rot_mat(1,3)/sin2 )
      end if
      if ( (abs(rot_mat(3,2))  .lt. 1.0e-10) .and.
     &     (abs(rot_mat(3,1))  .lt. 1.0e-10)       ) then
        angles(3)= 0.0
        error= error + 2            
      else      
        angles(3)= atan2( rot_mat(3,2)/sin2,-rot_mat(3,1)/sin2 )
      end if
      do i= 1, 3                            
        angles(i)= angles(i)*rtod !*angsign(i)
      end do
      return
      end   
c
c                
c
c
c     Copyright by Dwight Meglan 1990
c
      subroutine JcsAngles( LCS1, LCS2, angles, error )
      implicit none
c
c
c     Purpose: Compute the floating axis euler angles describing the orientation of
c              CS 2 relative to CS 1. The sequence is y, x, and z which corresponds
c              to the same sequence used Chao as well as Grood and Suntay to describe
c              joint orientation. It is referred to often as the Joint Coordinate 
c              System. Hence the JCS in the name. The order seems a bit different 
c              relative to what has been published but that is because the imbedded
c              CS of the segments is in a different alignment here relative Grood and
c              Suntay's paper. The y rotation is the fl/extension angle, x the ab/ad
c              angle, and z is in/external rotation. Storage of the angles in the 
c              output array is in this order also.
c             
c     Error Codes: 0 - no error
c                  1 - Angle 1 (flexion/extension) bad
c                  2 - Angle 2 (abduction/adduction) bad
c                  4 - Angle 3 (internal/external) bad
c                  3 - Angles 1 and 2 bad
c                  5 - Angles 1 and 3 bad
c                  6 - Angles 2 and 3 bad
c                  7 - Angles 1, 2, and 3 bad
c
      real*4    LCS1(3,3),LCS2(3,3),angles(3)
      integer*4 error
                               
      real*4    temp_mat(3,3),rot_mat(3,3),cos2,rtod,AngSign(3)
      integer*4 i
                       
      data AngSign / 1,-1,-1 /

      error= 0                     
      rtod= 45.0/atan(1.0)
      call MatTran( LCS2, temp_mat ) ! Invert matrix
      call MatMult( temp_mat, LCS1, rot_mat ) ! now calc lcs1 relative to lcs2
      cos2= sqrt( rot_mat(2,1)**2 + rot_mat(2,2)**2 )
      if ( cos2 .lt. 1.0e-10 ) then        ! special case...
        if ( rot_mat(2,3) .lt. 0.0 ) then
          angles(1)= atan2( rot_mat(1,2),rot_mat(1,1) )
          angles(2)= atan(1.0)*2.0
        else
          angles(1)= atan2( -rot_mat(1,2),rot_mat(1,1) )
          angles(2)= -atan(1.0)*2.0
        end if
        angles(3)= 0.0
        error= 7
        return
      else
        angles(2)= atan2( -rot_mat(2,3),cos2 )
      end if
      if ( (abs(rot_mat(1,3)) .lt. 1.0e-10) .and.
     &     (abs(rot_mat(3,3)) .lt. 1.0e-10)       ) then
        angles(1)= 0.0
        error= error + 1       
      else  
        angles(1)= atan2( rot_mat(1,3)/cos2,rot_mat(3,3)/cos2 )
      end if
      if ( (abs(rot_mat(2,1)) .lt. 1.0e-10) .and.
     &     (abs(rot_mat(2,2)) .lt. 1.0e-10)       ) then
        angles(3)= 0.0
        error= error + 2
      else
        angles(3)= atan2(  rot_mat(2,1)/cos2,rot_mat(2,2)/cos2 )
      end if
c
c     NOTE that the angles are negated to make their signs correct
c     according to the conventions used by the OSU Gait Lab
c
      do i= 1, 3
        angles(i)= angles(i)*rtod !*AngSign(i)
      end do          
      return
      end                      
c
c                
c
c
c     Copyright by Dwight Meglan 1990
c
      subroutine SwivelJntAnglesOld( LCS1, LCS2, angles, error )
      implicit none                           
c
c     Purpose:
c     Special set of euler angles whose order should work with Swivel3D
c     such that the angles will allow the results of ANALYZE to be displayed
c     on the Macintosh- These are really just absolute euler angles in the
c     order y-x-z in the swivel coordinate system or floating axis euler angles
c     in the order z-y-x in the ANZ coordinate system.  See the derivation
c     notes for more detail on this confusing statement...
c
c     This is the Old attempt...
c
c     Error Codes: 0 - no error
c                  1 - Angle 1 (y) bad
c                  2 - Angle 2 (x) bad
c                  4 - Angle 3 (z) bad
c                  3 - Angles 1 and 2 bad
c                  5 - Angles 1 and 3 bad
c                  6 - Angles 2 and 3 bad
c                  7 - Angles 1, 2, and 3 bad
c
      real*4    LCS1(3,3),LCS2(3,3),angles(3)
      integer*4 error
                               
      real*4    temp_mat(3,3),rot_mat(3,3),cos2,rtod
      integer*4 i                                                         
                       
      error= 0
      rtod= 45.0/atan(1.0)
      call MatTran( LCS2, temp_mat ) ! Invert matrix
      call MatMult( temp_mat, LCS1, rot_mat ) ! now calc lcs1 relative to lcs2
      cos2= sqrt( rot_mat(3,2)**2 + rot_mat(3,3)**2 )
      if ( cos2 .lt. 1.0e-10 ) then
        if ( rot_mat(3,1) .gt. 0.0 ) then
          angles(2)=  atan(1.0)*2.0
        else
          angles(2)= -atan(1.0)*2.0
        end if
        angles(1)= 0.0
        angles(3)= atan2( -rot_mat(2,3),rot_mat(2,2) )
        error= 7
        return
      else
        angles(2)= atan2( -rot_mat(3,1),cos2 )
      end if
      if ( (abs(rot_mat(2,1)) .lt. 1.0e-10) .and.
     &     (abs(rot_mat(1,1)) .lt. 1.0e-10)       ) then
        angles(1)= 0.0
        error= error + 1       
      else  
        angles(1)= atan2( rot_mat(2,1)/cos2,rot_mat(1,1)/cos2 )
      end if
      if ( (abs(rot_mat(3,2)) .lt. 1.0e-10) .and.
     &     (abs(rot_mat(3,3)) .lt. 1.0e-10)       ) then
        angles(3)= 0.0
        error= error + 2
      else
        angles(3)= atan2( rot_mat(3,2)/cos2,rot_mat(3,3)/cos2 )
      end if
      do i= 1, 3
        angles(i)= angles(i)*rtod
      end do          
      return
      end                  
c
c                
c
c
c     Copyright by Dwight Meglan 1990
c
      subroutine SwivelJntAngles( LCS1, LCS2, angles, error )
      implicit none                           
c
c     Special set of euler angles whose order should work with Swivel3D   
c     such that the angles will allow the results of ANALYZE to be displayed
c     on the Macintosh- These are really just absolute euler angles in the
c     order z-y-x in the swivel coordinate system or floating axis euler angles
c     in the order x-y-z in the ANZ coordinate system.  See the derivation
c     notes for more detail on this confusing statement...
c
c     Finally get this to work but it is an absolute pain to export data to Swivel3D
c     to generate a movie animation on the macintosh. Gave up and moved animations
c     on the VAXStation 8000- much more satisfying results.
c
c     Error Codes: 0 - no error
c                  1 - Angle 1 (y) bad
c                  2 - Angle 2 (x) bad
c                  4 - Angle 3 (z) bad
c                  3 - Angles 1 and 2 bad
c                  5 - Angles 1 and 3 bad
c                  6 - Angles 2 and 3 bad
c                  7 - Angles 1, 2, and 3 bad
c
      real*4    LCS1(3,3),LCS2(3,3),angles(3)
      integer*4 error
                               
      real*4    temp_mat(3,3),rot_mat(3,3),cos2,rtod
      integer*4 i                                                         
                       
      error= 0
      rtod= 45.0/atan(1.0)
      call MatTran( LCS2, temp_mat ) ! Invert matrix
      call MatMult( temp_mat, LCS1, rot_mat ) ! now calc lcs1 relative to lcs2
      cos2= sqrt( rot_mat(1,1)**2 + rot_mat(1,2)**2 )
      if ( cos2 .lt. 1.0e-10 ) then
        if ( rot_mat(1,3) .gt. 0.0 ) then
          angles(2)=  atan(1.0)*2.0
        else
          angles(2)= -atan(1.0)*2.0
        end if
        angles(3)= 0.0
        angles(1)= atan2( rot_mat(3,2),rot_mat(2,2) )
        error= 7
        return
      else
        angles(2)= atan2( rot_mat(1,3),cos2 )
      end if
      if ( (abs(rot_mat(2,3)) .lt. 1.0e-10) .and.
     &     (abs(rot_mat(3,3)) .lt. 1.0e-10)       ) then
        angles(1)= 0.0
        error= error + 1       
      else  
        angles(1)= atan2( -rot_mat(2,3)/cos2,rot_mat(3,3)/cos2 )
      end if
      if ( (abs(rot_mat(1,2)) .lt. 1.0e-10) .and.
     &     (abs(rot_mat(1,1)) .lt. 1.0e-10)       ) then
        angles(3)= 0.0
        error= error + 2
      else
        angles(3)= atan2( -rot_mat(1,2)/cos2,rot_mat(1,1)/cos2 )
      end if
      do i= 1, 3
        angles(i)= angles(i)*rtod
      end do          
      angles(3)= -angles(3) ! special because of upsidedown CS in Swivel3d
      return
      end                  
c  
c
c
c
c     Copyright by Dwight Meglan 1990
c
c     Purpose: Compute the center of area a spatial triangle.
c
      subroutine Tricen( mrkr1, mrkr2, mrkr3, cntr, error )
      implicit none       

      real*4    mrkr1(3),mrkr2(3),mrkr3(3),cntr(3)
 
      real*4    pta(3),ptb(3)
      integer*4 x,y,z,i,error
 
      data x,y,z / 1, 2, 3 /

c
c     First compute midpoints along lines from markers 1 to 2 and 2 to 3
c     
      do i= x,z
        pta(i)= mrkr1(i) + ( mrkr2(i)-mrkr1(i) )/2.0
        ptb(i)= mrkr2(i) + ( mrkr3(i)-mrkr2(i) )/2.0
      end do
c
c     now find intersection of lines drawn from vertices of triangle to
c     midpoint of opposite sides.  these are the angle bisector lines
c
      call Line_Xsect( ptb, mrkr1, mrkr3, pta, cntr, error )
      return
      end

c              
c
c
c
c     Copyright by Dwight Meglan 1990
c
c     Purpose: Compute the intersection point a two lines defined by two points
c              on each line: line A- pta1 and pta2, line B- ptb1 and ptb2.
c
c
c     error # code
c       0 : Intersection calculated
c       1 : Invalid line information- pta1 & pta2 or ptb1 & ptb2 are the same
c       2 : Lines are not planar- no intersection possible
c       3 : Line are planar, but parallel
c
      subroutine Line_Xsect( pta1, pta2, ptb1, ptb2, cntr, error )
      implicit none

      real*4    pta1(3), pta2(3), ptb1(3), ptb2(3), cntr(3)

      real*4    dcosa(3),dcosb(3),maga,magb,det,slopea(3),slopeb(3),ratioa,
     &          ratiob
      integer*4 x,y,z,error,i

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

      error= 0
c
c     First calculate the direction cosines of the lines and then check
c     whether they are coplanar and can therefore intersect
c
      maga= 0.0
      magb= 0.0
      do i= 1 , 3
        slopea(i)= pta1(i) - pta2(i)
        slopeb(i)= ptb1(i) - ptb2(i)
        maga= maga + slopea(i)**2
        magb= magb + slopeb(i)**2
      end do
      maga= sqrt(maga)                         
      magb= sqrt(magb)                                             
      if ( (maga .lt. 1.0e-10) .or. (magb .lt. 1.0e-10) ) then
        error= 1
        return
      end if
      do i= 1, 3
        dcosa(i)= slopea(i)/maga
        dcosb(i)= slopeb(i)/magb
      end do
      det= ( ptb1(x)-pta1(x) )*( slopea(y)*slopeb(z)-slopeb(y)*slopea(z) ) -
     &     ( ptb1(y)-pta1(y) )*( slopea(x)*slopeb(z)-slopeb(x)*slopea(z) ) +
     &     ( ptb1(z)-pta1(z) )*( slopea(x)*slopeb(y)-slopeb(x)*slopea(y) )
      if ( abs(det) .gt. 1.0e-10 ) then
        error= 2      
        return
      end if
c
c     If we made it this far then the lines either intersect or are parallel
c     to one another.  Check for parallel condition and if not then find
c     their intersection point.
c
      if ( ( abs(abs(dcosa(x))-abs(dcosb(x))) .lt. 1.0e-10 ) .and.
     &     ( abs(abs(dcosa(y))-abs(dcosb(y))) .lt. 1.0e-10 ) .and.
     &     ( abs(abs(dcosa(z))-abs(dcosb(z))) .lt. 1.0e-10 )       ) then
        error= 3
        return
      end if
      if ( (abs(dcosa(x)) .lt. 1.0e-10) .and.
     &     (abs(dcosb(x)) .lt. 1.0e-10)       ) then ! lines are in yz plane
        cntr(x)= pta1(x)
        if ( abs(slopea(z)) .lt. 1.0e-10 ) then
          error= 4
          return
        end if
        ratioa= slopea(y)/slopea(z)
        if ( abs(slopeb(z)) .lt. 1.0e-10 ) then
          error= 4
          return
        end if
        ratiob= slopeb(y)/slopeb(z)
        if ( abs(ratiob-ratioa) .lt. 1.0e-10 ) then
          error= 4
          return
        end if
        cntr(z)= ( ratiob*ptb1(z) - ratioa*pta1(z) + pta1(y) - ptb1(y) )/
     &           ( ratiob - ratioa )
        cntr(y)= ratioa*( cntr(z)-pta1(z) ) + pta1(y)
        
      else if ( (abs(dcosa(y)) .lt. 1.0e-10) .and.
     &          (abs(dcosb(y)) .lt. 1.0e-10)       ) then ! lines are xz plane
        cntr(y)= pta1(y)
        if ( abs(slopea(z)) .lt. 1.0e-10 ) then
          error= 4
          return
        end if
        ratioa= slopea(x)/slopea(z)
        if ( abs(slopeb(z)) .lt. 1.0e-10 ) then
          error= 4
          return
        end if
        ratiob= slopeb(x)/slopeb(z)
        if ( abs(ratiob-ratioa) .lt. 1.0e-10 ) then
          error= 4
          return
        end if
        cntr(z)= ( ratiob*ptb1(z) - ratioa*pta1(z) + pta1(x) - ptb1(x) )/
     &           ( ratiob - ratioa )
        cntr(x)= ratioa*( cntr(z)-pta1(z) ) + pta1(x)
        
      else if ( (abs(dcosa(z)) .lt. 1.0e-10) .and. 
     &          (abs(dcosb(z)) .lt. 1.0e-10)       ) then ! lines are in xy plane
        cntr(z)= pta1(z)
        if ( abs(slopea(y)) .lt. 1.0e-10 ) then
          error= 4
          return
        end if
        ratioa= slopea(x)/slopea(y)
        if ( abs(slopeb(y)) .lt. 1.0e-10 ) then
          error= 4
          return
        end if
        ratiob= slopeb(x)/slopeb(y)
        if ( abs(ratiob-ratioa) .lt. 1.0e-10 ) then
          error= 4
          return
        end if
        cntr(z)= ( ratiob*ptb1(y) - ratioa*pta1(y) + pta1(x) - ptb1(x) )/
     &           ( ratiob - ratioa )
        cntr(x)= ratioa*( cntr(y)-pta1(y) ) + pta1(x)
                      
      else if ( abs(dcosa(y)) .lt. 1.0e-10 ) then ! line A is in the xz plane
        cntr(y)= pta1(y)                         ! line B is not
        if ( abs(slopeb(y)) .lt. 1.0e-10 ) then
          error= 4
          return
        end if
        cntr(x)= ( slopeb(x)/slopeb(y) )*(cntr(y)-ptb1(y)) + ptb1(x)
        if ( abs(dcosa(x)) .lt. 1.0e-10 ) then
          if ( abs(slopeb(x)) .lt. 1.0e-10 ) then
            error= 4
            return
          end if
          cntr(z)= ( slopeb(z)/slopeb(x) )*(cntr(x)-ptb1(x)) + ptb1(z)
        else
          if ( abs(slopea(x)) .lt. 1.0e-10 ) then
            error= 4
            return
          end if
          cntr(z)= ( slopea(z)/slopea(x) )*(cntr(x)-pta1(x)) + pta1(z)
        end if

      else if ( abs(dcosb(y)) .lt. 1.0e-10 ) then ! line B is in the xz plane
        cntr(y)= ptb1(y)                         ! line A is not 
        if ( abs(slopea(y)) .lt. 1.0e-10 ) then
          error= 4
          return
        end if
        cntr(x)= ( slopea(x)/slopea(y) )*(cntr(y)-pta1(y)) + pta1(x)
        if ( abs(dcosa(x)) .lt. 1.0e-10 ) then
          if ( abs(slopeb(x)) .lt. 1.0e-10 ) then
            error= 4
            return
          end if
          cntr(z)= ( slopeb(z)/slopeb(x) )*(cntr(x)-ptb1(x)) + ptb1(z)
        else
          if ( abs(slopea(x)) .lt. 1.0e-10 ) then
            error= 4
            return
          end if
          cntr(z)= ( slopea(z)/slopea(x) )*(cntr(x)-pta1(x)) + pta1(z)
        end if
        
      else ! general intersection
        if ( abs(slopea(y)) .lt. 1.0e-10 ) then
          error= 4
          return
        end if
        ratioa= slopea(x)/slopea(y)
        if ( abs(slopeb(y)) .lt. 1.0e-10 ) then
          error= 4
          return
        end if
        ratiob= slopeb(x)/slopeb(y)
        if ( abs(ratiob-ratioa) .lt. 1.0e-10 ) then
          error= 4
          return
        end if
        cntr(y)= ( ratiob*ptb1(y) - ratioa*pta1(y) + pta1(x) - ptb1(x) )/
     &           ( ratiob - ratioa )
        cntr(x)= ratioa*(cntr(y)-pta1(y)) + pta1(x)
        if ( abs(dcosa(x)) .lt. 1.0e-10 ) then
          if ( abs(slopeb(x)) .lt. 1.0e-10 ) then
            error= 4
            return
          end if
          cntr(z)= ( slopeb(z)/slopeb(x) )*(cntr(x)-ptb1(x)) + ptb1(z)
        else
          if ( abs(slopea(x)) .lt. 1.0e-10 ) then
            error= 4
            return
          end if
          cntr(z)= ( slopea(z)/slopea(x) )*(cntr(x)-pta1(x)) + pta1(z)
        end if
      end if
      return             
      end
c
c
c
c
c     Copyright by Dwight Meglan 1990
c
c     Purpose: Find point on line (NewPt) which corresponds to the minimum distance between
c              the line and another point. Line is defined by its direction (LnDir)
c              and a point on it (LnPt).
c
      subroutine PerpenPtLine( LnPt, LnDir, Pt, NewPt )
      implicit none

      real*4    LnPt(3),LnDir(3),Pt(3),NewPt(3)

      real*4    s,TempVec(3),VecDot
      integer*4 i

      call VecSub( Pt,LnPt,TempVec )
      s= VecDot(TempVec,LnDir)
      do i= 1, 3
        NewPt(i)= s*LnDir(i) + LnPt(i)
      end do
      return
      end
c
c     Copyright 1992 by Dwight Meglan
c
c     Purpose: Convert rotation matrix to equivalent euler parameters. This
c              routine is error trapped to return a false value if the
c              calculation fails...
c
c              Euler parameter storage order is:
c              e(1)= cos(theta/2)
c              e(2)= sin(theta/2)*kx
c              e(3)= sin(theta/2)*ky
c              e(4)= sin(theta/2)*kz
c              
c              where theta = equivalent angle axis rotation
c                    kx,ky,kz = direction cosines of equivalent angle axis
c
      subroutine RMtoEP( r,ep,error )
      implicit none

      real*4    r(3,3),ep(4)
      logical*4 error

      error= .true.
      if ( 1.0+r(1,1)+r(2,2)+r(3,3) .lt. 0.0 ) return
      ep(1)= 0.5*sqrt(1.0 + r(1,1) + r(2,2) + r(3,3))
      if ( ep(1) .gt. 1.0e-10 ) then ! use normal methods, angle != 0,180
        ep(2)= (r(3,2)-r(2,3))/(4.0*ep(1))
        ep(3)= (r(1,3)-r(3,1))/(4.0*ep(1))
        ep(4)= (r(2,1)-r(1,2))/(4.0*ep(1))
      else
        if ( 1.0+r(1,1)-r(2,2)-r(3,3) .gt. 1.0e-10 ) then ! use ep(2)
          ep(2)= 0.24*sqrt(1.0+r(1,1)-r(2,2)-r(3,3))
          ep(3)= 0.5*r(1,2)/ep(2)
          ep(4)= 0.5*r(1,3)/ep(2) 
        else if ( 1.0-r(1,1)+r(2,2)-r(3,3) .gt. 1.0e-10 ) then ! use ep(3)
          ep(3)= 0.24*sqrt(1.0-r(1,1)+r(2,2)-r(3,3))
          ep(2)= 0.5*r(1,2)/ep(3)
          ep(4)= 0.5*r(2,3)/ep(3)
        else if ( 1.0-r(1,1)-r(2,2)+r(3,3) .gt. 1.0e-10 ) then ! use ep(4)
          ep(4)= 0.24*sqrt(1.0-r(1,1)-r(2,2)+r(3,3))
          ep(2)= 0.5*r(1,3)/ep(4)
          ep(3)= 0.5*r(2,3)/ep(4)
        else
          return ! error...                         
        end if
      end if
      error= .false.
      return
      end
c                            
c     Convert unit quaternion (euler parameters) to rotation matrix
c
      subroutine EPtoRM( ep,r )
      implicit none

      real*4 ep(4),r(3,3)
                       
      real*4 q13,q14,q23,q12,q34,q24,q1s,q2s,q3s,q4s,q(4)

      q(1)= ep(2)
      q(2)= ep(3)
      q(3)= ep(4)
      q(4)= ep(1)
	
      q13= 2.0*q(1)*q(3)
      q14= 2.0*q(1)*q(4)    
      q23= 2.0*q(2)*q(3)
      q12= 2.0*q(1)*q(2)
      q34= 2.0*q(3)*q(4)
      q24= 2.0*q(2)*q(4)
	                          
      q1s= 2.0*q(1)*q(1)
      q2s= 2.0*q(2)*q(2)
      q3s= 2.0*q(3)*q(3)
		
      r(1,1)= 1.0 - q2s - q3s   ! row 1 
      r(1,2)= q12 - q34
      r(1,3)= q13 + q24

      r(2,1)= q12 + q34			   ! row 2
      r(2,2)= 1.0 - q1s - q3s
      r(2,3)= q23 - q14

      r(3,1)= q13 - q24			   ! row 3
      r(3,2)= q23 + q14
      r(3,3)= 1.0 - q1s - q2s
      
      return
      end
c
c     3x3 Matrix determinate
c
      function det3(m)
      implicit none
 
      real*4   det3,m(3,3)

      det3= m(1,1)*(m(2,2)*m(3,3)-m(2,3)*m(3,2))
     &    - m(1,2)*(m(2,1)*m(3,3)-m(3,1)*m(2,3))
     &    + m(1,3)*(m(2,1)*m(3,2)-m(3,1)*m(2,2))
      return
      end
c
c
c
      subroutine adjoint3x3(g,gad)
      implicit none

      real*4 g(3,3),gad(3,3)

      gad(1,1)= g(2,2)*g(3,3) - g(3,2)*g(2,3)
      gad(2,1)= g(3,2)*g(1,3) - g(1,2)*g(3,3)
      gad(3,1)= g(1,2)*g(2,3) - g(2,2)*g(1,3)
                           
      gad(1,2)= g(2,3)*g(3,1) - g(3,3)*g(2,1)
      gad(2,2)= g(3,3)*g(1,1) - g(1,3)*g(3,1)
      gad(3,2)= g(1,3)*g(2,1) - g(2,3)*g(1,1)

      gad(1,3)= g(2,1)*g(3,2) - g(3,1)*g(2,2)
      gad(2,3)= g(3,1)*g(1,2) - g(1,1)*g(3,2)
      gad(3,3)= g(1,1)*g(2,2) - g(2,1)*g(1,2)
      return
      end
c
c     Convert a rotation by an angle, a, about a unit vector, k,
c     into a rotation matrix, r
c
c     k - direction cosines (unit vector) of axis of rotation
c     a - angle of rotation about axis (rad)
c     r - 3x3 rotation matrix
c
      subroutine eaa_rm( k,a,r )
      implicit none

      real*4    k(3),a,r(3,3)

      real*4    ca,sa,va,kx,ky,kz

      ca= cos(a)
      sa= sin(a)
      va= 1.0 - ca
      kx= k(1)
      ky= k(2)
      kz= k(3)
	                              
      r(1,1)= kx*kx*va + ca
      r(1,2)= kx*ky*va - kz*sa
      r(1,3)= kx*kz*va + ky*sa
	
      r(2,1)= kx*ky*va + kz*sa
      r(2,2)= ky*ky*va + ca
      r(2,3)= ky*kz*va - kx*sa

      r(3,1)= kx*kz*va - ky*sa
      r(3,2)= ky*kz*va + kx*sa
      r(3,3)= kz*kz*va + ca
      return
      end
c
c
c
      subroutine eafxyz_rm( ea,r )
      implicit none

      real*4   ea(3),r(3,3)

      real*4   s1,s2,s3,c1,c2,c3

      s1= sin(ea(1))
      c1= cos(ea(1))
      s2= sin(ea(2))
      c2= cos(ea(2))
      s3= sin(ea(3))
      c3= cos(ea(3))
      r(1,1)= c3*c2
      r(1,2)= c3*s2*s1 - s3*c1
      r(1,3)= c3*s2*c1 + s3*s1
      r(2,1)= s3*c2
      r(2,2)= s3*s2*s1 + c3*c1
      r(2,3)= s3*s2*c1 - c3*s1
      r(3,1)= -s2
      r(3,2)= c2*s1
      r(3,3)= c2*c1
      return
      end
c
c                        
c     Copyright by Dwight Meglan 1992
c
      function rm_eafxyz( rm,ea )
      implicit none
c             
c     Error Codes: 0 - no error
c                  1 - Angle 1 bad
c                  2 - Angle 2 bad
c                  4 - Angle 3 bad
c                  3 - Angles 1 and 2 bad
c                  5 - Angles 1 and 3 bad
c                  6 - Angles 2 and 3 bad
c                  7 - Angles 1, 2, and 3 bad
c
      real*4    ea(3),rm(3,3)
      integer*4 rm_eafxyz
                               
      real*4    cos2
                       
      rm_eafxyz= 0
      cos2= sqrt( rm(1,1)**2 + rm(2,1)**2 )
      if ( cos2 .lt. 1.0e-10 ) then        ! special case...
        if ( rm(3,1) .lt. 0.0 ) then
          ea(1)= atan2( rm(1,2),rm(2,2) )
          ea(2)= atan(1.0)*2.0
        else
          ea(1)= atan2( -rm(1,2),rm(2,2) )
          ea(2)= -atan(1.0)*2.0
        end if
        ea(3)= 0.0
        rm_eafxyz= 7
        return
      else
        ea(2)= atan2( -rm(3,1),cos2 )
      end if
      if ( (abs(rm(3,2)) .lt. 1.0e-10) .and.
     &     (abs(rm(3,3)) .lt. 1.0e-10)       ) then
        ea(1)= 0.0
        rm_eafxyz= rm_eafxyz + 1       
      else  
        ea(1)= atan2( rm(3,2)/cos2,rm(3,3)/cos2 )
      end if
      if ( (abs(rm(2,1)) .lt. 1.0e-10) .and.
     &     (abs(rm(1,1)) .lt. 1.0e-10)       ) then
        ea(3)= 0.0
        rm_eafxyz= rm_eafxyz + 2
      else
        ea(3)= atan2(  rm(2,1)/cos2,rm(1,1)/cos2 )
      end if
      return
      end                      
c
c
c
      subroutine avgvec( nfrm,mpn1,mpn2,avec )
      implicit none

      real*4    mpn1(3,*),mpn2(3,*),avec(3)
      integer*4 nfrm,i,j

      do i= 1, 3
        avec(i)= 0.0
        do j= 1, nfrm
          avec(i)= avec(i) + mpn1(i,j) - mpn2(i,j)
        end do
        avec(i)= avec(i)/float(nfrm)
      end do
      return            
      end
c
c
c
      subroutine avgmidpnt( nfrm,mpn1,mpn2,apnt )
      implicit none

      real*4    mpn1(3,*),mpn2(3,*),apnt(3)
      integer*4 nfrm,i,j

      do i= 1, 3
        apnt(i)= 0.0
        do j= 1, nfrm
          apnt(i)= apnt(i) + mpn1(i,j) + mpn2(i,j)
        end do
        apnt(i)= 0.5*apnt(i)/float(nfrm)
      end do
      return            
      end
