disp(' ');
disp(' mass_properties.m   ver 1.2  March 22, 2010 ');
disp(' ');
disp(' by Tom Irvine   Email: tomirvine@aol.com ');
disp(' ');
disp(' This program calculates the mass properties for a dynamic system ');
disp(' from its unconstrained mass and stiffness matrices. ');
disp(' ');
disp(' The model must have 6 dof per node. ');
disp('  ');
%
clear acc;
clear dis;
clear vel;
clear k;
clear m;
clear damp;
clear Eigenvalues;
clear ModeShapes;
clear omega;
clear omegan;
clear fn;
clear MST;
clear ModalMass;
clear part;
clear QTMQ;
clear r;
clear t;
clear f1;
clear f2;
clear x;
clear d;
clear v;
clear na;
clear nv;
clear nd;
clear Q;
clear mass;
clear stiffness;
clear max;
clear kq;
clear mq;
%
tpi=2.*pi;
%
disp(' ');
disp(' Enter the units system ');
disp(' 1=English  2=metric ');
iu=input(' ');
%
mass_scale=1;
%
if(iu==1)
     disp(' Select input mass unit ');
     disp('  1=lbm  2=lbf sec^2/in  ');
     imu=input(' ');
     if(imu==1)
         mass_scale=386;
         mass_unit='lbm';
     else
         mass_unit='lbf sec^2/in';      
     end
else
    disp(' mass unit = kg ');
    mass_unit='kg';     
end
%
disp(' ');
if(iu==1)
    disp(' stiffness unit = lbf/in ');
else
    disp(' stiffness unit = N/m ');
end
%
%
disp(' ');
disp(' Select file input method ');
disp('   1=file preloaded into Matlab ');
disp('   2=Excel file ');
file_choice = input('');
%
disp(' ');
disp(' Unconstrained Mass Matrix ');
%
if(file_choice==1)
        m = input(' Enter the matrix name:  ');
end
if(file_choice==2)
        [filename, pathname] = uigetfile('*.*');
        xfile = fullfile(pathname, filename);
%        
        m = xlsread(xfile);
%         
end
%
m=m/mass_scale;
%
mass=m;
%
disp(' ');
disp(' Unconstrained Stiffness Matrix ');
%
if(file_choice==1)
        k = input(' Enter the matrix name:  ');
end
if(file_choice==2)
        [filename, pathname] = uigetfile('*.*');
        xfile = fullfile(pathname, filename);
%        
        k = xlsread(xfile);
%         
end
stiffness=k;
%
size(m);
size(k);
%
num=max(size(m));
%
%% disp(' ');
%% disp(' The mass matrix is');
%% m
%% disp(' ');
%% disp(' The stiffness matrix is');
%% k
%
%% disp(' ');
%% iret=input(' Enter the number of degrees-of-freedom to retain: ');
nret=6;
disp(' ');
rnn=input(' Enter retained node number: ');
clear ret;
for(i=1:nret)
   iret(i)=((rnn-1)*6)+i;
end
disp(' ');
disp(' Retained dof ');
out1=sprintf(' %d %d %d %d %d %d \n',iret(1),iret(2),iret(3),iret(4),iret(5),iret(6));
disp(out1);
%
clear mr;
clear kr;
kr=zeros(nret,nret);
mr=zeros(nret,nret);
%
clear M11;
clear M12;
clear M21;
clear M22;
%
clear K11;
clear K12;
clear K21;
clear K22;
%
clear ngw;
ijk=nret+1;
ngw=zeros(num,1);
ngw(1:nret)=iret;
for(i=1:num)
    iflag=0;
    for(nv=1:nret)
      if(i==iret(nv))
          iflag=1;
      end
    end
    if(iflag==0)
        ngw(ijk)=i;
        ijk=ijk+1;
    end
end
%
%%%%%%%%%%%%%%%%%%%%% M11 K11 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
ic=1;
for(i=1:num)
   iflag=0;          
   for(nv=1:nret)
      if(i==iret(nv))
         iflag=1;
         break;
      end
   end
   if(iflag==1)
      jc=1;      
      for(j=1:num)
          jflag=0;
          for(nv=1:nret)
             if(j==iret(nv))
                jflag=1;
                break;
             end   
          end
          if(jflag==1)
 %            out1=sprintf(' i=%d j=%d ic=%d jc=%d \n',i,j,ic,jc);
 %            disp(out1);             
             M11(ic,jc)=m(i,j);
             K11(ic,jc)=k(i,j);
             jc=jc+1;
          end
%         out1=sprintf(' i=%d j=%d iflag=%d jflag=%d \n',i,j,iflag,jflag);
%         disp(out1);
      end
   end
   if(iflag==1)
       ic=ic+1;
   end
end
%
%%%%%%%%%%%%%%%%%%%%% M12 K12 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
ic=1;
for(i=1:num)
   iflag=0;          
   for(nv=1:nret)
      if(i==iret(nv))
         iflag=1;
         break;
      end
   end
   if(iflag==1)
      jc=1;      
      for(j=1:num)
          jflag=0;
          for(nv=1:nret)
             if(j==iret(nv))
                jflag=1;
                break;
             end   
          end
          if(jflag==0)
 %            out1=sprintf(' i=%d j=%d ic=%d jc=%d \n',i,j,ic,jc);
 %            disp(out1);             
             M12(ic,jc)=m(i,j);
             K12(ic,jc)=k(i,j);
             jc=jc+1;
          end
%         out1=sprintf(' i=%d j=%d iflag=%d jflag=%d \n',i,j,iflag,jflag);
%         disp(out1);
      end
   end
   if(iflag==1)
       ic=ic+1;
   end
end
%
%%%%%%%%%%%%%%%%%%%%% M21 K21 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
ic=1;
for(i=1:num)
   iflag=0;          
   for(nv=1:nret)
      if(i==iret(nv))
         iflag=1;
         break;
      end
   end
   if(iflag==0)
      jc=1;      
      for(j=1:num)
          jflag=0;
          for(nv=1:nret)
             if(j==iret(nv))
                jflag=1;
                break;
             end   
          end
          if(jflag==1)
 %            out1=sprintf(' i=%d j=%d ic=%d jc=%d \n',i,j,ic,jc);
 %            disp(out1);             
             M21(ic,jc)=m(i,j);
             K21(ic,jc)=k(i,j);
             jc=jc+1;
          end
%         out1=sprintf(' i=%d j=%d iflag=%d jflag=%d \n',i,j,iflag,jflag);
%         disp(out1);
      end
   end
   if(iflag==0)
       ic=ic+1;
   end
end
%
%%%%%%%%%%%%%%%%%%%%%%% M22 K22 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
ic=1;
for(i=1:num)
   iflag=0;          
   for(nv=1:nret)
      if(i==iret(nv))
         iflag=1;
         break;
      end
   end
   if(iflag==0)
      jc=1;      
      for(j=1:num)
          jflag=0;
          for(nv=1:nret)
             if(j==iret(nv))
                jflag=1;
                break;
             end   
          end
          if(jflag==0)
 %            out1=sprintf(' i=%d j=%d ic=%d jc=%d \n',i,j,ic,jc);
 %            disp(out1);             
             M22(ic,jc)=m(i,j);
             K22(ic,jc)=k(i,j);
             jc=jc+1;
          end
%         out1=sprintf(' i=%d j=%d iflag=%d jflag=%d \n',i,j,iflag,jflag);
%         disp(out1);
      end
   end
   if(iflag==0)
       ic=ic+1;
   end
end
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%% M11
%% if(iret<num)
%%    M12
%%    M21
%%    M22
%%end
%
%% K11
%% if(iret<num)
%%    K12
%%    K21
%%    K22
%% end
%
clear C;
clear CC;
%
d2=nret;
nmds=num-d2;
%
clear invK22;
clear QQQ;
disp(' ');
disp(' Select submatrix inverse method ');
disp(' 1=inv   2=pinv  3=backslash ');
im=input(' ');
if(im==1)
    invK22=inv(K22);
end
if(im==2)
    invK22=pinv(K22);
end
if(im==3)
    QQQ=eye(nmds,nmds);
    invK22=K22\QQQ;
end
%
if(iret<num)
    CC=-invK22*K21;
end
for i=1:d2
    for j=1:d2
         C(i,j)=0;
    end
    C(i,i)=1;
end
for i=1:nmds
    for j=1:d2
         if(nret<num)
            C(i+d2,j)=CC(i,j);
         else
            C(i+d2,j)=0;    
         end
     end
end
%
C;
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  Calculate eigenvalues and eigenvectors
%
clear mq;
clear kq;
clear m_partition;
clear k_partition;
%
m_partition(1:nret,1:nret)        =M11;
m_partition(1:nret,nret+1:num)    =M12;
m_partition(nret+1:num,1:nret)    =M21;
m_partition(nret+1:num,nret+1:num)=M22;
%
k_partition(1:nret,1:nret)        =K11;
k_partition(1:nret,nret+1:num)    =K12;
k_partition(nret+1:num,1:nret)    =K21;
k_partition(nret+1:num,nret+1:num)=K22;
%
%% disp(' Partitioned Matrices ');
%
%% m_partition
%% k_partition
%
%% disp(' Transformed matrices ');
%
mq=C'*m_partition*C;
kq=C'*k_partition*C;
%
disp(' ');
[fn,omega,ModeShapes,MST]=Generalized_Eigen(kq,mq,1);
%
omegan=omega;
%
mx=mq(1,1);
my=mq(2,2);
mz=mq(3,3);
Ixx=mq(4,4);
Ixy=mq(4,5);
Ixz=mq(4,6);
Iyy=mq(5,5);
Iyz=mq(5,6);
Izz=mq(6,6);
%
mmass=(mx+my+mz)/3;
%
xg= mq(2,6)/mmass;
yg=-mq(1,6)/mmass;
zg= mq(1,5)/mmass;
%
disp(' ');
disp(' Rigid-body Mass Matrix ');
disp(' ');
mq
%
disp(' ');
out1=sprintf(' CG coordinates relative to location of node %d',rnn);
disp(out1);
disp(' ');
out1=sprintf(' xg = %8.4g  ',xg);
out2=sprintf(' yg = %8.4g  ',yg);
out3=sprintf(' zg = %8.4g  ',zg);
disp(out1);
disp(out2);
disp(out3);
xg=-xg;
yg=-yg;
zg=-zg;
%
disp(' ');
out1=sprintf(' Mass (%s)',mass_unit);
disp(out1);
disp(' ');
out1=sprintf(' mx = %8.4g  ',mx);
out2=sprintf(' my = %8.4g  ',my);
out3=sprintf(' mz = %8.4g  ',mz);
disp(out1);
disp(out2);
disp(out3);
%
Ixx_cg=Ixx-mmass*(yg^2+zg^2);
Iyy_cg=Iyy-mmass*(xg^2+zg^2);
Izz_cg=Izz-mmass*(xg^2+yg^2);
%
Ixy_cg=Ixy+mmass*(xg*yg);
Ixz_cg=Ixz+mmass*(xg*zg);
Iyz_cg=Iyz+mmass*(yg*zg);
%
Icg=[Ixx_cg Ixy_cg Ixz_cg; Ixy_cg Iyy_cg Iyz_cg; Ixz_cg Iyz_cg Izz_cg];
disp(' ');
disp(' Mass Moments of Inertia about CG ');
disp(' ');
Icg