% R-RTR
% velocity and acceleration analysis

clear; clc; close all

PII_1 % position

fprintf('\n') 
fprintf('Velocity and acceleration analysis \n\n') 
n = -50.; % (rpm)
omega1_ = [0 0 pi*n/30]; 
alpha1_ = [0 0 0];
fprintf('omega1_ = [%g,%g,%6.3f] (rad/s)\n',omega1_) 
fprintf('alpha1_ = [%g,%g,%6.3f] (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_;
aB2_ = aB1_;
fprintf...
('vB1_ = vB2_ =[%6.3f,%6.3f,%g] (m/s)\n',vB1_)  
fprintf...
('aB1_ = aB2_ =[%6.3f,%6.3f,%g] (m/s^2)\n',aB1_)  
fprintf('\n')

omega3z=sym('omega3z','real'); 
vB32=sym('vB32','real');
omega3u_ = [ 0 0 omega3z]; 
% omega3z unknown (to be calculated)
% vB32 unknown (to be calculated)
vC_ = [0 0 0]; % C is fixed
% vB3_ = vC_ + omega3_ x rCB_   
% (B3 & C are points on link 3)
vB3_ = vC_ + cross(omega3u_,rB_-rC_); 
% point B2 is on link 2 and point B3 is on link 3
% vB3_ = vB2_ + vB3B2_
% between the links 2 and 3 there is a translational 
% joint B_T
% vB3B2_ is the relative velocity of B3 wrt 2
% vB3B2_ is parallel to the sliding direction BC
% vB3B2_ is written as a vector
vB3B2u_ = vB32*[cos(phi2) sin(phi2) 0]; 
% vB3 = vB2 + vB3B2
eqvB_ = vB3_ - vB2_ - vB3B2u_; 
% vectorial equation
% the component of the vectorial equation on x-axis
eqvBx = eqvB_(1); 
% the component of the vectorial equation on y-axis
eqvBy = eqvB_(2); 
% two equations eqvBx and eqvBy with two unknowns 

digits(3)
% print the equations for calculating omega3 and vB32
fprintf...
('vB3_ = vC_ + omega3_ x rCB_ = vB2_ + vB3B2_ => \n') 
qvBx=vpa(eqvBx);
fprintf('x-axis: %s = 0 \n',char(qvBx)) 
qvBy=vpa(eqvBy);
fprintf('y-axis: %s = 0 \n',char(qvBy)) 
fprintf('=>\n') 

% solve for omega3z and vB32 
solvB = solve(eqvBx,eqvBy); 
omega3zs=eval(solvB.omega3z); 
vB32s=eval(solvB.vB32);

omega3_ = [0 0 omega3zs]; 
omega2_ = omega3_;
vB3B2_ = vB32s*[cos(phi2) sin(phi2) 0];

fprintf('omega3z = %6.3f (rad/s)\n',omega3zs) 
fprintf('vB32 = %6.3f (m/s)\n',vB32s) 
fprintf('\n') 
fprintf('omega2_ = omega3_ = [%g,%g,%6.3f](rad/s)\n',omega3_) 
fprintf('vB32_ = [%6.3f,%6.3f,%g] (m/s)\n',vB3B2_)  
fprintf('\n') 

% Coriolis acceleration
aB3B2cor_ = 2*cross(omega3_,vB3B2_); 

alpha3z=sym('alpha3z','real'); 
aB32=sym('aB32','real'); % aB32 unknown

alpha3u_ = [0 0 alpha3z]; % alpha3z unknown
aC_ = [0 0 0]; % C is fixed
% aB3 acceleration of B3
aB3_=...
    aC_+cross(alpha3u_,rB_-rC_)...
    -dot(omega3_,omega3_)*(rB_-rC_); 
% aB3B2_ relative velocity of B3 wrt 2
% aB3B2_ parallel to the sliding direction BC

aB3B2u_ = aB32*[cos(phi2) sin(phi2) 0]; 
% aB3_ = aB2_ + aB3B2_ + aB3B2cor_
eqaB_ = aB3_ - aB2_ - aB3B2u_ - aB3B2cor_; 
% vectorial equation
eqaBx = eqaB_(1); % equation component on x-axis
eqaBy = eqaB_(2); % equation component on y-axis

% print the equations for calculating alpha3 and aB32
fprintf('aB32cor_ = [%6.3f,%6.3f,%g] (m/s^2)\n',aB3B2cor_)  
fprintf('\n') 
fprintf('aB3_=aC_+alpha3_ x rCB_-(omega3_.omega3_)rCB_=') 
fprintf('aB2_+aB3B2_+aB3B2cor_ =>\n') 
qaBx=vpa(eqaBx);
fprintf('x-axis: %s = 0 \n',char(qaBx)) 
qaBy=vpa(eqaBy);
fprintf('y-axis: %s = 0 \n',char(qaBy)) 
fprintf('=>\n');

solaB = solve(eqaBx,eqaBy);
alpha3zs=eval(solaB.alpha3z);
aB32s=eval(solaB.aB32);
alpha3_ = [0 0 alpha3zs];
alpha2_ = alpha3_;
aB32_ = aB32s*[cos(phi2) sin(phi2) 0];

fprintf('alpha3z = %6.3f (rad/s)\n', alpha3zs) 
fprintf('aB32 = %6.3f (m/s)\n', aB32s) 
fprintf('\n') 
fprintf('alpha2_=alpha3_=[%g,%g,%6.3f](rad/s^2)\n',alpha3_) 
fprintf('aB32_=[%6.3f,%6.3f,%g] (m/s^2)\n',aB32_) 
fprintf('\n') 

% vD_ velocity of D
% D & C points on link 3
vD_ = vC_ + cross(omega3_,rD_-rC_);
fprintf('vD_ = [%6.3f,%6.3f,%g] (m/s)\n', vD_) 
% aD_ acceleration of D
aD_=...
   aC_+cross(alpha3_,rD_-rC_)...
   -dot(omega3_,omega3_)*(rD_-rC_);  
fprintf('aD_ = [%6.3f,%6.3f,%g] (m/s^2)\n', aD_) 
fprintf('\n')

% eof

% Results 
% 
% 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)
% 
% Velocity and acceleration analysis 
% 
% omega1_ = [0,0,-5.236] (rad/s)
% alpha1_ = [0,0, 0.000] (rad/s^2)
% 
% vB1_ = vB2_ =[ 0.262,-0.453,0] (m/s)
% aB1_ = aB2_ =[-2.374,-1.371,0] (m/s^2)
% 
% vB3_ = vC_ + omega3_ x rCB_ = vB2_ + vB3B2_ => 
% x-axis: - 0.05*omega3z - 0.591*vB32 - 0.262 = 0 
% y-axis: 0.0366*omega3z - 0.807*vB32 + 0.453 = 0 
% =>
% omega3z = -7.732 (rad/s)
% vB32 =  0.211 (m/s)
% 
% omega2_ = omega3_ = [0,0,-7.732](rad/s)
% vB32_ = [ 0.125, 0.170,0] (m/s)
% 
% aB32cor_ = [ 2.636,-1.930,0] (m/s^2)
% 
% aB3_=aC_+alpha3_ x rCB_-(omega3_.omega3_)rCB_=aB2_+aB3B2_+aB3B2cor_ =>
% x-axis: - 0.591*aB32 - 0.05*alpha3z - 2.45 = 0 
% y-axis: 0.0366*alpha3z - 0.807*aB32 + 0.311 = 0 
% =>
% alpha3z = -34.865 (rad/s)
% aB32 = -1.196 (m/s)
% 
% alpha2_=alpha3_=[0,0,-34.865](rad/s^2)
% aB32_=[-0.706,-0.965,-0] (m/s^2)
% 
% vD_ = [ 0.936,-0.685,0] (m/s)
% aD_ = [-1.077,-10.324,0] (m/s^2)

