/**************************************************************************** * Step by step solution of system [M] X" + [C] X' + [K] X = F(t) * * by the "Wilson-Theta" Method * * ------------------------------------------------------------------------- * * SAMPLE RUN: * * * * 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 ndof03.cpp). * * * * * * How many degrees of freedom (d.o.f.): 3 * * * * Mass Matrix: * * M(1,1) = 2 * * M(1,2) = 0 * * M(1,3) = 0 * * M(2,2) = 1 * * M(2,3) = 0 * * M(3,3) = 3 * * * * Damping Matrix: * * 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 * * * * Stiffness Matrix: * * K(1,1) = 3 * * K(1,2) = -2 * * K(1,3) = 0 * * K(2,2) = 3 * * K(2,3) = -1 * * K(3,3) = 1 * * * * Starting time (sec.) = 0 * * Ending time......... = 60 * * Time increment...... = 0.1 * * * * Starting motion X(1) = 0 * * Starting motion X(2) = 0 * * Starting motion X(3) = 0 * * * * Starting speed V(1) = 0 * * Starting speed V(2) = 0 * * Starting speed V(3) = 0 * * * * Starting acceleration G(1) = 0 * * Starting acceleration G(2) = 0 * * Starting acceleration G(3) = 0 * * * * Theta = 1.4 * * * * Number of force componant: 1 * * * * Force (maximum) = 1000 * * Force frequency = 0.315 * * * * Output file ndof04.txt contains: * * * * Time=0.000000 Force=0.000000 * * Node Displacement Speed Acceleration * * --------------------------------------- * * 1 0.000000 0.000000 0.000000 * * 2 0.000000 0.000000 0.000000 * * 3 0.000000 0.000000 0.000000 * * Time=0.100000 Force=196.630695 * * 1 0.163063 4.891900 97.838009 * * 2 0.001055 0.031650 0.633007 * * 3 0.000001 0.000034 0.000681 * * Time=0.200000 Force=385.583992 * * 1 1.342110 20.695684 218.237660 * * 2 0.012156 0.238073 3.495455 * * 3 0.000017 0.000368 0.006008 * * Time=0.300000 Force=559.482258 * * 1 4.697087 48.346076 334.770183 * * 2 0.065333 0.944405 10.631169 * * 3 0.000120 0.002070 0.028018 * * Time=0.400000 Force=711.535677 * * 1 11.373234 86.853736 435.383013 * * 2 0.235185 2.675173 23.984201 * * 3 0.000575 0.008105 0.092686 * * Time=0.500000 Force=835.807361 * * 1 22.364561 134.263211 512.806499 * * 2 0.657746 6.127269 45.057710 * * 3 0.002104 0.025013 0.245478 * * Time=0.600000 Force=927.445153 * * 1 38.436915 188.003858 562.006436 * * 2 1.545163 12.115093 74.698783 * * 3 0.006349 0.065060 0.555465 * * - -------- -------- -------- * * Time=59.500000 Force=-998.889875 * * 1 1311.501557 -4027.911278 -5149.756316 * * 2 -2554.792157 9594.998127 10332.611203 * * 3 184.399735 -884.959493 -754.711909 * * Time=59.600000 Force=-988.651745 * * 1 885.667274 -4461.718120 -3526.380524 * * 2 -1550.158169 10432.392829 6415.282844 * * 3 92.748603 -941.879364 -383.685509 * * Time=59.700000 Force=-939.811951 * * 1 424.805036 -4726.111878 -1761.494639 * * 2 -481.806426 10865.002501 2236.910583 * * 3 -2.712865 -960.901048 3.251826 * * Time=59.800000 Force=-854.277432 * * 1 -53.549682 -4810.343067 76.870845 * * 2 608.748956 10874.810930 -2040.741986 * * 3 -98.140459 -941.188318 391.002771 * * Time=59.900000 Force=-735.387861 * * 1 -531.131704 -4710.618045 1917.629602 * * 2 1679.008275 10460.194795 -6251.580732 * * 3 -189.681955 -883.418370 764.396191 * * Time=60.000000 Force=-587.785252 * * 1 -989.652417 -4430.266778 3689.395729 * * 2 2687.136442 9636.034476 -10231.625630 * * 3 -273.627843 -789.759696 1108.777301 * * * * Maximum Displacement (node #1) = 2432.184004 * * ------------------------------------------------------------------------- * * REFERENCE: "Mécanique des vibrations linéaires By M. Lalanne, * * P. Berthier, J. Der Hagopian, Masson, Paris 1980" [BIBLI 16].* * * * C++ Release By J-P Moreau, Paris. * * (www.jpmoreau.fr) * ****************************************************************************/ #include #include //Label: e500 #define NMAX 16 #define MACH_EPS 2e-16 #define ABS fabs #define PI 4*atan(1.0) typedef double MAT[NMAX][NMAX]; typedef double MAT1[NMAX][2]; typedef double VEC[NMAX]; typedef int IVEC[NMAX]; MAT M, K, K1, A, C; MAT1 B1; VEC X0, V0, G0, F0, F1, F2, X1, V1, G1, X2; double D,DET,F,Fmax,freq,SUM,T0,T1,T2,T3,tmp; double A0,A1,A2,A3,A4,A5,A6,A7,A8,A9,XMAX; int I,J,N,N1; FILE *fp; /****************************************** * SOLVING A LINEAR MATRIX SYSTEM AX = B * * with Gauss-Jordan method using full * * pivoting at each step. During the pro- * * cess, original AA and BB matrices are * * destroyed to spare storage location. * * --------------------------------------- * * INPUTS: AA MATRIX N*N * * BB MATRIX N*M * * --------------------------------------- * * OUTPUS: AA INVERSE OF AA N*N * * DET DETERMINANT OF AA * * BB SOLUTION MATRIX N*M * * --------------------------------------- * * NOTA - If M=0 inversion of AA matrix * * only. * ******************************************/ void MATINV(int N,int M,MAT AA,MAT1 BB,double *DET) { IVEC PC,PL; VEC CS; double PV,PAV,temp,TT; int I,IK,J,JK,K; // Initializations : *DET= 1.0; for (I=1; I<=N; I++) { PC[I]= 0; PL[I]= 0; CS[I]= 0.0; } // Main loop for (K=1; K<=N; K++) { // Searching greatest pivot : PV=AA[K][K]; IK=K; JK=K; PAV= ABS(PV); for (I=K; I<=N; I++) for (J=K; J<=N; J++) { temp = ABS(AA[I][J]); if (temp > PAV) { PV=AA[I][J]; PAV= ABS(PV); IK=I; JK=J; } } // Search terminated, the pivot is in location I=IK, J=JK. // Memorizing pivot location: PC[K]=JK; PL[K]=IK; // DETERMINANT DET is actualised // If DET=0, ERROR MESSAGE and STOP // Machine dependant EPSMACH equals here 1e-20 if (IK!=K) *DET=-*DET; if (JK!=K) *DET=-*DET; *DET=*DET*PV; temp= ABS(*DET); if (temp < MACH_EPS) { printf("\n The determinant equals ZERO !!!\n"); return; } // POSITIONNING PIVOT IN K,K: if(IK!=K) for (I=1; I<=N; I++) { // EXCHANGE LINES IK and K of matrix AA: TT=AA[IK][I]; AA[IK][I]=AA[K][I]; AA[K][I]=TT; } if(M!=0) for (I=1; I<=M; I++) { TT=BB[IK][I]; BB[IK][I]=BB[K][I]; BB[K][I]=TT; } // PIVOT is at correct line if(JK!=K) for (I=1; I<=N; I++) { // EXCHANGE COLUMNS JK and K of matrix AA: TT=AA[I][JK]; AA[I][JK]=AA[I][K]; AA[I][K]=TT; } // The PIVOT is at correct column. // and is located in K,K. // Column K of matrix AA is stored in CS vector // then column K is set to zero. for (I=1; I<=N; I++) { CS[I]=AA[I][K]; AA[I][K]= 0.0; } CS[K]= 0.0; AA[K][K]= 1.0; // Line K of matrix AA is modified: temp= ABS(PV); if(temp < MACH_EPS) { printf("\n PIVOT TOO SMALL - STOP\n"); return; } for (I=1; I<=N; I++) AA[K][I]=AA[K][I]/PV; if (M!=0) for (I=1; I<=M; I++) BB[K][I]=BB[K][I]/PV; // Other lines of matrix AA are modified: for (J=1; J<=N; J++) { if (J==K) J++; for (I=1; I<=N; I++) // Line J of matrix AA is modified: AA[J][I]=AA[J][I]-CS[J]*AA[K][I]; if (M!=0) for (I=1; I<=M; I++) BB[J][I]=BB[J][I]-CS[J]*BB[K][I]; } // Line K is ready } //End of K loop // MATRIX AA INVERSION IS DONE - REARRANGEMENT OF MATRIX AA // EXCHANGE LINES for (I=N; I>0; I--) { IK=PC[I]; if (IK==I) goto fin1; // EXCHANGE LINES I and PC(I) of matrix AA: for (J=1; J<=N; J++) { TT=AA[I][J]; AA[I][J]=AA[IK][J]; AA[IK][J]=TT; } if (M!=0) for (J=1; J<=M; J++) { TT=BB[I][J]; BB[I][J]=BB[IK][J]; BB[IK][J]=TT; } // NO MORE EXCHANGE is NECESSARY // Go to next line fin1: ; } // for i // EXCHANGE COLUMNS: for (J=N; J>0; J--) { JK=PL[J]; if (JK==J) goto fin2; // EXCHANGE COLUMNS J ET PL(J) of matrix AA: for (I=1; I<=N; I++) { TT=AA[I][J]; AA[I][J]=AA[I][JK]; AA[I][JK]=TT; } fin2: ; // NO MORE EXCHANGE is NECESSARY // Go to next column. } // REARRANGEMENT TERMINATED. return; } //Matinv() void main() { fp = fopen("ndof4.txt","w"); printf("\n How many degrees of freedom (d.o.f.): "); scanf("%d", &N); //Data section printf("\n Mass Matrix:\n"); for (I=1; I<=N; I++) for (J=I; J<=N; J++) { printf(" M(%d,%d) = ",I,J); scanf("%lf", &tmp); M[I][J] = tmp; } printf("\n Damping Matrix:\n"); for (I=1; I<=N; I++) for (J=I; J<=N; J++) { printf(" C(%d,%d) = ",I,J); scanf("%lf", &tmp); C[I][J] = tmp; } printf("\n Stiffness Matrix:\n"); for (I=1; I<=N; I++) for (J=I; J<=N; J++) { printf(" K(%d,%d) = ",I,J); scanf("%lf", &tmp); K[I][J] = tmp; } //Complete Matrices by symmetry for (I=1; I<=N; I++) for (J=I; J<=N; J++) { K[J][I] = K[I][J]; C[J][I] = C[I][J]; M[J][I] = M[I][J]; } printf("\n"); printf(" Starting time (sec.) = "); scanf("%lf", &T0); printf(" Ending time......... = "); scanf("%lf", &T3); printf(" Time increment...... = "); scanf("%lf", &D); printf("\n"); for (I=1; I<=N; I++) { printf(" Starting motion X(%d) = ", I); scanf("%lf", &tmp); X0[I]=tmp; } printf("\n"); for (I=1; I<=N; I++) { printf(" Starting speed V(%d) = ", I); scanf("%lf", &tmp); V0[I]=tmp; } printf("\n"); for (I=1; I<=N; I++) { printf(" Starting acceleration G(%d) = ", I); scanf("%lf", &tmp); G0[I]=tmp; } printf("\n Theta = "); scanf("%lf", &T2); printf("\n Number of force componant: "); scanf("%d", &N1); printf("\n"); printf(" Force (maximum) = "); scanf("%lf", &Fmax); printf(" Force frequency = "); scanf("%lf", &freq); F = Fmax * sin(2.0 * PI * freq * T0); //end of data section fprintf(fp,"\n"); fprintf(fp, " Time=%f Force=%f\n", T0, F); fprintf(fp, " Node Displacement Speed Acceleration\n"); fprintf(fp, " ---------------------------------------\n"); for (I=1; I<=N; I++) fprintf(fp, " %d %f %f %f\n", I, X0[I], V0[I], G0[I]); for (I=1; I<=N; I++) F0[I] = 0.0; F0[N1] = F; A9 = T2 * D; A0 = 6.0 / (A9*A9); A1 = 3.0 / A9; A2 = 2.0 * A1; A3 = A9 / 2.0; A4 = A0 / T2; A5 = -A2 / T2; A6 = 1.0 - 3.0 / T2; A7 = D / 2.0; A8 = D*D / 6.0; // Construct K+A0*M+A1*C // --------------------- e500: for (I=1; I<=N; I++) F1[I] = 0.0; T1 = T0 + D; F = Fmax * sin(2.0 * PI * freq * T1); fprintf(fp, " Time=%f Force=%f\n", T1, F); F1[N1] = F; for (I=1; I<=N; I++) F2[I] = 0.0; for (I=1; I<=N; I++) { F2[I] = F0[I] + T2 * (F1[I] - F0[I]); for (J=1; J<=N; J++) { K1[I][ J] = K[I][ J] + A0 * M[I][ J] + A1 * C[I][ J]; F2[I] = F2[I] + M[I][ J] * (A0 * X0[J] + A2 * V0[J] + 2.0 * G0[J]); F2[I] = F2[I] + C[I][ J] * (A1 * X0[J] + 2.0 * V0[J] + A3 * G0[J]); } } //K1=INV(K1) MATINV(N,0,K1,B1,&DET); //X2=K1 MPY F2 for (I=1; I<=N; I++) { SUM = 0.0; for (J=1; J<=N; J++) SUM += K1[I][J] * F2[J]; X2[I] = SUM; } for (I=1; I<=N; I++) { G1[I] = A4 * (X2[I] - X0[I]) + A5 * V0[I] + A6 * G0[I]; V1[I] = V0[I] + A7 * (G1[I] + G0[I]); X1[I] = X0[I] + D * V0[I] + A8 * (G1[I] + 2.0 * G0[I]); if (ABS(X1[I]) > XMAX) XMAX = X1[I]; } for (I=1; I<=N; I++) fprintf(fp, " %d %f %f %f\n", I, X1[I], V1[I], G1[I]); for (I=1; I<=N; I++) { X0[I] = X1[I]; V0[I] = V1[I]; G0[I] = G1[I]; } T0 = T1; if (T1 < T3) goto e500; // next time step printf("\n Results in file ndof4.txt...\n\n"); fprintf(fp, "\n Maximum Displacement (node #1) = %f\n\n", XMAX); fclose(fp); } // end of file ndof4.cpp