% R_RRT_RTR Mechanism
% Velocity and acceleration analysis
% closed contour method

clear; clc; close all

PIV_1  % position analysis PIV_1.m

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

n = -500; % (rpm) angular speed of 1
omega1_ = [0 0 pi*n/30]; % angular velocity of 1
alpha1_ = [0 0 0]; % angular acceleration of 1

fprintf...
('omega1_ = [%g, %g, %6.3f] (rad/s)\n', omega1_) 
fprintf...
('alpha1_ = [%g, %g, %g] (rad/s^2)\n', alpha1_)  
fprintf('\n') 

vA_ = [0 0 0 ]; 
aA_ = [0 0 0 ];

vB1_ = vA_ + cross(omega1_,rB_); 
vB2_ = vB1_; 
aB1_ = aA_ + cross(alpha1_,rB_) - ...
    dot(omega1_,omega1_)*rB_;
vB2_ = vB1_; vB_ = vB1_; 
aB2_ = aB1_; aB_ = aB1_;
fprintf...
('vB_ = vB1_ = vB2_ = [%6.3f, %6.3f, %g] (m/s)\n',vB1_)  
fprintf...
('aB_ = aB1_ = aB2_ = [%6.3f, %6.3f, %g] (m/s^2)\n',aB1_)  
fprintf('\n')

fprintf('Contour I   \n\n')  
fprintf('Relative velocities \n\n') 

omega10_ = omega1_; 
syms omega21z omega32z v03
omega21v_=[0 0 omega21z];% angular velocity of 2 relative to 1
omega32v_=[0 0 omega32z];% angular velocity of 3 relative to 2 
v03v_ =[v03 0 0]; % velocity of D on 0 relative to D on 3  
% First contour velocity equations
eqIomega_ = omega10_ + omega21v_ + omega32v_; 
eqIvz = eqIomega_(3); % (1)
eqIv = cross(rB_, omega21v_) + cross(rD_, omega32v_) + v03v_;
eqIvx = eqIv(1); % (2) 
eqIvy = eqIv(2); % (3) 

Ivz=vpa(eqIvz,6);
fprintf('%s = 0 \n', char(Ivz)) 
Ivx=vpa(eqIvx,6);
fprintf('%s = 0 \n', char(Ivx)) 
Ivy=vpa(eqIvy,6);
fprintf('%s = 0 \n', char(Ivy)) 
fprintf('\n')

% Eqs.(1)(2)(3) =>
solIv = solve(eqIvz,eqIvx,eqIvy);
omega21_ = [ 0 0 eval(solIv.omega21z) ]; 
omega32_ = [ 0 0 eval(solIv.omega32z) ]; 
vD03 = eval(solIv.v03); 
vD03_ = [vD03 0 0];

fprintf('omega21_ = [ %g, %g, %6.3f ] (rad/s)\n', omega21_) 
fprintf('omega32_ = [ %g, %g, %6.3f ] (rad/s)\n', omega32_) 
fprintf('vD03 = %6.3f (m/s)\n', eval(solIv.v03)) 
fprintf('vD03_ = [ %6.3f, %d, %d ] (m/s)\n', vD03_)  
fprintf('\n') 

fprintf('Absolute velocities \n\n') 
omega20_ = omega10_+omega21_; % angular velocity of 2 
fprintf('omega20_= [%d, %d, %6.3f] (rad/s)\n',omega20_) 
% omega20_= [0, 0, 12.487] (rad/s)

vD3_ = -vD03_; % velocity of D
vD_ = vD3_;
fprintf('vD3_ = [ %6.3f, %d, %d ] (m/s)\n', vD3_) 
% vD3_ = [  8.546, 0, 0 ] (m/s)

fprintf('\n') 
fprintf('Relative accelerations \n\n') 

alpha10_ = alpha1_; 
syms alpha21z alpha32z a03
alpha21v_=[0 0 alpha21z];% angular acceleration of 2 relative to 1
alpha32v_=[0 0 alpha32z];% angular acceleration of 3 relative to 2 
a03v_=[a03 0 0];% acceleration of D on 0 relative to D on 3  
% First contour acceleration equations 
eqIalpha_ = alpha10_ + alpha21v_ + alpha32v_; 
eqIaz = eqIalpha_(3); % (4)
eqIa_=cross(rB_,alpha21v_)+cross(rD_,alpha32v_)+a03v_...
-dot(omega1_,omega1_)*rB_-dot(omega20_,omega20_)*(rD_-rB_);
eqIax = eqIa_(1); % (5)
eqIay = eqIa_(2); % (6)

Iaz=vpa(eqIaz,6);
fprintf('%s = 0 \n', char(Iaz)) 
Iax=vpa(eqIax,6);
fprintf('%s = 0 \n', char(Iax)) 
Iay=vpa(eqIay,6);
fprintf('%s = 0 \n', char(Iay))  
fprintf('\n')

% Eqs.(4)(5)(6) =>
solIa=solve(eqIaz,eqIax,eqIay);
alpha21_ = [ 0 0 eval(solIa.alpha21z) ]; 
alpha32_ = [ 0 0 eval(solIa.alpha32z) ]; 
aD03_ = [ eval(solIa.a03) 0 0]; 

fprintf('alpha21_ = [ %g, %g, %6.3f ] (rad/s^2)\n', alpha21_) 
fprintf('alpha32_ = [ %g, %g, %6.3f ] (rad/s^2)\n', alpha32_) 
fprintf('aD03 = %6.3f (m/s^2)\n', eval(solIa.a03)) 
fprintf('aD03_ = [ %6.3f, %d, %d ] (m/s^2)\n', aD03_)  
fprintf('\n')
% alpha21_ = [ 0, 0, 629.786 ] (rad/s^2)
% alpha32_ = [ 0, 0, -629.786 ] (rad/s^2)
% aD03 = 422.604 (m/s^2)
% aD03_ = [ 422.604, 0, 0 ] (m/s^2)

fprintf('Absolute accelerations \n\n') 
alpha20_ = alpha10_+alpha21_; % angular acceleration of 2 
aD3_ = -aD03_; % acceleration of D
aD_ = aD3_;
fprintf('alpha20_= [%d, %d, %6.3f] (rad/s^2)\n',alpha20_) 
fprintf('aD3_ = [ %6.3f, %d, %d ] (m/s^2)\n', aD3_) 
fprintf('\n')  
% alpha20_= [0, 0, 629.786] (rad/s^2)
% aD3_ = [ -422.604, 0, 0 ] (m/s^2)

fprintf('Contour II   \n\n')  
fprintf('Relative velocities \n\n') 

% symbolic unknowns
syms omega42z vE54 omega05z
omega42v_=[0 0 omega42z];% angular velocity of 4 relative to 2 
omega05v_=[0 0 omega05z];% angular velocity of 0 relative to 5 
vE54v_=vE54*[ cos(phi4) sin(phi4) 0];% velocity of E on 5 relative to 4 

eqIIomega_ = omega10_ + omega21_ + omega42v_ + omega05v_; 
eqIIvz = eqIIomega_(3); % (7)
eqIIv = cross(rB_,omega21_) + cross(rC_,omega42v_) ...
      + cross(rE_,omega05v_) + vE54v_;
eqIIvx = eqIIv(1); % (8)
eqIIvy = eqIIv(2); % (9)

digits(3)
IIvz=vpa(eqIIvz);
fprintf('%s = 0 \n', char(IIvz)) 
IIvx=vpa(eqIIvx);
fprintf('%s = 0 \n', char(IIvx)) 
IIvy=vpa(eqIIvy);
fprintf('%s = 0 \n', char(IIvy)) 
fprintf('\n')

% Eqs.(7)(8)(9) =>
solIIv=solve(eqIIvz,eqIIvx,eqIIvy);
omega42_ = [ 0 0 eval(solIIv.omega42z) ]; 
omega05_ = [ 0 0 eval(solIIv.omega05z) ]; 
vE54_ = eval(solIIv.vE54)*[ cos(phi4) sin(phi4) 0]; 

fprintf('omega42_ = [ %g, %g, %6.3f ] (rad/s)\n', omega42_) 
fprintf('omega05_ = [ %g, %g, %6.3f ] (rad/s)\n', omega05_) 
fprintf('vE54 = %6.3f (m/s)\n', eval(solIIv.vE54)) 
fprintf('vE54_ = [ %6.3f, %6.3f, %d ] (m/s)\n', vE54_)  
fprintf('\n') 

% omega42_ = [ 0, 0, -35.596 ] (rad/s)
% omega05_ = [ 0, 0, 23.109 ] (rad/s)
% vE54 =  3.716 (m/s)
% vE54_ = [  0.502,  3.681, 0 ] (m/s)

fprintf('Absolute velocity \n\n') 
omega40_ = omega20_+omega42_;
fprintf('omega40_=omega50_= [%d, %d, %6.3f] (rad/s)\n',omega40_) 
fprintf('\n') 
% omega40_=omega50_= [0, 0, -23.109] (rad/s)

fprintf('Relative accelerations \n\n')
 
syms alpha42z alpha05z aE54

alpha42v_=[0 0 alpha42z];% angular acceleration of 4 relative to 2  
alpha05v_=[0 0 alpha05z];% angular acceleration of 0 relative to 5   
aE54v_=aE54*[cos(phi4) sin(phi4) 0];% acceleration of E on 5 relative to 4


eqIIalpha_ = alpha10_ + alpha21_ + alpha42v_ + alpha05v_; 
eqIIaz = eqIIalpha_(3); % (10)
eqIIa_=...
cross(rB_,alpha21_)+cross(rC_,alpha42v_)+cross(rE_,alpha05v_)...
+aE54v_+2*cross(omega40_,vE54_)...
-dot(omega10_,omega10_)*rB_-dot(omega20_,omega20_)*(rC_-rB_)...
-dot(omega40_,omega40_)*(rE_-rC_);
eqIIax=eqIIa_(1); % (11)
eqIIay=eqIIa_(2); % (12)

IIaz=vpa(eqIIaz,6); fprintf('%s = 0 \n', char(IIaz)) 
IIax=vpa(eqIIax,6); fprintf('%s = 0 \n', char(IIax)) 
IIay=vpa(eqIIay,6); fprintf('%s = 0 \n', char(IIay)) 
fprintf('\n')

% Eqs.(10)(11)(12) =>
solIIa=solve(eqIIaz,eqIIax,eqIIay);
alpha42_ = [ 0 0 eval(solIIa.alpha42z) ]; 
alpha05_ = [ 0 0 eval(solIIa.alpha05z) ] ; 
aE54_ = eval(solIIa.aE54)*[ cos(phi4) sin(phi4) 0]; 

fprintf('alpha42_ = [ %g, %g, %6.3f ] (rad/s^2)\n', alpha42_) 
fprintf('alpha05_ = [ %g, %g, %6.3f ] (rad/s^2)\n', alpha05_) 
fprintf('aE54 = %6.3f (m/s^2)\n', eval(solIIa.aE54)) 
fprintf('aE54_ = [ %6.3f, %6.3f, %d ] (m/s^2)\n', aE54_)  
fprintf('\n') 

% alpha42_ = [ 0, 0, -104.567 ] (rad/s^2)
% alpha05_ = [ 0, 0, -525.219 ] (rad/s^2)
% aE54 = 109.981 (m/s^2)
% aE54_ = [ 14.847, 108.974, 0 ] (m/s^2)

fprintf('Absolute acceleration \n\n')
alpha50_ = - alpha05_;
alpha40_ = alpha50_;
fprintf('alpha40_=alpha50_=[%d, %d, %6.3f] (rad/s^2)\n',alpha50_)
% omega40_=omega50_= [0, 0, -23.109] (rad/s)

vC_ = vB_ + cross(omega20_,rC_-rB_);
fprintf('vC_=vC2_=vC4_=[%6.3f,%6.3f,%g] (m/s)\n',vC_) 
aC_ = aB_ + cross(alpha20_, rC_-rB_) + ...
    cross(omega20_,cross(omega20_, rC_-rB_));
fprintf('aC_=aC2_=aC4_=[%6.3f,%6.3f,%g] (m/s^2)\n',aC_) 
fprintf('\n')

vF_ = vC_ + cross(omega40_,rF_-rC_); 
aF_=aC_+cross(alpha40_,rF_-rC_)+...
cross(omega40_,cross(omega40_,rF_-rC_)); 
fprintf('vF_=[%6.3f,%6.3f,%g] (m/s)\n',vF_) 
fprintf('aF_=[%6.3f,%6.3f,%g] (m/s^2)\n',aF_) 
fprintf('\n') 

omega2_ = omega20_;
omega3_ = [0 0 0];
omega4_ = omega40_;
omega5_ = omega40_;

alpha2_ = alpha20_;
alpha3_ = [0 0 0];
alpha4_ = alpha40_;
alpha5_ = alpha40_;


% Contour
% Results 
% 
% phi = phi1 = 45 (degrees) 
% rA_ = [0, 0, 0] (m)
% rE_ = [ 0.300, -0.250, 0] (m)
% rB_ = [ 0.141,  0.141, 0] (m)
% rD_ = [ 0.734, 0.05, 0] (m)
% phi2 = -8.764 (degrees) 
% rC_ = [ 0.349,  0.109, 0] (m)
% phi4 = phi5 = 82.242 (degrees) 
% rF_ = [ 0.288, -0.336, 0] (m)
% 
% Velocity and acceleration analysis 
% 
% omega1_ = [0, 0, -52.360] (rad/s)
% alpha1_ = [0, 0, 0] (rad/s^2)
% 
% vB_ = vB1_ = vB2_ = [ 7.405, -7.405, 0] (m/s)
% aB_ = aB1_ = aB2_ = [-387.715, -387.715, 0] (m/s^2)
% 
% Contour I   
% 
% Relative velocities 
% 
% omega21z + omega32z - 52.3599 = 0 
% 0.141421*omega21z + 0.05*omega32z + v03 = 0 
% - 0.141421*omega21z - 0.734416*omega32z = 0 
% 
% omega21_ = [ 0, 0, 64.847 ] (rad/s)
% omega32_ = [ 0, 0, -12.487 ] (rad/s)
% vD03 = -8.546 (m/s)
% vD03_ = [ -8.546, 0, 0 ] (m/s)
% 
% Absolute velocities 
% 
% omega20_= [0, 0, 12.487] (rad/s)
% vD3_ = [  8.546, 0, 0 ] (m/s)
% 
% Relative accelerations 
% 
% alpha21z + alpha32z = 0 
% a03 + 0.141421*alpha21z + 0.05*alpha32z - 480.18 = 0 
% - 0.141421*alpha21z - 0.734416*alpha32z - 373.459 = 0 
% 
% alpha21_ = [ 0, 0, 629.786 ] (rad/s^2)
% alpha32_ = [ 0, 0, -629.786 ] (rad/s^2)
% aD03 = 422.604 (m/s^2)
% aD03_ = [ 422.604, 0, 0 ] (m/s^2)
% 
% Absolute accelerations 
% 
% alpha20_= [0, 0, 629.786] (rad/s^2)
% aD3_ = [ -422.604, 0, 0 ] (m/s^2)
% 
% Contour II   
% 
% Relative velocities 
% 
% omega05z + omega42z + 12.5 = 0 
% 0.109*omega42z - 0.25*omega05z + 0.135*vE54 + 9.17 = 0 
% 0.991*vE54 - 0.349*omega42z - 0.3*omega05z - 9.17 = 0 
% 
% omega42_ = [ 0, 0, -35.596 ] (rad/s)
% omega05_ = [ 0, 0, 23.109 ] (rad/s)
% vE54 =  3.716 (m/s)
% vE54_ = [  0.502,  3.681, 0 ] (m/s)
% 
% Absolute velocity 
% 
% omega40_=omega50_= [0, 0, -23.109] (rad/s)
% 
% Relative accelerations 
% 
% alpha05z + alpha42z + 629.786 = 0 
% 0.134997*aE54 - 0.25*alpha05z + 0.109424*alpha42z - 134.71 = 0 
% 0.990846*aE54 - 0.3*alpha05z - 0.348969*alpha42z - 303.03 = 0 
% 
% alpha42_ = [ 0, 0, -104.567 ] (rad/s^2)
% alpha05_ = [ 0, 0, -525.219 ] (rad/s^2)
% aE54 = 109.981 (m/s^2)
% aE54_ = [ 14.847, 108.974, 0 ] (m/s^2)
% 
% Absolute acceleration 
% 

% classical
% =>
% omega2z = 12.487 (rad/s)
% vD =  8.546 (m/s)
% 
% omega2_ = [0,0,12.487](rad/s)
% vD_ = [ 8.546, 0.000,0] (m/s)
% 
% 
% aD2_=aB_+alpha2_xrBD_-(omega2_.omega2_)rBD_
% aD3_=aD2_ =>
% x-axis:
% aD - 0.0914*alpha2z + 480.0 = 0
% y-axis:
% 373.0 - 0.593*alpha2z = 0
% =>
% alpha2z = 629.786 (rad/s^2)
% aD = -422.604 (m/s^2)
% 
% alpha2_ = [0,0,629.786](rad/s^2)
% aD_ = [-422.604, 0.000,0] (m/s^2)
% 
% alpha3_ = [0,0,0](rad/s^2)
% vC_=vC2_=vC4_=[ 7.804,-4.813,0] (m/s)
% aC_=aC2_=aC4_=[-399.926,-252.015,0] (m/s^2)
% 
% vE4_ = vC_ + omega4_ x rCE_ = vE5_ + vE45_ => 
% x-axis: 0.359*omega4z - 0.135*vE45 + 7.8 = 0 
% y-axis: - 0.049*omega4z - 0.991*vE45 - 4.81 = 0 
% =>
% omega4z = -23.109 (rad/s)
% vE45 = -3.716 (m/s)
% 
% omega4_=omega5_ = [0,0,-23.109](rad/s)
% vE45_ = [-0.502,-3.681,0] (m/s)
% 
% aE45cor_ = [  -170,  23.2,0] (m/s^2)
% 
% aE4_=vC_+alpha4_xrCE_+omega4_x(omega4_xrCE_)=
% aE4_=aE5_+aE45_+aE45cor_=>
% x-axis: 0.359*alpha4z - 0.135*aE45 - 204.0 = 0 
% y-axis: - 0.991*aE45 - 0.049*alpha4z - 83.3 = 0 
% =>
% alpha4z = 525.219 (rad/s^2)
% aE45 = -109.981 (m/s^2)
% 
% alpha4_=alpha5_ = [0,0,525.219](rad/s^2)
% aE45_ = [-14.847,-108.974,0] (m/s^2)
% 
% vF_=[-2.500,-3.409,0] (m/s)
% aF_=[-133.299,-45.808,0] (m/s^2)
