% 
% the 2-by-1 cantilever c cells wide and d cells high
%
%  with m = 4*c*d + c fibers
%
%  usage: mich1(c,d,Ea,F,nof)
%
% exmaple:  c=4; d=6; m=4*c*d+c; mich1(c,d,ones(m,1),-0.05,20)
%

function mich1(c,d,Ea,F,nof)

n = c*(d+1);	% nodes
m = 2*c*d + c*(d+1) + c*d  % fibers

A = zeros(m,2*n);
L = zeros(m,1);
h = sqrt(1/d^2 + 4/c^2);   % length of each diagonal bar
t = asin(1/d/h);
ct = cos(t);
st = sin(t);

for j=1:d,

    rc = (j-1)*4*c;
    cc = (j-1)*2*c+1;   % index of horizontal motion of SE node of 1st layer
    uc = cc + 2*c;

    A(1+rc,cc) = 1;
    L(1+rc) = 2/c;

    A(2+rc,[uc uc+1]) = [ct st];
    L(2+rc) = h;

    A(3+rc,[cc cc+1]) = [ct -st];
    L(3+rc) = h;

    A(4+rc,[cc+1 uc+1]) = [-1 1];
    L(4+rc) = 1/d;

    for i=1:c-1

        cc = cc + 2;
        uc = uc + 2;
        rc2 = rc + 4*i;

        A(1+rc2,[cc cc-2])  = [1 -1];
        L(1+rc2) = 2/c;

        A(2+rc2,[cc-2 cc-1 uc uc+1])  = [-ct -st ct st];
        L(2+rc2) = h;

        A(3+rc2,[cc cc+1 uc-2 uc-1])  = [ct -st -ct st];
        L(3+rc2) = h;

        A(4+rc2,[cc+1 uc+1])  = [-1 1];
        L(4+rc2) = 1/d;

   end

end

rc = m-c+1;
cc = cc+2;
A(rc,cc) = 1;
L(rc) = 2/c;

for i=1:c-1

    cc = cc+2;
    A(rc+i,[cc-2 cc]) = [-1 1];
    L(rc+i) = 2/c;

end

figure(1)
spy(A)
xlabel('degree of freedom','fontsize',16)
ylabel('fiber number','fontsize',16)
title(['Nonzero adjacencies, cx = ' num2str(c) ' cy = ' num2str(d)],'fontsize',16)

indf = c+c*(d+1);		% center right index
V = 20;
a0 = V./L/m;
S = A'*diag(Ea./L)*A;
f = zeros(size(S,1),1);
f(indf) = F;

figure(2)
fibdraw(c,d,zeros(2*n,1))
M(1) = getframe;

for j=2:nof

   x = gauss(S,f*(j-1)/(nof-1));

   fibdraw(c,d,x)

   M(j) = getframe;

end

hold off

movie(M,-4,10)

movie2avi(M,'bcant','quality',100);

return

function fibdraw(c,d,x)

n = c*(d+1);	% nodes
m = 2*c*d + c*(d+1) + c*d;  % fibers

for j=1:d,

    rc = (j-1)*4*c;
    cc = (j-1)*2*c+1;   % index of horizontal motion of SE node of 1st layer
    uc = cc + 2*c;

    xc(1+rc,:) = [0 2/c+x(cc)];
    yc(1+rc,:) = [(j-1)/d (j-1)/d+x(cc+1)];

    xc(2+rc,:) = [0 2/c+x(uc)];
    yc(2+rc,:) = [(j-1)/d j/d+x(uc+1)];

    xc(3+rc,:) = [0 2/c+x(cc)];
    yc(3+rc,:) = [j/d (j-1)/d+x(cc+1)];

    xc(4+rc,:) = [2/c+x(cc) 2/c+x(uc)];
    yc(4+rc,:) = [(j-1)/d+x(cc+1) j/d+x(uc+1)];

    for i=1:c-1

        cc = cc + 2;
        uc = uc + 2;
        rc2 = rc + 4*i;

        xc(1+rc2,:) = [i*2/c+x(cc-2) (i+1)*2/c+x(cc)];
        yc(1+rc2,:) = [(j-1)/d+x(cc-1) (j-1)/d+x(cc+1)];

        xc(2+rc2,:) = [i*2/c+x(cc-2) (i+1)*2/c+x(uc)];
        yc(2+rc2,:) = [(j-1)/d+x(cc-1) j/d+x(uc+1)];

        xc(3+rc2,:) = [i*2/c+x(uc-2) (i+1)*2/c+x(cc)];
        yc(3+rc2,:) = [j/d+x(uc-1) (j-1)/d+x(cc+1)];

        xc(4+rc2,:) = [(i+1)*2/c+x(cc) (i+1)*2/c+x(uc)];
        yc(4+rc2,:) = [(j-1)/d+x(cc+1) j/d+x(uc+1)];

   end

end

rc = m-c+1;
cc = cc+2;
xc(rc,:) = [0 2/c+x(cc)];
yc(rc,:) = [1 1+x(cc+1)];

for i=1:c-1

    cc = cc+2;
    xc(rc+i,:) = [i*2/c+x(cc-2) (i+1)*2/c+x(cc)];
    yc(rc+i,:) = [1+x(cc-1) 1+x(cc+1)];

end

clf
line(xc',yc')
hold on

fill([-.25 0 0 -.25],[-.1 -.1 1.1 1.1],'w')
axis equal
axis([-.3 2.2 -0.5 1.1]) 
axis off
hold off

%
% gauss.m
%
% usage 	x = gauss(A,b)
%
% where		A is an n-by-n matrix
%		b is an n-by-1 vector
%
% and		x is an n-by-1 vector that satisfies A x = b
%
% functions called: trisolve
%

function x = gauss(A,b)

n = length(b);

A(:,n+1) = b;			% augment A with b

for k=1:n-1,			% k is the column counter

    [y,i] = max(abs(A(k:n,k)));		% find biggest no. at or below row k
    r = i + k - 1;			% absolute row number

    if y < eps
       disp('oops, hit a small pivot')	% warning message
    end

    tmp = A(k,:);			% swap rows k and r
    A(k,:) = A(r,:);
    A(r,:) = tmp;

    for j=k+1:n				% eliminate, i.e., make zero
					% all elements below diagonal
        mn = -A(j,k)/A(k,k);

        A(j,:) = A(j,:) + mn*A(k,:);

    end

end

b = A(:,n+1);			% extract new b

if abs(A(n,n)) < eps
   disp('oops, hit a small pivot')	% warning message
end

x = trisolve(A,b);		% backsolve triangular system

return

%

function x = trisolve(A,b)

n = length(b);

x = zeros(n,1);
x(n) = b(n)/A(n,n);
for j=n-1:-1:1,
    x(j) = (b(j)-A(j,j+1:n)*x(j+1:n))/A(j,j);
end

return