% R-RTR-RTR
% Velocity and acceleration analysis
% Contour method

clear; clc; close;

AB = 0.14 ; AC = 0.06 ; AE = 0.25 ; CD = 0.15 ; DF=0.4; EG=0.5; % (m)
phi_input = 30; phi = phi_input*(pi/180); 
xA = 0; yA = 0; rA = [xA yA 0]; 
xC = 0 ; yC = AC ; rC = [xC yC 0]; 
xE = 0 ; yE = -AE ; rE = [xE yE 0];  

xB = AB*cos(phi); yB = AB*sin(phi); rB = [xB yB 0]; 

eqnD1 = '( xDsol - xC )^2 + ( yDsol - yC )^2 = CD^2 ';
eqnD2 = '( yB - yC ) / ( xB - xC ) = ( yDsol - yC ) / ( xDsol - xC )';
solD = solve(eqnD1, eqnD2, 'xDsol, yDsol');
xDpositions = eval(solD.xDsol); yDpositions = eval(solD.yDsol);
xD1 = xDpositions(1); xD2 = xDpositions(2);
yD1 = yDpositions(1); yD2 = yDpositions(2);
if (phi>=0 && phi<=pi/2)||(phi >= 3*pi/2 && phi<=2*pi)
   if xD1 <= xC xD = xD1; yD=yD1; else xD = xD2; yD=yD2; end
   else
   if xD1 >= xC xD = xD1; yD=yD1; else xD = xD2; yD=yD2; end
end 
rD = [xD yD 0]; 

phi2 = atan((yB-yC)/(xB-xC)); phi3 = phi2;
phi4 = atan((yD-yE)/(xD-xE))+pi; phi5 = phi4;
xF = xD + DF*cos(phi3) ; yF = yD + DF*sin(phi3) ; rF = [xF yF 0]; 
xG = xE + EG*cos(phi5) ; yG = yE + EG*sin(phi5) ; rG = [xG yG 0]; 

fprintf('Results \n') ; fprintf('\n');

fprintf('rA=[%g, %g, %g] (m)\n', rA); fprintf('rC=[%g, %g, %g] (m)\n', rC);
fprintf('rE=[%g, %g, %g] (m)\n', rE); fprintf('rB=[%g, %g, %g] (m)\n', rB);
fprintf('rD=[%g, %g, %g] (m)\n', rD); fprintf('rF=[%g, %g, %g] (m)\n', rF);
fprintf('rG=[%g, %g, %g] (m)\n', rG); 
fprintf('phi2=phi3=%g(degrees)\n',phi2*180/pi);
fprintf('phi4=phi5=%g(degrees)\n',phi4*180/pi);
plot([xA,xB],[yA,yB],'r-o',[xD,xF],[yD,yF],'b-o',[xE,xG],[yE,yG],'g-o') 
xlabel('x (m)'),ylabel('y (m)'), title('positions for \phi = 30 (deg)'),...
text(xA,yA,' A'), text(xB,yB,' B'), text(xC,yC,' C'), text(xD,yD,'  D'),...
text(xE,yE,' E'), text(xF,yF,' F'), text(xG,yG,' G'), grid

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

n = 50.; omega1 = [ 0 0 pi*n/30 ]; alpha1 = [0 0 0 ]; 
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 ];

vB1 = vA + cross(omega1,rB); vB2 = vB1; 
aB1 = aA + cross(alpha1,rB) - dot(omega1,omega1)*rB;

fprintf('vB = vB1 = vB2 = [ %g, %g, %g ] (m/s)\n', vB1); aB2 = aB1;
fprintf('aB = aB1 = aB2 = [ %g, %g, %g ] (m/s^2)\n', aB1); fprintf('\n');

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

omega10 = omega1; 
omega21v = [ 0 0 sym('omega21z','real') ]; 
omega03v = [ 0 0 sym('omega03z','real') ]; 
v32v = sym('vB32','real')*[ 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,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));

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, %g ] (rad/s)\n', omega21 );
fprintf('omega03 = [ %g, %g, %g ] (rad/s)\n', omega03 );
fprintf('vB32 = %g (m/s)\n', eval(solIv.vB32) );
fprintf('vB3B2 = [ %g, %g, %d ] (m/s)\n', vB3B2 ); fprintf('\n');

fprintf('Absolute velocities  \n'); fprintf('\n');
omega30 = - omega03;
omega20 = omega30;
vD3 = cross(omega30,rD-rC);
fprintf('omega20 = omega30 = [ %d, %d, %g ] (rad/s)\n', omega30 );
fprintf('vD3 = vD4 = [ %g, %g, %g ] (m/s)\n', vD3 );

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

alpha10 = alpha1; 
alpha21v = [ 0 0 sym('alpha21z','real') ]; 
alpha03v = [ 0 0 sym('alpha03z','real') ]; 
a32v = sym('aB32','real')*[ cos(phi2) sin(phi2) 0]; 

eqIalpha = alpha10 + alpha21v + alpha03v; 
eqIaz=eqIalpha(3);
eqIa1=cross(rB,alpha21v)+cross(rC,alpha03v)+a32v+2*cross(omega20,vB3B2);
eqIa2=-dot(omega1,omega1)*rB-dot(omega20,omega20)*(rC-rB);
eqIa=eqIa1+eqIa2;
eqIax=eqIa(1);
eqIay=eqIa(2);

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));

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, %g ] (rad/s^2)\n', alpha21 );
fprintf('alpha03 = [ %g, %g, %g ] (rad/s^2)\n', alpha03 );
fprintf('aB32 = %g (m/s^2)\n', eval(solIa.aB32) );
fprintf('aB3B2 = [ %g, %g, %d ] (m/s^2)\n', aB3B2 ); fprintf('\n');

fprintf('Absolute accelerations  \n'); fprintf('\n');
alpha30 = - alpha03;
alpha20 = alpha30;
aD3 = cross(alpha30,rD-rC)-dot(omega20,omega20)*(rD-rC);
fprintf('alpha20 = alpha30 = [ %d, %d, %g ] (rad/s^2)\n', alpha30 );
fprintf('aD3 = aD4 = [ %g, %g, %g ] (m/s^2)\n', aD3 );

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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

omega43v = [ 0 0 sym('omega43z','real') ]; 
omega05v = [ 0 0 sym('omega05z','real') ]; 
v54v = sym('vD54','real')*[ cos(phi4) sin(phi4) 0]; 

eqIIomega = omega30 + omega43v + omega05v; 
eqIIvz=eqIIomega(3);
eqIIv=cross(rC,omega30)+cross(rD,omega43v)+cross(rE,omega05v)+v54v;
eqIIvx=eqIIv(1);
eqIIvy=eqIIv(2);

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

solIIv=solve(eqIIvz,eqIIvx,eqIIvy);
omega43 = [ 0 0 eval(solIIv.omega43z) ]; 
omega05 = [ 0 0 eval(solIIv.omega05z) ] ; 
vD5D4 = eval(solIIv.vD54)*[ cos(phi4) sin(phi4) 0]; 

fprintf('omega43 = [ %g, %g, %g ] (rad/s)\n', omega43 );
fprintf('omega05 = [ %g, %g, %g ] (rad/s)\n', omega05 );
fprintf('vD54 = %g (m/s)\n', eval(solIIv.vD54) );
fprintf('vD5D4 = [ %g, %g, %d ] (m/s)\n', vD5D4 ); fprintf('\n');

fprintf('Absolute velocities  \n'); fprintf('\n');
omega50 = - omega05;
omega40 = omega50;
fprintf('omega40 = omega50 = [ %d, %d, %g ] (rad/s)\n', omega50 );

fprintf('\n');
fprintf('Relative accelerations  \n'); fprintf('\n');
 
alpha43v = [ 0 0 sym('alpha43z','real') ]; 
alpha05v = [ 0 0 sym('alpha05z','real') ]; 
a54v = sym('aD54','real')*[ cos(phi4) sin(phi4) 0]; 

eqIIalpha = alpha30 + alpha43v + alpha05v; 
eqIIaz=eqIIalpha(3);
eqIIa1=cross(rC,alpha30)+cross(rD,alpha43v)+cross(rE,alpha05v)+a54v;
eqIIa2=2*cross(omega40,vD5D4);
eqIIa3=-dot(omega30,omega30)*(rD-rC)-dot(omega40,omega40)*(rE-rD);
eqIIa=eqIIa1+eqIIa2+eqIIa3;
eqIIax=eqIIa(1);
eqIIay=eqIIa(2);

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));

solIIa=solve(eqIIaz,eqIIax,eqIIay);
alpha43 = [ 0 0 eval(solIIa.alpha43z) ]; 
alpha05 = [ 0 0 eval(solIIa.alpha05z) ] ; 
aD5D4 = eval(solIIa.aD54)*[ cos(phi4) sin(phi4) 0]; 

fprintf('alpha43 = [ %g, %g, %g ] (rad/s^2)\n', alpha43 );
fprintf('alpha05 = [ %g, %g, %g ] (rad/s^2)\n', alpha05 );
fprintf('aD54 = %g (m/s^2)\n', eval(solIIa.aD54) );
fprintf('aD5D4 = [ %g, %g, %d ] (m/s^2)\n', aD5D4 ); fprintf('\n');

fprintf('Absolute accelerations  \n'); fprintf('\n');
alpha50 = - alpha05;
alpha40 = alpha50;
fprintf('alpha40 = alpha50 = [ %d, %d, %g ] (rad/s^2)\n', alpha50 );