#include <math.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <conio.h>

#define SWITCHI(aaa,bbb); \
itemp=aaa;\
aaa=bbb;\
bbb=itemp;\

#define SWITCH(aaa,bbb); \
temp=aaa;\
aaa=bbb;\
bbb=temp;\

#define IDENTITY(aaa,nnn) \
for(i=0; i<nnn; i++)\
{ for(j=0; j<nnn; j++)\
{aaa[i][j]=0.;}aaa[i][i]=1.;}


#define ZERO(aaa,nnn,mmm) \
for(i=0; i<nnn; i++)\
{ for(j=0; j<mmm; j++)\
{aaa[i][j]=0.;}}

#define ZEROG(aaa,mmm,nnn) \
for(i=0; i<mmm; i++)\
{ for(j=0; j<nnn; j++)\
{aaa[i][j]=0.;}}

#define ZEROV(aaa) \
for(i=0; i<n; i++)\
{aaa[i]=0.;}


#define MATMUL(ccc,aaa,bbb,nra,nca,ncb) \
for(i=0; i<nra; i++)\
{for(j=0; j<ncb; j++)\
{for(k=0; k<nca; k++)\
{ ccc[i][j]+=aaa[i][k]*bbb[k][j];	}}}


#define MATMUL_TRANS(ccc,aaa,bbb,nra,nca,ncb) \
for(i=0; i<nra; i++)\
{for(j=0; j<ncb; j++)\
{for(k=0; k<nca; k++)\
{ ccc[i][j]+=aaa[k][i]*bbb[k][j];	}}}


#define MATEQUAL(aaa,bbb) \
for(iii=0; iii<n; iii++)\
{for(jjj=0; jjj<n; jjj++)\
{aaa[iii][jjj]=bbb[iii][jjj];}}\

#define TRANSPOSE(aaa,bbb) \
for(i=0; i<n; i++)\
{ for(j=0; j<n; j++)\
{aaa[i][j]=bbb[j][i];}}

#define PRINTMAT(aaa,nnn,mmm) \
for(iii=0; iii<nnn; iii++)\
{ for(jjj=0; jjj<mmm; jjj++)\
{printf(" %11.4g \t",aaa[iii][jjj]);}printf("\n");}

#define MAX 60000

#define MAX2 MAX*MAX

#define MAXN int(MAX/6.)+1

void input(void);
void output(void);
void GRB_matrix(void);

float refx,refy,refz;

double anum;

double GRB[MAX][6];

float a[MAX][6];

float mass[MAX];

float M2[6][6];

int mass_index[MAX][2];

float nodex[MAXN];
float nodey[MAXN];
float nodez[MAXN];

long i,j,k;

long ijk;

long iii,jjj;

long d2;

long nrg;

long nmds;

long n,nnn,mmm;
long mnz;

long iu;
long imu;

long node_num;

double mass_scale;

char mass_label[20];
char inertia_label[20];
char loc_label[20];

unsigned long nmn=MAX;
     
FILE *pFile[10];
char filename[10][FILENAME_MAX];

int main()
{
     printf("\n\n");
     printf(" mass_properties_GRB.cpp  ver 1.1  March 24, 2010 \n");
     printf(" by Tom Irvine \n\n");

     printf(" This program calculates the mass properties for a dynamic system \n");
     printf(" from its unconstrained mass and nodal location vector. \n\n");
     
     printf(" The model must have 6 dof per node. \n\n");
     
     printf("\n The maximum matrix size is: (%ld x %ld) \n",nmn,nmn);

     input();
     
     GRB_matrix();

     int mn=6;
     
     long ndof=long(node_num*6);

     ZERO(a,ndof,mn)  

     for(ijk=0;ijk<=mnz;ijk++)
     {
           i=mass_index[ijk][0];
           k=mass_index[ijk][1];
     
           for(j=0; j<mn; j++)
           {    
                
                if(fabs(GRB[k][j])>1.0e-10)
                {
                   a[i][j]+=mass[ijk]*GRB[k][j];   
               }  
           }                       
     }

 
     ZERO(M2,mn,mn)
       
     MATMUL_TRANS(M2,GRB,a,mn,ndof,mn)  

    
     output();

	 printf("\n\n Calculation complete. \n Press any key to exit.");
     getch();  
     
}
void output(void)
{
    printf("\n Output \n");
 
    fprintf(pFile[2],"\n Rigid-body Mass Matrix\n\n");
          
     for(i=0; i<d2; i++)
     {  
        for(j=0; j<d2; j++)
        {              
           if(fabs(M2[i][j])<1.0e-10)
           {
              M2[i][j]=0.;                       
           }            
           fprintf(pFile[2],"%8.4g \t",M2[i][j]);
        }
        fprintf(pFile[2],"\n");	       		
     }    

     
     printf("\n\n Rigid-body Mass Matrix\n\n");
     PRINTMAT(M2,d2,d2)
     
     double xg,yg,zg;
     
     double mmass=(M2[0][0]+M2[1][1]+M2[2][2])/3.;
     
     xg= M2[1][5]/mmass;
     yg=-M2[0][5]/mmass;
     zg= M2[0][4]/mmass;
     
 
     
     printf("\n\n Reference point (%s)\n\n",loc_label);

     printf(" refx = %8.4g  \n",refx);
     printf(" refy = %8.4g  \n",refy);
     printf(" refz = %8.4g  \n",refz);   
     
     fprintf(pFile[2],"\n\n Reference point (%s)\n\n",loc_label);

     fprintf(pFile[2]," refx = %8.4g  \n",refx);
     fprintf(pFile[2]," refy = %8.4g  \n",refy);
     fprintf(pFile[2]," refz = %8.4g  \n",refz);        
     

     printf("\n\n CG coordinates relative to reference point (%s)\n\n",loc_label);

     printf(" xg = %8.4g  \n",xg);
     printf(" yg = %8.4g  \n",yg);
     printf(" zg = %8.4g  \n",zg);   
     
     fprintf(pFile[2],"\n\n CG coordinates relative to reference point (%s)\n\n",loc_label);

     fprintf(pFile[2]," xg = %8.4g  \n",xg);
     fprintf(pFile[2]," yg = %8.4g  \n",yg);
     fprintf(pFile[2]," zg = %8.4g  \n",zg);      
     
     
     printf("\n\n Mass Values  (%s) \n\n",mass_label);  
     
     printf(" mx= %8.4g \n",M2[0][0]*mass_scale);
     printf(" my= %8.4g \n",M2[1][1]*mass_scale);   
     printf(" mz= %8.4g \n",M2[2][2]*mass_scale);
     
     
     fprintf(pFile[2],"\n\n Mass Values  (%s) \n\n",mass_label);
     
     fprintf(pFile[2]," mx= %8.4g \n",M2[0][0]*mass_scale);
     fprintf(pFile[2]," my= %8.4g \n",M2[1][1]*mass_scale);   
     fprintf(pFile[2]," mz= %8.4g \n",M2[2][2]*mass_scale);    
     
     
     double Ixx,Ixy,Ixz,Iyy,Iyz,Izz;
     double Ixx_cg,Ixy_cg,Ixz_cg,Iyy_cg,Iyz_cg,Izz_cg;
          
     Ixx=M2[3][3];
     Ixy=M2[3][4];
     Ixz=M2[3][5];
     Iyy=M2[4][4];
     Iyz=M2[4][5];
     Izz=M2[5][5];  
     
     Ixx_cg=Ixx-mmass*(pow(yg,2)+pow(zg,2));
     Iyy_cg=Iyy-mmass*(pow(xg,2)+pow(zg,2));
     Izz_cg=Izz-mmass*(pow(xg,2)+pow(yg,2));

     Ixy_cg=fabs(Ixy)-fabs(mmass*(xg*yg));
     Ixz_cg=fabs(Ixz)-fabs(mmass*(xg*zg));
     Iyz_cg=fabs(Iyz)-fabs(mmass*(yg*zg));
     
     float Imin;
     
     Imin=fabs(Ixx_cg);
     
     if(Imin<fabs(Iyy_cg))
     {
         Imin=fabs(Iyy_cg);
     }
     if(Imin<fabs(Izz_cg))
     {
         Imin=fabs(Izz_cg);
     }

     Imin/=10000;
     
     
     if(fabs(Ixy_cg) < Imin )
     {
            Ixy_cg=0.;                  
     }     
     if(fabs(Ixz_cg) < Imin )
     {
            Ixz_cg=0.;                  
     }  
     if(fabs(Iyz_cg) < Imin )
     {
            Iyz_cg=0.;                  
     }     



     printf("\n\n Mass Moments of Inertia Matrix about CG (%s)\n\n",inertia_label);
    
     printf(" %8.4g \t %8.4g \t %8.4g \n",Ixx_cg,Ixy_cg,Ixz_cg);  
     printf(" %8.4g \t %8.4g \t %8.4g \n",Ixy_cg,Iyy_cg,Iyz_cg);   
     printf(" %8.4g \t %8.4g \t %8.4g \n",Ixz_cg,Iyz_cg,Izz_cg);
     
     
     fprintf(pFile[2],"\n\n Mass Moments of Inertia Matrix about CG (%s)\n\n",inertia_label);
    
     fprintf(pFile[2]," %8.4g \t %8.4g \t %8.4g \n",Ixx_cg,Ixy_cg,Ixz_cg);  
     fprintf(pFile[2]," %8.4g \t %8.4g \t %8.4g \n",Ixy_cg,Iyy_cg,Iyz_cg);   
     fprintf(pFile[2]," %8.4g \t %8.4g \t %8.4g \n",Ixz_cg,Iyz_cg,Izz_cg);   

     
     printf("\n\n Output file = %s \n\n",filename[2]);
   
     fclose(pFile[2]);      
    
}
void GRB_matrix(void)
{
    printf("\n GRB Matrix \n");

    long jk;

    refx=0;
    refy=0;
    refz=0;
    
    float dx;
    float dy;
    float dz;

    for(i=0;i<node_num;i++)    
    {
       dx=-refx+nodex[i];
       dy=-refy+nodey[i];
       dz=-refz+nodez[i];
       
       jk=i*6; 
               
       GRB[jk+1][3]=-dz;
       GRB[jk+2][3]= dy;  
       GRB[jk+0][4]= dz;
       GRB[jk+2][4]=-dx;  
       GRB[jk+0][5]=-dy;
       GRB[jk+1][5]= dx; 

       GRB[jk+0][0]=1;
       GRB[jk+1][1]=1;  
       GRB[jk+2][2]=1;
       GRB[jk+3][3]=1;  
       GRB[jk+4][4]=1;
       GRB[jk+5][5]=1;  

    } 
    
    fprintf(pFile[2],"\n GRB Matrix \n");  
    
    for(i=0;i< long(node_num*6);i++)    
    {
       for(j=0;j<6;j++)
       {
          fprintf(pFile[2]," %8.4g\t", GRB[i][j]);
       }
       fprintf(pFile[2]," \n");     
    }

}
void input(void)
{
    printf("\n Enter the units system \n");
    printf(" 1=English  2=metric ");
    scanf("%d",&iu);

    mass_scale=1.;

    if(iu==1)
    {
        strcpy(loc_label,"inch");     
        strcpy(inertia_label,"lbf sec^2 in");                         
        strcpy(mass_label,"lbf sec^2/in");     
             
        printf("\n Select input mass unit \n");
        printf("  1=lbm  2=lbf sec^2/in  \n");
        scanf("%d",&imu);
        if(imu==1)
        {
           mass_scale=386.;
           strcpy(mass_label,"lbm");
        }
    }
    else
    {
        strcpy(loc_label,"meters");   
        strcpy(inertia_label,"kg m^2");    
                        
        printf(" mass unit = kg");
        strcpy(mass_label,"kg");
    }


    while(pFile[0] == NULL )
    {
       printf("\n\n Enter the input unconstrained mass matrix filename (%s)\n",mass_label);
       scanf("%s",filename[0]);   
       pFile[0] = fopen(filename[0], "rb");                  
    }  
   
 
    while(pFile[1] == NULL )
    {
       printf("\n Enter the input vector filename  (%s)\n",loc_label);
       scanf("%s",filename[1]);   
       pFile[1] = fopen(filename[1], "rb");                  
    }  

    printf("\n\n Enter the output reduced mass matrix filename \n");
    scanf("%s",filename[2]);   
    pFile[2] = fopen(filename[2], "w"); 
    

// reading

    n=0;
    while(1)
    {
      if(fscanf(pFile[0],"%lf",&anum)<=0)
      {
         break;
      }    
      else
      {
//         printf(" n=%ld  anum=%8.4g \n",n,anum); 
      }           
      n++;
    }
    rewind(pFile[0]);
   
    n=long(sqrt(n));

    if( n<2 || n>MAX)
	{
		printf("\n\n Maximum number of dofs exceeded. \n");
		printf("\n Press any key to exit.");
		getch();
		exit(1);
	}  
	else
	{
        printf("\n n=%ld \n",n);
    }

    printf("\n reading mass \n");
    
//*****************************************   

    long jvm=0;

    long ijk=0;
    
    for(i=0; i< n; i++)
    {
			for(j=0; j<n; j++)
			{
				if( fscanf(pFile[0],"%lf",&anum) <=0 )
				{break;}
				
				jvm++;
			
				 if(fabs(anum)> 1.0e-12)
				 {
				   mass[ijk]=float(anum/mass_scale);
				   mass_index[ijk][0]=i;
				   mass_index[ijk][1]=j;
                   				   
//				   printf("mass_index[%ld][%ld]=%d   mass[%ld]=%8.4g\n",i,j,mass_index[i][j],ijk,mass[ijk]);
				   
				   ijk++;
                 }
			}
    }
   
    
    mnz=ijk-1;
    
     printf("\n reading nodes \n"); 
    
    node_num=0;
    
    while(1)
    {
      iii=node_num;
      
      if(fscanf(pFile[1],"%f %f %f",&nodex[iii],&nodey[iii],&nodez[iii])<=0)
      {
         break;
      }    
      else
      {
//         printf(" n=%ld  anum=%8.4g \n",n,anum); 
      }           
      node_num++;
      
      if(node_num==MAXN)
      {
         break;          
      }
    }    
    
    if( long(node_num*6) !=n)
    {
        printf("\n\n Input Error \n");       
        printf("\n  iii=%ld  n=%ld  \n",iii,n);
        
        printf("\n press any key to exit \n");
        getch();
        exit(1);      
    }
     
    
    fclose(pFile[0]);
    fclose(pFile[1]);  

     nrg=6;
     
     d2=nrg;
     nmds=n-d2;
     
        
}