%========================================================================
% arquivo: QuatroBarras2glSimbolico.m
% Exemplo 3.1.1 do Doughty
% 
% iniciado em: 18/02/2015
% prof. Jorge Luiz Erthal
%========================================================================
clear all
close all
clc
syms C1 C2 C3
syms q1 q2 A B dq1dt dq2dt d2q1dt2 d2q2dt2
syms Ka1 Ka2 Kb1 Kb2
%------------------------------------------------------------------------
%% vetores
S=[A; B];
q=[q1; q2];
dqdt=[dq1dt; dq2dt];
d2qdt2=[d2q1dt2; d2q2dt2];
%------------------------------------------------------------------------
%% equações cinemáticas de posição
f = [C1*cos(q1)+B*cos(A)-C2*cos(q2)-C3;
     C1*sin(q1)+B*sin(A)-C2*sin(q2)];
%------------------------------------------------------------------------
%% velocidades
J = jacobian(f,S)
Q = jacobian(f,q)
% K=[Ka1 Ka2; 
%    Kb1 Kb2];
K=simplify(-inv(J)*Q)
Ka1=K(1,1);
Ka2=K(1,2);
Kb1=K(2,1);
Kb2=K(2,1);
dSdt=K*dqdt;
dAdt=dSdt(1);
dBdt=dSdt(2);
%------------------------------------------------------------------------
%% acelerações
for i=1:size(S,1)
    for j=1:size(S,1)
        dJdq1(i,j)=jacobian(J(i,j),S)*K(:,1)+diff(J(i,j),q(1));
        dJdq2(i,j)=jacobian(J(i,j),S)*K(:,2)+diff(J(i,j),q(2));
    end
end
dJdq1=simplify(dJdq1)
dJdq2=simplify(dJdq2)

for i=1:size(S,1)
    for j=1:size(q,1)
        dQdq1(i,j)=jacobian(Q(i,j),S)*K(:,1)+diff(Q(i,j),q(1));
        dQdq2(i,j)=jacobian(Q(i,j),S)*K(:,2)+diff(Q(i,j),q(2));
    end
end
dQdq1=simplify(dQdq1)
dQdq2=simplify(dQdq2)

L1=simplify(-inv(J)*(dJdq1*K+dQdq1))
L2=simplify(-inv(J)*(dJdq2*K+dQdq2))

d2Sdt2=K*d2qdt2+L1*dqdt*dq1dt+L2*dqdt*dq2dt;
d2Adt2=d2Sdt2(1);
d2Bdt2=d2Sdt2(2);
%------------------------------------------------------------------------
%% verificação
% clc
% Ka1=K(1,1);
% Ka2=K(1,2);
% Kb1=K(2,1);
% Kb2=K(2,2);
% 
% L111=simplify(C1/B*(1-Ka1)*sin(q1-A)+C1/(B^2)*Kb1*cos(q1-A)-L1(1,1));
% L112=simplify(-C2/B*Ka1*sin(A-q2)-(C2/(B^2))*Kb1*cos(A-q2)-L1(1,2));
% L121=simplify(C1*(1-Ka1)*cos(q1-A)-L1(2,1));
% L122=simplify(C2*Ka1*cos(A-q2)-L1(2,2));
% 
% resto1=[L111 L112;
%         L121 L122]
%     
% L211=simplify(-C1/B*Ka2*sin(q1-A)+C1/(B^2)*Kb2*cos(q1-A)-L2(1,1));
% L212=simplify(-C2/B*(Ka2-1)*sin(A-q2)-(C2/(B^2))*Kb2*cos(A-q2)-L2(1,2));
% L221=simplify(-C1*Ka2*cos(q1-A)-L2(2,1));
% L222=simplify(C2*(Ka2-1)*cos(A-q2)-L2(2,2));
% 
% resto2=[L211 L212;
%         L221 L222]    
%------------------------------------------------------------------------
%% dados numéricos
C1=1.5;
C2=2.2;
C3=3.5;
q1=0.85;
q2=0.25;
A=-0.125;
B=4.678;
dq1dt=-2.6;
dq2dt=3.5;
d2q1dt2=0.42;
d2q2dt2=0.68;
%------------------------------------------------------------------------
dAdtcalc=eval(dAdt);
dBdtcalc=eval(dBdt);

d2Adt2calc=eval(d2Adt2);
d2Bdt2calc=eval(d2Bdt2);
%------------------------------------------------------------------------

fprintf('dAdt = %f\n',dAdtcalc)
fprintf('dBdt = %f\n',dBdtcalc)
fprintf('d2Adt2 = %f\n',d2Adt2calc)
fprintf('d2Bdt2 = %f\n',d2Bdt2calc)