'===========================================================================
' Subject: MATRIX INVERSE ALGORITHM          Date: 09-27-99 (08:40)       
' Author:  Robert L. Roach                   Code: QB, QBasic, PDS        
' Origin:  rroach@cvd-pro.com              Packet: ALGOR.ABC
'===========================================================================
REM---------- MATRIX INVERSE ALGORITHM ----------
REM
REM     This program call the MATINV subroutine which finds
REM      the inverse of an A(,) matrix.
REM
REM-------------------------------------------------------------
        DEFDBL A-H, O-Z
        DEFINT I-N

        LL = 3: LLM = 3
        DIM A(LL, LL), AI(LL, LL), AO(LL, LL), B(LL, LL)

REM---------- MATRIX TO BE INVERTED
        A(1, 1) = 2
        A(2, 1) = 2
        A(3, 1) = 4
       
        A(1, 2) = 2
        A(2, 2) = 2
        A(3, 2) = 1
       
        A(1, 3) = 0
        A(2, 3) = 1
        A(3, 3) = 1

REM------- SAVE ORIGINAL MATRIX IN AO(,)
        FOR J = 1 TO LL
          FOR I = 1 TO LL
            AO(I, J) = A(I, J)
          NEXT
        NEXT

REM----------- IDENTITY MATRIX ------------
        FOR L2 = 1 TO LL
          FOR L1 = 1 TO LL
            AI(L1, L2) = 0
          NEXT
          AI(L2, L2) = 1
        NEXT

REM-------- PRINT THE MATRIX ---------
        CLS
        LOCATE 1, 10: PRINT "Original Matrix"
        ROW = 1
       
        LLL = LLM
        IF LLM > LL THEN LLL = LL
       
        FOR J = 1 TO LLL
          ROW = ROW + 1
          SUM = 0
          FOR I = 1 TO LLL
            COL = 2 + 10 * (I - 1)
            LOCATE ROW, COL
            PRINT USING " ##.##### "; AO(I, J)
          NEXT
        NEXT

REM-------- NOW INVERT THE MATRIX -----------
        GOSUB 1000


REM-------- CHECK BY MULTIPLYING THE INVERSE BY THE ORIGINAL
        FOR LA = 1 TO LL
          FOR LB = 1 TO LL
            B(LB, LA) = 0
            FOR L1 = 1 TO LL
              B(LB, LA) = B(LB, LA) + AO(L1, LA) * AI(LB, L1)
            NEXT
          NEXT
        NEXT

REM-------- PRINT THE INVERSE ---------
        ROW = ROW + 2
        LOCATE ROW, 10: PRINT "Inverse Matrix"
        LR1 = (LL - LLM) / 2
        LR1 = 1
        
        FOR J = LR1 TO LR1 + LLL - 1
'        FOR J = 1 TO LL
          ROW = ROW + 1
          SUM = 0
          FOR I = LR1 TO LR1 + LLL - 1
'          FOR I = 1 TO LL
            COL = 2 + 10 * (I - LR1)
            LOCATE ROW, COL
            PRINT USING " ##.######"; AI(I, J)
          NEXT
        NEXT

REM-------- PRINT THE CHECK MATRIX ---------
        ROW = ROW + 2
        LOCATE ROW, 10: PRINT "A*AI"
        FOR J = 1 TO LLL
          ROW = ROW + 1
          FOR I = 1 TO LLL
            COL = 2 + 10 * (I - 1)
            LOCATE ROW, COL
            PRINT USING " ##.##### "; B(I, J)
          NEXT
        NEXT

REM----------- DONE ---------------------
        END


1000 REM----------------- INVERT A MATRIX ----------------------
REM
REM      This subroutine inverts the matrix A.  The inverse
REM      is returned in the matrix AI.  Unfortunately, A itself
REM      is messed up upon completion, which is why I stored
REM      the original A matrix in AO in the main program.
REM
REM----------------------------------------------------------------------
        FOR LC = 1 TO LL

REM-------- CHECK DIAGONAL OF THIS ROW FOR ZERO
REM   Note:  if so, just add one of the other rows to it
          IF ABS(A(LC, LC)) < 1E-10 THEN
'            PRINT "    Aieee...a zero in the diagonal..."
            FOR LCA = LC + 1 TO LL
              IF LCA = LC GOTO 1090
              IF ABS(A(LC, LCA)) > 1E-10 THEN
                FOR LCB = 1 TO LL
                  A(LCB, LC) = A(LCB, LC) + A(LCB, LCA)
                  AI(LCB, LC) = AI(LCB, LC) + AI(LCB, LCA)
                NEXT
                GOTO 1100
              END IF
1090     NEXT
          END IF

1100 REM---- MAKE THE DIAGONAL UNITY BY DIVIDING THE ROW BY IT
          AIN = 1 / A(LC, LC)
          FOR LCA = 1 TO LL
            A(LCA, LC) = AIN * A(LCA, LC)
            AI(LCA, LC) = AIN * AI(LCA, LC)
          NEXT

REM-------- ELIMINATE THE REMAINING ELEMENTS IN THE LCth COLUMN
          FOR LCA = 1 TO LL
            IF LCA = LC GOTO 1150
            AF = A(LC, LCA)
            FOR LCB = 1 TO LL
              A(LCB, LCA) = A(LCB, LCA) - AF * A(LCB, LC)
              AI(LCB, LCA) = AI(LCB, LCA) - AF * AI(LCB, LC)
            NEXT
1150   NEXT

        NEXT

REM------------------------------------------------------------------------
         RETURN