% R-RTR
% dynamic forces

clear; clc; close all

PIII_2 % velocity and acceleration

%%%%%%%%%%%%%%%%%%%
rC1_ = rB_/2; 
rC2_ = rB_; 
rC3_ = (rD_+rC_)/2;
aC1_ = aB1_/2; 
aC2_ = aB1_; 
aC3_ = (aD_+aC_)/2; 

fprintf('\n')
fprintf('rC1_ = [%6.3f,%6.3f,%d] (m)\n',rC1_)
fprintf('rC2_ = [%6.3f,%6.3f,%d] (m)\n',rC2_)
fprintf('rC3_ = [%6.3f,%6.3f,%d] (m)\n',rC3_)
fprintf('aC1_ = [%6.3f,%6.3f,%d] (m/s^2)\n', aC1_)
fprintf('aC2_ = [%6.3f,%6.3f,%d] (m/s^2)\n', aC2_)
fprintf('aC3_ = [%6.3f,%6.3f,%d] (m/s^2)\n', aC3_)
fprintf...
('alpha1_ = [%d ,%d,%6.3f](rad/s^2)\n', alpha1_) 
fprintf...
('alpha2_ = [%d,%d,%6.3f](rad/s^2)\n', alpha2_) 
fprintf...
('alpha3_ = [%d,%d,%6.3f](rad/s^2)\n', alpha3_) 
fprintf('\n')

fprintf('\n')
fprintf('Dynamic force analysis \n')

h = 0.01; % height of the links
d = 0.001; % depth of the links
hSlider = 0.02; % height of the slider 
wSlider = 0.04; % depth of the slider 
rho = 8000; % density of the material
g = 9.807; % gravitational acceleration

fprintf('Link 1 \n')
m1 = rho*AB*h*d; % mass
G1_ = [0,-m1*g,0]; % force of gravity
Fin1_ = -m1*aC1_;  % force of inertia
IC1 = m1*(AB^2+h^2)/12; % mass moment of inertia
IA  = m1*(AB^2+h^2)/3 ; % A fixed point
alpha1_ = [0 0 0]; % angular acceleration
Min1_ = -IC1*alpha1_; % moment of inertia
%   fprintf('m1 = %6.3f (kg)\n', m1)
fprintf('G1_ = - m1 g_ = [%d,%6.3f,%d] (N)\n', G1_)
% fprintf('m1 aC1_ = [%6.3f,%6.3f,%d] (N)\n', m1*aC1_)
fprintf('Fin1_= - m1 aC1_ = [%6.3f,%6.3f,%d] (N)\n', Fin1_)
% fprintf('IC1 = %g (kg m^2)\n', IC1)
% fprintf('IA = %g (kg m^2)\n', IA)
% fprintf('IC1 alpha1_=[%6.3f,%6.3f,%d](N m)\n',IC1*alpha1_)
fprintf('Min1_=-IC1 alpha1_=[%d, %d, %d](N m)\n',Min1_)
fprintf('\n')

fprintf('Link 2 \n')
m2 = rho*hSlider*wSlider*d;
G2_ = [0,-m2*g,0];
Fin2_ = -m2*aC2_;
IC2 = m2*(hSlider^2+wSlider^2)/12;
Min2_ = -IC2*alpha2_;
% fprintf('m2 = %6.3f (kg)\n', m2)
fprintf('G2_ = - m2 g_ = [%d,%6.3f,%d] (N)\n', G2_)
% fprintf('m2 aC2_ = [%6.3f,%6.3f,%d] (N)\n', m2*aC2_)
fprintf('Fin2_= - m2 aC2_ =[%6.3f,%6.3f,%d] (N)\n', Fin2_)
% fprintf('IC2 = %6.3d (kg m^2)\n', IC2)
% fprintf('IC2 alpha2_=[%g,%g,%6.3d](N m)\n',IC2*alpha2_)
fprintf('Min2_=-IC2 alpha2_=[%d,%d,%6.3d](N m)\n',Min2_)
fprintf('\n')

fprintf('Link 3 \n')
m3 = rho*CD*h*d;
G3_ = [0,-m3*g,0];
Fin3_ = -m3*aC3_;
IC3 = m3*(CD^2+h^2)/12;
IC  = IC3+m3*(CD/2)^2; % C fixed point
Min3_ = -IC3*alpha3_;
% fprintf('m3 = %6.3f (kg)\n', m3)
fprintf('G3_ = - m3 g_ = [%d, %6.3f, %d] (N)\n', G3_)
% fprintf('m3 aC3_ = [%6.3f,%6.3f,%g] (N)\n', m3*aC3_)
fprintf('Fin3_ = - m3 aC3_ =[%6.3f,%6.3f,%d] (N)\n',Fin3_)
% fprintf('IC3 = %6.3d (kg m^2)\n', IC3)
% fprintf('IC3 alpha3_=[%d,%d,%6.3d](N m)\n',IC3*alpha3_)
fprintf('Min3_=-IC3 alpha3_=[%d,%d,%6.3d](N m)\n',Min3_)
fprintf('\n')

Me = 100; % N m
Me_ = -sign(omega3_(3))*[0,0,Me];
fprintf('Me_ = [%d, %d, %6.3f] (N m)\n',Me_)
fprintf('\n')


%  joint C rotation
syms F03x F03y
% unknown joint force of 0 on 3 at C 
F03_=[F03x, F03y, 0]; 
% for joint B translation:
% (F03_+G3_+Fin3_+F23_) .(rB_-rC_)=0
% F23_ perpendicular to BC: F32_.BC_ = 0 => 
eqF3 = (F03_+G3_+Fin3_)*(rB_-rC_).'; % (F1)
% for joint B rotation
% BC_xF03_+BC3_x(G3_+Fin3_)+Me_+Min2_+Min3_=0_
eqM32B_ = ...
cross(rC_-rB_,F03_)+cross(rC3_-rB_,G3_+Fin3_)...
+Me_+Min2_+Min3_;
eqM32z = eqM32B_(3); % (F2)
% Eq(F1) and Eq(F2) =>
solF03=solve(eqF3,eqM32z);
F03xs=eval(solF03.F03x);
F03ys=eval(solF03.F03y);
F03s_=[F03xs, F03ys, 0]; 
fprintf('F03_ = [%6.3f,%6.3f,%g] (N)\n',F03s_)
% F03_ = [-1302.149,953.291,0] (N)

% joint B rotation
syms F12x F12y 
F12_=[F12x, F12y, 0]; % unknown joint force of 1 on 2 at B 
% for joint B translation
% (F12_+G2_+Fin2_+F32_) .(rB_-rC_)=0
% F32_ = -F23_ perpendicular to BC: F32_.BC_ = 0 => 
eqF2 = (F12_+G2_+Fin2_)*(rB_-rC_).'; % (F3)
% for joint C rotation
% CB_x(F12_+G2_+Fin2_)+CC3_x(G3_+Fin3_)+Me_+Min2_+Min3_=0_
eqM23C_ = ...
cross(rB_-rC_,F12_+G2_+Fin2_)+cross(rC3_-rC_,G3_+Fin3_)...
+Me_+Min2_+Min3_;
eqM23z = eqM23C_(3); % (F4)
% Eq(F3) and Eq(F4) =>
solF12=solve(eqF2,eqM23z);
F12xs=eval(solF12.F12x);
F12ys=eval(solF12.F12y);
F12s_=[F12xs, F12ys, 0]; 
fprintf('F12_ = [%6.3f,%6.3f,%g] (N)\n',F12s_)
% F12_ = [1302.128,-953.181,0] (N)

% joint B translation
% for joint B rotation
% Min2_ - M23_ = 0_
M23_= Min2_; % 
fprintf('M23_ = [%d,%d,%6.6f] (N m)\n',M23_)
% M23_ = [0,0,0.000037] (N m)
syms F23x F23y 
F23_=[F23x, F23y, 0]; % unknown joint force of 2 on 3 at B 
% F23_ perpendicular to BC: F32.BC = 0 => 
eqF23 = (rB_-rC_)*F23_.'; % (F5)
% for joint C rotation
% CB_xF23_+CC3_x(G3_+Fin3_)+Me_+Min3_+Min3_=0_
eqM3C_ = ...
cross(rB_-rC_,F23_)+cross(rC3_-rC_,G3_+Fin3_)...
+Me_+Min3_+M23_;
eqM3Cz = eqM3C_(3); % (F6)
% Eq(F5) and Eq(F6) =>
solF23=solve(eqF23,eqM3Cz);
F23xs=eval(solF23.F23x);
F23ys=eval(solF23.F23y);
F23s_=[F23xs, F23ys, 0];
fprintf('F23_ = [%6.3f,%6.3f,%g] (N)\n',F23s_)
% F23_ = [1302.143,-953.235,0] (N)


% joint A rotation
syms F01x F01y Mm
F01_=[F01x, F01y, 0]; % unknown joint force of 0 on 1 at A 
Mm_= [0, 0, Mm]; % equilibrium moment
% for joint B rotation
% BA_xF01_+BC1_x(G1_+Fin1_)+Mm_+Min1_=0_
eqM1B_ = ...
cross(rA_-rB_,F01_)+cross(rC1_-rB_,G1_+Fin1_)+Mm_+Min1_;
eqM1Bz = eqM1B_(3); % (F7)
% for joint B translation
% (F01+G1_+Fin1_+G2_+Fin2_+F32_) .(rB_-rC_)=0
% F32_ = -F23_ perpendicular to BC: F32_.BC_ = 0 => 
eqF12 = (F01_+G1_+Fin1_+G2_+Fin2_)*(rB_-rC_).'; % (F8)
% for joint C rotation
% CA_xF01_+CC1_x(G1_+Fin1_)+CB_x(G2_+Fin2_)+CC3_x(G3_+Fin3_)
% Mm_+Me_+Min2_+Min3_=0_
eqM123C_ = ...
 cross(rA_-rC_,F01_)+cross(rC1_-rC_,G1_+Fin1_)...
+cross(rB_-rC_,G2_+Fin2_)+cross(rC3_-rC_,G3_+Fin3_)...
+Mm_+Me_+Min2_+Min3_;
eqM123z = eqM123C_(3); % (F9)

% Eq(F7), Eq(F8) and Eq(F9) =>
solF01=solve(eqM1Bz, eqF12, eqM123z);
F01xs=eval(solF01.F01x);
F01ys=eval(solF01.F01y);
Mms=eval(solF01.Mm);
F01s_=[F01xs, F01ys, 0];
Mms_=[0, 0, Mms];
fprintf('F01_ = [%6.3f,%6.3f,%g] (N)\n',F01s_)
fprintf('Mm_ = [%d,%d,%6.3f] (N m)\n',Mms_)


% 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)
% rC1_ = [ 0.043, 0.025,0] (m)
% rC2_ = [ 0.087, 0.050,0] (m)
% rC3_ = [ 0.094, 0.061,0] (m)
% aC1_ = [-1.187,-0.685,0] (m/s^2)
% aC2_ = [-2.374,-1.371,0] (m/s^2)
% aC3_ = [-0.538,-5.162,0] (m/s^2)
% alpha1_ = [0 ,0, 0.000](rad/s^2)
% alpha2_ = [0,0,-34.865](rad/s^2)
% alpha3_ = [0,0,-34.865](rad/s^2)
% 
% 
% Dynamic force analysis 
% Link 1 
% G1_ = - m1 g_ = [0,-0.078,0] (N)
% Fin1_= - m1 aC1_ = [ 0.009, 0.005,0] (N)
% Min1_=-IC1 alpha1_=[0, 0, 0](N m)
% 
% Link 2 
% G2_ = - m2 g_ = [0,-0.063,0] (N)
% Fin2_= - m2 aC2_ =[ 0.015, 0.009,0] (N)
% Min2_=-IC2 alpha2_=[0,0,3.719e-05](N m)
% 
% Link 3 
% G3_ = - m3 g_ = [0, -0.118, 0] (N)
% Fin3_ = - m3 aC3_ =[ 0.006, 0.062,0] (N)
% Min3_=-IC3 alpha3_=[0,0,7.880e-04](N m)
% 
% Me_ = [0, 0, 100.000] (N m)
% 


% F03_ = [-1302.149,953.291,0] (N)
% F12_ = [1302.128,-953.181,0] (N)
% M23_ = [0,0,0.000037] (N m)
% F23_ = [1302.143,-953.235,0] (N)
% F01_ = [1302.118,-953.108,0] (N)
% Mm_ = [0,0,-147.651] (N m)
% 
% F23_ = [1302.143,-953.235,0] (N)
% F12_ = [1302.128,-953.181,0] (N)
% M23_ = [0,0,0.000037] (N m)
% F03_ = [-1302.149,953.291,0] (N)
% Mm_ = [0,0,-147.651] (N m)
% F01_ = [1302.118,-953.108,0] (N)

