% 13-09-23, Numerical solution of 3-D motion where mass is attached in spring by momentum principle
%           The mass can sway and oscillate

Iter    = 1;                 % 1 --> Predictor corrector 
DTpause = 0.0;               % s to pause between plotting

% Initial parameters
L0      = 2;                 % Equilibrium length for spring (m) with no mass attached
M       = 1;                 % Mass (kg)
K       = 5;                 % Spring constant (N/m)
g       = 9.8;               % Gravitational acceleration (m/s^2)  = 9.8 m/s^2

dT      = 0.002;             % Time step (s)
Plot    = 100;               % 0 No plot,  > 0, how many points to omit, 1 -> plot all points
EndT    = 10;


% Equilibrium position:  K*(Z+L0) = - M*g   -->  Z = -L0 - M*g/K
% X = 0,  Y = 0, Z = -L0 - M*g/K


% 1) No movement:   K*(Z+L0) = - M*g   -->  Z = -L0 - M*g/K
%{
IniPos = [0 0  -L0 - M*g/K ];
IniV   = [0 0  0  ];
%}

% 2) Oscillation and swaying
%{
IniPos = [1 1 -2.3];         % Initial coordinates (m)   = [X0 Y0 Z0]
IniV   = [1 10 1 ];          % Initial velocities (m/s)  = [Vx0 Vy0 Vz0]
%}

% 3) Oscillation only
%{
IniPos = [0 0 -2.8];
IniV   = [0 0  0  ];
%}

% 4) rotation
%{
K      = 1000;               % Spring constant (N/m)
Xini   = 0.5;
Length = L0 + M*g/K*cos( Xini/L0 );
Zini   = sqrt( Length^2 - Xini^2 );
IniPos = [Xini 0 -Zini ];
IniV   = [0 1  0  ];
Plot   = 50;                 % 0 No plot,  > 0, how many points to omit, 1 -> plot all points
EndT   = 10;
%}

% 5) rotation (larger initial speed)  -->  Sphere
%{
K      = 1000;                 % Spring constant (N/m)
Xini   = 0.5;
Length = L0 + M*g/K*cos( Xini/L0 );
Zini   = sqrt( Length^2 - Xini^2 );
IniPos = [Xini 0 -Zini ];
IniV   = [0 10  0  ];
%}

% 6) rotation (very large initial speed) -->  Disc
%
K      = 1000;               % Spring constant (N/m)
Xini   = 0.5;
Length = L0 + M*g/K*cos( Xini/L0 );
Zini   = sqrt( Length^2 - Xini^2 );
IniPos = [Xini 0 -Zini ];
IniV   = [0 100 0 ];
Plot   = 5;                  % 0 No plot,  > 0, how many points to omit, 1 -> plot all points
EndT   = 10;
%


% 7) 2-dimensional pendlum  --> Small oscillation
%{
K      = 1000;                 % Spring constant (N/m)
Xini   = 0.5;
Length = L0 + M*g/K*cos( Xini/L0 );
Zini   = sqrt( Length^2 - Xini^2 );
IniPos = [Xini 0 -Zini ];
IniV   = [ 0 0 0 ];
%}

% 8) 2-dimensional pendlum  --> Large oscillation
%{
K      = 1000;               % Spring constant (N/m)
Xini   = 0.5;
Length = L0 + M*g/K*cos( Xini/L0 );
Zini   = sqrt( Length^2 - Xini^2 );
IniPos = [Xini 0 Zini ];
IniV   = [0 0 0 ];
%}



T      = 0:dT:EndT;             % Time vector (s)

Mg     = M*g;                % To improve calculation speed
Nloop  = length(T);          % Number of iterations
 
% INITIALIZE COORDINATES
X    = zeros( Nloop, 1 );   Y    = zeros( Nloop, 1 );   Z    = zeros( Nloop, 1 );
Vx   = zeros( Nloop, 1 );   Vy   = zeros( Nloop, 1 );   Vz   = zeros( Nloop, 1 );

X(1)  = IniPos(1);  Y(1)  = IniPos(2);   Z(1)  = IniPos(3);
Vx(1) = IniV(1);    Vy(1) = IniV(2);     Vz(1) = IniV(3);



% Start itertion
CPUT  = cputime;
for I = 2:Nloop
   I1    = I-1;
   % Momentum for the system
	PxNow = M * Vx(I1);                 % Momentum: kg*m/s
	PyNow = M * Vy(I1);
	PzNow = M * Vz(I1);
   
   % Net force acting on the system:
   % 1) Spring force:   Fvec = -K*S*Lhat      Lhat = unit vector in L direction = Lvec / |Lvec|
   % 2) Gravitation: m*g in Z-direction
   L     = sqrt( X(I1)^2 + Y(I1)^2 + Z(I1)^2 );  % Distance (m) from where spring is attached   
   S     = L - L0;                               % Strech of the spring (m)
   Coeff = K*S/L;
   FxNet = - Coeff * X(I1);         % Net force: N/m*m = N = kg m / s^2
   FyNet = - Coeff * Y(I1);         % Net force: N/m*m = N = kg m / s^2
   FzNet = - Coeff * Z(I1) - Mg;    % kg m/ s^2  + gravitation in + Z direction
   
   % New momentum after time step dT
   PxNew = PxNow + FxNet * dT;
   PyNew = PyNow + FyNet * dT;
   PzNew = PzNow + FzNet * dT;
   
   % New speed after time step dT
   Vx(I) = PxNew / M;
   Vy(I) = PyNew / M;
   Vz(I) = PzNew / M;
   
   % New position after time step dT
   X(I) = X(I1) + 0.5* ( Vx(I1) + Vx(I) ) * dT;
   Y(I) = Y(I1) + 0.5* ( Vy(I1) + Vy(I) ) * dT;
   Z(I) = Z(I1) + 0.5* ( Vz(I1) + Vz(I) ) * dT;
   
   
   % Iteration
   if Iter == 1
      % Net force acting on the system:
      Xmean = 0.5*( X(I1) + X(I) );
      Ymean = 0.5*( Y(I1) + Y(I) );
      Zmean = 0.5*( Z(I1) + Z(I) );

      L     = sqrt( Xmean^2 + Ymean^2 + Zmean^2 );   % Distance (m) from where spring is attached
      S     = L - L0;                                % Strech of the spring (m)
      Coeff = K*S/L;
      FxNet = - Coeff * Xmean;         % Net force: N/m*m = N = kg m / s^2
      FyNet = - Coeff * Ymean;         % Net force: N/m*m = N = kg m / s^2
      FzNet = - Coeff * Zmean - Mg;    % kg m/ s^2  + gravitation in + Z direction
   
      % New momentum after time step dT
      PxNew = PxNow + FxNet * dT;
      PyNew = PyNow + FyNet * dT;
      PzNew = PzNow + FzNet * dT;
   
      % New speed after time step dT
      Vx(I) = PxNew / M;
      Vy(I) = PyNew / M;
      Vz(I) = PzNew / M;
   
      % New position after time step dT
      X(I) = X(I1) + 0.5* ( Vx(I1) + Vx(I) ) * dT;
      Y(I) = Y(I1) + 0.5* ( Vy(I1) + Vy(I) ) * dT;
      Z(I) = Z(I1) + 0.5* ( Vz(I1) + Vz(I) ) * dT;
   end
   
end  % for I = 2:Nloop



% Plot results
if Plot > 0
   figure(1);  hold off
   PlotH = plot3( X(1), Y(1), Z(1), 'ro', [0 X(1)], [0 Y(1)], [0 Z(1)], 'k-', ...
                        'markersize', 10, 'MarkerFaceColor', 'r', 'Linewidth',3);
   MinX = min( [ -1.1*abs(max(X))  -1  ] );
   MaxX = max( [  1.1*abs(max(X))   1  ] );
   MinY = min( [ -1.1*abs(max(Y))  -1  ] );
   MaxY = max( [  1.1*abs(max(Y))   1  ] );
   MinZ = -1.1*abs(min(Z));
   MaxZ = max( [  1.1*max(Z)   0  ] );

   axis([  MinX MaxX  MinY MaxY  MinZ MaxZ ])
   axis equal
   axis([  MinX MaxX  MinY MaxY  MinZ MaxZ ])   
   hold on
   grid on
   xlabel('X');      ylabel('Y');      zlabel('Z');
   Iprev = 1;
   %set(gca,'visible','on','DrawMode','fast')
   set(gca,'visible','on')
   for I = Plot:Plot:Nloop
      % Draw blue line between points
      PlotG = plot3( [X(Iprev) X(I)], [Y(Iprev) Y(I)], [Z(Iprev) Z(I)], 'b--');
      % Set new position for the mass
      set( PlotH(1), 'XData', X(I), 'YData', Y(I), 'ZData', Z(I) )
      set( PlotH(2), 'XData', [0 X(I)], 'YData', [0 Y(I)], 'ZData', [0 Z(I)] )

      title([num2str(T(I),'%6.4f'),' (s)'])
      xlabel('X');      ylabel('Y');      zlabel('Z');
      drawnow
      Iprev = I;
      %axis equal
      
      pause(DTpause)
      pause(Plot*dT)
   end
end

% Plot Kinetic, potential and total energies
figure(2)
Ekin  = 0.5*M*( Vx.^2 + Vy.^2 + Vz.^2 );
Epotg = M*g*( Z - L0 );
L     = sqrt( X.^2 + Y.^2 + Z.^2 );   % Distance (m) from where spring is attached
S     = L - L0;                       % Strech of the spring (m)

Epots = 0.5*K*S.^2;
Etot  = Ekin + Epotg + Epots;
%D     = round(Nloop/100);
%plot( T, Ekin,'b-', T,Epotg,'m--', T,Epots,'k-.', T(1:D:end),Etot(1:D:end),'ko' )
plot( T, Ekin,'b-', T,Epotg,'m--', T,Epots,'k-.', T,Etot,'ko' )
legend('Kinetic','Gravitational','Spring','Total')
xlabel('Time (s)')
ylabel('Energy (J)')

MaxEerror =  max( abs((Etot-Etot(1))/Etot(1)) ) * 100;

fprintf('\n Ready: took %4.3f s: Maximum energy error %5.4e %%\n\n',  cputime - CPUT, MaxEerror)
