% Program 3.3
% R-TRR
% Velocity and acceleration analysis
clear; clc; close;
AC = 0.1; BC = 0.30; xA = 0; yA = 0; xC = 0.1; yC = 0.;
phi = pi/4; phi1=phi;
rA = [ xA, yA, 0 ]; rC = [ xC, yC, 0 ];
xD=1.5*BC*cos(phi); yD=1.5*BC*sin(phi); rD = [ xD, yD, 0 ];
% position of B
eqB1 = 'xBsol*sin(phi) = yBsol*cos(phi)';
eqB2 = 'yBsol^2+(xC-xBsol)^2-BC^2 = 0';
solB = solve(eqB1, eqB2, 'xBsol, yBsol');
xBpositions = eval(solB.xBsol); yBpositions = eval(solB.yBsol);
xB1 = xBpositions(1); xB2 = xBpositions(2); 
yB1 = yBpositions(1); yB2 = yBpositions(2); 
if (phi>=0 && phi<= pi) 
    if yB1 >= 0 xB = xB1; yB=yB1; else xB = xB2; yB=yB2; end
end
if (phi>pi && phi<=2*pi)
   if yB1 < 0 xB = xB1; yB=yB1; else xB = xB2; yB=yB2; end
end
rB = [ xB, yB, 0 ];
phi3 = atan((yB-yC)/(xB-xC));
fprintf('Results \n'); fprintf('\n');
fprintf('rB =  [ %g, %g, %g ] (m)\n', rB);
fprintf('rC =  [ %g, %g, %g ] (m)\n', rC);
fprintf('rD =  [ %g, %g, %g ] (m)\n', rD);
fprintf('phi3 = %g (degrees) \n', phi3*180/pi);
% Graphic of the mechanism
plot([xA,xD],[yA,yD],'r-o', [xB,xC],[yB,yC],'b-o');
xlabel('x (m)'), ylabel('y (m)'),...
title('positions for \phi = 45 (deg)'),...
text(xA,yA,' A'),text(xB,yB,'  B'),text(xC,yC,'  C'),text(xD,yD,'  D');

fprintf('\n');
fprintf('Velocity and acceleration analysis \n'); fprintf('\n');

n = 30; % (rpm)  driver link
omega1 = [ 0 0 pi*n/30 ]; omega2 = omega1;
alpha1 = [0 0 0 ]; alpha2 = alpha1;
fprintf('omega1 =  [ %g, %g, %g ] (rad/s)\n', omega1);
fprintf('alpha1 =  [ %g, %g, %g ] (rad/s^2)\n', alpha1 ); fprintf('\n');

vA = [0 0 0 ]; aA = [0 0 0 ]; vC = [0 0 0 ]; aC = [0 0 0 ]; 

vB1 = vA + cross(omega1,rB); 
aB1 = aA + cross(alpha1,rB) - dot(omega1,omega1)*rB;
fprintf('vB = vB1 =  [ %g, %g, %g ] (m/s)\n', vB1);
fprintf('aB = aB1 =  [ %g, %g, %g ] (m/s^2)\n', aB1); fprintf('\n');

% angular velocity of link 3
omega3z = sym('omega3z','real'); % omega3z unknown
omega3 = [ 0 0 omega3z ];
vB21 = sym('vB21','real'); % vB21 unknown
% vB2B1 parallel to the sliding direction
vB2B1 = [ vB21*cos(phi1) vB21*sin(phi1) 0 ];
% vB2 = vB3 = vC + omega3 x (rB-rC)
vB3 = vC + cross(omega3,rB-rC);
vB2 = vB3;

eqvB = vB2 - ( vB1 + vB2B1 ); % vB2 = vB1 + vB2B1
eqvBx = eqvB(1); % equation component on x-axis
eqvBy = eqvB(2); % equation component on y-axis
solvB = solve(eqvBx,eqvBy);
omega3zs = eval(solvB.omega3z);
vB21s = eval(solvB.vB21);
Omega3 = [0 0 omega3zs];
VB21 = vB21s*[cos(phi1) sin(phi1) 0];
VB3 = vC + cross(Omega3,rB-rC);

% print the equations for calculating omega3z and vB21
fprintf('vB2 = vB1 + vB2B1 => \n');
qvBx=vpa(eqvBx,6); fprintf('x-axis: %s = 0 \n', char(qvBx));
qvBy=vpa(eqvBy,6); fprintf('y-axis: %s = 0 \n', char(qvBy)); 
fprintf('=>\n');
fprintf('omega3z =  %g (rad/s)\n', omega3zs);
fprintf('vB21 =  %g (m/s)\n', vB21s); fprintf('\n');
fprintf('omega3 = [ %g, %g, %g ] (rad/s)\n', Omega3 );
fprintf('vB2B1 =  [ %g, %g, %d ] (m/s)\n', VB21 ); fprintf('\n');
fprintf('vB3 =  [ %g, %g, %d ] (m/s)\n', VB3 ); fprintf('\n');

% angular acceleration of link 3
alpha3z = sym('alpha3z','real'); % alpha3z unknown
alpha3 = [ 0 0 alpha3z ];
aB21 = sym('aB21','real'); % aB21 unknown
% aB2B1 parallel to the sliding direction
aB2B1 = [ aB21*cos(phi1) aB21*sin(phi1) 0 ];
% aB2 = aB3 = aC + alpha3 x (rB-rC)  - (omega3.omega3)(rB-rC) 
aB3 = aC + cross(alpha3,rB-rC) - dot(Omega3,Omega3)*(rB-rC);
aB2 = aB3;
aB2B1cor = 2*cross(omega1,VB21); % aB2B1cor = 2 omega1 x vB2B1  
eqaB = aB2 - ( aB1 + aB2B1 + aB2B1cor ); % aB2 = aB1 + aB2B1 + aB2B1cor
eqaBx = eqaB(1); % equation component on x-axis
eqaBy = eqaB(2); % equation component on y-axis
solaB = solve(eqaBx,eqaBy);
alpha3zs = eval(solaB.alpha3z);
aB21s = eval(solaB.aB21);
Alpha3 = [0 0 alpha3zs];
AB21 = aB21s*[cos(phi1) sin(phi1) 0];
AB3 = aC + cross(Alpha3,rB-rC) - dot(Omega3,Omega3)*(rB-rC);

% print the equations for calculating alpha3z and aB21
fprintf('aB2B1cor = 2 omega1 x vB2B1 = [ %g, %g, %d] (m/s^2)\n', aB2B1cor); 
fprintf('\n');
fprintf('aB2 = aB1 + aB2B1 + aB2B1cor => \n');
qaBx=vpa(eqaBx,6); fprintf('x-axis: %s = 0 \n', char(qaBx));
qaBy=vpa(eqaBy,6); fprintf('y-axis: %s = 0 \n', char(qaBy)); 
fprintf('=>\n');
fprintf('alpha3z =  %g (rad/s^2)\n', alpha3zs);
fprintf('aB21 =  %g (m/s^2)\n', aB21s); fprintf('\n');
fprintf('alpha3 = [ %g, %g, %g ] (rad/s^2)\n', Alpha3 );
fprintf('aB2B1 =  [ %g, %g, %d ] (m/s^2)\n', AB21 ); 
fprintf('aB3 =  [ %g, %g, %d ] (m/s^2)\n', AB3 ); fprintf('\n');

omega23 = omega2 - Omega3;
alpha23 = alpha2 - Alpha3;
fprintf('omega23 = [ %g, %g, %g ] (rad/s)\n', omega23 );
fprintf('alpha23 = [ %g, %g, %g ] (rad/s^2)\n', alpha23 );