%%
%% 4.3 Roll pendulum (Computational example)
% The suspension point {K_1}of a plane pendulum slides frictionless along the x-axis. The pendulum body{K_2} has the distance L from the suspension point. Both bodies have the same mass ${m_1} = {m_2} = m$ and the connection between {K_1} and{K_2} is massless
%
% We choose as generalized coordinates x and \phi.
% Coordinates: \[\vec r = \left( {\begin{array}{*{20}{c}}
% {{x_1}}\\
% {{x_2}}\\
% {{y_2}}
% \end{array}} \right) = \left( {\begin{array}{*{20}{c}}
% x\\
% {x + L\sin \varphi }\\
% {l\cos \varphi }
% \end{array}} \right),\quad \left( {{y_1} = 0!} \right)\]
% Parameters: mass, gravitational acceleration, pendulum length m=g=L=1, energy E=0.9999
%

clear all
syms x L m g phi E E0 positive
r_v=[x;x+L*sin(phi);-L*cos(phi)]
Par=[E0==0.9999 m==1 g==1 L==1]

%
%% 1 Ground vectors 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,[x,phi])
G=simplify(J'*J)
G_i=G^(-1)

%% 2 Hamiltonian
% H={1\over 2m}(p_r,p_\varphi)G_i
% \left({p_r\atop p_\varphi}\right)
% +V(r) mit V(r)=-mgL\cos\varphi

syms p_x p_phi real
p_v=[p_x;p_phi]
H=1/(2*m)*(p_v'*G_i*p_v)-m*g*L*cos(phi);
H=expand(H)

% p_x is a constant of motion: \frac{\partial p_x}{\partial t}=-\frac{\partial H}{\partial x}=0

-diff(H,x)

%
%% 3 Special case \[\vec r = \left( {\begin{array}{*{20}{c}}
% {{x_1}}\\
% {{x_2}}\\
% {{y_2}}
% \end{array}} \right) = \left( {\begin{array}{*{20}{c}}
% x\\
% {x + L\sin \varphi }\\
% {l\cos \varphi }
% \end{array}} \right),\quad \left( {{y_1} = 0!} \right)\]p_x=0
% We calculate for this case the angular momentum p_\phi as a function of \phi and the energy H=E and from this the reversal points \[{\phi _0} = {\left. \phi  \right|_{{p_\phi } = 0}}.
% Hamiltonian

H3=sube(H,p_x==0)

% Momentum p_\phi from E=H(\phi,p_\phi) \ \to\ p_\phi(\phi,E) for movement from left to right

p3_phi=solve(E0==H3,p_phi,'IgnoreProperties',true)
double(sube(p3_phi,[Par,phi==0])) %p3_phi(2) from left to right
p3_phi=p3_phi(2)

% Reversal points \phi_0 at p_\phi(\phi,E)=0

phi0=solve(p3_phi,phi,'IgnoreProperties', true)
double(sube(phi0,Par))
phi0=min(ans)

%
%% 4 Period time
% T=2\int\limits_{t(-|\varphi_0|)}^{t(|\varphi_0|)}dt=
% 2\int\limits_{t(-|\varphi_0|)}^{t(|\varphi_0|)}{\dot \varphi \over \dot \varphi}dt=
% 2\int\limits_{-|\varphi_0|}^{|\varphi_0|}{1\over \dot \varphi}d \varphi

dphi_t=diff(H3,p_phi)
sube(dphi_t,[p_phi==p3_phi,Par]);
T=double(vpaintegral(2/ans,phi,-phi0,phi0))

%
%% 5 Equations of motion
% Equations:  \dot x={\partial H\over\partial p_x},\
% \dot\varphi={\partial H\over\partial p_\varphi},\
% \dot p_\varphi=-{\partial H\over\partial\varphi}
% Initial values: x(0)=0,\ p_\varphi(0)=0,\ \varphi(0)=\varphi_0

dx_t=subs(diff(H,p_x),p_x,0)
dphi_t=subs(diff(H,p_phi),p_x,0)
dp_phi_t=-subs(diff(H,phi),p_x,0)

%
%% 6 Numerical solution

syms x_n(t) phi_n(t) p_phi_n(t)
var=[x==x_n(t) phi==phi_n(t) p_phi==p_phi_n(t)]
sube([dx_t;dphi_t;dp_phi_t],[var,Par])
f=odeFunction(ans,[x_n(t) phi_n(t) p_phi_n(t)])
[t sol]=ode23s(f,linspace(0,T,100),[0;-phi0;0],odeset('RelTol',1e-8));
X=sol(:,1);Phi=sol(:,2);P_phi=sol(:,3);

%
%% 7 Momentum
% p(t)=\sqrt{(p_x,p_\phi)G_i  \Big({p_x \atop p_\phi} \Big) } für p_x=0

p_v
p7=sqrt(p_v'*G_i*p_v)
sube(p7,[p_x==0 Par])
matlabFunction(ans)
p=ans(P_phi,Phi);

%
%% 8 Graphics

plot(t,X,'r',t,Phi,'b',t,P_phi,'c',t,p,'g',[0 T],[0 0],':k')
axis([0 double(T) 3*[- 1 1]])
xlabel('Time \sl t')
legend('x(t)','phi(t)','p_{phi}','p(t)','Location','southeast')
hold off

%
%% 9 Animation \rightarrow
%  'Roll_Pendulum.gif'

r_v
r_vp=sube(r_v,Par)

%

Nt=size(X,1);

%

s=matlabFunction(r_vp(1));x1=s(X);fx1=@(Ta)x1(round(1+(Nt-1)*Ta));
s=matlabFunction(r_vp(2));x2=s(Phi,X);fx2=@(Ta)x2(round(1+(Nt-1)*Ta));
s=matlabFunction(r_vp(3));y2=s(Phi);fy2=@(Ta)y2(round(1+(Nt-1)*Ta));
ft=@(Ta)t(round(1+(Nt-1)*Ta));

%

c1=.8*[1 1 0]; c2=[1 .2 .2];
r=0.1; n=1;

figure
hold on
plot([-1 1],[0 0],'Color',.5*[1 1 1],'LineWidth',10)
fanimator( @(Ta)plot([fx1(Ta) fx2(Ta)],[0 fy2(Ta)],'k','LineWidth',3), 'AnimationRange',[0 1],'FrameRate',100);
fanimator( @(Ta)rectangle('Position',r*[-1 -1 2 2]+[fx1(Ta) 0 0 0],'FaceColor',c1), 'AnimationRange',[0 1],'FrameRate',100);
fanimator( @(Ta)rectangle('Position',r*[-1 -1 2 2]+[fx2(Ta) fy2(Ta) 0 0],'Curvature',[1 1],'FaceColor',c2), 'AnimationRange',[0 1],'FrameRate',100);
fanimator( @(Ta)text(0,1.2,sprintf('t= %2.1f',ft(Ta)),'HorizontalAlignment','center'), 'AnimationRange',[0 1],'FrameRate',100);
axis([-1.1 1.1 -1.1 1.1]); axis square; axis off
hold off

%

 % playAnimation

%

writeAnimation('Roll_Pendulum.gif','Loopcount',inf)

%