c
c
c	Spoor and Veldpaus algorithm to compute the relative change in
c	rotation and position of a rigid body for which more than 3 points
c	have known before and after locations known
c	
c	Return values of spvp
c	
c	0		Normal execution
c  ERR_RB_RANK Rank of mm matrix is less than 3, meaning a bad marker topology
c	ERR_RB_EVL  A 0 eigenvalue was computed indicating a bad marker topology ?
c
c	Implemented by Dwight Meglan, Sept. 13, 1991
c
      function spvp( p1,p2,np,pn1,pn2,tf,emx,emn,eav,esd )
      implicit none  

      include 'rbody.inc'
                                                         
      integer*4 spvp,np,pn1(*),pn2(*)
      real*4    p1(3,*),p2(3,*),tf(3,4),emx,emn,eav,esd

      real*4    c1(3),c2(3),m(3,3),mv(3,2),md(3,3),sum,
     &          mm(3,3),dif(3),evl(3),evc(3,3),m1(3),m2(3),m3(3),det3
      integer*4 i,j,k,eigen3x3,ierr
 
c
c     Error trap for too many markers
c          
      if ( np .gt. MAX_RBMRK ) then
        spvp= ERR_RB_NP
        return
      else if ( np .lt. 3 ) then
        spvp= ERR_RB_3PT
        return
      end if
c
c     Compute average of p1 and p2 */
c
      do j= 1, 3
        c1(j)= 0.0
        c2(j)= 0.0
        do i= 1, np 
          c1(j)= c1(j) + p1(j,pn1(i))
          c2(j)= c2(j) + p2(j,pn2(i))
        end do
        c1(j)= c1(j)/np
        c2(j)= c2(j)/np
      end do
c
c     Compute M matrix
c
      do i= 1, 3
        do j= 1, 3
          m(i,j)= 0.0
          do k=1, np
            m(i,j)= m(i,j) + p2(i,pn2(k))*p1(j,pn1(k))
          end do
          m(i,j)= m(i,j)/np - c2(i)*c1(j)
        end do
      end do
c
c     Compute MtM: NOTE that the order of array subscripting is VERY
c     important in the inner loop since m transpose is being used here.
c     Also, only part of matrix is computed outright since it is symmetric 
c
      do i= 1, 3
        do j= 1, i
          mm(j,i)= 0.0
          do k= 1, 3
            mm(j,i)= mm(j,i) + m(k,i)*m(k,j)
          end do
          if ( i .ne. j ) mm(i,j)= mm(j,i)
        end do
      end do
c
c     Rank check of mm so that invalid marker configurations
c     will detected (less than 3 or colinear) and return value
c     indicating an error sent back
c                                               
c      if ( abs(det3(mm)) .lt. RB_TINY ) then
c        spvp= ERR_RB_RANK
c        return
c      end if
c
c     Compute eigenvalues and eigenvectors
c
      ierr= eigen3x3( mm,evc,evl )
      if ( ierr .lt. 0 ) then
        spvp= ERR_RB_ED0
        return
      end if                       
      call eigsrtd( evl,evc,3 )
c
c     Error trap for 0.0 eigenvalues
c
      do i= 1, 3
        if ( abs(evl(i)) .lt. RB_TINY ) then
c          spvp= ERR_RB_EV
c          return
           evl(i)= RB_TINY
        end if
        if ( evl(i) .lt. 0.0 ) evl(i)= -evl(i)
      end do
c
c     Compute m1 and m2 vectors- called mv here
c     NOTE:	the storage order of mv is important. It is mandatory
c           for the call to vxprod to calculate the vector cross 
c           product correctly
c
      do i= 1, 2
        do j= 1, 3
          mv(j,i)= 0.0
          do k= 1, 3
            mv(j,i)= mv(j,i) + m(j,k)*evc(k,i)
          end do
        end do
      end do
c
c     Assemble Rotation matrix
c     NOTE the indexing of v so that the matrix multiplication of
c     md*vt in the last line is correct- vt is v transpose 
c
      do j= 1, 3
        m1(j)= mv(j,1)
        m2(j)= mv(j,2)
        do i= 1, 2
          md(j,i)= mv(j,i)/sqrt(evl(i))
        end do
      end do
      call VecX( m1,m2,m3 )
      do j= 1, 3
        md(j,3)= m3(j) / (sqrt(evl(1))*sqrt(evl(2)))
      end do
c
c     Before final calculation of the rotation matrix, recompute the last
c     eigenvector from the cross product of the first two eigenvectors. 
c     This should already be the case but in testing I found that fo some
c     cases it comes out of the eigen3x3 routine with the negative of what
c     it should be. This extra procedure is also in the Spoor-Veldpaus 
c     implementation of Tom Crippen.
c
      call VecX( evc(1,1),evc(1,2),evc(1,3) )
      do i= 1, 3
        do j= 1, 3
          tf(i,j)= 0.0
          do k= 1, 3
            tf(i,j)= tf(i,j) + md(i,k)*evc(j,k)
          end do
        end do
      end do
c
c     Compute displacement
c
      do i= 1, 3
        sum= 0.0
        do j= 1, 3
          sum= sum + tf(i,j)*c1(j)
        end do
        tf(i,4)= c2(i) - sum
      end do  
c
c     Compute Error Estimates of recomputing 2nd position points from 1st 
c     position points and the calculated rotation and translation of the body
c
      call rb_est_err(p1,p2,np,pn1,pn2,tf,emx,emn,eav,esd)
      spvp= RB_OK
      return
      end                                                             
c
c     Shell for Herman Woltring's DISP3D routine
c
      function disp3ds( p1,p2,np,pn1,pn2,tf,emx,emn,eav,esd,sf )
      implicit none  

      integer*4 disp3ds,np,pn1(*),pn2(*)
      real*4    p1(3,*),p2(3,*),tf(3,4),emx,emn,eav,esd,sf

      real*4    x(3,10),y(3,10),r(3,3),p(3)
      integer*4 i,j,ierr 

      include 'rbody.inc'
c
c     Error trap for too many or too few markers
c          
      if ( np .gt. MAX_RBMRK ) then
        disp3ds= ERR_RB_NP
        return
      else if ( np .lt. 3 ) then
        disp3ds= ERR_RB_3PT
        return
      end if

      do i= 1, np
        do j= 1, 3
          x(j,i)= p1(j,pn1(i))
          y(j,i)= p2(j,pn2(i))
        end do
      end do
      sf= -1.0

      call disp3d( x,y,np,p,r,sf,ierr )

      if ( ierr .lt. 0 ) then ! error
        if ( ierr .eq. -1 ) then
          disp3ds= ERR_RB_DSP1
          return
        else if ( ierr .eq. -2 ) then
          disp3ds= ERR_RB_DSP2
          return
        else if ( ierr .eq. -3 ) then
          disp3ds= ERR_RB_DSP3
          return
        else if ( ierr .eq. -4 ) then
          disp3ds= ERR_RB_DSP4
          return
        else if ( ierr .eq. -5 ) then
          disp3ds= ERR_RB_DSP5
          return
        end if
      end if

      do i= 1, 3
        do j= 1, 3
          tf(i,j)= r(i,j)
        end do
        tf(i,4)= p(i)
      end do
c
c     Compute Error Estimates of recomputing 2nd position points from 1st 
c     position points and the calculated rotation and translation of the body
c
      call rb_est_err(p1,p2,np,pn1,pn2,tf,emx,emn,eav,esd)
      disp3ds= RB_OK
      return
      end                                                             
C
C******************************************************************************
C
C DISP3D.FOR, Woltring 1983-02-06 (text expanded 1991-07-16)
C
C PURPOSE:
C *******      
C
C       to calculate the translation vector, pure rotation matrix, and optional
C       scaling factor in the 3-D equiform transformation, based upon observed
C       coordinates Y in the second coordinate system of at least three non-co-
C       linear markers with known coordinates X in the first coordinate system.
C
C MATHEMATHICAL MODEL:
C *******************
C
C       Y(I) + V(I) = S * R * X(I) + P;  I = 1,...,N>=3.
C
C       with: X(I) The 3-D coordinate vector of the I-th marker with respect
C                  to the first coordinate system
C             Y(I) the 3-D coordinate vector of the I-th marker as observed
C                  in the second coordinate system
C             V(I) error term in the observed Y(I), modeled as additive
C             S    Optional scale factor between the two coordinate systems
C             R    Pure rotation matrix between the two coordinate systems
C             P    Translation vector of the origin of the second system
C                  with respect to the first system
C
C DESCRIPTION:
C ***********
C
C       For the given X(I) and Y(I), those values for P, R, and possibly S
C       are estimated for which the sum of squared residuals SUM[V(I)'V(I)]
C       is minimized. If S is nonpositive on input, the optimal value for S
C       is estimated; if S is positive on input, this value is used. On out-
C       put, DET(R) = +1, and the sign of S reflects the similarity between
C       the two coordinate systems. For coplanar distributions in X and/or
C       Y, S is taken > 0. If S = 0.0 on output, either the X- or Y-coordi-
C       nates, or both, ar colinear, signifying algorithm failure.
C
C CALLING CONVENTION:
C ******************
C
C       CALL DISP3D(X,Y,N,P,R,S)
C
C MEANING OF PARAMETERS:
C *********************
C
C       X(3,N) - Input array containing known coordinates w.r.t. the first
C                coordinate system
C       Y(3,N) - Input array containing observed coordinates w.r.t. the
C                second coordinate system
C       N      - Input variable: number of marker coordinates >= 3
C       P(3)   - Output array containing the translation vector of the
C                first coordinate system's origin w.r.t. the second
C                coordinate system
C       R(3,3) - Output array containing the rotation matrix relating the
C                first coordinate system to the second coordinate system
C       S      - Input/output variable. If nonpositive on input, the
C                estimated scaling factor is returned; if positive on
C                input, its value is left unmodified and used as an a
C                priori scaling factor. If a zero value is returned,
C                either the x- or y-coordinates, or both, are colinear.
C
C
C REFERENCES:
C **********
C
C       (1) Tienstra, M. (1969), A method for the calculation of orthogonal
C           rotation matrices and its application in photogrammetry and other
C           disciplines. Ph.D.-thesis, Technical University of Delft, The
C           Netherlands. Waltman, Delft.
C
C       (2) Woltring, H.J. (1981), On definition and calculus of rigid-body
C           kinematics using spatial marker coordinates. Submitted for publi-
C           cation to Journal of Biomechanics. [Ultimately published as Veld-
C           paus et al., Journal of Biomechanics, January 1988.]
C
C******************************************************************************
C
        SUBROUTINE DISP3D(X,Y,N,P,R,S,IERR)
C
        IMPLICIT REAL*8 (D), LOGICAL (L)
        DIMENSION X(3,N),Y(3,N),P(3),R(3,3),AVX(3),
     1  DT(3,3),DD(3,3),DTADJ(3,3)
        ICYCL(I) = MOD(I,3) + 1
        DSQ(D) = D * D
C              
C***    Calculate mean values X0, Y0, and DT matrix
C
        IF (N.LT.3) GO TO 200
        DO 20 I=1,3
           DSX = 0D0
           DSY = 0D0
           DO 10 K=1,N
              DSX = DSX + X(I,K)
              DSY = DSY + Y(I,K)
   10      CONTINUE
           AVX(I) = DSX/N       !X0
           P(I)   = DSY/N       !Y0
   20   CONTINUE
C       DSS = TRACE ( [X(I) - X0].[X(I) - X0]' )
        DSS = 0.0
        DO 40 I=1,3
           DSX = AVX(I)
           DO 30 K=1,N
              DSS = DSS + DSQ(X(I,K) - DSX)
   30      CONTINUE
   40   CONTINUE
        IF (DSS.EQ.0D0) GO TO 210
C       DT = [Y(I) - Y0].[X(I) - X0]' / DSS
        DO 70 I=1,3
           DSX = AVX(I)
           DO 60 J=1,3
              DSY = P(J)
              D1 = 0D0
              DO 50 K=1,N
                 D1 = D1 + (Y(J,K) - DSY) * (X(I,K) - DSX)
   50         CONTINUE
              DT(J,I) = D1 / DSS
   60      CONTINUE
   70   CONTINUE
C
C***    Calculate DD, DT ADJOINT, and principal minor sums
C
C       DD = DT'DT
        DO 100 I=1,3
           DO 90 J=1,I
              DSS = 0D0
              DO 80 K=1,3
                 DSS = DSS + DT(K,I)*DT(K,J)
   80         CONTINUE
              DD(I,J) = DSS
              IF (I.NE.J) DD(J,I) = DSS
   90      CONTINUE
  100   CONTINUE
C       DTADJ and principal minor sums D1, D2, D3
        D1 = DD(1,1) + DD(2,2) + DD(3,3)
        D2 = 0D0
        DO 120 I1=1,3
           J1 = ICYCL(I1)
           K1 = ICYCL(J1)
           DO 110 I2=1,3
              J2 = ICYCL(I2)
              K2 = ICYCL(J2)
              DTADJ(I2,I1) = DT(J1,J2)*DT(K1,K2) - DT(J1,K2)*DT(K1,J2)
  110      CONTINUE
           D2 = D2 + (DD(J1,J1)*DD(K1,K1) - DD(J1,K1)*DD(K1,J1))
  120   CONTINUE
        D2 = 4 * D2     !times 4
        DETT = 0D0
        IF (N.GT.3)  DETT =
     1  DT(1,1)*DTADJ(1,1) + DT(1,2)*DTADJ(2,1) + DT(1,3)*DTADJ(3,1)
        D3SQ = 8 * DABS(DETT)   !8 times square root
C
C***    Calculate T-value and S if required
C
        DTO = 1D0
        IF (S.NE.0.0) DTO = ABS(S)
        DO 130 I=1,25
           DTN = D2 + D3SQ*DTO
           IF (DTN.LT.0D0) DTN = 0D0
           DTN = DSQRT(D1 + DSQRT(DTN))
           IF (DABS(DTN-DTO).LE.1D-15*DABS(DTO)) GO TO 140
           DTO = DTN
  130   CONTINUE
        GO TO 220
  140   IF (DTN.EQ.0D0) GO TO 230
        IF (S.LE.0.0) S = DTN
        S = SIGN(S,SNGL(DETT))
D       TYPE 700, I,DTN
D 700   FORMAT(' ***DEBUG DISP3D: ITER =',I3,', DTN =',1PD20.13)
C
C***    Calculate rotation matrix R
C
        DSS = (D1 + DTN*DTN) / 2
        DO 150 I=1,3
           DD(I,I) = DD(I,I) - DSS
  150   CONTINUE
        DSS = DABS(DETT) + DTN*(D1 - DSS)
        IF (DSS.EQ.0D0) GO TO 240
        DO 170 I=1,3
           DO 160 J=1,3
            R(I,J) = ( DT(I,1)*DD(1,J)+DT(I,2)*DD(2,J)+DT(I,3)*DD(3,J)
     1         - DTN*DTADJ(J,I) ) / DSS
  160      CONTINUE
  170   CONTINUE
C
C***    Calculate translation vector P
C
        DO 190 I=1,3
           DSS = 0D0
           DO 180 J=1,3
              DSS = DSS + R(I,J) * DBLE(AVX(J))
  180      CONTINUE
           P(I) = P(I) - S * DSS
  190   CONTINUE
C
C***    Ready
C
        ierr= 0
        RETURN

  200   S = 0.0
        ierr= -1
        RETURN

  210   S = 0.0
        ierr= -2
        RETURN

  220   S = 0.0
        ierr= -3
        RETURN

  230   S = 0.0
        ierr= -4
        RETURN

  240   S = 0.0
        ierr= -5
        RETURN
        END
c
c    Estimate the difference between the measured marker position in location 2
c    of the rigid body and those computed from the first location and the evaluated
c    translation and rotation of the body.
c
      subroutine rb_est_err(p1,p2,np,pn1,pn2,tf,emx,emn,eav,esd)
      implicit none

      include 'rbody.inc'

      real*4    p1(3,1),p2(3,1),tf(3,4),emx,emn,eav,esd
      integer*4 np,pn1(1),pn2(1)

      real*4    c1(3),sum,v1(3),v2(3),dif(3),mag_dif(MAX_RBMRK),VecMag
      integer*4 i,j

      do i= 1, 3
        sum= 0.0
        do j= 1, np
          sum= sum + p1(i,pn1(j))
        end do
        c1(i)= sum/np
      end do
      emx= 0.0
      emn= 0.0
      eav= 0.0
      esd= 0.0
      do i= 1, np
        do j= 1, 3
          v1(j)= p1(j,pn1(i)) - c1(j)
        end do
        call VecChangeCS( tf,v1,v2 )
        do j= 1, 3
          dif(j)= v2(j) + tf(j,4) + c1(j) - p2(j,pn2(i))
        end do
        mag_dif(i)= VecMag(dif)
        eav= eav + mag_dif(i)
        if ( mag_dif(i) .gt. emx ) emx= mag_dif(i)
        if ( mag_dif(i) .lt. emn ) emn= mag_dif(i)
      end do
      eav= eav/np
      do i= 1, np
        esd= esd + (mag_dif(i)-eav)**2
      end do
      esd= sqrt(esd)/(np-1)
      return
      end
c
c
c
      subroutine handle_rb_err( errnum,np,bn,frm )
      implicit none

      integer*4 errnum,np,bn,frm

      include 'rbody.inc'
c
c     Error conditions common to all rigid body routines
c
      if ( errnum .eq. ERR_RB_NP ) then
        write(6,100) bn,np,MAX_RBMRK
100     format(' Error- The number of markers on body # ',i2,' is ',i2/
     &         '        but the maximum allowed is ',i2)
      else if ( errnum .eq. ERR_RB_3PT ) then
        write(6,105) bn,np
105     format(' Error- The number of markers on body # ',i2,' is ',i2/
     &         '        but there must be at least 3 markers present')
c                         
c     Schuta error conditions
c
      else if ( errnum .eq. ERR_RB_GE_S1 ) then
        write(6,110) bn,frm
110     format(' Error- The schuta routine failed on body # ',i2,' at frame # ',i3/
     &         '        because column # 1 of the matrix being solved with'/
     &         '        was singular')

      else if ( errnum .eq. ERR_RB_GE_S2 ) then
        write(6,120) bn,frm
120     format(' Error- The schuta routine failed on body # ',i2,' at frame # ',i3/
     &         '        because column # 2 of the matrix being solved with'/
     &         '        was singular')

      else if ( errnum .eq. ERR_RB_GE_S3 ) then
        write(6,130) bn,frm
130     format(' Error- The schuta routine failed on body # ',i2,' at frame # ',i3/
     &         '        because column # 3 of the matrix being solved with'/
     &         '        was singular')

      else if ( errnum .eq. ERR_RB_Q_RM ) then
        write(6,140) bn,frm
140     format(' Error- The schuta routine failed on body # ',i2,' at frame # ',i3/
     &         '        because the quaternion could not be transformed into'/
     &         '        a rotation matrix')

      else if ( errnum .eq. ERR_RB_P1_VN ) then
        write(6,150) bn,frm
150     format(' Error- The schuta routine failed on body # ',i2,' at frame # ',i3/
     &         '        because the vector normalization of the first body'/
     &         '        vectors failed')

      else if ( errnum .eq. ERR_RB_P2_VN ) then
        write(6,160) bn,frm
160     format(' Error- The schuta routine failed on body # ',i2,' at frame # ',i3/
     &         '        because the vector normalization of the second body'/
     &         '        vectors failed')


      else if ( errnum .eq. ERR_RB_RANK ) then
        write(6,170) bn,frm
170     format(' Error- The spvp routine failed on body # ',i2,' at frame # ',i3/
     &         '        because the mm matrix is singular')

      else if ( errnum .eq. ERR_RB_ED0 ) then
        write(6,180) bn,frm
180     format(' Error- The spvp routine failed on body # ',i2,' at frame # ',i3/
     &         '        while calculating the eigenvectors of the mm matrix')

      else if ( errnum .eq. ERR_RB_EV ) then
        write(6,190) bn,frm
190     format(' Error- The spvp routine failed on body # ',i2,' at frame # ',i3/
     &         '        because an eigenvalue of the mm matrix was very small')

      else if ( errnum .eq. ERR_RB_MLIN ) then
        write(6,200) bn,frm
200     format(' Error- The poldec routine failed on body # ',i2,' at frame # ',i3/
     &         '        because the marker distribution is colinear')

      else if ( errnum .eq. ERR_RB_NCNV ) then
        write(6,210) bn,frm
210     format(' Error- The poldec routine failed on body # ',i2,' at frame # ',i3/
     &         '        because the iteration to find beta1 and beta2 did not',
     &                  ' converge')

      else if ( errnum .eq. ERR_RB_MINV ) then
        write(6,220) bn,frm
220     format(' Error- The poldec routine failed on body # ',i2,' at frame # ',i3/
     &         '        because the c matrix could not be inverted')

      else if ( errnum .eq. ERR_RB_SVD ) then
        write(6,230) bn,frm
230     format(' Error- The wolsvd routine failed on body # ',i2,' at frame # ',i3/
     &         '        because of an error in the SVD routine')

      else if ( errnum .eq. ERR_RB_MLINW ) then
        write(6,240) bn,frm
240     format(' Error- The wolsvd routine failed on body # ',i2,' at frame # ',i3/
     &         '        because the marker distribution is colinear')

      else if ( errnum .eq. ERR_RB_DSP1 ) then
        write(6,250) bn,frm
250     format(' Error- The disp3d routine failure type 1 on body # ',i2,
     &         ' at frame # ',i3)

      else if ( errnum .eq. ERR_RB_DSP2 ) then
        write(6,260) bn,frm
260     format(' Error- The disp3d routine failure type 2 on body # ',i2,
     &         ' at frame # ',i3)
            
      else if ( errnum .eq. ERR_RB_DSP3 ) then
        write(6,270) bn,frm
270     format(' Error- The disp3d routine failure type 3 on body # ',i2,
     &         ' at frame # ',i3)

      else if ( errnum .eq. ERR_RB_DSP4 ) then
        write(6,280) bn,frm
280     format(' Error- The disp3d routine failure type 4 on body # ',i2,
     &         ' at frame # ',i3)

      else if ( errnum .eq. ERR_RB_DSP5 ) then
        write(6,290) bn,frm
290     format(' Error- The disp3d routine failure type 5 on body # ',i2,
     &         ' at frame # ',i3)
      end if

      return
      end   
c
c**	compute the eigen values of the 3 X 3 matrix in mat
c**	return the vectors in vct and the values in val
c**	this routine is sort of like what is done in numerical recipes in C
c**	except for it is only for 3 X 3 matrices, and it eliminates the
c**	largest element like the original jacobi routine.  (There are only
c**	3 elements to look at so it isn't too hard to do.
c
c

      function eigen3x3( mat,vct,val )
      implicit none

      integer*4 eigen3x3
      real*4    mat(3,3),vct(3,3),val(3)

      integer*4 i,j,maxr,maxc,nrot
      real*4    tmp,tmp2,max,x
      real*8    t,theta,tau,sphi,cphi
                     
c
c     initialize the eigen vectors to an identity matrix
c
      eigen3x3= 0
      do i= 1, 3
        do j= 1, 3
          vct(i,j)= 0.0
        end do
        vct(i,i)= 1.0
        val(i)= mat(i,i)
      end do
c
c     Start iteration
c
      nrot= 0
      max= 1.0
      do while ( max .gt. 1.0e-9 )
        max= 0.0
        do i= 1, 2
          do j= i+1, 3
            x= abs(mat(i,j))
            if ( x .gt. max ) then
              max= x
              maxr= i
              maxc= j
            end if
          end do
        end do
c                    
c       check if this value is really small
c
        if ( max .lt. 1.0e-9 ) then
          eigen3x3= nrot
          return
        end if
c
c       if not, then we will zero this row and column
c
        if ( abs(mat(maxr,maxc)) .lt. 1.0e-18 ) then
          eigen3x3= -1
          return
        end if
        theta= 0.5*(val(maxc)-val(maxr))/mat(maxr,maxc)
        t= 1.0/(abs(theta)+sqrt(theta*theta+1.0))
        if ( theta .lt. 0.0 ) t= -t
        cphi= 1.0/sqrt(t*t+1.0)
        sphi= t*cphi
        tau= sphi/(1.0+cphi)
        tmp= t*mat(maxr,maxc)
        val(maxr)= val(maxr) - tmp
        val(maxc)= val(maxc) + tmp
        mat(maxr,maxc)= 0.0
        if ( maxr .gt. 1 ) then
          tmp=  mat(1,2)
          tmp2= mat(1,3)
          mat(1,2)= tmp -  sphi*(tmp2+tau*tmp)
          mat(1,3)= tmp2 + sphi*(tmp-tau*tmp2)
        else if ( maxc .eq. 2 ) then
          tmp=  mat(1,3)
          tmp2= mat(2,3)
          mat(1,3)= tmp  - sphi*(tmp2+tau*tmp)
          mat(2,3)= tmp2 + sphi*(tmp-tau*tmp2)
        else
          tmp=  mat(1,2)
          tmp2= mat(2,3)
          mat(1,2)= tmp  - sphi*(tmp2+tau*tmp)
          mat(2,3)= tmp2 + sphi*(tmp-tau*tmp2)
        end if
c
c       calculate the eigenvectors
c
        do i= 1, 3
          tmp= vct(i,maxr)
          tmp2= vct(i,maxc)
          vct(i,maxr)= tmp  - sphi*(tmp2+tau*tmp)
          vct(i,maxc)= tmp2 + sphi*(tmp-tau*tmp2)
        end do
        nrot= nrot + 1
      end do     
      eigen3x3= nrot
      return
      end
c
c     Sort a list of eigenvectors so that the eigenvalues are decreasing
c                           
      subroutine eigsrtd( val,vct,n )
      implicit none

      integer*4 n
      real*4    val(n),vct(n,n)

      integer*4 i,j,k
      real*4    tmp,max

      do i= 1, n - 1
        k= i
        max= val(k)
        do j= i+1, n
          if ( val(j) .gt. max ) then
            k= j     
            max= val(k)
          end if
        end do        
        if ( k .ne. i ) then
          tmp= val(i)
          val(i)= val(k)
          val(k)= tmp
          do j= 1, n
            tmp= vct(j,i)
            vct(j,i)= vct(j,k)
            vct(j,k)= tmp
          end do
        end if
      end do
      return
      end
c
c     Sort a list of eigenvectors so that the eigenvalues are increasing
c
      subroutine eigsrti( val,vct,n )
      implicit none

      integer*4 n
      real*4    val(n),vct(n,n)

      integer*4 i,j,k
      real*4    tmp,min
                
      do i= 1, n - 1
        k= i
        min= val(k)
        do j= i+1, n
          if ( val(j) .lt. min ) then
            k= j     
            min= val(k)
          end if
        end do        
        if ( k .ne. i ) then
          tmp= val(i)
          val(i)= val(k)
          val(k)= tmp
          do j= 1, n
            tmp= vct(j,i)
            vct(j,i)= vct(j,k)
            vct(j,k)= tmp
          end do
        end if
      end do
      return
      end
