joints = [0 0 0.5 1 1;...
	  0 1 2   1 0];

force  = [0 1 1 1 0;...
	  0 0 0 0 0];
force  = [0 1      0       -1     0;...
	  0 1.0133 -1.3571 1.0133 0];
force=force(:);

fixed_joints = [1 5];

bars     = [1 2 2 3 4;...
	    2 3 4 4 5];

EA       = [1 1 1 1 1] * 10;

% compute stiffness i.e. EA/l
l = joints(:,bars(2,:))-joints(:,bars(1,:));
l = sqrt(l(1,:).^2+l(2,:).^2);
stiffness = EA./l;

% plot truss
figure(1), clf,
plot_truss(joints,bars)

% build elongation matrix
A=build_elongationmatrix(joints,bars);

% set fixed joints
remove_cols=[(fixed_joints-1)*2+1, (fixed_joints-1)*2+2];
leave_cols=setdiff(1:size(A,2),remove_cols);

A=A(:,leave_cols);
force = force(leave_cols);

% build system matrix
K = A'*diag(stiffness)*A;

% solve system
x = K\force;

% construct solution
solution = zeros(length(joints)*2,1);
solution(leave_cols) = x;

% plot solution
figure(2)
clf
plot_truss(joints+[solution(1:2:end)';solution(2:2:end)'],bars)

do_eigenstuff = 1;
if do_eigenstuff
  [V,D] = eig(K);
  modes = zeros(length(joints)*2,length(V));
  modes(leave_cols,:) = V*0.1;
  
  for i=1:length(V)
    figure
    plot_truss(joints+[modes(1:2:end,i)';modes(2:2:end,i)'],bars)
  end
end
