% msd_fdbt.m
%
% Used to plot the solution of a mass-spring-damper system using
% a finite difference approach.
%
% Gary P. Scavone
% McGill University, 2004.

% Simulation Parameters

fs = 44100;      % sample rate
T = 1/fs;        % sample period
N = 10000;        % iterations

% System Parameters

m = 0.5;          % mass in kg
k = 5000000;      % spring constant (N/m)
w0 = sqrt(k/m);   % natural angular frequency of system
f0 = w0/(2*pi);   % natural frequency of system (no damping)
mu = 0.5;         % damping constant
x0 = 0;        % initial displacement
v0 = 4;           % initial velocity

% System Equation: m(x'') + mu(x') + kx = 0, where x' = dx/dt

% Using the Laplace Transform:
%
% m(s^2*X(s) - s*x0 - v0) + mu(s*X(s) - x0) + k X(s) = 0
%
% X(s) = (m*x0*s + m*v0 + mu*x0) / (m*s^2 + mu*s + k)

% Now do s-plane to z-plane mapping for the backward finite
% difference: s = (1-z^-1)/T and s^2 = (1 - 2z^-1 + z^2)/T^2.
% Note that the constant initial condition terms in the numerator
% should be scaled by the sample rate (by a factor of 1/T) when
% converted from continuous to discrete time. Another way to
% think about this is a continuous delta function needs to be
% scaled by 1/T to maintain the same "integrated area" over time
% and to properly bandlimit it: delta(x) -> delta[n]/T:
%
% X(z) = 1/T*(b0 + b1*z^-1) / (a0 + a1*z^-1 + a2*z^-2),
%
% where:
b0 = m*x0*T + m*v0*T^2 + mu*x0*T^2;
b1 = -m*x0*T;
a0 = m + mu*T + k*T^2;
a1 = -(2*m + mu*T);
a2 = m;

% Now find the difference equation via the inverse z-Transform
%
% x[n] = b0/(T*a0)*delta[n] + b1/(T*a0)*delta[n-1] - a1/a0*x[n-1] - a2/a0*x[n-2]

% Displacement Initialization
x = zeros(1,N);
xm1 = 0;        % x[n-1]
xm2 = xm1;      % x[n-2]

% Now do simulation:
for n = 1:N
  
  x(n) = -xm1*a1 - xm2*a2;
  if n == 1
    x(n) = x(n) + b0/T;
  end
  if n == 2
      x(n) = x(n) + b1/T;
  end
  x(n) = x(n) / a0;
  xm2 = xm1;
  xm1 = x(n);
  
end

plot(x)
grid
xlabel('Time Steps (samples)');
ylabel('Mass Displacement (m)');

hold on;

% Now do the same with the bilinear transform s-plane to z-plane
% mapping: s = alpha*(1-z^-1)/(1+z^-1)
%
% X(z) = 1/T*(b0 + b1*z^-1 + b2*z^-2) / (a0 + a1*z^-1 + a2*z^-2),
%
% where:
alpha = 2 / T;
%alpha = f0 / ( tan(2*pi*f0*T/2));
a0 = m*alpha^2 + mu*alpha + k;
a1 = 2*(k - m*alpha^2);
a2 = (m*alpha^2 - mu*alpha + k);
b0 = (m*x0*alpha + m*v0 + mu*x0);
b1 = 2*(mu*x0 + m*v0);
b2 = (m*v0 + mu*x0 - m*alpha*x0);

% Now find the difference equation via the inverse z-Transform
%
% x[n] = b0/(T*a0)*delta[n] + b1/(T*a0)*delta[n-1] + b2/(T*a0)*delta[n-2] - a1/a0*x[n-1] - a2/a0*x[n-2]
% Now do simulation:

% Displacement Initialization (accounted for in delta functions)
x = zeros(1,N);
xm1 = 0.0;      % x[n-1]
xm2 = xm1;      % x[n-2]

for n = 1:N
  
  x(n) = -xm1*a1 - xm2*a2;
  if n == 1
    x(n) = x(n) + b0/T;
  end
  if n == 2
      x(n) = x(n) + b1/T;
  end
  if n == 3
      x(n) = x(n) + b2/T;
  end
  x(n) = x(n) / a0;
  xm2 = xm1;
  xm1 = x(n);
  
end

plot(x, 'r')
legend('Backward FD', 'Bilinear Transform');
hold off
