************************************************************************
*   FUNCTION:   Routines for solving linear systems of equations.      *
*   AUTHORS:    Lawrence Shampine, Richard Allen, Steven Pruess  for   *
*               the text  Fundamentals of Numerical Computing          *
*   DATE:       February 7, 1996                                       *
************************************************************************
      SUBROUTINE FACTOR(A,MAXROW,NEQ,COND,PVTIDX,FLAG,TEMP)
C
      INTEGER MAXROW,NEQ,PVTIDX(*),FLAG
      DOUBLE PRECISION A(MAXROW,*),COND,TEMP(*)
C
C  FACTOR decomposes the matrix A using Gaussian elimination
C  and estimates its condition number.  FACTOR may be used in
C  conjunction with SOLVE to solve A*x=b.
C
C  Input variables:
C     A      = matrix to be triangularized.
C     MAXROW = maximum number of equations allowed; the declared row 
C              dimension of A.
C     NEQ    = actual number of equations to be solved;  NEQ cannot
C              exceed MAXROW.
C  Output variables:
C     A      = the upper triangular matrix U in its upper portion
C              and a permuted version of a lower triangular matrix
C              I-L such that (permutation matrix)*A = L*U; a
C              record of interchanges is kept in PVTIDX.
C     FLAG   = an integer variable that reports whether or not the
C              matrix A has a zero pivot.  A value of FLAG = 0 
C              means all pivots were nonzero;  if positive, the
C              first zero pivot occurred at equation FLAG and the
C              decomposition could not be completed.  If FLAG = -1
C              then there is an input error (NEQ or MAXROW not positive
C              or NEQ > MAXROW).
C     COND   = an estimate of the condition number of A (unless 
C              FLAG is nonzero).
C     PVTIDX = the pivot vector which keeps track of row inter-
C              changes; also, 
C                    PVTIDX(NEQ) = (-1)**(number of interchanges).
C     TEMP   = a vector of dimension NEQ used for a work area.
C
C  The determinant of A can be obtained on output from
C     DET(A) = PVTIDX(NEQ) * A(1,1) * A(2,2) * ... * A(NEQ,NEQ).
C
C  Declare local variables and initialize:
      DOUBLE PRECISION ANORM,DNORM,T,YNORM
      INTEGER I,J,K,M
      DOUBLE PRECISION ZERO,ONE
      DATA ZERO/0.D0/,ONE/1.D0/
C
      IF ((NEQ .LE. 0) .OR. (MAXROW .LE. 0) .OR. (NEQ .GT. MAXROW)) THEN
         FLAG = -1
         RETURN
      ENDIF
      FLAG = 0
      COND = ZERO
      PVTIDX(NEQ) = 1
      IF (NEQ .EQ. 1) THEN
C
C        NEQ = 1 is a special case.
C
         IF (A(1,1) .EQ. ZERO) THEN
            FLAG = 1
         ELSE
            COND = ONE
         ENDIF
         RETURN
      ENDIF
C
C     Compute infinity-norm of A for later condition number estimation.
C
      ANORM = ZERO
      DO 15 I = 1,NEQ
         T = ZERO
         DO 10 J = 1,NEQ
            T = T+ABS(A(I,J))
   10    CONTINUE
         ANORM = MAX(T,ANORM)
   15 CONTINUE
C
C     Gaussian elimination with partial pivoting.
C
      DO 40 K = 1,NEQ-1
C
C        Determine the row M containing the largest element in 
C        magnitude to be used as a pivot.
C
         M = K
         DO 20 I = K+1,NEQ
            IF (ABS(A(I,K)) .GT. ABS(A(M,K))) M = I
   20    CONTINUE
C
C        Check for a nonzero pivot; if all possible pivots are zero,
C        matrix is numerically singular.
C
         IF (A(M,K) .EQ. ZERO) THEN
            FLAG = K
            RETURN
         ENDIF             
         PVTIDX(K) = M
         IF (M .NE. K) THEN
C
C           Interchange the current row K with the pivot row M. 
C
            PVTIDX(NEQ) = -PVTIDX(NEQ)
            DO 25 J = K,NEQ
               T = A(M,J)
               A(M,J) = A(K,J)
               A(K,J) = T
   25       CONTINUE
         ENDIF
C
C        Eliminate subdiagonal entries of column K.
C
         DO 35 I = K+1,NEQ
            T = A(I,K)/A(K,K)
            A(I,K) = -T
            IF (T .NE. ZERO) THEN
               DO 30 J = K+1,NEQ
                  A(I,J) = A(I,J)-T*A(K,J)
   30          CONTINUE
            ENDIF
   35    CONTINUE
   40 CONTINUE
C
      IF (A(NEQ,NEQ) .EQ. ZERO) THEN
         FLAG = NEQ
         RETURN
      ENDIF
C
C     Estimate the condition number of A.
C
      DO 50 K = 1,NEQ
         T = ZERO
         DO 45 I = 1,K-1
            T = T+A(I,K)*TEMP(I)
   45    CONTINUE
         TEMP(K) = -(SIGN(ONE,T)+T)/A(K,K)
   50 CONTINUE
      DO 60 K = NEQ-1,1,-1
         T = ZERO
         DO 55 I = K+1,NEQ
            T = T+A(I,K)*TEMP(K)
   55    CONTINUE
         TEMP(K) = T
         M = PVTIDX(K)
         IF (M .NE. K) THEN
            T = TEMP(M)
            TEMP(M) = TEMP(K)
            TEMP(K) = T
         ENDIF
   60 CONTINUE
      DNORM = ZERO
      DO 65 I = 1,NEQ
         DNORM = MAX(DNORM,ABS(TEMP(I)))
   65 CONTINUE
      CALL SOLVE(A,MAXROW,NEQ,PVTIDX,TEMP)
      YNORM = ZERO
      DO 70 I = 1,NEQ
         YNORM = MAX(YNORM,ABS(TEMP(I)))
   70 CONTINUE
      COND = ANORM*YNORM/DNORM
      RETURN
      END
C-----------------------------------------------------------------------
      SUBROUTINE SOLVE(A,MAXROW,NEQ,PVTIDX,B)
C
      INTEGER MAXROW,NEQ,PVTIDX(*)
      DOUBLE PRECISION A(MAXROW,*),B(*)
C
C  SOLVE solves the linear system A*x=b using the factorization
C  obtained from FACTOR.  Do not use SOLVE if a zero pivot has
C  been detected in FACTOR.
C
C  Input variables:
C     A      = an array returned from FACTOR containing the 
C              triangular decomposition of the coefficient matrix.
C     MAXROW = as in FACTOR.
C     NEQ    = number of equations to be solved.
C     PVTIDX = vector of information about row interchanges obtained 
C                from FACTOR.
C     B      = right hand side vector b.
C  Output variables:
C     B      = solution vector x.
C
C  Local variables:
      INTEGER I,J,K,M
      DOUBLE PRECISION T
C
C     Forward elimination.
C
      IF (NEQ .GT. 1) THEN
         DO 20 K = 1,NEQ-1
            M = PVTIDX(K)
            T = B(M)
            B(M) = B(K)
            B(K) = T
            DO 10 I = K+1,NEQ
               B(I) = B(I)+A(I,K)*T
   10       CONTINUE
   20    CONTINUE
C
C        Back substitution.
C
         DO 40 I = NEQ,1,-1
            DO 30 J = I+1,NEQ
               B(I) = B(I)-A(I,J)*B(J)
   30       CONTINUE
            B(I) = B(I)/A(I,I)
   40    CONTINUE
      ELSE
         B(1) = B(1)/A(1,1)
      ENDIF
      RETURN
      END
C=======================================================================