%
% pyr.m
%
% deformation of a right pyramid
%

% original vertices

x0 = [0 1 0 0];
y0 = [0 0 1 0];
z0 = [1 0 0 0];

% plot edges

plot3([x0(1) x0(4)],[y0(1) y0(4)],[z0(1) z0(4)]);
hold on
plot3([x0(1) x0(2)],[y0(1) y0(2)],[z0(1) z0(2)]);
plot3([x0(1) x0(3)],[y0(1) y0(3)],[z0(1) z0(3)]);
plot3([x0(3) x0(4)],[y0(3) y0(4)],[z0(3) z0(4)]);
plot3([x0(2) x0(4)],[y0(2) y0(4)],[z0(2) z0(4)]);
plot3([x0(2) x0(3)],[y0(2) y0(3)],[z0(2) z0(3)]);

% construct the adjacency and stiffness matrices

s = 1/sqrt(2);

A = zeros(6,12);

A(1,[3 12]) = [1 -1];
A(2,[4 10]) = [1 -1];
A(3,[1 3 4 6]) = s*[-1 1 1 -1];
A(4,[2 3 8 9]) = s*[1 1 -1 -1];
A(5,[8 11]) = [-1 1];
A(6,[4 5 7 8]) = s*[1 1 -1 -1];

S = A'*A;

% construct and plot a random consistent load

f = A'*randn(6,1)/8;

plot3([x0(1) x0(1)+f(1)],[y0(1) y0(1)+f(2)],[z0(1) z0(1)+f(3)],'k','linewidth',3)
plot3([x0(2) x0(2)+f(4)],[y0(2) y0(2)+f(5)],[z0(2) z0(2)+f(6)],'k','linewidth',3)
plot3([x0(3) x0(3)+f(7)],[y0(3) y0(3)+f(8)],[z0(3) z0(3)+f(9)],'k','linewidth',3)
plot3([x0(4) x0(4)+f(10)],[y0(4) y0(4)+f(11)],[z0(4) z0(4)+f(12)],'k','linewidth',3)

% compute the displacements

d = pinv(S)*f;

% displace the vertices

x = x0 + d(1:3:12)';
y = y0 + d(2:3:12)';
z = z0 + d(3:3:12)';

% plot the deformed edges

plot3([x(1) x(4)],[y(1) y(4)],[z(1) z(4)],'r');
plot3([x(1) x(2)],[y(1) y(2)],[z(1) z(2)],'r');
plot3([x(1) x(3)],[y(1) y(3)],[z(1) z(3)],'r');
plot3([x(3) x(4)],[y(3) y(4)],[z(3) z(4)],'r');
plot3([x(2) x(4)],[y(2) y(4)],[z(2) z(4)],'r');
plot3([x(2) x(3)],[y(2) y(3)],[z(2) z(3)],'r');

hold off

% polish

axis equal
view(-37.5,-30)
title('Original is blue, deformed is red, force is black', 'fontsize',16)
