% R_RTR Mechanism
% velocity and acceleration
% closed contour method
clear; clc; close all

PIII_1

% phi = phi1 = 30 (degrees) 
% rA_ = [ 0.000, 0.000,0] (m)
% rC_ = [ 0.050, 0.000,0] (m)
% rB_ = [ 0.087, 0.050,0] (m)
% phi2 = phi3 = 53.794 (degrees) 
% rD_ = [ 0.139, 0.121,0] (m)
digits(3)


fprintf('\n') 
fprintf('Closed contour equations  \n\n')  
fprintf('Relative velocities \n\n') 

n = -50.; % (rpm) 
omega1_ = [0 0 pi*n/30]; % (rad/s)
alpha1_ = [0 0 0];

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

vB1_ = vA_ + cross(omega1_,rB_); 
vB2_ = vB1_; 
aB1_ = aA_ ...
    + cross(alpha1_,rB_) - dot(omega1_,omega1_)*rB_;
aB2_ = aB1_;


% symbolic unknowns
syms omega21z omega03z vB32

omega10_ = omega1_; 
omega21v_ = [ 0 0 omega21z]; 
omega03v_ = [ 0 0 omega03z]; 
v32v_ = vB32*[ cos(phi2) sin(phi2) 0];

eqIomega_ = omega10_ + omega21v_ + omega03v_; 
eqIvz = eqIomega_(3);
eqIv = cross(rB_,omega21v_) + cross(rC_,omega03v_) + v32v_;
eqIvx=eqIv(1);
eqIvy=eqIv(2);

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

solIv=solve(eqIvz,eqIvx,eqIvy);
omega21_ = [ 0 0 eval(solIv.omega21z) ]; 
omega03_ = [ 0 0 eval(solIv.omega03z) ]; 
vB3B2_ = eval(solIv.vB32)*[ cos(phi2) sin(phi2) 0]; 

fprintf('omega21_ = [ %g, %g, %6.3f ] (rad/s)\n', omega21_) 
fprintf('omega03_ = [ %g, %g, %6.3f ] (rad/s)\n', omega03_) 
fprintf('vB32 = %6.3f (m/s)\n', eval(solIv.vB32)) 
fprintf('vB3B2_ = [ %6.3f, %6.3f, %d ] (m/s)\n', vB3B2_)  
fprintf('\n') 

fprintf('Absolute velocities \n\n') 
omega30_ = - omega03_;
omega20_ = omega30_;
vC_ = [0 0 0]; % C is fixed
vD_ = vC_ + cross(omega30_, rD_-rC_);
fprintf('omega20_=omega30_= [%d, %d, %6.3f] (rad/s)\n',omega30_) 
fprintf('vD_ = [ %6.3f, %6.3f, %g ] (m/s)\n', vD_) 

% vB32 =  0.211 (m/s)
% vB3B2_ = [  0.125,  0.170, 0 ] (m/s)
% omega20_=omega30_= [0, 0, -7.732] (rad/s)
% vD_ = [  0.936, -0.685, 0 ] (m/s)


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

syms alpha21z alpha03z aB32

alpha10_ = [ 0 0 0 ];  
alpha21v_ = [ 0 0 alpha21z]; 
alpha03v_ = [ 0 0 alpha03z]; 
a32v_ = aB32*[ cos(phi2) sin(phi2) 0]; 

eqIalpha_ = alpha10_ + alpha21v_ + alpha03v_; 
eqIaz = eqIalpha_(3);
eqIa_=cross(rB_,alpha21v_)+cross(rC_,alpha03v_)...
     +a32v_+2*cross(omega20_,vB3B2_)...
-dot(omega1_,omega1_)*rB_-dot(omega20_,omega20_)*(rC_-rB_);
eqIax = eqIa_(1);
eqIay = eqIa_(2);

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

solIa = solve(eqIaz,eqIax,eqIay);
alpha21_ = [ 0 0 eval(solIa.alpha21z) ]; 
alpha03_ = [ 0 0 eval(solIa.alpha03z) ]; 
aB3B2_ = eval(solIa.aB32)*[ cos(phi2) sin(phi2) 0]; 

fprintf('alpha21_ = [ %g, %g, %6.3f ] (rad/s^2)\n', alpha21_) 
fprintf('alpha03_ = [ %g, %g, %6.3f ] (rad/s^2)\n', alpha03_) 
fprintf('aB32 = %6.3f (m/s^2)\n', eval(solIa.aB32)) 
fprintf('aB3B2_ = [ %6.3f, %6.3f, %d ] (m/s^2)\n', aB3B2_)  
fprintf('\n') 

fprintf('Absolute accelerations \n\n') 
alpha30_ = - alpha03_;
alpha20_ = alpha30_;
aC_ = [0 0 0 ]; 
aD_=aC_+cross(alpha30_,rD_-rC_)-dot(omega20_,omega20_)*(rD_-rC_);
fprintf('alpha20_=alpha30_=[%d,%d,%6.3f] (rad/s^2)\n',alpha30_) 
fprintf('aD_ = [ %6.3f, %6.3f, %g ] (m/s^2)\n', aD_) 


omega2_ = omega20_;
omega3_ = omega2_;
alpha2_ = alpha20_;
alpha3_ = alpha2_;


% aB32 = -1.196 (m/s^2)
% aB3B2_ = [ -0.706, -0.965, 0 ] (m/s^2)
% 
% Absolute accelerations 
% 
% alpha20_=alpha30_=[0,0,-34.865] (rad/s^2)
% aD_ = [ -1.077, -10.324, 0 ] (m/s^2)


% eof
