SphericalPendulumConstraintsSoft.html  (MotionGenesis input/output).
   (1) % MotionGenesis file:  SphericalPendulumConstraintsSoft.txt 
   (2) % Problem:  Pendulum on a string (wrecking ball or tether ball)
   (3) % Copyright (c) 2009 Motion Genesis LLC.  All rights reserved.
   (4) %--------------------------------------------------------------------
   (5) NewtonianFrame  N
   (6) Particle        Q
   (7) %--------------------------------------------------------------------
   (8) Variable   x'',  y'',  z''        % Nx>, Ny>, Nz> measures of Q's position from No.
   (9) Constant   g = 9.8 m/s^2          % Earth's gravitational acceleration.
   (10) Constant   k = 100 N/m            % Linear spring  constant (when string is flexible).
   (11) Constant   zeta = 0 noUnits       % For linear damping constant (when string is flexible).
   (12) Constant   bAir = 0 N*s/m         % Aerodynamic damping constant.
   (13) Constant   Ln = 0.3 m             % Natural length of spring.
   (14) Constant   epsilonL = 1.0E-17 m   % Avoid divide-by-zero error when L is near zero.
   (15) Q.SetMass( m = 1 kg )
   (16) %--------------------------------------------------------------------
   (17) %       Position, velocity, acceleration
   (18) Q.Translate( No,  x*Nx> + y*Ny> + z*Nz> )
-> (19) p_No_Q> = x*Nx> + y*Ny> + z*Nz>
-> (20) v_Q_N> = x'*Nx> + y'*Ny> + z'*Nz>
-> (21) a_Q_N> = x''*Nx> + y''*Ny> + z''*Nz>

   (22) %--------------------------------------------------------------------
   (23) %       L, L', L'' are useful for both constraints and forces.
   (24) %       L = Q.GetDistance(No) + epsilonL.
   (25) Specified  L+,  L',  L'' 
   (26) SolveQuadraticPositiveRootDt( L^2 - Q.GetDistanceSquared(No),  L )
-> (27) L = sqrt(x^2+y^2+z^2)
-> (28) L' = (x*x'+y*y'+z*z')/L
-> (29) L'' = -(L'^2-x'^2-y'^2-z'^2-x*x''-y*y''-z*z'')/L

   (30) L += epsilonL
-> (31) L = epsilonL + sqrt(x^2+y^2+z^2)

   (32) %--------------------------------------------------------------------
   (33) %       String's spring tension on Q  (spring can stretch, but cannot compress).
   (34) %       In other words, the spring is active when  SpringStretch > 0.
   (35) Specified  SpringStretch'         % Spring stretch and its time-derivative.
   (36) SpringStretch = L - Ln
-> (37) SpringStretch = L - Ln

   (38) isSpringActive = ceil( sign(SpringStretch)/2 )
-> (39) isSpringActive = ceil(0.5*sign(SpringStretch))

   (40) TensionSpring = isSpringActive * k * SpringStretch 
-> (41) TensionSpring = k*SpringStretch*isSpringActive

   (42) QsUnitVectorFromNo> = Q.GetPosition(No) / L 
-> (43) QsUnitVectorFromNo> = x/L*Nx> + y/L*Ny> + z/L*Nz>

   (44) FSpring> = -TensionSpring * QsUnitVectorFromNo>
-> (45) FSpring> = -TensionSpring*x/L*Nx> - TensionSpring*y/L*Ny> - TensionSpr
        ing*z/L*Nz>

   (46) %--------------------------------------------------------------------
   (47) %       String's damper force on Q,  active when SpringStretch' > 0.
   (48) %       Damper is active only when spring is active and stretching. 
   (49) %       Note: To facilitate picking a damping constant, use damping ratio
   (50) %             with zeta=0 for no damping, zeta=1 for critical damping.
   (51) b = 2 * zeta * sqrt(m*k)
-> (52) b = 2*sqrt(m)*zeta*sqrt(k)

   (53) SpringStretch' = Dt( SpringStretch )
-> (54) SpringStretch' = L'

   (55) isElongating = ceil( sign(SpringStretch')/2 )
-> (56) isElongating = ceil(0.5*sign(SpringStretch'))

   (57) isDamperActive = isSpringActive * isElongating
-> (58) isDamperActive = isSpringActive*isElongating

   (59) TensionDamper = isDamperActive * b * SpringStretch'
-> (60) TensionDamper = b*SpringStretch'*isDamperActive

   (61) FDamper> = -TensionDamper * QsUnitVectorFromNo>
-> (62) FDamper> = -TensionDamper*x/L*Nx> - TensionDamper*y/L*Ny> - TensionDam
        per*z/L*Nz>

   (63) %--------------------------------------------------------------------
   (64) %       Air's damping force on Q is  -bAir * magV * (v / magV).
   (65) FAir> = -bAir * Q.GetVelocity(N)
-> (66) FAir> = -bAir*x'*Nx> - bAir*y'*Ny> - bAir*z'*Nz>

   (67) %--------------------------------------------------------------------
   (68) %       Rewrite net force on Q in terms of Fx, Fy, Fz.
   (69) FNet> = -m*g*Ny> + FAir> + FSpring> + FDamper>
-> (70) FNet> = (-(TensionSpring+TensionDamper)*x/L-bAir*x')*Nx> + (-m*g-(Tens
        ionSpring+TensionDamper)*y/L-bAir*y')*Ny> + (-(TensionSpring+TensionDa
        mper)*z/L-bAir*z')*Nz>

   (71) Fx = Dot( FNet>, Nx> );    Fy = Dot( FNet>, Ny> );    Fz = Dot( FNet>, Nz> )
-> (72) Fx = -(TensionSpring+TensionDamper)*x/L - bAir*x'
-> (73) Fy = -m*g - (TensionSpring+TensionDamper)*y/L - bAir*y'
-> (74) Fz = -(TensionSpring+TensionDamper)*z/L - bAir*z'

   (75) FNet> := Fx*Nx> + Fy*Ny> + Fz*Nz>
-> (76) FNet> = Fx*Nx> + Fy*Ny> + Fz*Nz>

   (77) Q.AddForce( FNet> )
-> (78) Force_Q> = Fx*Nx> + Fy*Ny> + Fz*Nz>

   (79) %--------------------------------------------------------------------
   (80) %       Equations of motion.
   (81) Zero[1] = Dot( Nx>,  Q.GetDynamics() )
-> (82) Zero[1] = m*x'' - Fx

   (83) Zero[2] = Dot( Ny>,  Q.GetDynamics() )
-> (84) Zero[2] = m*y'' - Fy

   (85) Zero[3] = Dot( Nz>,  Q.GetDynamics() )
-> (86) Zero[3] = m*z'' - Fz

   (87) %--------------------------------------------------------------------
   (88) %       Track kinetic energy, gravitational energy, spring energy.
   (89) KE = Q.GetKineticEnergy()
-> (90) KE = 0.5*m*(x'^2+y'^2+z'^2)

   (91) PEGravity = System.GetForceGravityPotentialEnergy( -g*Ny>, No )
-> (92) PEGravity = m*g*y

   (93) PESpring = 1/2 * k * SpringStretch^2 * isSpringActive
-> (94) PESpring = 0.5*k*SpringStretch^2*isSpringActive

   (95) %--------------------------------------------------------------------
   (96) %       Calculate power and work associated with damping forces.
   (97) PowerDamper = Dot( FDamper>,  Q.GetVelocity(N) )
-> (98) PowerDamper = -TensionDamper*(x*x'+y*y'+z*z')/L

   (99) PowerAir = Dot( FAir>,  Q.GetVelocity(N) )
-> (100) PowerAir = -bAir*(x'^2+y'^2+z'^2)

   (101) Variable  WorkDamper' = PowerDamper 
-> (102) WorkDamper' = PowerDamper

   (103) Variable  WorkAir'    = PowerAir
-> (104) WorkAir' = PowerAir

   (105) %--------------------------------------------------------------------
   (106) %       Kinetic energy + potential energy - work done should remain constant.
   (107) EnergySum = KE + PEGravity + PESpring - WorkDamper - WorkAir 
-> (108) EnergySum = PESpring + PEGravity + KE - WorkAir - WorkDamper

   (109) %--------------------------------------------------------------------
   (110) %       Calculate other output quantities.
   (111) Tension = TensionSpring + TensionDamper 
-> (112) Tension = TensionSpring + TensionDamper

   (113) q = GetAngleBetweenUnitVectors( -Ny>,  QsUnitVectorFromNo> )
-> (114) q = 3.141593 - acos(y/L)

   (115) %--------------------------------------------------------------------
   (116) %       Integration parameters and initial values for work.
   (117) Input  tFinal = 16 sec,  tStep = 0.005 sec,  absError := 1.0E-07,  relError = 1.0E-07
   (118) Input  WorkAir = 0 Joules,  WorkDamper = 0 Joules
   (119) %--------------------------------------------------------------------
   (120) %       Default (zero) initial values for variables (override below).
   (121) Input  x  = 0 m/s,  y  = 0 m/s,  z  = 0 m/s
   (122) Input  x' = 0 m/s,  y' = 0 m/s,  z' = 0 m/s  
   (123) %--------------------------------------------------------------------
   (124) %       Initial values for variables correspond to swinging spring.
   (125) % Input  x := Input(Ln) * sinDegrees(1) m,  y := -Input(Ln) * cosDegrees(1) m 
   (126) % OutputPlot  t sec,  L  m
   (127) % OutputPlot  t sec,  q  degrees
   (128) %--------------------------------------------------------------------
   (129) %       Alternate input values to swing from -90 deg to 150 deg and free-fall.
   (130) Input  tFinal := 2.0 sec, tStep := 0.001 sec, k := 10000 N/m,  zeta := 1.0 noUnits
   (131) Input  x := -Input(Ln) m,  y' := -2.35 m/s
   (132) OutputPlot  x m,  y m
   (133) OutputPlot  t sec,  Tension Newton
   (134) %--------------------------------------------------------------------
   (135) %       Alternate input values to spiral like a tether-ball.
   (136) % Input  tFinal := 16.0 sec,  tStep := 0.001 sec,  k := 5.0E6 N/m,  zeta := 1.0 noUnits,  bAir := 0.5 N*sec/m
   (137) % Input  x := -Input(Ln) m,   z' := -70 m/s
   (138) % OutputPlot  x m, z m
   (139) % OutputPlot  t sec, KE Joules,  WorkAir Joules,  EnergySum Joules
   (140) %--------------------------------------------------------------------
   (141) %       List output quantities and solve ODEs.
   (142) Output  t sec,  q deg,  L m,  SpringStretch m,  L' m/s,  L'' m/s,  x m,  y m, z m,  Tension Newton,  &
        KE Joules,  PEGravity Joules,  PESpring Joules,  WorkDamper Joules,  WorkAir Joules,  EnergySum Joules
   (143) ODE( Zero, x'', y'', z'' )  SphericalPendulumConstraintsSoft

   (144) %--------------------------------------------------------------------
Saved by Motion Genesis LLC.   Portions copyright (c) 2009-2017 Motion Genesis LLC. Rights reserved. Only for use with MotionGenesis.