function  latticevibes()
%  Find vibrations of a truss with point masses at joints
% Structure can be 1,2 or 3 dimensional.
% Animation only works if structure is 2-dimensional
% The dimension is determined by the number of columns in the array
% that defines points. 
disp('*********************************')
disp(['Started at   '   datestr(now)])
clear all

%GET TRUSS DEFINITION
%[points springs masses constraints] = springmass1b();
[points springs masses constraints] = deftruss1();

%ASSEMBLE MASS AND STIFFNESS MATRICES
[M K meanings n] = assembly(points, springs,masses,constraints);


%FIND MODE SHAPES AND FREQUENCIES
[V D] = eig(K,M);  % Same as  eig(M^-1 * K)  
D = abs(D);        % Computer gets confused by -0.0
disp('Mode shapes V.  Each column is a mode.'); disp(V)
disp('meanings of rows of V. Col 1 is node, col 2 is direction.')
disp(meanings) 
disp('Frequencies, one for each mode.')
disp(sqrt(diag(D)))

%PICK MODES AND PLOT THEM
r=zeros(n.K,1); % list of mode amplitudes, start with zeros
r(1)=.5;       % pick mode and amplitude, MUST BE <= n.K !!!!!!!!
%r(6) = .05
tspan=linspace(0,30*pi,300);
%PLOT MODE SHAPES
showmode(points, springs,masses,constraints, meanings, V, D, r,n,tspan)

disp(['Ended at     '   datestr(now)])
disp('*********************************')
end %Main driver
%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%% find numbers of rows and cols in various matrices
function n=findnumbers(points, springs, masses, constraints)
%COUNTS numbers of points, dimensions, rows, springs,constraints
n.points  = length(points(:,1));
n.dim     = length(points(1,:))-1;  % 1,2 or 3 spatial dimensions
n.rows    = n.points*n.dim;           % number of rows & cols in unconst. K
n.spr     = length(springs(:,1));   % number of springs
if isempty(constraints) 
    n.const = 0;
else
   n.const   = length(constraints(:,1));
end
n.K = n.rows - n.const;
end
%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%% Assemble the big stiffness and mass matrix
function [M K meanings n] = assembly(points, springs,masses,constraints)
n = findnumbers(points, springs, masses, constraints); %count sizes

M        = zeros(n.rows);
K        = zeros(n.rows);
meanings = zeros(n.dim*n.points,2);  % first col is node, 2nd = dir.

%Make up M matrix and meanings matrix
for i=1:n.points
    % Fill in diagonal blocks of ndim X ndim
    j    = n.dim*(i-1) + 1; %first row of block
    rows = j:j+n.dim-1;     %block of rows: 1,2 or 3 dep on dim.
    M(rows,rows) = masses(i,2)*eye(n.dim);
    %Each row of meanings is a node and a direction
    meanings(rows,1) = i;            % node number
    meanings(rows,2) = [1:n.dim]';    % which direction
end

%ASSEMBLY
%Fill in K matrix, one spring at a time: "Assembly"
for i=1:n.spr
    k =  springs(i,3);
    pt1 = springs(i,1); 
    pt2 = springs(i,2); 
    r =  points(pt2,2:end) ... % Pos of one end rel to other
        -points(pt1,2:end);
   ur = r/norm(r);             % unit vector along bar
   k  = k*ur'*ur;              % k * direction cosines = little k matrix
   
   
   r1 = n.dim*(pt1-1)+1;  %starting row of K for one   end
   r2 = n.dim*(pt2-1)+1;  %starting row of K for other end
   rows =  [[r1:r1+n.dim-1] [r2:r2+n.dim-1] ];% rows of K to insert little k
    
   K(rows,rows) = K(rows,rows) ...  % add little k to K in 4 places
                 +  [ [ k  -k];
                    [  -k   k] ];
end

%DELETE CONSTRAINED DIRECTIONS (Make M and K smaller)
rows=[]; %list of rows and columns to delete
if n.const ne 0 then;  %Only do this if there ARE constraints
 for i = 1:n.const
    row = n.dim*(constraints(i,1)-1) + constraints(i,2); %which row to delete
    rows = [rows;row];
 end
 K(rows,:) = [];  %delete constrained rows of K
 K(:,rows) = [];  %delete constrained cols of K
 M(rows,:) = [];  %delete constrained rows of M
 M(:,rows) = [];  %delete constrained cols of M
 meanings(rows,:)=[];  % delete meaningless (constrained) rows
end

end  
%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Do animations
function showmode(points, springs, ...
    masses,constraints, meanings, V,D, r,n,tspan)
%ASSUMES 2-dimensional lattice (two coords for each joint)
if n.dim ~= 2
    disp('ANIMATION ONLY WORKS FOR 2 spacial dimensions')
    return % stop the animation
end
clf
pointsold=points;   %have to save these for whole function
om = sqrt(diag(D)); %frequencies are sqrt of e-values


%Scaling for animation plot
axispts=[ min(points(:,2))-1 max(points(:,2))+1 ...
          min(points(:,3))-1 max(points(:,3))+1];

%START MAIN ANIMATION LOOP
for t=tspan;    %Loop for drawing the whole figure once
    pause(.001);
 points = pointsold;  % points gets modified each time step of anim.
 
 %Sine function for om .ne. 0, linear in time if om == 0
 foft=zeros(n.K,1);  % zero array of f(t) (one zero for each DOF).
 for i =1:n.K
     if om(i) >1e-6    %if evalue is close to zero, call it zero
         foft(i) = sin(om(i)*t);
     else   
         foft(i) = t;
     end
 end
 
 
 %superposition of normal modes
 d=V*(r.* foft); % list of displacements of moving nodes
     for i=1:n.K % add displacement to moving points positions
       joint = meanings(i,1);
       dir   = meanings(i,2); 
       points(joint,1+dir) = pointsold(joint,1+dir) + d(i);                   
     end
     
%DRAW ONE FRAME
     figure(1); clf; hold on
     
     for i=1:n.const  % Plot constraints
        xoff=0; yoff=0;   %zero the segment end offsets
        point = constraints(i,1); dir = constraints(i,2);
        if dir==1 yoff=.2;
           else   xoff = .2; 
        end  % pick direction of segment
        x=pointsold(point,2); y=pointsold(point,3); %keep const in place
        plot([x-xoff x+xoff],[y-yoff y+yoff],'k','linewidth',8)
     end
 
     for i=1:n.spr   % Plot springs
         plot([points(springs(i,1),2) points(springs(i,2),2)],...
              [points(springs(i,1),3) points(springs(i,2),3)],'linewidth',2)
     end
     
     for i=1:n.points % Plot points
         plot(points(i,2),points(i,3),'ro','markersize',30,'MarkerFaceColor','g')
     end
     
     for i=1:n.points % Type joint numbers
         text(points(i,2)-.04,points(i,3),num2str(i),'fontsize',20)
     end
     
   
    
 
 axis equal
 axis(axispts)
 
 hold off
 

end % End of drawing loop, one drawing complete


end %End of animation function
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%VARIOUS TRUSSES DEFINED BELOW

%%%%%%%%%%%%%%%%%%%%%
function [points springs masses constraints] = springmass1a()
% Locations of points
points = [% #  x y z (1,2 or 3 of them, for 1D,2D or 3D)
            1  0 ;
            2  1 ;
               ];        
% Mass of masses, has to match number of points
masses = [ % #  m     (node number, mass of node)
             1  1;
             2  1;
                   ];         
% Points springs connect to, and their stiffnesses
springs = [% n1  n2 k  (connection points, stiffness)
             1   2  1];
 % Kinematic Constraints 
constraints = [% n  1  node number,  dir of const
                 1  1;
                 ];
end
%%%%%%%%%%%%%%%%%%%%%

function [points springs masses constraints] = springmass1b()
% Locations of points
points = [% #  x y z (1,2 or 3 of them, for 1D,2D or 3D)
            1  0 0;
            2  1 0;
               ];        
% Mass of masses, has to match number of points
masses = [ % #  m     (node number, mass of node)
             1  1;
             2  1;
                   ];         
% Points springs connect to, and their stiffnesses
springs = [% n1  n2 k  (connection points, stiffness)
             1   2  1];
 % Kinematic Constraints 
constraints = [% n  1  node number,  dir of const
                 1  1;
                 1  2
                 2  2
                 ];
end
%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%
function [points springs masses constraints] = springmass2()
% Locations of points
points = [% #  x y z (1,2 or 3 of them, for 1D,2D or 3D)
            1  0  0 ;
            2  1  0;
            3  2  0   ];        
% Mass of masses, has to match number of points
masses = [ % #  m     (node number, mass of node)
             1  1;
             2  1;
             3  1];         
% Points springs connect to, and their stiffnesses
springs = [% n1  n2 k  (connection points, stiffness)
             1   2  1;
             2   3  1];
 % Kinematic Constraints 
constraints = [% n  1  node number,  dir of const
                 1  1;
                 1  2;
                 2  2;
                 3  2 ];
end
%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%
function [points springs masses constraints] = springmass3()
% Locations of points
points = [% #  x y z (1,2 or 3 of them, for 1D,2D or 3D)
            1  0  0 ;
            2  1  0;
            3  2  0;
            4  3  0;
            5  4  0];        
% Mass of masses, has to match number of points
masses = [ % #  m     (node number, mass of node)
             1  1;
             2  1;
             3  1;
             4  1;
             5  1];         
% Points springs connect to, and their stiffnesses
springs = [% n1  n2 k  (connection points, stiffness)
             1   2  1;
             2   3  1;
             3   4  1;
             4   5  1];
 % Kinematic Constraints 
constraints = [% n  1  node number,  dir of const
                 1  1;
                 1  2;
                 2  2;
                 3  2;
                 4  2;
                 5  1;
                 5  2];
end
%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%
function [points springs masses constraints] = defsquare()
% Locations of points
points = [% #  x y z (1,2 or 3 of them, for 1D,2D or 3D)
            1 0 0 
            2 1 0
            3 1 1
            4 0 1];
        
% Mass of masses, has to match number of points
masses = [ % #  m     (node number, mass of node)
             1  1;
             2  1;
             3  1
             4  1];
         
% Points springs connect to, and their stiffnesses
springs = [% n1  n2 k  (connection points, stiffness)
             1 2  1
             2 3 1
             3 4 1
             4 1 1
                   ];
 
% Kinematic Constraints 
constraints = [% n  1  node number,  dir of const
                  ];
end
%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%
function [points springs masses constraints] = deftrussbar()
points = [
          1  0 0
          2  1 1
          
                 ];
             
springs =[
          1  2  1
                  ];
masses  =[
          1 1
          2 1];
      
constraints =[
              1  1
              1  2];

end
%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%
function [points springs masses constraints] = deftruss1()
%This is the example from section 8.5 of the Inman text
% Locations of points
points = [% #  x y z (1,2 or 3 of them, for 1D,2D or 3D)
            1  0            0 ;
            2  sqrt(3)/2  -.5;
            3  0           -1   ];
        
% Mass of masses, has to match number of points
masses = [ % #  m     (node number, mass of node)
             1  1;
             2  1;
             3  1];
         
% Points springs connect to, and their stiffnesses
springs = [% n1  n2 k  (connection points, stiffness)
             1   2  1;
             2   3  1];
 
% Kinematic Constraints 
constraints = [% n  1  node number,  dir of const
                 1  1;
                 1  2;
                 3  1;
                 3  2 ];
end
%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%
function [points springs masses constraints] = deftruss2()
% Locations of points
points = [% #  x y z (1,2 or 3 of them, for 1D,2D or 3D)
            1  0            0 ;
            2  1            0 ;
            3  .5          sqrt(3)/2   ];
        
% Mass of masses, has to match number of points
masses = [ % #  m     (node number, mass of node)
             1  1;
             2  1;
             3  1];
         
% Points springs connect to, and their stiffnesses
springs = [% n1  n2 k  (connection points, stiffness)
             1   2  1;
             2   3  1;
             3   1  1];
 
% Kinematic Constraints 
constraints = [% n  1  node number,  dir of const
                  ];
end
%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%
function [points springs masses constraints] = deftruss3()
% Locations of points
for i=1:6
    s = 2*pi/6;
    points(i,:)= [i cos(i*s), sin(i*s)];
end
         
% Points springs connect to, and their stiffnesses
springs = [% n1  n2 k  (connection points, stiffness)
             1   2  1;
             2   3  1;
             3   4  1;
             4   5  1;
             5   6  1;
             6   1  1;
             1   4  1;
             2   5  1;
             3   6  1];
         
% Mass of masses, has to match number of points
masses = [1:6; ones(1,6)]';

 
% Kinematic Constraints 
constraints = [% n  1  node number,  dir of const
               %4 1;
               %4 2;
               %5 2
               ];
end
%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%
function [points springs masses constraints] = deftruss4()
% Locations of points
 points = [
            1 0 0
            2 1 0
            3 0 1
            4 1 1
            5 0 2
            6 1 2
            7 0 3
            8 1 3];
% Points springs connect to, and their stiffnesses
springs = [% n1  n2 k  (connection points, stiffness)
             1 3 1
             1 4 1
             3 4 1
             2 4 1
             3 4 1
             3 6 1
             3 5 1
             4 6 1
             5 6 1
             5 8 1
             5 7 1
             6 8 1
             7 8 1
             ];
         
% Mass of masses, has to match number of points
masses = [1:8; ones(1,8)]';

 
% Kinematic Constraints 
constraints = [% n  d  node number,  dir of const
               1 1
               1 2
               2 1
               2 2
               ];
end
%%%%%%%%%%%%%%%%%%%%%

