'*************************************************************************
'*       Response of a N d.o.f. Mass-Spring System with Damping          *
'*              to a sinusoidal Force By a Direct Method                 *
'* --------------------------------------------------------------------- *
'* EXPLANATION:                                                          *
'* Let us consider the 3 d.o.f. Mass-Spring system with viscous damping: *
'*                                                                       *
'*                      --> x1          --> x2    k     --> x3           *
'*          \|    k     *----*    2k    *----*--/\/\/\--*----*           *
'*          \|--/\/\/\--| 2m |--/\/\/\--| m  |  ---*    | 3m |           *
'*          \|          *----*          *----*---| |----*----*           *
'*                      --> F1(t)               ---*                     *
'*                                                c                      *
'*                      | 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 |                                *
'*                                                                       *
'*                       | 0  0  0   |                                   *
'* The Damping Matrix is | 0  0  0   | assuming c=0.5.                   *
'*                       | 0  0  0.5 |                                   *
'*                                                                       *                     
'* The resonance fresuencies (neglecting c) 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:                                                           *
'*                                                                       *
'* How many degrees of freedom (d.o.f.): 3                               *
'*                                                                       *
'* Input Mass Matrix [M]:                                                *
'*  M(1,1) = 2                                                           *
'*  M(1,2) = 0                                                           *
'*  M(1,3) = 0                                                           *
'*  M(2,2) = 1                                                           *
'*  M(2,3) = 0                                                           *
'*  M(3,3) = 3                                                           *
'*                                                                       *
'* Viscous Damping...: VIS                                               *
'* Structural Damping: STR                                               *
'*                                                                       *
'* Your choice: VIS                                                      *
'*                                                                       *
'* Input Stiffness Matrix [K]:                                           *
'*  K(1,1) = 3                                                           *
'*  K(1,2) = -2                                                          *
'*  K(1,3) = 0                                                           *
'*  K(2,2) = 3                                                           *
'*  K(2,3) = -1                                                          *
'*  K(3,3) = 1                                                           *
'*                                                                       *
'* Input Damping Matrix [C]:                                             *
'*  C(1,1) = 0                                                           *
'*  C(1,2) = 0                                                           *
'*  C(1,3) = 0                                                           *
'*  C(2,2) = 0                                                           *
'*  C(2,3) = 0                                                           *
'*  C(3,3) = 0.5                                                         *
'*                                                                       *
'* Input Excitation Vector:                                              *
'*  F1(1) = 1000                                                         *
'*  F1(2) = 0                                                            *
'*  F1(3) = 0                                                            *
'*  F2(1) = 0                                                            *
'*  F2(2) = 0                                                            *
'*  F2(3) = 0                                                            *
'*                                                                       *
'* Number of d.o.f. to calculate: 3                                      *
'*                                                                       *
'* Frequency Sweep                                                       *
'* ---------------                                                       *
'* Starting Frequency: 0.31                                              *
'* Ending Frequency..: 0.32                                              *
'*                                                                       *
'* Linear frequency step.....: LIN                                       *
'* Logarithmic frequency step: LOG                                       *                        
'*                                                                       *
'* Your choice: LIN                                                      *
'*                                                                       *
'* Frequency Step....: 0.0005                                            *
'*                                                                       *
'*                                                                       *
'* Frequency (Hz)   Displacement   Phase (deg.)                          *
'* --------------------------------------------                          *
'*     0.3100          240.66        0.0                                 *
'*     0.3105          264.73        0.0                                 *
'*     0.3110          294.72        0.0                                 *
'*     0.3115          333.08        0.0                                 *
'*     0.3120          383.85        0.0                                 *
'*     0.3125          454.17        0.0                                 *
'*     0.3130          557.90        0.0                                 *
'*     0.3135          725.80        0.0                                 *
'*     0.3140         1041.65        0.0                                 *
'*     0.3145         1824.42        0.0                                 *
'*     0.3150         4347.56        0.0  <-- 3rd resonance              *
'*     0.3155         2248.74       -0.0                                 *
'*     0.3160         1151.46       -0.0                                 *
'*     0.3165          757.64       -0.0                                 *
'*     0.3170          560.61       -0.0                                 *
'*     0.3175          443.06       -0.0                                 *
'*     0.3180          365.13       -0.0                                 *
'*     0.3185          309.74       -0.0                                 *
'*     0.3190          268.36       -0.0                                 *
'*     0.3195          236.29       -0.0                                 *
'*     0.3200          210.72       -0.0                                 *
'*                                                                       *              
'* --------------------------------------------------------------------- *
'* 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 NDOF03

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

PI = 4# * ATN(1#)

CLS
PRINT
INPUT " How many degrees of freedom (d.o.f.): ", N
N2 = 2 * N

DIM M(N, N) AS DOUBLE
DIM K1(N, N) AS DOUBLE
DIM K2(N, N) AS DOUBLE

DIM A(N2, N2), X(N2), B(N2), B1(N2, 1)
DIM C9 AS INTEGER
DIM I1 AS DOUBLE
DIM M1 AS DOUBLE

PRINT
PRINT " Input Mass Matrix [M]:"
PRINT
FOR I = 1 TO N
  FOR J = I TO N
    PRINT " M("; I; ","; J; ") = "; : INPUT "", M(I, J)
  NEXT J
NEXT I

PRINT
PRINT " Viscous Damping...: VIS"
PRINT " Structural Damping: STR"
PRINT
INPUT " Your choice: ", ANS$

IF ANS$ = "VIS" THEN GOTO 600

'case structural damping
C9 = 2
PRINT
PRINT " Input stiffness Matrix [K1]:"
FOR I = 1 TO N
  FOR J = I TO N
    PRINT " K1("; I; ","; J; ") = "; : INPUT "", K1(I, J)
  NEXT J
NEXT I
PRINT
PRINT " Input Damping Matrix [K2]:"
FOR I = 1 TO N
  FOR J = I TO N
    PRINT " K2("; I; ","; J; ") = "; : INPUT "", K2(I, J)
  NEXT J
NEXT I
PRINT
GOTO 900

600 'case viscous damping
C9 = 1
PRINT
PRINT " Input stiffness Matrix [K]:"
FOR I = 1 TO N
  FOR J = I TO N
    PRINT " K("; I; ","; J; ") = "; : INPUT "", K1(I, J)
  NEXT J
NEXT I
PRINT
PRINT " Input Damping Matrix [C]:"
FOR I = 1 TO N
  FOR J = I TO N
    PRINT " C("; I; ","; J; ") = "; : INPUT "", K2(I, J)
  NEXT J
NEXT I
PRINT

900 'Complete Matrices by symmetry
FOR I = 1 TO N
  FOR J = I TO N
    K1(J, I) = K1(I, J)
    K2(J, I) = K2(I, J)
    M(J, I) = M(I, J)
  NEXT J
NEXT I

PRINT
PRINT " Input Excitation Vector:"
FOR I = 1 TO N
  PRINT " F1("; I; ") = "; : INPUT " ", B(I)
NEXT I
PRINT
FOR I = N + 1 TO N2
  J = I - N
  PRINT " F2("; J; ") = "; : INPUT " ", B(I)
NEXT I

PRINT
INPUT " Number of d.o.f. to calculate: ", N3
PRINT

'calculate numerical response of d.o.f. #N3
PRINT
PRINT " Frequency Sweep"
PRINT " ---------------"
PRINT
1400 INPUT " Starting frequency = ", F1
INPUT " Ending frequency...= ", F2
PRINT
PRINT " Linear frequency step.....: LIN"
PRINT " Logarithmic frequency step: LOG"
PRINT
INPUT " Your choice: ", ANS$
PRINT

IF ANS$ = "LOG" THEN GOTO 1660

'case linear step
INPUT " Frequency step: ", F3
GOTO 1750

1660 'case log. step
IF F1 = 0 THEN
  PRINT " CAUTION - Starting frequency must be > 0 for log. step."
  GOTO 1400
ELSE
  INPUT " Number of constant log. increments from F1 to F2: ", F3
  F3 = 10 ^ ((LOG(F2) - LOG(F1)) / LOG(10#) / F3)
END IF

1750 'print header
PRINT
PRINT " Frequency (Hz)   Displacement   Phase (deg.)"
PRINT " --------------------------------------------"

F = F1 'Initialize frequency

1760 W = 2# * PI * F
FOR I = 1 TO N
  I1 = I + N
  FOR J = 1 TO N
    J1 = J + N
    A(I, J) = K1(I, J) - W * W * M(I, J)
    A(I1, J1) = A(I, J)
    IF C9 = 1 THEN
      A(I1, J) = K2(I, J) * W
    ELSE
      A(I1, J) = K2(I, J)
    END IF
    A(I, J1) = -A(I1, J)
  NEXT J
NEXT I

'A=INV(A)
 NOLD = N: N = N2: M = 0: GOSUB 1000  'A=INV(A)
 N = NOLD


'X=A MPY B
FOR I = 1 TO N2
  SUM = 0#
  FOR J = 1 TO N2
    SUM = SUM + A(I, J) * B(J)
  NEXT J
  X(I) = SUM
NEXT I

M1 = SQR(X(N3) ^ 2 + X(N3 + N) ^ 2)
IF M1 = 0 THEN
  T1 = 0#: GOTO 1800
ELSE
  O1 = X(N3) / M1
  I1 = -X(N3 + N) / M1
  IF O1 = -1# THEN
    T1 = PI: GOTO 1800
  ELSEIF O1 = 1# THEN
    T1 = 0#: GOTO 1800
  ELSE
    IF I1 >= 0# THEN
      'T1=ACOS(O1)
      T1 = ATN(SQR(1 - O1 ^ 2) / O1)
    ELSE
      'T1=2*PI-ACOS(O1)
      T1 = 2 * PI - ATN(SQR(1 - O1 ^ 2) / O1)
    END IF
  END IF
END IF

1800 T1 = T1 / PI / 180  'Convert phase in degrees
'print line of results
PRINT USING "   ###.####"; F;
PRINT USING "       ######.##"; M1;
PRINT USING "      ###.# "; T1

IF ANS$ = "LIN" THEN
  F = F + F3
ELSE
  F = F * F3
END IF

IF F <= F2 THEN GOTO 1760  'next frequency     

PRINT
END 'of main program


'*******************************************
'*  SOLVING A LINEAR MATRIX SYSTEM AX = B1 *
'*  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               *
'*            B1  MATRIX N*M               *
'* --------------------------------------- *
'* OUTPUS:    A   INVERSE OF A N*N         *
'*            DET DETERMINANT OF A         *
'*            B1  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 = B1(IK, I)
        B1(IK, I) = B1(K, I)
        B1(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
        B1(K, I) = B1(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
          B1(J, I) = B1(J, I) - CS(J) * B1(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 = B1(I, J)
        B1(I, J) = B1(IK, J)
        B1(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

'end of file ndof03.bas