% MotionGenesis file:  MGWrapParticleAroundVerticalCylinder3D.txt
% Copyright (c) 2016 Motion Genesis LLC.  Only for use with MotionGenesis.
%--------------------------------------------------------------------
NewtonianFrame  N                % Cylinder with Nz> along symmetry axis.
RigidFrame      A                % Associated with theta rotation about +Nz>
RigidFrame      B                % Taut string with By> directed towards Q.
Particle        Q                % Particle at end of string.
Point           BN( B )          % Point of string B in contact with cylinder N.
Point           P                % Path point:  Traces out wrap on cylinder.
%--------------------------------------------------------------------
Variable    qA''                 % Angle from Nx> to Ax> with sense +Nz>.
Variable    qB''                 % Angle from Ay> to By> with sense +Ax>.
Variable     z''                 % Nz> measure of P's position from No.
Variable    L''                  % Length of deployed string (between P and Q).
Variable    Tension              % Tension in string.
Constant    R = 3 cm             % Radius of cylinder N.
Constant    g = 9.8 m/s^2        % Earth's gravitational acceleration.
Q.SetMass( m = 100 g )
%--------------------------------------------------------------------
%   Rotational and translational kinematics.
A.RotateZ( N, qA )
B.RotateX( A, qB )
P.Translate( No, R*Ax> + z*Az> ) 
Q.Translate( P,  L*By> ) 
BN.SetPositionVelocity( Q,  -L*By>, B )
%--------------------------------------------------------------------
%   Constraint 1: Total length of string does not change.
%   Constraint 2: Path point P only moves in By> direction (no sliding in Bx> direction).
%   Alternate:    v_BN_N> = 0>.
%   Let  s = Additional length of string wrapped around cylinder after t = 0.
%        s = FindByIntegration =  L0 - L  
Variable s' = Dot( P.GetVelocity(N), By> )
Input    s  = 0 cm                     % Initial value of s.
Constant    L0 = 70 cm                 % Initial value  of L.
SolveDt( s = L0 - L,   L )             % Find L', L'' in terms of theta', theta''.
SolveDt( Dot( P.GetVelocity(N), Bz> ),  z' )
%--------------------------------------------------------------------
%   Add relevant contact/distance forces.
Q.AddForce(  -Tension*By> - m*g*Nz> )
%--------------------------------------------------------------------
%   Form equations of motion via F = m*a and solve for qA'', qB'', Tension.
Dynamics[1] = Dot(  Q.GetDynamics(),  Bx>  )
Dynamics[2] = Dot(  Q.GetDynamics(),  By>  )
Dynamics[3] = Dot(  Q.GetDynamics(),  Bz>  )
Solve( Dynamics = 0,   qA'',  qB'', Tension )
%--------------------------------------------------------------------
%   Calculations for plotting Q's trajectory in N and solving initial values.
xQ = Dot(  Q.GetPosition(No),  Nx>  )
yQ = Dot(  Q.GetPosition(No),  Ny>  )
zQ = Dot(  Q.GetPosition(No),  Nz>  )
%--------------------------------------------------------------------
%   Use Q's initial position to solve for initial values of qA, qB, z.
SolveSetInput(  [xQ;  yQ;  zQ] = [R;  L0;  0],  qA =  0 deg,  qB =  0 deg,  z =  0 cm )
%--------------------------------------------------------------------
%   Use Q's initial velocity to solve for initial value of qA', qB'.
%   Note: Already used Bz> measure of P's velocity in N to solve for z'.
vx = Explicit( Dot( Q.GetVelocity(N), Ax> ),  qA', qB' )
vy = Explicit( Dot( Q.GetVelocity(N), Ay> ),  qA', qB'  )
vz = Explicit( Dot( Q.GetVelocity(N), Az> ),  qA', qB'  )
SolveSetInput( [vx; vz] = [-4;  0],   qA' = 4 rad/sec,  qB' = 0 rad/sec )
%--------------------------------------------------------------------
%   Calculations for plotting angular momentum and kinetic + potential energy.
KineticEnergy = Q.GetKineticEnergy()
GravityPotentialEnergy = Q.GetForceGravityPotentialEnergy( -g*Nz>, No )
MechanicalEnergy = KineticEnergy + GravityPotentialEnergy
HNo = Dot(  Q.GetAngularMomentum( No ),  Nz>  )
HBN = Dot(  Q.GetAngularMomentum( BN ),  Nz>  )
vSpeed = Q.GetSpeed( N )
%--------------------------------------------------------------------
%   List output quantities, provide integration parameters, and solve ODEs.
Output  t sec,  xQ cm,  yQ cm, zQ cm,  L cm, qA deg, qB deg, z m, Tension N, MechanicalEnergy Joules, vSpeed cm/s, HNo kg*m^2/s^2, HBN kg*m^2/s^2
Input  tFinal = 1.7259 sec,  tStep = 0.002 sec,  absError = 1.0E-07
ODE()  MGWrapParticleAroundVerticalCylinder3D
%--------------------------------------------------------------------
Save  MGWrapParticleAroundVerticalCylinder3D.html
Quit