% *********************************************************************
%                                       CAPSIF.M
% *********************************************************************

% Internal forces in Planar Subsystems

% **************
%      Input
% **************

% eai = (matrix)- Each row lists E,A,I the length (l) and the angle
% fi (measured in degress counterclockwise from the local bar
% axis), as many rows as bars in the structure. 

% kc = (matrix) - Each row has the three coefficients of the local
% flexural stiffness (typically 4 4 2).

% w = vector with the bar number and the uniform load acting on the bar (only bars with uniform
% loads need to be specified (positive when directed oposite to the positive y-y axis for the bar).
% (see note on output for positive y-y).

% sml = (matrix) - Used to specify the effect of loads that act on bars but are not uniform
% this matrix can be input as null [] if there are no bars with special loads.
% Each row has 7 values, the first is the bar number and the other six are the
% fixed end forces due to the special loads in local member coordinates (see sign convention for output).

% jl = vector of lateral loads (taken from Capsld.m)

% *****************************
%           Output
% *****************************

% delta = displacements (lateral motion should be close to what you get from Capsld.m)
% res = internal forces in the bars - one bar per row -in the following order
% axial shear and moment at node i, axial shear and moment at node j. 
% Sign convention:
% 	The bar axis going from node i to j forms the local x axis
%  the y axis is positive in such a way that the z axis (in a right hand
%	system is directed towards the viewer, moments are positive when
%	clockwise.

function [res,delta]=capsif(jc,eai,kc,w,sml,jl);

% Initialize the stiffness matrix and the load vector
lm=lmg1(jc);
[nb,qq]=size(lm);
dof=max(lm);
dof=max(dof);
stif=zeros(size(dof));
Q=zeros(dof,1);

% Create the special member load matrix 
a=isempty(sml);
if a~=1;
[nsb,dum]=size(sml);
else
   nsb=1;
sml=[1 0 0 0 0 0 0];   
end;

loc=sml(:,1);
sml1=zeros(nb,6);
sml1(loc,:)=sml(:,2:7);

% Create the local member load matrix
mlm=zeros(nb,6);
for j=1:length(w(:,2));
   le=eai(j,4);
   mlm(w(j,1),1:6)=[0 w(j,2)*le/2 -w(j,2)*le^2/12 0 w(j,2)*le/2 w(j,2)*le^2/12];
end;
mlmt=mlm+sml1;
ml=mlmt;

% Assemble the stiffness matrix and the part of the load vector
% that derives from member loads.
for i=1:nb
% At the level of distortions
kl=[kc(i,1) kc(i,3);kc(i,3) kc(i,2)];
c=eai(i,1)*eai(i,3)/eai(i,4);
kl=c*kl;
kl=[kl [0 0]'];
comp=[0 0 eai(i,1)*eai(i,2)/eai(i,4)];
kl=[kl;comp];

% Transfer to 6*6 in local coordinates
T1=[0 -1/eai(i,4) 1 0 1/eai(i,4) 0;0 -1/eai(i,4) 0 0 1/eai(i,4) 1;-1 0 0 1 0 0];
k1=T1'*kl*T1;

% Rotate
T2=[cos(eai(i,5)*pi/180) sin(eai(i,5)*pi/180) 0; -sin(eai(i,5)*pi/180) cos(eai(i,5)*pi/180) 0; 0 0 1];
dc=zeros(3);
T2=[T2 dc;dc T2];
k2=T2'*k1*T2;
fef1=T2'*ml(i,:)';

% Assemble the locator matrix
T3=zeros(6,dof);
for j=1:6
a=lm(i,j);
if a==0;
T3=T3;
else;
T3(j,a)=1;
end;
end;

% Contribution to global stiffness and to global load vector
kgs=T3'*k2*T3;
fef2=T3'*fef1;
stif=stif+kgs;
Q=Q+fef2;
end;

jlp=[jl;zeros(dof-length(jl),1)];
% Change the sign of Q, add the joint loads and solve
L=-Q+jlp;
delta=inv(stif)*L;

%Compute the internal forces due to joint displacements
res=[];

for i=1:nb
% Distortion matrix
kl=[kc(i,1) kc(i,3);kc(i,3) kc(i,2)];
c=eai(i,1)*eai(i,3)/eai(i,4);
kl=c*kl;
kl=[kl [0 0]'];
comp=[0 0 eai(i,1)*eai(i,2)/eai(i,4)];
kl=[kl;comp];

% Take the stiffness to the 6x6 format
T1=[0 -1/eai(i,4) 1 0 1/eai(i,4) 0;0 -1/eai(i,4) 0 0 1/eai(i,4) 1;-1 0 0 1 0 0];
k1=T1'*kl*T1;

% Assemble T2
T2=[cos(eai(i,5)*pi/180) sin(eai(i,5)*pi/180) 0; -sin(eai(i,5)*pi/180) cos(eai(i,5)*pi/180) 0; 0 0 1];
dc=zeros(3);
T2=[T2 dc;dc T2];

% Assemble T3
T3=zeros(6,dof);
for j=1:6
a=lm(i,j);
if a==0;
T3=T3;
else;
T3(j,a)=1;
end;
end;

% Obtain distortions
dd=T2*T3*delta;

% Obtain forces 
forces=k1*dd;

res=[res;forces'];
end;

% Add the fixed end member forces
res=res+ml;

% *************** SUBPROGRAMS *******************
% Subprogram to generate lm
function [lm]=lmg1(jc);
[nb dum]=size(jc);
a=max(jc);
nc=a(2);
lm=[];

for j=1:nb;
   n1=jc(j,1);
   n2=jc(j,2);
   if n1==0;
      e1=[0 0 0];
   e2=[n2 n2+nc n2+2*nc];
else;
   e1=[n1 n1+nc n1+2*nc];
   e2=[n2 n2+nc n2+2*nc];
   end;
   y=[e1 e2];
   lm=[lm;y];
end;