% hangingMasses.m
% Find equilibrium positions and simulate dynamics of two lamps hanging from springs
% JMS, Mar.2021

% Set parameters
clear
rh = [-1,0,1; 3,3,3];    % positions of the ceil hooks (m)
k = [6,5,3,4];           % spring constants (N/m)
m = [1,0.5];             % hanging masses (kg)
g = [0;-9.8];            % gravitational acceleration vector (m/s^2)
dt = 0.1;                % integration time step (s)
tmax = 10;               % integration time (s)
v0avg = 0;               % mean random initial velocities
cf = 0.5;                % friction coefficient Fdrag=-cf*v; 

% Find K and f0 such that f=-r*K+f0, where f(ixy,im) and r(ixy,im) are the forces
% and coordinates (ixy=1:2) of the two masses (im=1:2), and K(im,jm) is a 2x2 matrix
% f1 = -k1*(r1-rh1)-k2*(r1-rh2)-k4*(r1-r2)+m1*g = -r1*(k1+k2+k4)-r2*(-k4)+(k1*rh1+k2*rh2+m1*g)
% f2 = -k4*(r2-r1)-k3*(r2-rh3)+m2*g = -r1*(-k4)-r2*(k4+k3)+(k3*rh3+m2*g)
K = [k(1)+k(2)+k(4),-k(4); -k(4),k(3)+k(4)];
f0 = [k(1)*rh(:,1)+k(2)*rh(:,2)+m(1)*g, k(3)*rh(:,3)+m(2)*g];

% Solve r*K=f0
req = f0/K;
fprintf('Equilibrium positions (x1,y1),(x2,y2) = (%5.3f %5.3f) m  (%5.3f %5.3f) m\n',req)

% Find trajectory
force = @(r,v,t)(-r*K+f0-cf*v);
r0 = [-0.5,0.5;2.5,2.5];
v0 = v0avg*randn(2,2);
[r,v] = newtonRK(force,m,r0,v0,dt,tmax);

% Plot trajectory
nt = size(r,3);
for it = 1:nt
  scatter(r(1,:,it),r(2,:,it),300,'filled'), hold on
  plot([rh(1,1),   r(1,1,it)],[rh(2,1),   r(2,1,it)],'k','LineWidth',2)
  plot([rh(1,2),   r(1,1,it)],[rh(2,2),   r(2,1,it)],'k','LineWidth',2)
  plot([rh(1,3),   r(1,2,it)],[rh(2,3),   r(2,2,it)],'k','LineWidth',2)
  plot([ r(1,1,it),r(1,2,it)],[ r(2,1,it),r(2,2,it)],'k','LineWidth',2)
  plot([-1.5,1.5],[3,3],'k','LineWidth',2)
  hold off
  axis equal
  axis([-1.5,1.5,1,3.5])
  xlabel('x (m)')
  ylabel('y (m)')
  set(gca,'FontSize',15)
  grid on
  pause(dt)
end
fprintf('      Final positions (x1,y1),(x2,y2) = (%5.3f %5.3f) m  (%5.3f %5.3f) m\n',r(:,:,nt))
