function [] = hw4_sub()
%Solution to HW4G by TSD 10/18/06
global   nn ne nm ndim nep nqn
global   nq nf nmpc
global   XYZ CTAB AE QTAB FTAB MATERIAL
global   FIDI_NAME ns
global   k l m K L F P Q


%10 Read Input Data File for 2D truss 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
GetData2D(FIDI_NAME);

%2  Write a script to calculate 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].
Bar_Lengths;

%3  Write a script to draw the truss by plotting each element (graph the endpoints) in the same figure.
figure(1);
hold on; axis equal;
Draw_Truss
Label_Nodes
hold off

%4  Write a script to calculate k=AE/L for each element.
%Store the results in (l x ne) row vector [k.e]
for e=1:ne
    k(e)=AE(e)/L(e);
end
k

%5  Write a script, 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.
F=zeros(ns,1);
for i=1:nf
    F(FTAB(i,1))=FTAB(i,2);
end
F

%6  Write a Script, given nq and QTAB, to initialize the column vector Q with the specified displacements.
%Note: To create Q a value must be given. For now, let the Q(i) be zero except where another value is given.
Q=zeros(ns,1);
for i=1:nq
    Q(QTAB(i,1))=QTAB(i,2);
end
Q

%7  Write a script to calculate Ke and Ge for each element.
%Assemble the results in K using K=Sum(Ge'*Ke*Ge)
K=zeros(ns,ns);
Ge=zeros(2*nqn,ns);
for e=1:ne
    M=[l(e)*l(e) l(e)*m(e);l(e)*m(e) m(e)*m(e)]
    Ke=k(e)*[M -M;-M M]
    Ge=0*Ge;
    Ge(1,2*CTAB(e,1)-1)=1;
    Ge(2,2*CTAB(e,1))=1;
    Ge(3,2*CTAB(e,2)-1)=1;
    Ge(4,2*CTAB(e,2))=1
    K=K+Ge'*Ke*Ge
end

%8  Write a script to apply the boundary conditions and solves for displacements.
%Assume that Q(1) ... Q(nq) are the known displacements. Some may be non-zero.
%Partition Q, K, and F, solve for the free displacements QF=inv(KF)*(FF-KEF'*QE.
%list the suscripts of the elements in D and find T | D=TQ

QF_Solver

%9  Write a Script to calculate the Reactions FE=KE*QE+KEF*QF.
%Complete Q and F by including the calculated values of QF and FE.


%11  Write a script to calculate the nodal forces for each element Fe=Ke*Qe 
%Save the bar force for each element in col. vector P. P(e)=sqrt(Fe2x^2+Fe2y^2)
P=zeros(ne,1)
for e=1:ne
    M=[l(e)*l(e) l(e)*m(e);l(e)*m(e) m(e)*m(e)];
    Ke=k(e)*[M -M;-M M]
    Ge=0*Ge;
    Ge(1,2*CTAB(e,1)-1)=1;
    Ge(2,2*CTAB(e,1))=1;
    Ge(3,2*CTAB(e,2)-1)=1;
    Ge(4,2*CTAB(e,2))=1
    Fe=Ke*Ge*Q
    P(e)=[l(e) m(e)]*Fe(3:4)
end

%12  Draw truss in solid lines, label nodes, draw deformed truss in broken
%lines with green tension members and red compression members
figure(2);
hold on; axis equal;
Draw_Truss
Label_Nodes
Draw_Deformed_Truss
hold off


%=====
function []= GetData2D(FIDI_NAME)
%GetData2D(FIDI_NAME) reads input data for 2D truss from FIDI_NAME.
%FIDI_NAME (FIDI ~ File ID for Input) is the name of the input data file.
global   nn ne nm ndim nep nqn
global   nq nf nmpc
global   XYZ CTAB AE QTAB FTAB MATERIAL
global   ns


FIDI = fopen(FIDI_NAME, 'r');	%Open input file
disp(blanks(1));		%Write blanks to screen
TEXT = fgets(FIDI)		%Read captions, 4 lines
TEXT = fgets(FIDI)
TEXT = fgets(FIDI)
TEXT = fgets(FIDI)
DATA = str2num(fgets(FIDI));
[nn ne nm ndim nep nqn] = deal(DATA(1), DATA(2), DATA(3), DATA(4), DATA(5), DATA(6))
TEXT = fgets(FIDI)
DATA = str2num(fgets(FIDI));
[nq nf nmpc] = deal(DATA(1), DATA(2), DATA(3));

TEXT = fgets(FIDI)
XYZ = zeros(nn,3);		%make node coordinates table
for i=1:nn
    DATA = str2num(fgets(FIDI));
    for j=1:ndim
        XYZ(DATA(1),j) = DATA(1+j);
    end
end

TEXT = fgets(FIDI)
CTAB = zeros(ne,2);		%make CTAB: connectivity table
AE = zeros(ne,1); 		%make AE: members property table
for i=1:ne
    DATA = str2num(fgets(FIDI));
    [CTAB(DATA(1),1) CTAB(DATA(1),2) AE(DATA(1))] = deal(DATA(2), DATA(3), DATA(4));
end

TEXT = fgets(FIDI)
QTAB = zeros(nq,2);		%make QTAB: essential displacements table
for i=1:nq
    DATA = str2num(fgets(FIDI));
    [QTAB(i,1) QTAB(i,2)] = deal(DATA(1), DATA(2));
end

TEXT = fgets(FIDI)
FTAB = zeros(nf,2);		%make FTAB: specified forces table
for i=1:nf
    DATA = str2num(fgets(FIDI));
    [FTAB(i,1) FTAB(i,2)] = deal(DATA(1), DATA(2));
end

TEXT = fgets(FIDI)
MATERIAL = zeros(nm,2);		%make FTAB: specified forces table
for i=1:nm
    DATA = str2num(fgets(FIDI));
    nmp=1;				%number of material properties
    for j=1:nmp
        MATERIAL(DATA(1),j) = DATA(1+j);
    end
end

TEXT = fgets(FIDI)
status = fclose('all');
ns = nn*nqn
return;
%==========


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 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)
    L(e)=sqrt(Lx*Lx+Ly*Ly)
    l(e)=Lx/L(e)
    m(e)=Ly/L(e)
end
return;
%==========

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

global XYZ CTAB ne 

Xmin=XYZ(CTAB(1,1),1);
Ymin=XYZ(CTAB(1,1),2);
Xmax=Xmin;
Ymax=Ymin;
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);
    plot([Xe1 Xe2], [Ye1 Ye2]);
    Xmin=min([Xmin Xe1 Xe2]);
    Xmax=max([Xmax Xe1 Xe2]);
    Ymin=min([Ymin Ye1 Ye2]);
    Ymax=max([Ymax Ye1 Ye2]); 
end
e=max(Xmax-Xmin,Ymax-Ymin)/32;	%add border space
plot(Xmin-e,Ymin-e);
plot(Xmax+e,Ymax+e);
return;
%==========

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

function [] = Draw_Deformed_Truss()
%draw deformed truss
global XYZ CTAB P Q ne 
mag=32
for e=1:ne
    n1=CTAB(e,1);
    n2=CTAB(e,2);
    Xe2=XYZ(n2,1)+mag*Q(2*n2-1);
    Xe1=XYZ(n1,1)+mag*Q(2*n1-1);
    Ye2=XYZ(n2,2)+mag*Q(2*n2);
    Ye1=XYZ(n1,2)+mag*Q(2*n1);
    if P(e)<0
        plot([Xe1 Xe2], [Ye1 Ye2], 'r--');  %red compression bars
    else
        plot([Xe1 Xe2], [Ye1 Ye2], '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 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;
%==========