function u = mkp(elems, nodes, mats, f0, u0)

% Direct Stiffness Method/Finite Element Method
% (METODA KONECNYCH PRVKU)
%
% u = mkp(elems, nodes, mats, forces, fixs)
%
% vstup:
%
%   elems - elementy
%     [ el1_nd1 el1_nd2;
%       el2_nd1 el2_nd2;
%       ...                  ]
%
%   nodes - uzly
%     [ nd1_x nd1_y;
%       nd2_x nd2_y;
%       ...          ]
%
%   mats - materialove vlastnosti elementu
%     [ mat_el1, mat_el2, ... ]
%
%   f0 - sily pusobici v uzlech
%     [ nd1 f_x f_y;
%       nd2 f_x f_y;
%       ...          ]
%
%   u0 - homogenni Dirichletovy okrajove podminky (u_d = 0)
%     [ nd1 dof;
%       nd2 dof;
%       ...      ]
%
%  vystup:
%
%    u - posuvy v uzlech
%

% pocet stupnu volnosti v uzlu
NDOF = 2;

% pocet uzlu a elementu
NNODES = length(nodes);
NELEMS = length(elems);
N = NNODES*NDOF;

% mapa: uzel + stupen volnosti --> cislo rovnice
map = 1:N;
map = reshape(map, NDOF, NNODES)';

% vykresli nezdeformovanou sit
figure
hold on
axis equal
kresli_geom(elems, nodes, 'b:');

% globalni matice tuhosti a vektro sil
K = zeros(N);
f = zeros(N,1);

% sestaveni globalni matice tuhosti
for ielem = 1:NELEMS,
  el = elems(ielem,:);
  % souradnice uzlu elementu
  local_nodes = [ nodes(el(1),:); nodes(el(2),:)];
  % vypocet lokalni matice tuhosti a vektoru sil
  Ke = assemble_local_K( el, local_nodes, mats(ielem) );
  % pricte lokalni matici tuhosti ke globalni
  local_map = [ map(el(1),:) map(el(2),:) ];
  K(local_map,local_map) = K(local_map,local_map) + Ke;
end

% vektor sil
f = zeros(N,1);
for ibc = 1:size(f0,1),
  eq = [map(f0(ibc,1),1) map(f0(ibc,1),2) ]; 
  f(eq,1) = f0(ibc,2:3)';
end

% homogenni Dirichletova okrajova podminka
for ibc = 1:size(u0,1),
  eq = map(u0(ibc,1), u0(ibc,2));
  K(eq,:) = zeros(1,N);
  K(:,eq) = zeros(N,1);
  K(eq,eq) = 1;
  f(eq,1) = 0;
end

% reseni
u_ = K\f;

% nove souradnice
u = reshape(u_, NDOF, NNODES)';

% vykresli zdeformovanou sit
kresli_geom(elems, nodes + u, 'k');

kresli_sily(nodes + u, f0)
kresli_posuvy0(nodes + u, u0)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% vykresleni predepsanych nulovych posuvu
function kresli_posuvy0(nodes, u0)

for i = 1:size(u0,1),
  nd = nodes(u0(i,1),:)
  if u0(i,2) == 1
    plot(nd(1), nd(2), 'r>', 'MarkerSize', 12, 'LineWidth', 2);
  elseif u0(i,2) == 2
    plot(nd(1), nd(2), 'r^', 'MarkerSize', 12, 'LineWidth', 2);
  end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% vykresleni predepsanych sil
function kresli_sily(nodes, sily)

for i = 1:size(sily,1),
  nd = nodes(sily(i,1),:)
  quiver(nd(1), nd(2), sily(i,2), sily(i,3), 'r', 'LineWidth', 2);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% vykresleni geometrie
function kresli_geom(elems, nodes, col)

for ielem=1:size(elems,1),
  node1 = nodes(elems(ielem,1),:);
  node2 = nodes(elems(ielem,2),:);
  plot([node1(1) node2(1)], [node1(2) node2(2)], col);
  plot([node1(1) node2(1)], [node1(2) node2(2)], ['o', col]);
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% sestaveni lokalni matice tuhosti
function [K,f] = assemble_local_K(elem, nodes, mat)

d = nodes(2,:) - nodes(1,:);
l = sqrt( d*d' ); % delka elementu

K = mat/l * [ 1 0 -1 0;
	      0 0 0 0;
	      -1 0 1 0;
	      0 0 0 0 ];

c = d(1)/l;
s = d(2)/l; % cos, sin
T0 = [ c s; -s c ];  % matice rotace
T = [ T0 zeros(2,2); zeros(2,2) T0 ];
K = T'*K*T; % local -> global ss

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%