% Unit 3 (MATLAB Course @ UAM)
% (c) Alexander Knebe

function [r] = equilib3m(m, k, p)
% Input:
%   m(3)   : particle masses (kg)
%   k(5,5) : spring constants (N/m)
%   p(2)   : (x4, x5) coordinates (m)
% Output:
%   r(2,3) : equilibrium points of each mass (m)

% the system of 6 equations for the unknowns xi, yi (i=1,2,3):
%   -(k12+k13+k14)*x1 + k12          *x2 + k13      *x3                                                     = -k14*x4
%    k12          *x1 - (k12+k23+k25)*x2 + k23      *x3                                                     =         -k25*x5
%    k13          *x1 + k23          *x2 - (k13+k23)*x3                                                     = 0
%                                                       -(k12+k13+k14)*y1 + k12          *y2 + k13      *y3 =                 -k14*y4 + m1*g
%                                                        k12          *y1 - (k12+k23+k25)*y2 + k23      *y3 =         -k25*x5         + m2*g
%                                                        k13          *y1 + k23          *y2 - (k13+k23)*y3 =                           m3*g

%====================================================================
% function to solve system
%====================================================================
g = 9.81;

x4=p(1);
x5=p(2);
y4=0;
y5=0;

% put all the left hand side coefficient into one matrix A
A = [ -(k(1,2)+k(1,3)+k(1,4)), k(1,2), k(1,3)
      k(1,2), -(k(1,2)+k(2,3)+k(2,5)), k(2,3)
      k(1,3), k(2,3), -(k(1,3)+k(2,3)) ];
A = [ A, zeros(3); zeros(3), A ];

% define the right hand side vector
b = [ -k(1,4)*x4;
      -k(2,5)*x5;
       0;
      -k(1,4)*y4+m(1)*g;
      -k(2,5)*y5+m(2)*g; m(3)*g ];
  
% solve the system
r = inv(A)*b;

r = [r(1),r(2),r(3);
     r(4),r(5),r(6)];
end