% g, cm, sec

addpath('../two_dim_fea');

points = [ 0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3;
           0, 0, 0, 0, 1, 1, 1, 1, 2, 2, 2, 2, 3, 3, 3, 3 ];
triangles = [ 1, 2, 5;
              2, 3, 6;
              3, 4, 7;
              6, 5, 2;
              7, 6, 3;
              8, 7, 4;
              5, 6, 9;
              6, 7, 10;
              7, 8, 11;
              10,  9, 6;
              11, 10, 7;
              12, 11, 8;
               9, 10, 13;
              10, 11, 14;
              11, 12, 15;
              14, 13, 10;
              15, 14, 11;
              16, 15, 12 ];

npoints = size(points,2);
ntriangles = size(triangles,1);
thickness = 1;
elastic = Body(npoints, points, ntriangles, triangles, thickness);

% E = 0.1 MPa; \nu = 0.48; rho = 1 g/cm^2
Young = 1.0*1e+6; nu = 0.48; density = 1.00;
[ lambda, mu ] = Lame_constants( Young, nu );
elastic = elastic.mechanical_parameters(density, lambda, mu);

% stiffness matrix
elastic = elastic.calculate_stiffness_matrix;
K = elastic.Stiffness_Matrix;

% constraints
nconstraints = 12;
A = elastic.constraint_matrix([1, 2, 3, 4, 14, 15]);
dy = -0.3;
b = [ 0;0; 0;0; 0;0; 0;0; 0;dy; 0;dy ];

mat = [ K, -A; -A', zeros(nconstraints,nconstraints) ];
vec = [ zeros(2*npoints,1); -b ];
sol = mat \ vec;
un = sol(1:2*npoints);

figure('position', [0, 0, 800, 800]);
set(0,'defaultAxesFontSize',32);
set(0,'defaultTextFontSize',32);

clf;
elastic.draw(zeros(2, npoints));
xlim([-1,4]); ylim([-1,4]); grid on; pbaspect([1 1 1]);
saveas(gcf, 'get_started_natural.png');

clf;
elastic.draw(reshape(un, [2, npoints]));
xlim([-1,4]); ylim([-1,4]); grid on; pbaspect([1 1 1]);
saveas(gcf, 'get_started_deformed.png');
