%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  Solution to Problem 3 in the homework assignment,
%  Updating and Downdating Matrix Factorizations:
%     A Change in Plans
%
%  We solve two truss problems, the first through matrix
%  factorization and the second using the Sherman-Morrison-Woodbury
%  formula.
%
%  First truss:  All members are equal length.  
%                Assume a load of 50 at C.
%
%                  B           D
%                 / \         / \
%                /   \       /   \
%               /     \     /     \
%              /       \   /       \
%             /         \ /         \
%            A --------- C ----------E
%
%  problem3.m       Dianne P. O'Leary  12/2005
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% The original problem  A f = b, where f is the force vector
% and b is the load vector.
% The variables (columns) are the unknown forces:
% The equations (rows) balance horizontal and vertical forces 
%  at each node.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

c = cos(pi/3);
s = sin(pi/3);
l = -50;

%     Ah Av AB BC CD DE BD AC CE Ev
A = [ -1  0  c  0  0  0  0  1  0  0   % horizontal forces at A
       0  1  s  0  0  0  0  0  0  0   % vertical   forces at A
       0  0 -c  c  0  0  1  0  0  0   % horizontal forces at B
       0  0  s  s  0  0  0  0  0  0   % vertical   forces at B
       0  0  0 -c  c  0  0 -1  1  0   % horizontal forces at C
       0  0  0  s  s  0  0  0  0  0   % vertical   forces at C
       0  0  0  0 -c  c -1  0  0  0   % horizontal forces at D
       0  0  0  0  s  s  0  0  0  0   % vertical   forces at D
       0  0  0  0  0 -c  0  0 -1  0   % horizontal forces at E
       0  0  0  0  0  s  0  0  0 -1]; % vertical   forces at E

b = [  0; 0; 0; 0; 0; l; 0; 0; 0; 0];

f = A \ b

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  Solve the modified problem: move node E so that member CE is 
%  2 times the length of the other members.
%  The resulting matrix is A - Z V^T.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Angle DCE remains 60 degrees.
% Angle CDE is 90 degrees
% Angle DEC is 30 degrees.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

cosE =  cos(pi/6)
sinE =  sin(pi/6)
cosC =  cos(pi/3);

gamma = pi/6;   % = angle at D between member 4 and horizontal.
cosgam = cos(gamma);
singam = sin(gamma);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% New column 6 of matrix:
% Member 4 applies horizontal force at D with factor  cos gamma    
%                  vertical   force at D with factor  sin gamma    
% Member 4 applies horizontal force at E with factor -cos gamma    
%                  vertical   force at E with factor  sin gamma    
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

V = zeros(10,1);
V(6,1) = 1;

Z = [A(:,6) - [0 0 0 0   0      0   cosgam singam -cosgam singam]'];

[L,U,P]=lu(A);

fmod = sherman_mw(L,U,b,Z,V,P)

fmodtru = (A-Z*V')\b;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Display the results.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

disp(sprintf('The original vector of forces:'))
disp(f')
disp(sprintf('The vector of forces after the node is moved:'))
disp(fmod')
disp(sprintf('The difference between the Sherman-Morrison-Woodbury'))
disp(sprintf(' answer and the answer computed with backslash: %e',...
     norm(fmod-fmodtru)))