%-----------------------------------------------------------------
% Script 'Discretization Methods', Example 4.6
% (c) 01/2005 by A. Schmidt, IAM, Uni Stuttgart
%-----------------------------------------------------------------

%-----------------------------------------------------------------
% remark: in the example, no boundary conditions are given and
%         no time-integration scheme is applied. Thus, in this
%         program we assume
%         - the rod to be fixed at the right side (node n_s)
%         - a force f acting on the left side (node 1)
%         - the rod being at rest and undeformed at time t=0
%         - the problem is solved twice using
%           * the central difference method (index cd)
%           * Newmark`s method (index nm)
%-----------------------------------------------------------------
clear all

% Variables
l = 1.0;                            % length of the rod
E = 7.1e+10;                        % Young`s modulus of the rod
A = 0.0001;                         % cross section area of the rod
rho = 2700.0;                       % density of the material
fo = 1.0;                           % external force
n_s = 6;                            % number of nodes (without ghost points)
dt = 0.000005;                      % time step size
t_max = 0.01;                       % overall simulation time

delta = 0.5;                        % Newmark parameter #1
alpha = 0.25;                       % Newmark parameter #2

% Parameters
n_t = floor(t_max/dt);              % number of temporal nodes (time int.)
a_0 = 1.0/alpha/dt^2;               % abbreviation for Newmark method
a_2 = 1.0/alpha/dt;                 % abbreviation for Newmark method
a_3 = 0.5/alpha-1.0;                % abbreviation for Newmark method

%-----------------------------------------------------------------
% Calculation of the mass matrix M [Eq. (4.240)]
% first row (i=1)
M(1,1) = 2.0;
M(1,2) = 1.0;

% middle block
for i=2:1:n_s-2
  M(i,i-1) = 1.0;
  M(i,i) = 4.0;
  M(i,i+1) = 1.0;
end

% last row (i=n_s-1)
M(n_s-1,n_s-2) = 1.0;
M(n_s-1,n_s-1) = 4.0;

M = M*rho*A*l/(n_s-1.0)/6.0;

% Calculation of the stiffness matrix K [Eq. (4.240)]
% first row (i=1)
K(1,1) = 1.0;
K(1,2) = -1.0;

% middle block
for i=2:1:n_s-2
  K(i,i-1) = -1.0;
  K(i,i) = 2.0;
  K(i,i+1) = -1.0;
end

% last row (i=n_s-1)
K(n_s-1,n_s-2) = -1.0;
K(n_s-1,n_s-1) = 2.0;

K = K*E*A*(n_s-1.0)/l;

% Calculation of the external force vector f [Eq. (4.240)]
f(1:n_s-1) = 0.0;
f(1) = fo;
f = f';                             % make f a column vector

%-----------------------------------------------------------------
% Calculation / time integration using central difference method
% Displacements at time t=0 (and t<0): initial conditions
u_cd(1:n_s,1) = 0.0;

% Displacement at right side: boundary condition
u_cd(n_s,2:n_t) = 0.0;

% calculation of the inverse of M: M_inv
M_inv = inv(M);

% Time integration, [cf to Eq. (4.249)]
% first time step using ghost points
u_cd(1:n_s-1,2) = 2.0*u_cd(1:n_s-1,1)-u_cd(1:n_s-1,1)+M_inv*(f-K*u_cd(1:n_s-1,1))*dt^2;

% time stepping procedure
for i=2:1:n_t
  u_cd(1:n_s-1,i+1) = 2.0*u_cd(1:n_s-1,i)-u_cd(1:n_s-1,i-1)+M_inv*(f-K*u_cd(1:n_s-1,i))*dt^2;
end

%-----------------------------------------------------------------
% Calculation / time integration using Newmark`s method
% Displacements, velocities and accelerations at time t=0: ic
u_nm(1:n_s,1) = 0.0;
vel(1:n_s-1,1) = 0.0;               % defined only for n-1 nodes
acc(1:n_s-1,1) = 0.0;               % defined only for n-1 nodes

% Displacement at right side: boundary condition
u_nm(n_s,2:n_t) = 0.0;

% effective stiffness matrix K_eff Eq. (4.263)
K_eff = K+a_0*M;

% time stepping procedure
for i=1:1:n_t

  % effective load vector Eq. (4.264)
  f_eff = f+M*(a_0*u_nm(1:n_s-1,i)+a_2*vel+a_3*acc);

  % new displacements, Eq. (4.259), accelerations Eq. (4.257) and velocities, Eq. (4.253)
  u_nm(1:n_s-1,i+1) = K_eff\f_eff;
  acc_new = a_0*(u_nm(1:n_s-1,i+1)-u_nm(1:n_s-1,i))-a_2*vel-a_3*acc;
  vel_new = vel+((1.0-delta)*acc+delta*acc_new)*dt;
  
  vel = vel_new;                    % update of velocities for next inc.
  acc = acc_new;                    % update of accelerations for next inc.
end

%-----------------------------------------------------------------
% Output
% sample points (time)
for i=1:1:n_t+1
  t(i) = (i-1)*dt;
end

% plot
figure(1)
plot(t,u_cd(1,:),'k',t,u_nm(1,:),'r')
xlabel('time / s')
ylabel('displacement / m')
title('displacement of node #1    black: cent diff    red: Newmark')