function [S2,M2,mass,C,dof]=mass_condense(stiff,mass,dof)
%
n=dof;
%
% Diagonal Pivot
%
for i=1:n-1
    max=abs(mass(i,i)/stiff(i,i));
%
    for k=2:n
        if( abs(mass(k,k)/stiff(k,k))>max)
            max=abs(mass(k,k)/stiff(k,k));
% Row switch
            for j=1:n
                temp=mass(i,j);
                mass(i,j)=mass(k,j);
                mass(k,j)=temp;
%
                temp=stiff(i,j);
                stiff(i,j)=stiff(k,j);
                stiff(k,j)=temp;
            end
% Column switch
            for j=1:n
                temp=mass(j,i);
                mass(j,i)=mass(j,k);
                mass(j,k)=temp;
%    
                temp=stiff(j,i);
                stiff(j,i)=stiff(j,k);
                stiff(j,k)=temp;
            end
        end
   end
end
%
d2=dof/2;
%
for i=1:d2
    for j=1:d2
        M11(i,j)=mass(i,j);
        K11(i,j)=stiff(i,j);      
    end
end
n=1;
for i=1:d2
    for j=1:d2
        M21(i,j)=mass(i+d2,j);
        K21(i,j)=stiff(i+d2,j);       
    end
    n=n+1;
end 
%
for i=1:d2
    for j=1:d2
        M22(i,j)=mass(i+d2,j+d2);
        K22(i,j)=stiff(i+d2,j+d2);       
    end 
end    
%
for i=1:d2
    for j=1:d2
        M12(i,j)=mass(i,j+d2);
        K12(i,j)=stiff(i,j+d2);       
    end 
end  
CC=-inv(K22)*K21;
for i=1:d2
    for j=1:d2
         C(i,j)=0.;    
    end
    C(i,i)=1.;
end
for i=1:d2
    for j=1:d2
         C(i+d2,j)=CC(i,j);    
     end
end
%
S2=C'*stiff*C;
M2=C'*mass*C;
dof=d2;