#include<stdio.h>
#include<math.h>
#define Nmax 11
void MATINV(int n,double a[Nmax][Nmax],int *er);
void MASSMAT(double am[Nmax][Nmax]);
/*--------------------------------------------------------------------*/

int main()
{
    int n=6;
    double a[Nmax][Nmax];
    int er;
    int i,j,k;
    double b[Nmax][Nmax];
    double c[Nmax][Nmax];

    /*
    a[1][1]= 0.35355339;a[1][2]=-0.35355339;a[1][3]=0         ;a[1][4]= 0;
    a[2][1]=-0.35355339;a[2][2]= 1.35355339;a[2][3]=0         ;a[2][4]=-1;
    a[3][1]= 0         ;a[3][2]= 0         ;a[3][3]=1.35355339;a[3][4]= 0.35355339;
    a[4][1]= 0         ;a[4][2]=-1         ;a[4][3]=0.35355339;a[4][4]= 1.35355339;
    */

    MASSMAT(a); /*n=6*/

    for(i=1;i<=n;i++){
        for(j=1;j<=n;j++){
            b[i][j]=a[i][j];
        }
    }

    MATINV(n,a,&er);

    printf("error=%d\n",er);
    printf("逆行列\n");
    for(i=1;i<=n;i++){
        printf("%f %f %f %f %f %f\n",a[i][1],a[i][2],a[i][3],a[i][4],a[i][5],a[i][6]);
    }

    for(i=1;i<=n;i++){
        for(j=1;j<=n;j++){
            c[i][j]=0.0;
            for(k=1;k<=n;k++){
                c[i][j]=c[i][j]+a[i][k]*b[k][j];
            }
        }
    }
    printf("検算結果\n");
    for(i=1;i<=n;i++){
        printf("%f %f %f %f %f %f\n",c[i][1],c[i][2],c[i][3],c[i][4],c[i][5],c[i][6]);
    }

}
//------------------------------------------------------------------------------


void MATINV(int n,double a[Nmax][Nmax],int *er)
{
    /*****************************************/
    /*  逆行列計算                           */
    /*  山田嘉昭著：マトリックス法材料力学   */
    //****************************************/
    int i,j,k,l,l1,irow=0,icol=0,jrow=0,jcol=0;
    double amax,s,t;
    double determ=1.0;
    double factor=1.0e-30;
    int ipivot[n+1];
    double pivot[n+1];
    int index[n+1][3];

    for(j=1;j<=n;j++){
        ipivot[j]=0;
    }

    for(i=1;i<=n;i++){

        amax=0.0;
        for(j=1;j<=n;j++){
            if(ipivot[j]-1!=0){
                for(k=1;k<=n;k++){
                    if(0<ipivot[k]-1)goto owari;
                    if(ipivot[k]-1<0){
                        if(fabs(amax)-fabs(a[j][k])<0){
                            irow=j;
                            icol=k;
                            amax=a[j][k];
                        }
                    }
                }
            }
        }
        ipivot[icol]=ipivot[icol]+1;

        if((irow+icol)%2!=0){
            determ=-determ;
        }
        if(irow-icol!=0){
            for(l=1;l<=n;l++){
                s=a[irow][l];
                a[irow][l]=a[icol][l];
                a[icol][l]=s;
            }
        }
        index[i][1]=irow;
        index[i][2]=icol;
        pivot[i]=a[icol][icol];
        if(fabs(pivot[i])<factor)goto okashii;
        if(determ<=1.0E+30){
            determ=determ*pivot[i];
        }
        a[icol][icol]=1.0;
        for(l=1;l<=n;l++){
            a[icol][l]=a[icol][l]/pivot[i];
        }

        for(l1=1;l1<=n;l1++){
            if(l1-icol!=0){
                t=a[l1][icol];
                a[l1][icol]=0.0;
                for(l=1;l<=n;l++){
                    a[l1][l]=a[l1][l]-a[icol][l]*t;
                }
            }
        }

    }

    for(i=1;i<=n;i++){
        l=n+1-i;
        if(index[l][1]-index[l][2]!=0){
            jrow=index[l][1];
            jcol=index[l][2];
            for(k=1;k<=n;k++){
                s=a[k][jrow];
                a[k][jrow]=a[k][jcol];
                a[k][jcol]=s;
            }
        }
    }

    owari:
        *er=0; /*正常終了*/
        return;

    okashii:
        *er=1; /*異常終了*/

}
//------------------------------------------------------------------------------

void MASSMAT(double am[Nmax][Nmax])
{
    /*****************************/
    /* 平面骨組要素質量行列      */
    /*****************************/
    int i,j;
    double AL=1.0;

    for(i=1;i<=6;i++){
        for(j=1;j<=6;j++){
            am[i][j]=0.0;
        }
    }

    am[1][1]=1.0/3.0;
    am[2][1]=0.0;
    am[3][1]=0.0;
    am[4][1]=1.0/6.0;
    am[5][1]=0.0;
    am[6][1]=0.0;
    am[2][2]=13.0/35.0;
    am[3][2]=11.0*AL/210.0;
    am[4][2]=0.0;
    am[5][2]=9.0/70.0;
    am[6][2]=-13.0*AL/420.0;
    am[3][3]=AL*AL/105.0;
    am[4][3]=0.0;
    am[5][3]=13.0*AL/420.0;
    am[6][3]=-AL*AL/140.0;
    am[4][4]=1.0/3.0;
    am[5][4]=0.0;
    am[6][4]=0.0;
    am[5][5]=13.0/35.0;
    am[6][5]=-11.0*AL/210.0;
    am[6][6]=AL*AL/105.0;

    /*対称項セット*/
    for(i=1;i<=5;i++){
        for(j=i+1;j<=6;j++){
            am[i][j]=am[j][i];
        }
    }

}
//------------------------------------------------------------------------------