'*********************************************************************
'*   Frequencies and eigenmodes, masses and modal stiffnesses of a   *
'*   Mass-Spring undamped system represented by Motion Equation:     *
'*                 [M] . [X]" + [K] . [X] = [0}                      *
'* ----------------------------------------------------------------- *
'* EXPLANATION:                                                      *
'* Let us take the 3 D.O.F. Mass-Spring System below:                *
'*                                                                   *
'*     \|    k     ------    2k    ------    k     ------            *
'*     \|--/\/\/\--| 2m |--/\/\/\--| m  |--/\/\/\--| 3m |            *
'*     \|          ------          ------          ------            *
'*                                                                   *
'*                      | 2  0  0 |                                  *
'* The Mass Matrix M is | 0  1  0 |  assuming m=1.                   *
'*                      | 0  0  3 |                                  *
'*                                                                   *
'*                           |  3 -2  0 |                            *
'* The Stiffness Matrix K is | -2  3 -1 |  assuming k=1.             *
'*                           |  0 -1  1 |                            *
'*                                                                   *
'* The resonance fresuencies are:                                    *
'*             F1 = w1/2pi = 0.05161 sqr(k/m)                        *
'*             F2 = w2/2pi = 0.1431 sqr(k/m)                         *
'*             F3 = w3/2pi = 0.3151 sqr(k/m)                         *
'* with the corresponding modal vectors:                             *
'*             |   1   |         |   1    |         |    1    |      *
'*      phi1 = | 1.395 |  phi2 = | 0.6914 |  phi3 = | -2.420  |      *
'*             | 2.038 |         |-0.4849 |         |  0.2249 |      *
'*                                                                   *
'* (See program ndof01.bas).                                         *
'* ----------------------------------------------------------------- *
'* SAMPLE RUN:                                                       *
'*                                                                   *
'* Order of system: 3                                                *
'*                                                                   *
'* M(1,1) = 2                                                        *
'* M(1,2) = 0                                                        *
'* M(1,3) = 0                                                        *
'* M(2,2) = 1                                                        *
'* M(2,3) = 0                                                        *
'* M(3,3) = 3                                                        *
'*                                                                   *
'* K(1,1) = 3                                                        *
'* K(1,2) = -2                                                       *
'* K(1,3) = 0                                                        *
'* K(2,2) = 3                                                        *
'* K(2,3) = -1                                                       *
'* K(3,3) = 1                                                        *
'*                                                                   *
'* Number of eigenvectors asked for: 3                               *
'*                                                                   *
'* What precision for eigenvalues: 1e-5                              *
'*                                                                   *
'* Maximum number of iterations: 100                                 *
'*                                                                   *
'*                                                                   *
'* Eigenvector #1, Convergence after 10 iterations.                  *
'* Eigenvector #2, Convergence after 9 iterations.                   *
'* Eigenvector #3, Convergence after 2 iterations.                   *
'*                                                                   *
'*                                                                   *
'* Eigenvalues:                                                      *
'*                                                                   *
'* L(1) =  .1051733215666635                                         *
'* L(2) =  .8086120631010683                                         * 
'* L(3) =  3.919453983079086                                         * 
'*                                                                   *
'* Pulsations:                                                       *
'*                                                                   *
'* W(1) =  .3243043656299796                                         *
'* W(2) =  .8992285933515839                                         *
'* W(3) =  1.979761092424812                                         *
'*                                                                   *
'* Frequencies:                                                      *
'*                                                                   *
'* F(1) =  5.161464285629262D-02                                     *
'* F(2) =  .1431166756014764                                         *
'* F(3) =  .3150887640004196                                         *
'*                                                                   *
'* Eigenvectors:                                                     *
'*                                                                   *
'* E.V.  1                                                           *
'*              1                                                    *
'*              1.394826678433337                                    *
'*              2.037781999496389                                    *
'*                                                                   *
'* E.V.  2                                                           *
'*              1                                                    *
'*              .691383466647192                                     *
'*             -.4849013087124237                                    *
'*                                                                   *
'* E.V.  3                                                           *
'*              1                                                    *
'*             -2.41959287715014                                     *
'*              .2249034647250999                                    *
'*                                                                   *
'* Modal Mass Matrix:                                                *
'*                                                                   *
'*  16.40321  -0.00001   0.00000                                     *
'*  -0.00001   3.18340  -0.00003                                     *
'*   0.00000  -0.00003   8.00617                                     *
'*                                                                   *
'* Modal Stiffness Matrix:                                           *
'*                                                                   *
'*   1.72517   0.00000  -0.00000                                     *
'*   0.00000   2.57413   0.00001                                     *
'*  -0.00000   0.00001  31.38059                                     *
'*                                                                   *
'* ----------------------------------------------------------------- *
'* REFERENCE: "Mécanique des vibrations linéaires By M. Lalanne,     *
'*            P. Berthier, J. Der Hagopian, Masson, Paris 1980"      *
'*            [BIBLI 16].                                            *
'*                                                                   *
'*                        Quick Basic Release By J-P Moreau, Paris.  *
'*                                    (www.jpmoreau.fr)              *
'*********************************************************************
'Program NDOF02

DEFDBL A-H, O-Z
DEFINT I-N

PI = 4# * ATN(1#)

CLS
PRINT
INPUT " Order of system: ", N
PRINT

DIM M(N, N) AS DOUBLE
DIM K(N, N) AS DOUBLE

DIM A(N, N), B(N, N), C(N, N), S(N, N)
DIM R(N, N), R1(N, N), R2(N, N)

'auxiliary vectors for subroutine 1000
DIM PC(N), PL(N), CS(N)

DIM M1 AS DOUBLE

FOR I = 1 TO N
  FOR J = I TO N
    PRINT " M("; I; ","; J; ") = "; : INPUT "", M(I, J)
  NEXT J
NEXT I
PRINT

FOR I = 1 TO N
  FOR J = I TO N
    PRINT " K("; I; ","; J; ") = "; : INPUT "", K(I, J)
  NEXT J
NEXT I

A1 = 0#: A2 = 0#
FOR I = 1 TO N
  FOR J = I TO N
    A1 = A1 + M(I, I)
    A2 = A2 + K(I, I)
    M(J, I) = M(I, J)
    K(J, I) = K(I, J)
  NEXT J
NEXT I

PRINT
INPUT " Number of eigenvectors asked for: ", N1
K8 = 1
DIM K1(N, N) AS DOUBLE
DIM D(N, N)

PRINT
INPUT " What precision for eigenvalues: ", P
P = P * P
PRINT
INPUT " Maximum number of iterations: ", N3
PRINT

A0 = A2 / A1 / 10#
FOR I = 1 TO N
  FOR J = 1 TO N
    K1(I, J) = K(I, J) + A0 * M(I, J)
  NEXT J
NEXT I

'K1=INV(K1)
 FOR I = 1 TO N
   FOR J = 1 TO N
     A(I, J) = K1(I, J)
   NEXT J
 NEXT I
 M = 0: GOSUB 1000'A=INV(A)
 FOR I = 1 TO N
   FOR J = 1 TO N
     K1(I, J) = A(I, J)
   NEXT J
 NEXT I

 'D=K1 MPY M
 FOR I = 1 TO N
   FOR J = 1 TO N
     A(I, J) = K1(I, J)
     B(I, J) = M(I, J)
   NEXT J
 NEXT I
 GOSUB 2000 'C=A MPY B
 FOR I = 1 TO N
   FOR J = 1 TO N
     D(I, J) = C(I, J)
   NEXT J
 NEXT I

 DIM V(N, N1), S1(N, N)
 DIM L(N1) AS DOUBLE
 'V=0
  FOR I = 1 TO N
   FOR J = 1 TO N1
     V(I, J) = 0#
   NEXT J
 NEXT I
 'S1=D
  FOR I = 1 TO N
   FOR J = 1 TO N
     S1(I, J) = D(I, J)
   NEXT J
 NEXT I

 DIM X(N), F(N), F1(N)
 DIM T(N, N), T1(N, N)

800 FOR I = 1 TO N
   X(I) = 1#
 NEXT I

 K9 = 0

820 K9 = K9 + 1
 'F=S1 MPY X
 FOR I = 1 TO N
   SUM = 0#
   FOR J = 1 TO N
     SUM = SUM + S1(I, J) * X(J)
   NEXT J
   F(I) = SUM
 NEXT I
 W2 = 1# / F(1)
 L(K8) = W2 - A0
 FOR I = 1 TO N
   F(I) = F(I) * W2
 NEXT I
 FOR I = 1 TO N
   F1(I) = F(I) - X(I)
 NEXT I
 D1 = 0#: M1 = 0#
 FOR I = 1 TO N
   D1 = D1 + F1(I) * F1(I)
   M1 = M1 + F(I) * F(I)
   X(I) = F(I)
 NEXT I

970 IF K9 - N3 <= 0 THEN GOTO 1070
 PRINT " No convergence after"; K9; " Iterations."
 STOP

1070 IF P - D1 / M1 < 0# THEN GOTO 820
 PRINT " Eigenvector #"; K8; ", Convergence after"; K9; " iterations."
 FOR I = 1 TO N
   V(I, K8) = X(I)
 NEXT I
 IF K8 >= N1 THEN GOTO 1490  'print results

1160 N2 = N - K8
 'T=TRN(V)
 FOR I = 1 TO N
   FOR J = 1 TO N1
     T(J, I) = V(I, J)
   NEXT J
 NEXT I

 'T1=T MPY M
 FOR I = 1 TO N1
   FOR J = 1 TO N
     A(I, J) = T(I, J)
     B(I, J) = M(I, J)
   NEXT J
 NEXT I
 GOSUB 2000 'C=A MPY B
 FOR I = 1 TO N1
   FOR J = 1 TO N
     T1(I, J) = C(I, J)
   NEXT J
 NEXT I

 FOR I = 1 TO K8
   FOR J = 1 TO N
     IF J <= K8 THEN
       R(I, J) = T1(I, J)
       GOTO 1290
     END IF
     J1 = J - K8
     R1(I, J1) = T1(I, J)
1290 NEXT J
 NEXT I

 'R=INV(R)
 FOR I = 1 TO K8
   FOR J = 1 TO K8
     A(I, J) = R(I, J)
   NEXT J
 NEXT I
 NOLD = N: N = K8: M = 0: GOSUB 1000'A=INV(A)
 FOR I = 1 TO K8
   FOR J = 1 TO K8
     R(I, J) = A(I, J)
   NEXT J
 NEXT I
 'Restore N
 N = NOLD

 'R2(N,N2)=R(N,K8) MPY R1(N,N2)
  FOR I = 1 TO K8
    FOR J = 1 TO N2
      SUM = 0#
      FOR K = 1 TO K8
        SUM = SUM + R(I, K) * R1(K, J)
        R2(I, J) = SUM
      NEXT K
    NEXT J
  NEXT I

  'S=Identity Matrix
  FOR I = 1 TO N
   FOR J = 1 TO N
     IF J = I THEN
       S(I, J) = 1#
     ELSE
       S(I, J) = 0#
     END IF
   NEXT J
 NEXT I

 FOR I = 1 TO K8
   S(I, I) = 0
   FOR J = K8 + 1 TO N
     J1 = J - K8
     S(I, J) = -R2(I, J1)
   NEXT J
 NEXT I

 'S1=D MPY S
 FOR I = 1 TO N
   FOR J = 1 TO N
     A(I, J) = D(I, J)
     B(I, J) = S(I, J)
   NEXT J
 NEXT I
 GOSUB 2000 'C=A MPY B
 FOR I = 1 TO N
   FOR J = 1 TO N
     S1(I, J) = C(I, J)
   NEXT J
 NEXT I

 K8 = K8 + 1
 GOTO 800  'go to next eigenvalue/eigenvector

'results section
1490 PRINT
 PRINT " Eigenvalues:"
 PRINT
 FOR I = 1 TO N1
   PRINT " L("; I; ") = "; L(I)
 NEXT I
 PRINT
 PRINT " Pulsations:"
 PRINT
  FOR I = 1 TO N1
   PRINT " W("; I; ") = "; SQR(L(I))
 NEXT I
 PRINT
 PRINT " Frequencies:"
 PRINT
  FOR I = 1 TO N1
   PRINT " F("; I; ") = "; SQR(L(I)) / 2# / PI
 NEXT I
 PRINT
 PRINT " Eigenvectors:"
 PRINT
 FOR J = 1 TO N1
   PRINT
   PRINT " E.V. "; J
   FOR I = 1 TO N
     PRINT "             "; V(I, J)
   NEXT I
 NEXT J

 'S1(N,N1)=M(N,N) MPY V(N,N1)
 FOR I = 1 TO N
   FOR J = 1 TO N1
     SUM = 0#
     FOR K = 1 TO N
       SUM = SUM + M(I, K) * V(K, J)
       S1(I, J) = SUM
     NEXT K
   NEXT J
 NEXT I

 'T1=TRN(V)
 FOR I = 1 TO N
   FOR J = 1 TO N1
     T1(J, I) = V(I, J)
   NEXT J
 NEXT I

 'R(N1,N1)=T1(N1,N) MPY S1(N,N1)
  FOR I = 1 TO N1
   FOR J = 1 TO N1
     SUM = 0#
     FOR K = 1 TO N
       SUM = SUM + T1(I, K) * S1(K, J)
       R(I, J) = SUM
     NEXT K
   NEXT J
 NEXT I

'print modal mass matrix
 PRINT
 PRINT " Modal Mass Matrix:"
 PRINT
 FOR I = 1 TO N1
   FOR J = 1 TO N1
     PRINT USING "####.#####"; R(I, J);
   NEXT J
   PRINT
 NEXT I

 'S1(N,N1)=K(N,N) MPY V(N,N1)
  FOR I = 1 TO N
   FOR J = 1 TO N1
     SUM = 0#
     FOR K = 1 TO N
       SUM = SUM + K(I, K) * V(K, J)
       S1(I, J) = SUM
     NEXT K
   NEXT J
 NEXT I
 'R(N1,N1)=T1(N1,N) MPY S1(N,N1)
   FOR I = 1 TO N1
   FOR J = 1 TO N1
     SUM = 0#
     FOR K = 1 TO N
       SUM = SUM + T1(I, K) * S1(K, J)
       R(I, J) = SUM
     NEXT K
   NEXT J
 NEXT I

 'print modal stiffness matrix
 PRINT
 PRINT " Modal Stiffness Matrix:"
 PRINT
 FOR I = 1 TO N1
   FOR J = 1 TO N1
     PRINT USING "####.#####"; R(I, J);
   NEXT J
   PRINT
 NEXT I
 PRINT

 END 'of main program


'*******************************************
'*  SOLVING A LINEAR MATRIX SYSTEM AX = B  *
'*  with Gauss-Jordan method using full    *
'*  pivoting at each step. During the pro- *
'* cess, original A and B matrices are     *
'* destroyed to spare storage location.    *
'* --------------------------------------- *
'* INPUTS:    A   MATRIX N*N               *
'*            B   MATRIX N*M               *
'* --------------------------------------- *
'* OUTPUS:    A   INVERSE OF A N*N         *
'*            DET  DETERMINANT OF A        *
'*            B   SOLUTION MATRIX N*M      *
'* --------------------------------------- *
'* NOTA - If M=0 inversion of A matrix     *
'*        only.                            *
'*******************************************
1000 'Subroutine MATINV
'Labels:  100, 200, 300
'Initializations
  DET = 1#
  FOR I = 1 TO N
    PC(I) = 0#
    PL(I) = 0#
    CS(I) = 0#
  NEXT I

  'Main loop
  FOR K = 1 TO N
    'Searching greatest pivot
    PV = A(K, K)
    IK = K: JK = K
    PAV = ABS(PV)
    FOR I = K TO N
      FOR J = K TO N
        temp = ABS(A(I, J))
        IF temp > PAV THEN
          PV = A(I, J)
          PAV = ABS(PV)
          IK = I: JK = J
        END IF
      NEXT J
    NEXT I

    'Search terminated, the pivot is in location I=IK, J=JK.
    'Memorizing pivot location:

    PC(K) = JK
    PL(K) = IK

    'DETERMINANT DET is updated
    'If DET=0, ERROR MESSAGE and STOP
    'Machine dependant EPSMACH equals here 2E-16
    IF IK <> K THEN
      DET = -DET
    END IF
    IF JK <> K THEN
      DET = -DET
    END IF
    DET = DET * PV
    temp = ABS(DET)
    IF temp < EPSMACH THEN
      PRINT #2,
      PRINT #2, " The determinant equals ZERO !!!"
      END   'stop program
    END IF

    'POSITIONNING PIVOT IN K,K:}
    IF IK <> K THEN
      FOR I = 1 TO N
        'EXCHANGE LINES IK and K of matrix A:}
        TT = A(IK, I)
        A(IK, I) = A(K, I)
        A(K, I) = TT
      NEXT I
    END IF

    IF M <> 0 THEN
      FOR I = 1 TO M
        TT = B(IK, I)
        B(IK, I) = B(K, I)
        B(K, I) = TT
      NEXT I
    END IF

    'PIVOT is at correct line
    IF JK <> K THEN
      FOR I = 1 TO N
      'EXCHANGE COLUMNS JK and K of matrix A:
        TT = A(I, JK)
        A(I, JK) = A(I, K)
        A(I, K) = TT
      NEXT I
    END IF

    'The PIVOT is at correct column.
    'and is located in K,K.

    'Column K of matrix A is stored in CS vector
    'then column K is set to zero.
    FOR I = 1 TO N
      CS(I) = A(I, K)
      A(I, K) = 0#
    NEXT I

    CS(K) = 0#
    A(K, K) = 1#
    'Line K of matrix A is modified:
    temp = ABS(PV)
    IF temp < EPSMACH THEN
      PRINT #2,
      PRINT #2, " PIVOT TOO SMALL - STOP"
      END   'stop program
    END IF
    FOR I = 1 TO N
      A(K, I) = A(K, I) / PV
    NEXT I

    IF M <> 0 THEN
      FOR I = 1 TO M
        B(K, I) = B(K, I) / PV
      NEXT I
    END IF

    'Other lines of matrix A are modified:
    FOR J = 1 TO N
      IF J = K THEN
        GOTO 100
      END IF
      FOR I = 1 TO N
        'Line J of matrix A is modified:
        A(J, I) = A(J, I) - CS(J) * A(K, I)
      NEXT I
      IF M <> 0 THEN
        FOR I = 1 TO M
          B(J, I) = B(J, I) - CS(J) * B(K, I)
        NEXT I
      END IF
100 NEXT J
  'Line K is ready
  NEXT K 'end K loop

  'MATRIX A INVERSION IS DONE - REARRANGEMENT OF MATRIX A

  'EXCHANGE LINES
  FOR I = N TO 1 STEP -1
    IK = INT(PC(I))
    IF IK = I THEN
      GOTO 200
    END IF
    'EXCHANGE LINES I and PC(I) of matrix A:
    FOR J = 1 TO N
      TT = A(I, J)
      A(I, J) = A(IK, J)
      A(IK, J) = TT
    NEXT J
    IF M <> 0 THEN
      FOR J = 1 TO M
        TT = B(I, J)
        B(I, J) = B(IK, J)
        B(IK, J) = TT
      NEXT J
    END IF
    'NO MORE EXCHANGE is NECESSARY
    'Go to next line.
200 NEXT I 'End of loop i=N to 1 step -1

  'EXCHANGE COLUMNS
  FOR J = N TO 1 STEP -1
    JK = INT(PL(J))
    IF JK = J THEN
      GOTO 300
    END IF
    'EXCHANGE COLUMNS J ET PL(J) of matrix A:
    FOR I = 1 TO N
      TT = A(I, J)
      A(I, J) = A(I, JK)
      A(I, JK) = TT
    NEXT I
    'NO MORE EXCHANGE is NECESSARY
    'Go to next column.}
300 NEXT J
'REARRANGEMENT TERMINATED
RETURN  'End of subroutine 1000


'*******************************************
'*    MULTIPLICATION OF TWO SQUARE REAL    *
'*    MATRICES                             *
'* --------------------------------------- *
'* INPUTS:    A  MATRIX N*N                *
'*            B  MATRIX N*N                *
'*            N  INTEGER                   *
'* --------------------------------------- *
'* OUTPUTS:   C  MATRIX N*N PRODUCT A*B    *
'*                                         *
'*******************************************
2000 'Start multiplication
  FOR I = 1 TO N
    FOR J = 1 TO N
      SUM = 0#
      FOR K = 1 TO N
        SUM = SUM + A(I, K) * B(K, J)
        C(I, J) = SUM
      NEXT K
    NEXT J
  NEXT I
RETURN

'end of file ndof02.bas