function [f1,f2] = climberForce( m, d1, d2, d, phi, tau )

% Climber hands force vector
% Input:
%   m      : climber mass (kg)
%   d1     : climber legs length (m)
%   d2     : climber arms length (m)
%   d      : distance between feet and hands (m)
%   phi    : wall inclination from horizontal (rad)
%   tau    : body torque between arms and legs (N*m)
% Output:
%   f1(2,1) : column vector with horizontal and vertical feet force (N)
%   f2(2,1) : column vector with horizontal and vertical hands force (N)
% Algorithm:
%   Call climberPosition to find r1 and r2. Then solve
%   the equilibrium eqs:
%     f1x+f2x = 0             horizontal force
%     f1y+f2y-m*g = 0         vertical force
%     x1*f1y-y1*f1x-tau = 0   legs torque
%     x2*f2y-y2*f2x+tau = 0,  arms torque
%   as A*f=b => f=A\b, with f=[f1x;f1y;f2x;f2y].
%   Finally, separate f into f1 and f2  
% J.M.Soler, Feb.2017

  % Set internal parameters
  g = 9.81;  % gravitational acceleration (m/s^2)
  
  % Find body geometry
  [r1,r2] = climberPosition( d1, d2, d, phi );
  
  % Solve equilibrium equations for climber forces:
  % f1x+f2x = 0
  % f1y+f2y-m*g = 0
  % cross(r1,f1)+tau = x1*f1y-y1*f1x-tau = 0
  % cross(r2,f2)-tau = x2*f2y-y2*f2x+tau = 0
  % Set these equations as A*f=b, with f=[f1x;f1y;f2x;f2y]
  A = [1, 0, 1, 0
       0, 1, 0, 1
       -r1(2), +r1(1), 0, 0
       0, 0, -r2(2), +r2(1)];
  b = [0; m*g; +tau; -tau];
  f = A\b;
  
  % Separate feet and hands forces
  f1 = [f(1);f(2)];
  f2 = [f(3);f(4)];
  
end
