% rotatingString.m
% Solution to problem 2 of control 2. 2018/02/12
% J.M.Soler, Feb.2018

% Set parameters
clear
n = 10;       % number of particles
R = 0.15;     % pulling arm length (m)
w = 2*pi;     % pulling arm angular velocity (rad/s)
d0 = 0.05;    % unstretched springs length (m)
k = 100;      % springs constant (N/m)
m = 0.1;      % particles mass (kg)
g = 9.8;      % gravitatinal acceleration (m/s^2)
tmax = 10;    % simulation time (s)
dt = 0.01;    % simulation time interval (s)
dtplot = 0.2; % time interval between video frames (s)

% Set initial positions and velocities
r0(1,1:n+1) = R;
r0(2,:) = 0;
r0(3,n+1) = 0;               % put 'particle' n+1 at arm tip
for j = n:-1:1               % descending loop on true particles
    mj = m*j;                % total mass below spring j
    dj = d0+mj*g/k;          % equilibrium length of spring j
    r0(3,j) = r0(3,j+1)-dj;  % equilibrium height of particle j
end
r0 = r0(:,1:n);              % select true particles
v0 = zeros(3,n);             % initial velocities

% Find trajectory
myforce = @(r,v,t)rotatingStringForces(r,t,R,w,d0,k,m);
r = rungeKutta( myforce, m*ones(1,n), r0, v0, dt, tmax );

% Plot trajectory
ndt = dtplot/dt;     % number of steps between frame plots
figure(1)
nt = size(r,3);      % number of time steps
for it = 1:ndt:nt    % plot every ndt steps
    t = it*dt;
    plot3([r(1,:,it),R*cos(w*t),0], ...  % plot a line for arm and springs
          [r(2,:,it),R*sin(w*t),0], ...
          [r(3,:,it),0,0]),                   hold on
    plot3(r(1,:,it),r(2,:,it),r(3,:,it),'o'), hold off  % discrete masses
    axis equal
    axis([-1,1,-1,1,-1.5,0])   % fix box to avoid it being resized
    grid on
    xlabel('x (m)')
    ylabel('y (m)')
    zlabel('z (m)')
    pause(dtplot)
end
