% Position analysis
% R-RRR

clear; clc; close all 

% Input data 
AB=0.15; %(m)
BC=0.35; %(m)
CD=0.25; %(m) 
xD=0.30; %(m) 
yD=0; %(m) 	
phi = pi/4 ; %(rad)

xA = 0; yA = 0; 
rA_ = [xA yA 0]; 
rD_ = [xD yD 0]; 

xB = AB*cos(phi); 
yB = AB*sin(phi); 
rB_ = [xB yB 0]; 

% Position of joint C 
% Distance formula: BC=constant
syms xCsol yCsol
eqnC1 = (xCsol - xB )^2 + ( yCsol - yB )^2 - BC^2;
% Distance formula: CD=constant
eqnC2 = (xCsol - xD )^2 + ( yCsol - yD )^2 - CD^2;

% Simultaneously solve above equations
solC_ = solve(eqnC1, eqnC2, xCsol, yCsol);
% Two solutions for xC - vector form
xCpositions = eval(solC_.xCsol);
% Two solutions for yC - vector form
yCpositions = eval(solC_.yCsol);
% Separate the solutions in scalar form
% first  component of the vector xCpositions
xC1 = xCpositions(1);
% second component of the vector xCpositions
xC2 = xCpositions(2);
% first  component of the vector yCpositions
yC1 = yCpositions(1); 
% second component of the vector yCpositions
yC2 = yCpositions(2); 

% select the correct position for C 
% for the given input angle

if xC1 > xD
    xC = xC1; yC=yC1;
else
    xC = xC2; yC=yC2;
end
rC_ = [xC yC 0]; % Position vector of C 

% Angles of the links with the horizontal
phi2 = atan((yB-yC)/(xB-xC));
phi3 = atan((yD-yC)/(xD-xC));

fprintf('Results \n\n')
fprintf('rA_ =  [ %6.3f, %6.3f, %g ] (m)\n', rA_)
fprintf('rD_ =  [ %6.3f, %6.3f, %g ] (m)\n', rD_)
fprintf('rB_ =  [ %6.3f, %6.3f, %g ] (m)\n', rB_)
fprintf('rC_ =  [ %6.3f, %6.3f, %g ] (m)\n', rC_)
fprintf('phi2 = %6.3f (degrees) \n', phi2*180/pi)
fprintf('phi3 = %6.3f (degrees) \n', phi3*180/pi)

% Graphic of the mechanism
figure(1)
plot([xA,xB],[yA,yB],'r-o','LineWidth',1.5) 
hold on % holds the current plot
plot([xB,xC],[yB,yC],'b-o','LineWidth',1.5)
hold on 
plot([xD,xC],[yD,yC],'k-o','LineWidth',1.5)
% adds major grid lines to the current axes
grid on
xlabel('x (m)'), ylabel('y (m)')
text(xA,yA,'  A') 
text(xB-0.03,yB,'B')
text(xC,yC,'  C') 
text(xD,yD,'  D') 
ad = 0.5;
axis([-ad ad -ad ad])
pivot0_scale = 0.008;  
plot_pivot0( xA, yA, pivot0_scale, 0)
plot_pivot0( xD, yD, pivot0_scale, 0)

% print -dpng -r300 file_name
% print -dpdf -cmyk f1
 
% eof

% rA_ =  [  0.000,  0.000, 0 ] (m)
% rD_ =  [  0.300,  0.000, 0 ] (m)
% rB_ =  [  0.106,  0.106, 0 ] (m)
% rC_ =  [  0.441,  0.206, 0 ] (m)
% phi2 = 16.613 (degrees) 
% phi3 = 55.540 (degrees) 
