%%
%% 4.4 Roller coaster (Computational example)
%
%
%

clear all

%
%% 1 Parameterization of the track
% Trajectory: \[\vec r = \left( {\begin{array}{*{20}{c}}
% x\\
% y\\
% z
% \end{array}} \right) = \left( {\begin{array}{*{20}{c}}
% {\sin \left( {2\pi q} \right)}\\
% {\frac{1}{2}\sin \left( {4\pi q} \right)}\\
% {\frac{1}{4}\cos \left( {2\pi q} \right)}
% \end{array}} \right),\quad q \in \left[ {0,1} \right]\]
% Parameters: energy E=0.2501, mass and gravitational acceleration m=g=1

syms q p E t real
Par=[E==0.25001]
r_v=[sin(2*pi*q);sin(4*pi*q)/2;cos(2*pi*q)/4]
figure
fplot3(r_v(1),r_v(2),r_v(3))
view([-7.5 64.])

%
%% 2 Jacobian and metric
% Jacobian J, metric G and contravariant metric G_i:   J=\left( {
% \partial x_i \over\partial q_j
% }\right), \
% G=J\prime J, \
% G_i=G^{-1}

J=jacobian(r_v,q)
G=J'*J
G_i=G^-1;

%
%% 3 Hamiltonian
% H={1\over 2m}pG_ip
% +mgz(q)

H=p*G_i*p+r_v(3)

%
%% 4 Momentum field
% H(q,p)=E\ \to\ p(q,E)

e1=isolate(H==E,p)

%
%% 5 Period time
% T=\int\limits_{t(0)}^{t(1)}dt=
% \int\limits_{t(0)}^{t(1)}{m\dot q\over G_ip}dt=
% \int\limits_0^1{m\over G_ip}dq

T=double(vpaintegral(G/sube(p,[e1,Par]),0,1))

%
%% 6 Equations of motion and numerical solution
% m\dot q=G_ip(q),\ m=1

syms Q(t)
N=150;
G_i*sube(p,e1);
odeFunction(sube(ans,[Par,q==Q(t)]),Q(t));
[tn,q_n]=ode45(ans,linspace(0,T,N),0);

%
%% 7 Animation of the movement\to  'Roller_Coaster.gif'

figure
n=1;
fplot3(r_v(1),r_v(2),r_v(3),'Color',.5*[.9 .9 1],'LineWidth',3);
hold on
fplot3(r_v(1),r_v(2),0*r_v(3)-0.29,'Color',.8*[1 1 1],'LineWidth',4);
K=10;
for k=1:K
    sube(r_v,q==(k-0.5)/K);
    plot3(ans(1)*[1 1],ans(2)*[1 1],[-0.29 ans(3)],...
        'Color',.5*[1 .5 0],'LineWidth',4)
end    
axis equal; axis off

% Tangent vector for car plot

diff(r_v,q);
ta=simplify(ans/sqrt(ans'*ans));

eps=.1;
ta_x=matlabFunction(eps*ta(1));
ta_y=matlabFunction(eps*ta(2));
ta_z=matlabFunction(eps*ta(3));

% Plot

rx=matlabFunction(r_v(1));
ry=matlabFunction(r_v(2));
rz=matlabFunction(r_v(3));

Nt=size(tn,1);
ft=@(t)(tn(round(1+(Nt-1)*t)));
fq=@(t)(q_n(round(1+(Nt-1)*t)));

Car=@(t0)plot3([rx(fq(t0))-ta_x(fq(t0)) rx(fq(t0))+ta_x(fq(t0))],[ry(fq(t0))-ta_y(fq(t0)) ry(fq(t0))+ta_y(fq(t0))],[rz(fq(t0))-ta_z(fq(t0)) rz(fq(t0))+ta_z(fq(t0))],'r','LineWidth',14);
Car_S=@(t0)plot3([rx(fq(t0))-ta_x(fq(t0)) rx(fq(t0))+ta_x(fq(t0))],[ry(fq(t0))-ta_y(fq(t0)) ry(fq(t0))+ta_y(fq(t0))],-0.27*[1 1],'Color',.7*[1 1 1],'LineWidth',14);

fanimator(Car,'AnimationRange',[0 1],'FrameRate',100);
fanimator(Car_S,'AnimationRange',[0 1],'FrameRate',100);
fanimator( @(t0)text(0,1.2,sprintf('Time t=%.1f',ft(t0)),'FontSize',20), 'AnimationRange',[0 1],'FrameRate',100);

%

%playAnimation

%

writeAnimation('Roller_Coaster.gif','LoopCount',inf)