% Solve vibrating mass-spring system using eigenvalues
%
% Inputs (corresponds to figure 4.1 in Ma221 textbook)
%   n = number of bodies
%   m = column vector of n masses
%   b = column vector of n damping constants
%   k = column vector of n spring constants
%   x0= column vector of n rest positions of bodies
%   x = column vector of n initial displacements from rest
%   v = column vector of n initial velocities of bodies
%   dt = time step
%   tfinal = final time to integrate system
%
% Outputs
%   graph body positions
%
% Compute mass matrix
M = diag(m);
% Compute damping matrix
B = diag(b);
% Compute stiffness matrix
if n>1,
   K = diag(k) + diag([k(2:n);k(n)]) - diag(k(2:n),1) - diag(k(2:n),-1);
else
   K = k;
end
% Compute matrix governing motion, find its eigenvalues and eigenvectors
A = [[-inv(M)*B, -inv(M)*K];[eye(n),zeros(n)]];
[V,D]=eig(A);
dD = diag(D);
iV = inv(V);
i=1;
Y = [v;x];
T = 0;
steps = round(tfinal/dt);
% Compute positions and velocities
for i = 2:steps
  t = (i-1)*dt;
  T = [T,t];
  Y(1:2*n,i) = V * ( diag(exp(t*dD)) * ( iV * [v;x] ) );
% Y(1:2*n,i) = expm(t*A)*[v;x];
end
Y = real(Y);
hold off, clf
subplot(1,2,1)
Locations = Y(n+1:2*n,:)+x0*ones(1,i);
for j=1:n,
  if (rem(j,4)==1),
    color = 'w';
  elseif (rem(j,4)==2),
    color = 'r';
  elseif (rem(j,4)==3),
    color = 'g';
  elseif (rem(j,4)==0),
    color = 'b';
  end
  plot(T,Locations(j,:),color),
  hold on
  axis([0,tfinal,min(min(Locations)),max(max(Locations))])
end
title('Positions')
xlabel('Time')
grid
subplot(1,2,2)
Velocities = Y(1:n,:);
for j=1:n,
  if (rem(j,4)==1),
    color = 'w';
  elseif (rem(j,4)==2),
    color = 'r';
  elseif (rem(j,4)==3),
    color = 'g';
  elseif (rem(j,4)==0),
    color = 'b';
  end
  plot(T,Velocities(j,:),color),
  hold on
  axis([0,tfinal,min(min(Velocities)),max(max(Velocities))])
end
title('Velocities')
xlabel('Time')
grid