% ビームの動的変形（断面積一定）
% 時間 [ 0, tpush ] : 下端固定　上端は力 fpush で押す
% 時間 [ tpush, tend ] : 下端の制約を外す．上端の力をなくす．
%                        下端が床に接触しているときは，床から反力を受ける
%                        床反力はペナルティ法で表す

L = 10; A = 2; % ビームの長さと断面積（一定）
n = 6; % 節点の個数
h = L/(n-1);
E = 10; % 弾性率（ヤング率）
c = 10;  % 粘性率
rho = 1; % 密度
alpha = 2000; % 制約安定化法の定数
fpush = 4; % 押し込み力
Evfloor = 1000; cvfloor = 0; % 床の弾性と粘性 (弾性率/長さ)
%Evfloor = E/L; cvfloor = c/L; % 床の弾性と粘性 (弾性率/長さ)
tpush = 20; tend = 40;

e0 = (4/6)*ones(n,1); e0(1) = (2/6); e0(n) = (2/6);
e1 = (1/6)*ones(n,1);
M = (rho*A*h)*spdiags([e1 e0 e1], -1:1, n, n); % 慣性行列

e0 = 2*ones(n,1); e0(1) = 1; e0(n) = 1;
e1 = (-1)*ones(n,1);
K = (E*A/h)*spdiags([e1 e0 e1], -1:1, n, n); % 剛性行列
B = (c*A/h)*spdiags([e1 e0 e1], -1:1, n, n); % 粘性行列

constraints = zeros(n,1); constraints(1) = 1; % 制約行列（下端固定）
p = n; % 力の作用点（上端）
external_force = @(t,n,un,vn) pushing_force_param (t,n,un,vn, p,fpush,tpush); % [o,tpush] の間，外力 -fpush
beam_dynamic_equation = @(t,q) beam_supported_dynamic_equation_param (t,q, n,M,B,K,constraints,alpha,external_force);
interval = [0,tpush];
qinit = [ zeros(n,1); zeros(n,1) ];
[timepush,qpush] = ode45(beam_dynamic_equation, interval, qinit);

p = 1; % 力の作用点（下端）
external_force = @(t,n,un,vn) floor_force_param(t,n,un,vn, p,A,Evfloor,cvfloor); % 下端の床反力
beam_dynamic_equation = @(t,q) beam_free_dynamic_equation_param (t,q, n,M,B,K,external_force);
interval = [tpush,tend];
qinit = qpush(end, :);
[timefree,qfree] = ode45(beam_dynamic_equation, interval, qinit);

time = [ timepush; timefree ];
q = [ qpush; qfree ];

clf;
hold on;
for k=1:n
    plot(time,q(:,k)+(k-1)*h);
end
hold off;
xlabel("time");
ylabel("position");
saveas(gcf,'beam_pushed_released_position.png');
fprintf("節点の位置\n");
%fprintf("一時停止：何かのキーを押してください\n");
%pause;

clf;
hold on;
for t = [0:2:tend]
    i = nearest_index(time,t);
    plot([time(i);time(i)], [q(i,1)+(1-1)*h;q(i,n)+(n-1)*h], 'b-o');
end
%plot(time, q(:,1), time, q(:,n)+(n-1)*h);
hold off;
xlabel("time");
ylabel("position");
saveas(gcf,'beam_pushed_released_body.png');
fprintf("ボディの変形と運動\n");
%fprintf("一時停止：何かのキーを押してください\n");
%pause;

plot(time, q(:,n)-q(:,1)+L, time, L*ones(size(time,1),1),'--');
xlabel("time");
ylabel("length");
ylim([6,12]);
saveas(gcf,'beam_pushed_released_body_length.png');
fprintf("ボディの長さ\n");
%fprintf("一時停止：何かのキーを押してください\n");
%pause;
