function [] = hw6_sub()
% A solution by TSD 11/16/06

global   nn ne nm ndim nep nqn
global   nq nf nwl npl
global   XYZ CTAB E A I REL QTAB FTAB MATERIAL WTAB PTAB
global   FIDI_NAME ns
global   e l m n K L M F P Q
global   A E I L M REL l m n

%1 Read Input Data File for 2D frame and calculate ns, the order of the system (total number of displacements).
%ANSWER = input('Input Data File Name: ', 's');	%Get typed name of FIDI
%if size(ANSWER) ~= 0
%    FIDI_NAME = ANSWER
%end
Get_2D_Frame_Data(FIDI_NAME);

%2 Find member lengths Lx, Ly, Lz=0, L, and then l=cos(theta)=L/lx, m=sin(theta)=L/Ly, n=L/Lz for each of the ne elements.
% Store the results in (1 x ne) row vectors [L.e], [l.e], [m.e], & [n.e].
Bar_Lengths;

%3 In Figure 1, draw the frame by plotting each member.
%4 Label the nodes with node index.
figure(1);
hold on; axis equal;
Draw_Frame
Label_Nodes
hold off


%5 Given nf and FTAB, to initialize the column vector F with the specified applied forces.
% Note: an applied force is zero unless some other value is specified.
% Include the unknown reactions, set to zero for now.
F=zeros(ns,1);
for i=1:nf
    F(FTAB(i,1))=FTAB(i,2);
end
F

%6 Given nq and QTAB, to initialize the column vector Q with the specified displacements.
% Include the unknown displacements, set to zero for now.
Q=zeros(ns,1);
for i=1:nq
    Q(QTAB(i,1))=QTAB(i,2);
end
Q

%7 Assemble the stiffness matrix K=Sum(Ge'*Ke*Ge)
% %% k=zeros(nqn,nqn)
K=zeros(ns,ns);
A
E
I
for e=1:ne
    Ge=Make_Ge(e)
    Ke=Make_Ke(e)
    K=K+Ge'*Ke*Ge
end

%8 Apply the Boundary Conditions and solve for displacements QF and reactions F
% Include the solved displacements in Q and the solved reactions in F
QF_Solver

% Find the bar forces P(e) for each member from
% the calculated nodal forces for each element Fe=Ke*Qe 

P=zeros(ne,1)
for e=1:ne
    Ke=Make_Ke(e)
    Ge=Make_Ge(e)
    Fe=Ke*Ge*Q
    P(e)=[l(e) m(e)]*Fe(4:5) %Pathagoris' theorem gives only magnatitude.
end

%9 In Figure 2, draw the frame in solid lines, label nodes, draw displaced frame in broken
% lines with green tension members and red compression members
figure(2);
hold on; axis equal;
Draw_Frame
Label_Nodes
Draw_Displaced_Frame
hold off

%=====
% SUBORDINATE FUNCTIONS
%=====

function [] = Bar_Lengths()
%Function calculates bar lengths Lx, Ly, L, l=cos(theta) and m=sin(theta) for each of the ne elements.
%Store the results in (1 x ne) row vectors [L.e], [l.e] and [m.e].
global ne l m n L CTAB XYZ

for e=1:ne
    Lx=XYZ(CTAB(e,2),1)-XYZ(CTAB(e,1),1)
    Ly=XYZ(CTAB(e,2),2)-XYZ(CTAB(e,1),2)
    Lz=XYZ(CTAB(e,2),3)-XYZ(CTAB(e,1),3)
    L(e)=sqrt(Lx*Lx+Ly*Ly+Lz*Lz)
    l(e)=Lx/L(e)
    m(e)=Ly/L(e)
    n(e)=Lz/L(e)
end
return;
%==========

function [] = Draw_Frame()
%draw 2D truss

global XYZ CTAB ne 

Xmin=XYZ(CTAB(1,1),1);
Ymin=XYZ(CTAB(1,1),2);
Zmin=XYZ(CTAB(1,1),3);
Xmax=Xmin;
Ymax=Ymin;
Zmax=Zmin;
for e=1:ne
    Xe2=XYZ(CTAB(e,2),1);
    Xe1=XYZ(CTAB(e,1),1);
    Ye2=XYZ(CTAB(e,2),2);
    Ye1=XYZ(CTAB(e,1),2);
    Ze2=XYZ(CTAB(e,2),3);
    Ze1=XYZ(CTAB(e,1),3);    
    plot3([Xe1 Xe2], [Ye1 Ye2], [Ze1 Ze2]);
    Xmin=min([Xmin Xe1 Xe2]);
    Xmax=max([Xmax Xe1 Xe2]);
    Ymin=min([Ymin Ye1 Ye2]);
    Ymax=max([Ymax Ye1 Ye2]); 
    Zmin=min([Zmin Ze1 Ze2]);
    Zmax=max([Zmax Ze1 Ze2]); 
end
e=max(Xmax-Xmin,Ymax-Ymin);	%add border space
e=max(e,Zmax-Zmin)/32;
plot3(Xmin-e,Ymin-e,Zmin-e);
plot3(Xmax+e,Ymax+e,Zmax+e);
return;
%==========

function [] = Label_Nodes()
%Label nodes
global XYZ nn
for i=1:nn
    text(XYZ(i,1), XYZ(i,2), XYZ(i,3), num2str(i));
end
return;
%==========

function [] = Draw_Displaced_Frame()
%draw deformed truss
global XYZ CTAB P Q ne

mag=100
for e=1:ne
    n1=CTAB(e,1);
    n2=CTAB(e,2);
    Xe2=XYZ(n2,1)+mag*Q(3*n2-2);
    Xe1=XYZ(n1,1)+mag*Q(3*n1-2);
    Ye2=XYZ(n2,2)+mag*Q(3*n2-1);
    Ye1=XYZ(n1,2)+mag*Q(3*n1-1);
    Ze2=XYZ(n2,3);  %third displacement is nodal rotation, not z-translation.
    Ze1=XYZ(n1,3);
    if P(e)<0
        plot3([Xe1 Xe2], [Ye1 Ye2], [Ze1 Ze2], 'r--');  %red compression bars
    else
        plot3([Xe1 Xe2], [Ye1 Ye2], [Ze1 Ze2], 'g--');  %green tension bars
    end
end
return;
%==========

function [] = QF_Solver()
%Given QTAB nq, ns, and the system F=K*Q, transforms to similar system H=C*D
%The essential boundary conditions are mixed in Q, but known by their inclusion in QTAB.
%The essential boundary conditions are placed in the upper part of D via D=T*Q
%Then C=T*K*T' and H=T*F. H=C*D is solved for D by partitions
%Reverse transformation gives Q=T'*D and F=T'*H
global QTAB F K Q nq ns
 
%In truth table I, I(i) is 1 if Q(i) is an essential displacement in QTAB.
I=zeros(ns,1);
for i=1:nq
    I(QTAB(i,1))=1;
end
I
%Copy the indices of the essential displacenemts from QTAB to V(1:nq)
V=zeros(ns,1);
V(1:nq,1) = QTAB(:,1);
%Write the remaining indices in V(nq+1:ns)
j=nq+1;
for i=1:ns
    if I(i)==0
        V(j)=i;
        j=j+1;
    end
end
V
%Make transform T from the indices in V. 
T=zeros(ns)
for i=1:ns
    T(i,V(i))=1;
end
T
%find the similar system H=CD using the transform matrix T
D=T*Q
H=T*F
C=T*K*T';
%Solve by partitions. The free displacements are DF=inv(CF)*(HF-CEF'*DE)
DE=D(1:nq);
CE=C(1:nq,1:nq);
CF=C(nq+1:ns,nq+1:ns);
CEF=C(1:nq,nq+1:ns);
HF=H(nq+1:ns);
DF=inv(CF)*(HF-CEF'*DE);
%The Reactions are HE=CE*DE+CEF*DF. Insert parts DF in D and HE in H
HE=CE*DE+CEF*DF;
D(nq+1:ns)=DF
H(1:nq)=HE
%Transform the results back to the original
Q=T'*D
F=T'*H
return;
%==========

function [ke] = Make_Ke(e)
%Function calculates member stiffness matrix in global coordinates for frame member e.
%First calaculate ke in local coordinates, and rotational transform M.
%Return the results in Ke = M'*ke*M.
global A E I L M REL l m n nqn

AE=A(e)*E(e);
EI=E(e)*I(e);
LL=L(e)*L(e); LLL=LL*L(e);
M=[l(e) m(e) 0 0 0 0; -m(e) l(e) 0 0 0 0; 0 0 1 0 0 0; 0 0 0 l(e) m(e) 0; 0 0 0 -m(e) l(e) 0; 0 0 0 0 0 1];
ke=zeros(2*nqn,2*nqn);
%FF default
    ke(1,:)=[AE/L(e) 0 0 -AE/L(e) 0 0];
    ke(2,:)=[0 12*EI/LLL 6*EI/LL 0 -12*EI/LLL 6*EI/LL];
    ke(3,:)=[0 6*EI/LL 4*EI/L(e) 0 -6*EI/LL 2*EI/L(e)];
    ke(4,:)=[-AE/L(e) 0 0 AE/L(e) 0 0];
    ke(5,:)=[0 -12*EI/LLL -6*EI/LL 0 12*EI/LLL -6*EI/LL];
    ke(6,:)=[0 6*EI/LL 2*EI/L(e) 0 -6*EI/LL 4*EI/L(e)];
%emd FF default
if REL(e,:)==[0,1] %FP   
    ke(1,:)=[AE/L(e) 0 0 -AE/L(e) 0 0];
    ke(2,:)=[0 3*EI/LLL 3*EI/LL 0 -3*EI/LLL 0];
    ke(3,:)=[0 3*EI/LL 3*EI/L(e) 0 -3*EI/LL  0];
    ke(4,:)=[-AE/L(e) 0 0 AE/L(e) 0 0];
    ke(5,:)=[0 -3*EI/LLL -3*EI/LL 0 3*EI/LLL 0];
    ke(6,:)=[0 0 0 0 0 0 ]; %FP
end
if REL(e,:)==[1,0] %PF
    ke(1,:)=[AE/L(e) 0 0 -AE/L(e) 0 0];
    ke(2,:)=[0 3*EI/LLL 0 0 -3*EI/LLL 3*EI/LL];
    ke(3,:)=[0 0 0 0 0 0];
    ke(4,:)=[-AE/L(e) 0 0 AE/L(e) 0 0];
    ke(5,:)=[0 -3*EI/LLL  0 0 3*EI/LLL -3*EI/LL];
    ke(6,:)=[0 3*EI/LL 0 0 -3*EI/LL 3*EI/L(e)];
end
if REL(e,:)==[1,1]  %PP
    ke(1,:)=[AE/L(e) 0 0 -AE/L(e) 0 0];
    ke(2,:)=[0 0 0 0 0 0];
    ke(3,:)=[0 0 0 0 0 0];
    ke(4,:)=[-AE/L(e) 0 0 AE/L(e) 0 0];
    ke(5,:)=[0 0 0 0 0 0];
    ke(6,:)=[0 0 0 0 0 0];  %PP
end
ke=M'*ke*M
return;
%==========

function [Ge] = Make_Ge(e)
%Function makes Ge (2(nqn) x ns), the transformation matrix relating global indices to
%local indices of displacement and nodal forces from Connectivity Table CTAB
%for element (e).
global CTAB nqn ns

Ge=zeros(2*nqn,ns);
Ge(1,3*CTAB(e,1)-2)=1;
Ge(2,3*CTAB(e,1)-1)=1;
Ge(3,3*CTAB(e,1)  )=1;
Ge(4,3*CTAB(e,2)-2)=1;
Ge(5,3*CTAB(e,2)-1)=1;
Ge(6,3*CTAB(e,2)  )=1
return;
%==========