%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This m-file forms the finite element model of a beam system and solves it.
% G.K. Ananthasuresh for MEAM 310 class in Spring 1997.
% And, modified in Spring 1998.
% Inputs:
%			Ae	--> A vector containing the cross-section areas of all the elements
%			Le	-->	A vector containing the lengths of all the elements
%			Ee	-->	A vector containing the Young's moduli of all the elements
%			nx	-->	A vector containing the X-coordinates of all the nodes
%			ny	-->	A vector containing the Y-coordinates of all the nodes
%		  ncon	-->	A matrix containing the nodal-connectivity of all the elements
%			F	-->	A vector containing the applied external forces
%		dispID	--> A vector containing the displacement boundary condition information
%			Inertia --> 	A vector containing the Inertia of all the elements
%
% Outputs:
%		u		-->	The displacements of all the dof
%		Rdisp	-->	The reaction forces at the dof which are specified to be fixed
%		P		--> A matrix containing the X and Y internal reaction forces at the two
%					ends of each element, and flag that	tells you if the element
%					is in tension (1), compression (-1) or no	stress (0).
%		King	--> The global stiffness matrix before the application of the
%					boundary conditions.
%		SE		--> The strain energy in the entire truss
%
function [u,Rdisp,P1,P2,Ksing,SE] = fembeam1(Ae, Le, Ee, nx, ny, ncon, NELEM, ...
    NNODE, F, dispID,dispVal,Ine)

K = zeros(3*NNODE,3*NNODE);		% Initialize global stiffness matrix
k = zeros(6,6);						% Initialize local stiffness matrix

for ie=1:NELEM,
    eye = ncon(ie,1);
    jay = ncon(ie,2);

    % Form the transformation matrix, Lambda.
    L = Le(ie);
    A = Ae(ie);
    E = Ee(ie);
    In= Ine(ie);
    lox = (nx(jay)-nx(eye))/L; mox = (ny(jay)-ny(eye))/L;
    loy = -mox; moy = lox;
    Lambda = [ lox mox  0   0   0   0   ; ...
        -mox lox  0   0   0   0   ; ...
        0   0   1   0   0   0   ; ...
        0   0   0  lox  mox 0   ; ...
        0   0   0 -mox  lox 0   ; ...
        0   0   0   0   0   1  ];
    % Form local element matrix
    k(1,1)=E*A/L;			k(1,4)=-k(1,1);
    k(2,2)=12*E*In/(L^3);		k(2,3)=6*E*In/(L^2);
    k(2,5)=-k(2,2);		k(2,6)=k(2,3);
    k(3,2)=k(2,3);		k(3,3)=4*E*In/L;
    k(3,5)=-k(2,3);		k(3,6)=2*E*In/L;
    k(4,1)=-k(1,1);		k(4,4)=k(1,1);
    k(5,2)=-k(2,2);		k(5,3)=-k(2,3);
    k(5,5)=k(2,2);		k(5,6)=-k(2,3);
    k(6,2)=k(2,3);		k(6,3)=k(3,6);
    k(6,5)=-k(2,3);		k(6,6)=k(3,3);

    klocal = Lambda' * k * Lambda;


    % Form ID matrix to assemble klocal into the global stiffness matrix, K.
    id1 = 3*(eye-1) + 1;
    id2 = id1 + 1;
    id3 = id2 + 1;
    id4 = 3*(jay-1) + 1;
    id5 = id4 + 1;
    id6 = id5 + 1;

    K(id1,id1) = K(id1,id1) + klocal(1,1);
    K(id1,id2) = K(id1,id2) + klocal(1,2);
    K(id1,id3) = K(id1,id3) + klocal(1,3);
    K(id1,id4) = K(id1,id4) + klocal(1,4);
    K(id1,id5) = K(id1,id5) + klocal(1,5);
    K(id1,id6) = K(id1,id6) + klocal(1,6);

    K(id2,id1) = K(id2,id1) + klocal(2,1);
    K(id2,id2) = K(id2,id2) + klocal(2,2);
    K(id2,id3) = K(id2,id3) + klocal(2,3);
    K(id2,id4) = K(id2,id4) + klocal(2,4);
    K(id2,id5) = K(id2,id5) + klocal(2,5);
    K(id2,id6) = K(id2,id6) + klocal(2,6);

    K(id3,id1) = K(id3,id1) + klocal(3,1);
    K(id3,id2) = K(id3,id2) + klocal(3,2);
    K(id3,id3) = K(id3,id3) + klocal(3,3);
    K(id3,id4) = K(id3,id4) + klocal(3,4);
    K(id3,id5) = K(id3,id5) + klocal(3,5);
    K(id3,id6) = K(id3,id6) + klocal(3,6);

    K(id4,id1) = K(id4,id1) + klocal(4,1);
    K(id4,id2) = K(id4,id2) + klocal(4,2);
    K(id4,id3) = K(id4,id3) + klocal(4,3);
    K(id4,id4) = K(id4,id4) + klocal(4,4);
    K(id4,id5) = K(id4,id5) + klocal(4,5);
    K(id4,id6) = K(id4,id6) + klocal(4,6);

    K(id5,id1) = K(id5,id1) + klocal(5,1);
    K(id5,id2) = K(id5,id2) + klocal(5,2);
    K(id5,id3) = K(id5,id3) + klocal(5,3);
    K(id5,id4) = K(id5,id4) + klocal(5,4);
    K(id5,id5) = K(id5,id5) + klocal(5,5);
    K(id5,id6) = K(id5,id6) + klocal(5,6);

    K(id6,id1) = K(id6,id1) + klocal(6,1);
    K(id6,id2) = K(id6,id2) + klocal(6,2);
    K(id6,id3) = K(id6,id3) + klocal(6,3);
    K(id6,id4) = K(id6,id4) + klocal(6,4);
    K(id6,id5) = K(id6,id5) + klocal(6,5);
    K(id6,id6) = K(id6,id6) + klocal(6,6);
end

% Store unlaltered K as Ksing before applying the boundary conditions.
Ksing = K;

%det(K)
%inv(K);
%pause

% Imposing displacement boundary conditions
% ------------------------------------------
% dispID array contains the dof which are assigned specified values.
[sm,sn] = size(dispID);
Ndbc = sn;
for nd=1:Ndbc,
    for nr=1:3*NNODE-nd+1,
        F(nr) = F(nr) - K(nr,dispID(nd)-nd+1) * dispVal(nd);
    end
    K = matcut(K,dispID(nd)-nd+1);
    F = veccut(F,dispID(nd)-nd+1);
end

% To solve for unknown displacements.
U = inv(K)*F;
SE = .5*U'*K*U;

% Results
% ---------------
% "u" for all nodes (including those where values were specified)
u = zeros(3*NNODE,1);

for iu=1:Ndbc,
    u(dispID(iu)) = 12345.12345;
end

iuc = 0;
for iu=1:3*NNODE,
    if u(iu) == 12345.12345
        iuc = iuc+1;
    else
        u(iu) = U(iu-iuc);
    end
end

for iu=1:Ndbc,
    u(dispID(iu)) = dispVal(iu);
end


%----------------------------------------------
% Computation of reactions at constrained nodes
%----------------------------------------------
R = Ksing*u;
Rdisp = zeros(1,Ndbc);
for iu=1:Ndbc,
    Rdisp(iu) = R(dispID(iu));
end

%-------------------------------------------
% Computation of internal reaction forces
% and storing in P(NNODE,4)
%-------------------------------------------
%M = zeros(NNODE,1);

for ie=1:NELEM,

    eye = ncon(ie,1);
    jay = ncon(ie,2);


    % Form the transformation matrix, Lambda.
    L = Le(ie); A = Ae(ie);
    E = Ee(ie);
    In= Ine(ie);
    lox = (nx(jay)-nx(eye))/L; mox = (ny(jay)-ny(eye))/L;
    loy = -mox; moy = lox;
    Lambda = [ lox mox  0   0   0   0   ; ...
        -mox lox  0   0   0   0   ; ...
        0   0   1   0   0   0   ; ...
        0   0   0  lox  mox 0   ; ...
        0   0   0 -mox  lox 0   ; ...
        0   0   0   0   0   1  ];
    % Form local element matrix
    k(1,1)=E*A/L;			k(1,4)=-k(1,1);
    k(2,2)=12*E*In/(L^3);	k(2,3)=6*E*In/(L^2);
    k(2,5)=-k(2,2);		k(2,6)=k(2,3);
    k(3,2)=k(2,3);		k(3,3)=4*E*In/L;
    k(3,5)=-k(2,3);		k(3,6)=2*E*In/L;
    k(4,1)=-k(1,1);		k(4,4)=k(1,1);
    k(5,2)=-k(2,2);		k(5,3)=-k(2,3);
    k(5,5)=k(2,2);		k(5,6)=-k(2,3);
    k(6,2)=k(2,3);		k(6,3)=k(3,6);
    k(6,5)=-k(2,3);		k(6,6)=k(3,3);

    kelem = Lambda' * k * Lambda;


    % Form ID matrix to identify respective displacements.
    id1 = 3*(eye-1) + 1;
    id2 = id1 + 1;
    id3 = id2 + 1;
    id4 = 3*(jay-1) +1;
    id5 = id4 + 1;
    id6 = id5 + 1;

    uelem = [u(id1) u(id2) u(id3) u(id4) u(id5) u(id6)];

    Rint = kelem*uelem';

    P1(ie,1) = Rint(1);
    P1(ie,2) = Rint(2);
    P1(ie,3) = Rint(3);
    P1(ie,4) = Rint(4);
    P1(ie,5) = Rint(5);
    P1(ie,6) = Rint(6);

    ulocal = Lambda*uelem';
    Rint_local = k*ulocal;

    P2(ie,1) = Rint_local(1);
    P2(ie,2) = Rint_local(2);
    P2(ie,3) = Rint_local(3);
    P2(ie,4) = Rint_local(4);
    P2(ie,5) = Rint_local(5);
    P2(ie,6) = Rint_local(6);

end

