% Program 2.6
% R-RRT
% Position analysis
% Path of C2 (mass center of link BC)

clear; clc; close all  
AB = .5; BC = 1; xA = 0; yA = 0; yC = 0;

incr = 0 ;

for phi=0:pi/10:2*pi,

xB = AB*cos(phi); yB = AB*sin(phi);
xC = xB + sqrt(BC^2-yB^2);

incr = incr + 1;
xC2(incr)=(xB+xC)/2; yC2(incr)=(yB+yC)/2;

% Graphic of the mechanism
subplot(2,1,1),... 
plot( [xA,xB],[yA,yB],'r-',... 
[xB,xC],[yB,yC],'b-' ),... 
hold on 
xlabel('x (m)'), ylabel('y (m)'),... 
title('Graphic of the mechanism'),... 
text(xC2,yC2,'C2'),... 
axis([-0.6 1.6 -0.6 0.6]),...

end % end for

% Path of C2 (mass center of link 2)
subplot(2,1,2),...  
plot(xC2, yC2, '-ko'),... 
xlabel('x (m)'), ylabel('y (m)'),... 
title('Path described by C2'), grid