function [r1,r2] = climberPosition( d1, d2, d, phi )
% Climber's feet and hands positions, relative to the body center
% Input:
%   d1   : legs length (m)
%   d2   : arms length (m)
%   d    : distance between feet and hands (m)
%   phi  : wall inclination from horizontal (rad)
% Output:
%   r1(2,1) : feet position vector, from body center (m)
%   r2(2,1) : hands position vector, from body center (m)
% Algorithm:
%   Use the triangular equation d2^2=d^2+d1^2-2*d*d1*cos(phi1) to
%   obtain phi1. Find position vectors r0 and r2 of body center and
%   hands, from feet. Subtract r0 to change origin to body center.
% J.M.Soler, Feb.2017

  % Check lengths
  if d>d1+d2
    error('climberPosition ERROR: d>d1+d2')
  end
  
  % Find legs angle relative to wall
  phi1 = acos((d^2+d1^2-d2^2)/(2*d*d1));
  
  % Find body geometry
  r0 = d1*[cos(phi+phi1);sin(phi+phi1)]; % body center, from feet
  r2 = d*[cos(phi);sin(phi)];            % hands position, from feet
  r1 = -r0;                              % feet position, from body center
  r2 = r2-r0;                            % hands position, from body center
  
end
  