/*二質量浮筏振動頻率計算(98.6),以及對于不同質量比,阻尼比
和干擾激勵頻率時的隔振效果計算,有數據壓縮防溢出功能(98.4)
對每個彈性支承均求旋轉變換矩陣,可用于輻射式布置(98.4)*/

/*矩陣相加*/
void MATADD(double A[][6],double B[][6],double (*C)[6],int M,int N)
{ int I,J;
  for(I=0;I<M;I++)
    for(J=0;J<N;J++)
    *(*(C+I)+J)=A[I][J]+B[I][J];
  }



void MATMPY(double A[][6],double B[][6],double (*C)[6],int M,int N,int R8) /*矩陣相乘*/
{ int I,J,K;
  for(I=0;I<M;I++)
    for(J=0;J<R8;J++)
    {
     **C=0;
     for(K=0;K<N;K++)
    *(*(C+I)+J)=*(*(C+I)+J)+A[I][K]*B[K][J];
     }
}


void ZMATX(double (*A)[6],int N)                          /*矩陣轉置*/
{ int I,J,L,KU1;
  double W1;
   KU1=N-1;
    for(I=0;I<KU1;I++)
    {
     L=I+1;
     for(J=L;J<N;J++)
     {
      W1=*(*(A+I)+J);
      *(*(A+I)+J)=*(*(A+J)+I);
      *(*(A+J)+I)=W1;
      }
     }
  }


void DA(double (*A)[6],double B[][6],int M,int N)       /*將矩陣B賦值給矩陣A*/
 {
   int I,J;
   for(I=0;I<M;I++)
     for(J=0;J<N;J++)
      *(*(A+I)+J)=B[I][J];
 }



 

 /*求坐標變換矩陣D和T,總坐標系下的子剛度矩陣K1*/
void DTMATX(double (*K1)[6],double K0[][6],double A[],double B[],double C[],int N,double R[][6])
 {
  int Z,I,J,L;
  double T[6][6],F[3][3],D[3][3],T1[6][6],T2[6][6];
    for(I=0;I<3;I++)
      for(J=0;J<3;J++)
      if(I==J)F[I][J]=1.0;
      else F[I][J]=0.0;
   D[0][0]=0.0;
   D[1][1]=0.0;
   D[2][2]=0.0;
   D[0][1]=-C[N-1];
   D[0][2]=B[N-1];
   D[1][0]=C[N-1];
   D[1][2]=-A[N-1];
   D[2][0]=-B[N-1];
   D[2][1]=A[N-1];
    for(I=0;I<3;I++)
      for(J=0;J<3;J++)
      T[I][J]=F[I][J];
    for(I=3;I<6;I++)
      for(J=3;J<6;J++)
      {L=I-3;
       Z=J-3;
       T[I][J]=F[L][Z];
       }
   for(I=3;I<6;I++)
      for(J=0;J<3;J++)
      {L=I-3;
       T[I][J]=D[L][J];
       }
   for(I=0;I<3;I++)
      for(J=3;J<6;J++)
      T[I][J]=0.0;

   MATMPY(T,R,T1,6,6,6);
   MATMPY(T1,K0,T2,6,6,6);
   ZMATX(T1,6);
   MATMPY(T2,T1,K1,6,6,6);

 }


 #include<stdio.h>
 #include<math.h>
 main()
 {
  int NG,NG1,NK,NJ,NK1,NK2,N,H,Z,I,J,J1,L,L1,L2,JJ;
  double AA1[3],BB1[3],C1[3],C2[3],CC[3],CC1[3],CC2[3],CC3[3],DD3[3],DD1[3];
  double DD2[3],Y1,Y2,Y3,Y4,R1[3][3],S5[6][6],W5[6][6],M[12][12],M01[6][6];
  double M02[6][6],KB[6],KC[6],KK[6][6],T[3],I1[3],I2[3],K11[6][6],K22[6][6];
  double M11[6][6],M22[6][6],GX,GY,GZ,K[12][12],M1,M2,A[40],B[40],C[40],ALF[120];
  double BET[120],GAM[120],K01[6][6],K02[6][6],WW,WW1,WW2,D1,D2,DD,WN[12],PACE;
  double K1[3],K2[3],F0,S,U,W,W1,TA[7],R[6][6],SJ[6][6],WJ[6][6],F;
  printf("輸入上層柴油機質量M2:\n");
  scanf("%f",&M2);
  printf("輸入筏體質量M1:\n");
  scanf("%f",&M1);
  printf("輸入筏體轉動慣量I1[3]:\n");
  for(I=0;I<3;I++)
   scanf("%f",&I1[I]);
  printf("輸入上層柴油機轉動慣量I2[3]:\n");
  for(I=0;I<3;I++)
   scanf("%f",&I2[I]);
  printf("輸入筏體下彈簧總數NJ:\n");
  scanf("%d",&NJ);
  printf("輸入彈簧總數NK:\n");
  scanf("%d",&NK);
  printf("輸入彈簧總數+質量總數 NG:\n");
  scanf("%d",&NG);
   printf("輸入彈簧和質量X坐標值A[NG]:\n");
  for(I=0;I<NG;I++)
   scanf("%f",&A[I]);
   printf("輸入彈簧和質量Y坐標值B[NG]:\n");
  for(I=0;I<NG;I++)
   scanf("%f",&B[I]);
  printf("輸入彈簧和質量Z坐標值C[NG]:\n");
  for(I=0;I<NG;I++)
   scanf("%f",&C[I]);
  printf("輸入上層彈簧剛度KB[6]:\n");
  for(I=0;I<6;I++)
   scanf("%f",&KB[I]);
  printf("輸入下層彈簧剛度KC[6]:\n");
  for(I=0;I<6;I++)
   scanf("%f",&KC[I]);
  NG1=2*NG;
  printf("輸入每個彈簧剛性主軸及質量慣性軸與總坐標的夾角(度)ALF[NG1]:\n");
  for(I=0;I<NG1;I++)
   scanf("%f",&ALF[I]);
   printf("輸入每個彈簧剛性主軸及質量慣性軸與總坐標的夾角(度)BET[NG1]:\n");
  for(I=0;I<NG1;I++)
   scanf("%f",&BET[I]);
   printf("輸入每個彈簧剛性主軸及質量慣性軸與總坐標的夾角(度)GAM[NG1]:\n");
  for(I=0;I<NG1;I++)
   scanf("%f",&GAM[I]);
  U=M1/M2;
  for(I=0;I<3;I++)
  I1[I]=I1[I]*U/1.0682;
   for(L=0;L<3;L++)
     {K1[L]=NJ*KC[L];
      K2[L]=(NK-NJ)*KB[L];
     }
   for(N=0;N<NG1;N++)
     {ALF[N]=ALF[N]*3.1415926/180;
      BET[N]=BET[N]*3.1415926/180;
      GAM[N]=GAM[N]*3.1415926/180;
      }

   for(L=0;L<6;L++)             /*求K01[6],K02[6]*/
     for(J=0;J<6;J++)
      if(L==J)
        {K01[L][J]=KC[L];
         K02[L][J]=KB[L];
         }
      else
        {K01[L][J]=0.0;
         K02[L][J]=0.0;
         }
   for(L=0;L<6;L++)             /*求S5[6],W5[6]*/
     for(J=0;J<6;J++)
        {S5[L][J]=0.0;
         W5[L][J]=0.0;
         }

    for(N=0;N<NJ;N++)
    { for(L=0;L<3;L++)
        for(J=0;J<3;J++)
          R1[L][J]=0.0;
      L1=3*(N-1);
      R1[0][0]=(double)cos(ALF[L1+0]);
      R1[1][0]=(double)cos(ALF[L1+1]);
      R1[2][0]=(double)cos(ALF[L1+2]);
      R1[0][1]=(double)cos(BET[L1+0]);
      R1[1][1]=(double)cos(BET[L1+1]);
      R1[2][1]=(double)cos(BET[L1+2]);
      R1[0][2]=(double)cos(GAM[L1+0]);
      R1[1][2]=(double)cos(GAM[L1+1]);
      R1[2][2]=(double)cos(GAM[L1+2]);
      for(L=0;L<3;L++)
        for(J=0;J<3;J++)
         R[L][J]=R1[L][J];
      for(H=3;H<6;H++)
        for(J=3;J<6;J++)
         {L=H-3;
          Z=J-3;
          R[H][J]=R1[L][Z];
          }
       for(L=0;L<3;L++)
        for(J=3;J<6;J++)
          R[L][J]=0.0;
       for(L=3;L<6;L++)
        for(J=0;J<3;J++)
          R[L][J]=0.0;

       DTMATX(KK,K01,A,B,C,N,R);       /*得到子剛度矩陣KK[6][6]*/
       MATADD(S5,KK,SJ,6,6);           
       DA(S5,SJ,6,6);                  /*將SJ[6][6]賦值給S5[6][6]*/
     }
   H=NJ;
   for(N=H;N<NK;N++)
    {  for(L=0;L<3;L++)
         for(J=0;J<3;J++)
           R1[L][J]=0.0;
        L2=3*(N-1);
      R1[0][0]=(double)cos(ALF[L2+0]);
      R1[1][0]=(double)cos(ALF[L2+1]);
      R1[2][0]=(double)cos(ALF[L2+2]);
      R1[0][1]=(double)cos(BET[L2+0]);
      R1[1][1]=(double)cos(BET[L2+1]);
      R1[2][1]=(double)cos(BET[L2+2]);
      R1[0][2]=(double)cos(GAM[L2+0]);
      R1[1][2]=(double)cos(GAM[L2+1]);
      R1[2][2]=(double)cos(GAM[L2+2]);
       for(L=0;L<3;L++)
        for(J=0;J<3;J++)
          R[L][J]=R1[L][J];
       for(H=3;H<6;H++)
        for(J=3;J<6;J++)
         {L=H-3;
          Z=J-3;
          R[H][J]=R1[L][Z];
          }
       for(L=0;L<3;L++)
        for(J=3;J<6;J++)
          R[L][J]=0.0;
       for(L=3;L<6;L++)
        for(J=0;J<3;J++)
          R[L][J]=0.0;
       DTMATX(KK,K02,A,B,C,N,R);        /*得到子剛度矩陣KK[6][6]*/
       MATADD(W5,KK,WJ,6,6);
       DA(W5,WJ,6,6);                   /*將WJ[6][6]賦值給W5[6][6]*/
     }
   MATADD(S5,W5,K11,6,6);              /*得到剛度子矩陣K11[6][6]*/
   printf("剛度子矩陣K11[6][6]:\n");
    for(L=0;L<6;L++)
       {for(J=0;J<6;J++)
          printf("%f  ",K11[L][J]);
         printf("\n");
        }
   DA(K22,W5,6,6);                     /*將W5[6][6]賦值給K22[6][6]*/
   printf("剛度子矩陣K22[6][6]:\n");   /*輸出剛度子矩陣K22[6][6]*/
    for(L=0;L<6;L++)
       {for(J=0;J<6;J++)
          printf("%f  ",K22[L][J]);
         printf("\n");
        }

   for(L=0;L<6;L++)
     for(J=0;J<6;J++)
      K[L][J]=K11[L][J];
   for(L=0;L<6;L++)
     for(J=6;J<12;J++)
     {Z=J-6;
      K[L][J]=-K22[L][Z];
      }
   for(H=6;H<12;H++)
      for(J=6;J<12;J++)
     {L=H-6;
      Z=J-6;
      K[H][J]=K22[L][Z];
      }
   for(H=6;H<12;H++)
      for(J=0;J<6;J++)
     {L=H-6;
      K[H][J]=-K22[L][J];
      }                               /*得到總剛度矩陣K[12][12]*/
   printf("總剛度矩陣K[12][12]:\n");
   for(L=0;L<12;L++)
     {for(J=0;J<12;J++)
          printf("%f  ",K[L][J]);
         printf("\n");
      }
   for(L=0;L<3;L++)
     for(J=0;J<3;J++)
     {M01[L][J]=0.0;
      M02[L][J]=0.0;
      }
   M01[0][0]=M1;
   M01[1][1]=M1;
   M01[2][2]=M1;
   M02[0][0]=M2;
   M02[1][1]=M2;
   M02[2][2]=M2;
   for(H=3;H<6;H++)
     for(J=3;J<6;J++)
     { L=H-3;
       if(H==J){M01[H][J]=I1[L];M02[H][J]=I2[L];}
       else {M01[H][J]=0.0;M02[H][J]=0.0;}
	 }                                 /*得到M01[6][6],M02[6][6]*/
   NK1=NK+1;
   NK2=NK+2;
   L1=3*(NK1-1);                 /*以下求中間筏體子矩陣*/
   L2=3*(NK2-1);
   for(L=0;L<3;L++)
     for(J=0;J<3;J++)
      R1[L][J]=0.0;
   R1[0][0]=cos(ALF[L2+0]);
   R1[1][0]=cos(ALF[L2+1]);
   R1[2][0]=cos(ALF[L2+2]);
   R1[0][1]=cos(BET[L2+0]);
   R1[1][1]=cos(BET[L2+1]);
   R1[2][1]=cos(BET[L2+2]);
   R1[0][2]=cos(GAM[L2+0]);
   R1[1][2]=cos(GAM[L2+1]);
   R1[2][2]=cos(GAM[L2+2]);           /*得到R1[3][3]*/
   for(L=0;L<3;L++)
     for(J=0;J<3;J++)
      R[L][J]=R1[L][J];
   for(H=3;H<6;H++)
     for(J=3;J<6;J++)
     {L=H-3;
      Z=J-3;
      R[H][J]=R1[L][Z];
      }
   for(L=0;L<3;L++)
     for(J=3;J<6;J++)
      R[L][J]=0.0;
   for(L=3;L<6;L++)
     for(J=0;J<3;J++)
      R[L][J]=0.0;                 /*得到R[6][6]*/ 
   DTMATX(M11,M01,A,B,C,NK2,R);    /*得到質量子矩陣M11[6][6]*/ 
   printf("質量子矩陣M11[6][6]:\n");
   for(L=0;L<6;L++)
     {for(J=0;J<6;J++)
          printf("%f  ",M11[L][J]);
         printf("\n");
      }
   for(L=0;L<3;L++)
     for(J=0;J<3;J++)
      R1[L][J]=0.0;
   R1[0][0]=cos(ALF[L1+0]);
   R1[1][0]=cos(ALF[L1+1]);
   R1[2][0]=cos(ALF[L1+2]);
   R1[0][1]=cos(BET[L1+0]);
   R1[1][1]=cos(BET[L1+1]);
   R1[2][1]=cos(BET[L1+2]);
   R1[0][2]=cos(GAM[L1+0]);
   R1[1][2]=cos(GAM[L1+1]);
   R1[2][2]=cos(GAM[L1+2]);
   for(L=0;L<3;L++)
     for(J=0;J<3;J++)
       R[L][J]=R1[L][J];
   for(H=3;H<6;H++)
     for(J=3;J<6;J++)
       {L=H-3;
        Z=J-3;
        R[H][J]=R1[L][Z];
        }
   for(L=0;L<3;L++)
     for(J=3;J<6;J++)
       R[L][J]=0.0;
   for(L=3;L<6;L++)
     for(J=0;J<3;J++)
       R[L][J]=0.0;                 /*得到R[6][6]*/
   DTMATX(M22,M02,A,B,C,NK1,R);     /*得到質量子矩陣M22[6][6]*/
   printf("質量子矩陣M22[6][6]:\n");
   for(I=0;I<6;I++)
     {for(J=0;J<6;J++)
          printf("%f  ",M22[L][J]);
         printf("\n");
      }
   for(L=0;L<12;L++)
     for(J=0;J<12;J++)
       M[L][J]=0.0;
   for(L=0;L<6;L++)
     for(J=0;J<6;J++)
       M[L][J]=M11[L][J];
   for(H=6;H<12;H++)
     for(J=6;J<12;J++)
       {L=H-6;
        Z=J-6;
        M[H][J]=M22[L][Z];
	    }                         /*得到總質量矩陣M[12][12]*/   
   printf("總質量矩陣M[12][12]:\n");
   for(I=0;I<12;I++)
     {for(J=0;J<12;J++)
          printf("%f  ",M[L][J]);
         printf("\n");
      }

}