(*
Dynamics of a Washer on a Rod
2PC mode, using terminal velocity
*)
l1=1;(*cm*)
l2=2 l1;
l3=0.4;
scale=3;
gbar=981;
g=gbar/(l1);
I3=1/2 (l1^2+l2^2)/(l1^2);
I1=I3/2;
(*data obtained from video*)
etad0=73.3;
(*obtain psi0 from \dot{\phi}*)
phidec=180;(* degree *)
psi0=ArcCos[360/phidec l3/l1];
theta0=ArcCos[l3/(l1 Cos[psi0]^2)];
(*theta0=51.3/180 Pi;*)
Print["psi0=",psi0,"rad"];
Print["theta0=",theta0,"rad"];

phi0=60/180 Pi;
eta0=0/180 Pi;
zg0=0;
tmax=1;
(*terminal velocity*)
eta[t_]:=eta0+etad0 t
phi[t_]:=eta[t] Cos[theta0]Cos[psi0]+phi0;
zg[t_]:=-scale(l1 eta[t]Cos[theta0]Cos[psi0]Sin[psi0]);
(* the components of the center of mass in \bar{E} frame*)
rg1[t_]:=scale(Cos[eta[t]](l3-l1 Cos[theta0])-Sin[eta[t]]l1 Sin[psi0]Sin[theta0]);
rg2[t_]:=scale(Sin[eta[t]](l3-l1 Cos[theta0])+Cos[eta[t]]l1 Sin[psi0]Sin[theta0]);
rg3[t_]:=zg[t]+scale l1 Cos[psi0]Sin[theta0];
(* the components of the e3 vertor in \bar{E} frame*)
e31[t_]:=Sin[theta0]Cos[eta[t]]-Cos[theta0]Sin[psi0]Sin[eta[t]];
e32[t_]:=Sin[theta0]Sin[eta[t]]+Cos[theta0]Sin[psi0]Cos[eta[t]];
e33[t_]:=Cos[theta0]Cos[psi0];
(* the components of the ebar vector (principal moment axis) in \bar{E} frame*)
eb11[t_]:=Cos[eta[t]]Cos[phi[t]]Cos[theta0]+Sin[eta[t]](Cos[psi0]Sin[phi[t]]+Cos[phi[t]]Sin[psi0]Sin[theta0]);
eb12[t_]:=-Cos[eta[t]]Cos[psi0]Sin[phi[t]]+Cos[phi[t]](Cos[theta0]Sin[eta[t]]-Cos[eta[t]]Sin[psi0]Sin[theta0]);
eb13[t_]:=Sin[phi[t]]Sin[psi0]-Cos[phi[t]]Cos[psi0]Sin[theta0];
eb21[t_]:=-Cos[phi[t]]Cos[psi0]Sin[eta[t]]+Sin[phi[t]](Cos[eta[t]]Cos[theta0]+Sin[eta[t]]Sin[psi0]Sin[theta0]);
eb22[t_]:=Cos[theta0]Sin[eta[t]]Sin[phi[t]]+Cos[eta[t]](Cos[phi[t]]Cos[psi0]-Sin[phi[t]]Sin[psi0]Sin[theta0]);
eb23[t_]:=-Cos[phi[t]]Sin[psi0]-Cos[psi0]Sin[phi[t]]Sin[theta0];
(*to draw objects*)
l3rod={Brown,Cylinder[{{0,0,-100 scale},{0,0,100 scale}},scale l3]};
light1={"Directional",White,{{20,20,20},{0,0,0}}};
light2={"Directional",White,{{-20,20,20},{0,0,0}}};
light3={"Directional",White,{{20,-20,20},{0,0,0}}};
light4={"Directional",White,{{-20,-20,-20},{0,0,0}}};

bunkatu=24;
pthe=2 Pi/bunkatu;
kizami=36;
pang=2 Pi/kizami;
viewang=-60/180 Pi;

Manipulate[uplinebold={Black,Cylinder[{{scale l1 (eb11[t])+rg1[t],scale l1 (eb12[t])+rg2[t],scale l1 (eb13[t])+rg3[t]},{scale l2 (eb11[t])+rg1[t],scale l2 (eb12[t])+rg2[t],scale l2 (eb13[t])+rg3[t]}},0.3],Cylinder[{{scale l1 (-eb11[t])+rg1[t],scale l1 (-eb12[t])+rg2[t],scale l1 (-eb13[t])+rg3[t]},{scale l2 (-eb11[t])+rg1[t],scale l2 (-eb12[t])+rg2[t],scale l2 (-eb13[t])+rg3[t]}},0.3]};
upline={Black,Table[Cylinder[{{scale l1 (Cos[n pang] eb11[t]+Sin[n pang] eb21[t])+rg1[t],scale l1 (Cos[n pang] eb12[t]+Sin[n pang] eb22[t])+rg2[t],scale l1 (Cos[n pang] eb13[t]+Sin[n pang] eb23[t])+rg3[t]},{scale l2 (Cos[n pang] eb11[t]+Sin[n pang] eb21[t])+rg1[t],scale l2 (Cos[n pang] eb12[t]+Sin[n pang] eb22[t])+rg2[t],scale l2 (Cos[n pang] eb13[t]+Sin[n pang] eb23[t])+rg3[t]}},0.1],{n,1,kizami}]};
polup=Polygon[Table[{scale l2 (Cos[n pthe] eb11[t]+Sin[n pthe] eb21[t])+rg1[t],scale l2 (Cos[n pthe] eb12[t]+Sin[n pthe] eb22[t])+rg2[t],scale l2 (Cos[n pthe] eb13[t]+Sin[n pthe] eb23[t])+rg3[t]},{n,0,bunkatu}]->Table[{scale l1 (Cos[n pthe] eb11[t]+Sin[n pthe] eb21[t])+rg1[t],scale l1 (Cos[n pthe] eb12[t]+Sin[n pthe] eb22[t])+rg2[t],scale l1 (Cos[n pthe] eb13[t]+Sin[n pthe] eb23[t])+rg3[t]},{n,0,bunkatu}]];
Graphics3D[{l3rod,polup,upline,uplinebold},PlotRange->{{-20,20},{-20,20},{-30,30}},Lighting->{light1,light2,light3,light4},ViewPoint->{10 Cos[viewang],10 Sin[viewang],0},Boxed->False],{t,0,tmax,1/420}]