function [] = quartercar ( dt, ihfcn );
% QuarterCar
%
% Dieses m-file ist Teil der Arbeitsmaterialien des 
%   Schuelerseminars "Bewegungssimulation mit dem Computer"
% am Fachbereich Mathematik und Informatik der
% Martin-Luther-Universitaet Halle-Wittenberg, Deutschland
%
% Author :      Prof. Dr. M. Arnold, martin.arnold@mathematik.uni-halle.de
% Version of :  Oct 7, 2005
%
% Bewegungssimulation
%
% Parameter:
%   dt     (input)  : Zeitschritt
%   ihfcn  (input)  : control flag "function h(x)"
%                       =1 .. sinusoidal
%                       =2 .. table look-ups
%
% Beispiel:
%   quartercar ( 1.0e-3, 2 );

% -> Fahrweg einlesen
if ihfcn==2,
    hdat = load ( 'h.dat' );
else
    hdat = [];
end;

% -> Modellparameter
t0    = 0.0;            % Anfangszeit
te    = 10.0;           % Endzeit
ys0   = 0.0;            % Anfangshoehe
vs0   = 0.0;            % Anfangsgeschwindigkeit
yus0  = 0.0;            % Anfangshoehe
vus0  = 0.0;            % Anfangsgeschwindigkeit

vx    = 2.0;            % Geschwindigkeit
hmax  = 0.1;            % Amplitude der Weganregung
len   = 2.0;            % Wellenlaenge der Weganregung

ms    = 290.0;          % Masse
ks    = 16182;          % Federsteifigkeit
ds    = 1000;           % Daempfung

mus   = 59.0;           % Masse
ktire = 190000;         % Federsteifigkeit
dtire = 0.0;            % Daempfung

% -> Initialisierung der Ergebnisdaten
nstep = round ( (te-t0)/dt );
t   = zeros ( 1+nstep, 1 );
ys  = zeros ( 1+nstep, 1 );
vs  = zeros ( 1+nstep, 1 );
yus = zeros ( 1+nstep, 1 );
vus = zeros ( 1+nstep, 1 );

t(1)   = t0;   
ys(1)  = ys0;    vs(1)  = vs0;
yus(1) = yus0;   vus(1) = vus0;

% -> Berechnung der Ergebnisse
tn   = t0;   
ysn  = ys0;    vsn  = vs0;
yusn = yus0;   vusn = vus0;
for istep=1:nstep, 
    [ h, hp ] = hfcn ( tn, vx, len, ihfcn, hdat );
    fs = - ks    * ( ysn - yusn ) - ds    * ( vsn - vusn ); 
    ft = - ktire * ( yusn - h )   - dtire * ( vusn - hp ); 
    tn = tn + dt;       % neuer Zeitpunkt
    ysb  = ysn  + dt*vsn;
    vsb  = vsn  + dt*fs/ms;
    yusb = yusn + dt*vusn;
    vusb = vusn + dt*(ft-fs)/mus;
    [ h, hp ] = hfcn ( tn, vx, len, ihfcn, hdat );
    fsb = - ks    * ( ysb - yusb ) - ds    * ( vsb - vusb ); 
    ftb = - ktire * ( yusb - h )   - dtire * ( vusb - hp ); 
    ysn  = ysn  + dt*(vsn+vsb)/2;
    vsn  = vsn  + dt*(fs+fsb)/(2*ms);
    yusn = yusn + dt*(vusn+vusb)/2;
    vusn = vusn + dt*(ft+ftb-fs-fsb)/(2*mus);
    t(istep+1)   = tn;
    ys(istep+1)  = ysn;
    vs(istep+1)  = vsn;
    yus(istep+1) = yusn;
    vus(istep+1) = vusn;
end;

% -> Graphische Ausgabe
hvec  = zeros ( size(t) );
hpvec = zeros ( size(t) );
for it=1:length(t),
    [ hvec(it), hpvec(it) ] = hfcn ( t(it), vx, len, ihfcn, hdat );
end;

figure(1)
plot ( t, hvec, 'g', 'LineWidth', 4  );
hold on;
plot ( t, yus, 'r', t, ys, 'b', 'LineWidth', 2  );
hold off;
legend ( 'h(x(t))', 'y_r', 'y_f' );
xlabel ( 't  [s]' );
ylabel ( 'y  [m]' );
set ( gca, 'XLim', [ t0 te ] );
axis ( [ 0 10 -0.2 0.2 ] );

figure(2)
plot ( t(2:end), diff(yus)./diff(t), 'r', t(2:end), diff(ys)./diff(t), 'b', 'LineWidth', 2 );
legend ( 'v_r', 'v_f' );
xlabel ( 't  [s]' );
ylabel ( 'v  [m/s]' );
set ( gca, 'XLim', [ t0 te ] );
axis ( [ 0 10 -2.0 2.0 ] );

figure(3)
plot ( t(2:end-1), diff(yus,2)./(diff(t(2:end)).^2), 'r', t(2:end-1), diff(ys,2)./(diff(t(2:end)).^2), 'b', 'LineWidth', 2 );
legend ( 'a_r', 'a_f' );
xlabel ( 't  [s]' );
ylabel ( 'a  [m/s^2]' );
set ( gca, 'XLim', [ t0 te ] );
axis ( [ 0 10 -20.0 20.0 ] );

% -> Fahrweg
function [ h, hp ] = hfcn ( t, vx, len, ihfcn, hdat ),

if ihfcn==1,
    h  = 0.1 * sin ( 2*pi*vx*t / len );
    hp = 0.1 * 2 * pi * vx / len * cos ( 2*pi*vx*t / len );
elseif ihfcn==2,
    delx = diff ( hdat(1:2,1) );
    x    = vx * t;
    icur = 1 + min  ( floor ( x / delx ), length(hdat(:,1))-2 );
    hdel = ( hdat(icur+1,2) - hdat(icur,2) ) / ( hdat(icur+1,1) - hdat(icur,1) );
    h    = hdat(icur,2) + hdel * ( x - hdat(icur,1) );
    hp   = hdel * vx;
elseif ihfcn==3,
    x    = vx * t;
    if x<=4.0,
        h = 0.0;
    else
        h = -0.05;
    end;
    hp   = 0.0;
end;
