% Exercício 11.2
% Síntese de mecanismos para movimentação de objetos
% Mabie

clear all
close all
clc

XA=[2; 2; 1.5];
YA=[1; 2; 3];
XB=[3; 3; 1.5];
YB=[1; 2; 4];
%% centro OA
X1=XA(1);
X2=XA(2);
X3=XA(3);
Y1=YA(1);
Y2=YA(2);
Y3=YA(3);
K=[2*(X2-X1) 2*(Y2-Y1);
   2*(X3-X2) 2*(Y3-Y2)];
U=[X1^2-X2^2+Y1^2-Y2^2;
   X2^2-X3^2+Y2^2-Y3^2];
OA=-K^(-1)*U;
%% centro OB
X1=XB(1);
X2=XB(2);
X3=XB(3);
Y1=YB(1);
Y2=YB(2);
Y3=YB(3);
K=[2*(X2-X1) 2*(Y2-Y1);
   2*(X3-X2) 2*(Y3-Y2)];
U=[X1^2-X2^2+Y1^2-Y2^2;
   X2^2-X3^2+Y2^2-Y3^2];
OB=-K^(-1)*U
%% comprimentos dos elos
a=sqrt((XA(1)-OA(1))^2+(YA(1)-OA(2))^2);
b=sqrt((XB(1)-XA(1))^2+(YB(1)-YA(1))^2);
c=sqrt((XB(1)-OB(1))^2+(YB(1)-OB(2))^2);
d=sqrt((OB(1)-OA(1))^2+(OB(2)-OA(2))^2);
%% apresentação dos resultados
clc
fprintf('      XA        YA        XB        YB\n')
PA = [XA YA XB YB];
disp(PA)
fprintf('a = %8.4f\nb = %8.4f\nc = %8.4f\nd = %8.4f\n',a,b,c,d)
%% análise
%-------------------------------------------------------------
%% Posição
%-------------------------------------------------------------
syms q A B
syms C1x C1y C2 C3 C4
%% POSIÇÃO
disp('Equações da posição')
F = [ C2*cos(q)+C3*cos(A)-C4*cos(B)-C1x;
      C2*sin(q)+C3*sin(A)-C4*sin(B)+C1y];
S = [A; B];
%% VELOCIDADE
disp('Matriz Jacobiana')
jaco = jacobian(F,S);
jacoinv = simplify(inv(jaco));
detjaco = simplify(det(jaco));
%-------------------------------------------------------------
%% dados numéricos
%-------------------------------------------------------------
disp('Dados Numéricos')
C1x=OB(1)-OA(1);
C1y=OA(2)-OB(2);
C2=a;
C3=b;
C4=c;
%-------------------------------------------------------------
% número de pontos
NP = 10;
% configuração 1
qmin = -12.6*pi/180;
qmax = 40.5*pi/180;
a0 = 0*pi/180;  
b0 = -20*pi/180;
% configuração 2
% qmin = -8*pi/180;
% qmax = 54*pi/180;
% a0 = -23*pi/180;  
% b0 = -15*pi/180;
%-------------------------------------------------------------
%% Análises
%-------------------------------------------------------------
% dimensionamento dos vetores
vetq=zeros(NP,1);
vetA=zeros(NP,1);
vetB=zeros(NP,1);
vetdetjaco=zeros(NP,1);
%-------------------------------------------------------------
ZERO=10^(-6);     % tolerancia para o metodo de Newton Raphson
s=[a0; b0];
for i = 1:NP
    vetq(i)=qmin+(i-1)*(qmax-qmin)/(NP-1);
    %-------------------------------------------------------------
    %% análise geral de posição
    %-------------------------------------------------------------

    f=[1;1];          %funçao f cuja norma e maior que ZERO
    while norm(f) > ZERO
        q = vetq(i)
        A = s(1)
        B = s(2)
        norm(f)
        f = eval(F)
        s=s-eval(jacoinv)*f
    end
    % distribuição do resultado nas variáveis secundárias
    A=s(1);
    B=s(2);
    vetA(i)=A;     % armazenamento no vetor
    vetB(i)=B;     % armazenamento no vetor
    vetdetjaco(i)=eval(detjaco);
    
end
%-------------------------------------------------------------
%% Gráficos da análise geral
%-------------------------------------------------------------
figure('Name','Análise Geral')    % título na moldura superior
% gráficos geminados
% primeiro gráfico
subplot(2,2,1)                    % gráfico da esquerda (posição 1)
plot(vetq*180/pi,vetA*180/pi,...
    'LineWidth',2)                % espessura da linha
grid on                           % linhas de grade
xlabel('q (^o)')                  % rótulo do eixo x
ylabel('A(q) (^o)')               % rótulo do eixo y
title('Posição do elo intermediário')    % título do gráfico
xlim([qmin*180/pi qmax*180/pi])
% segundo gráfico
subplot(2,2,2)                    % gráfico da direita (posição 2)
plot(vetq*180/pi,vetB*180/pi,...
    'r',...                       % cor da curva
    'LineWidth',2)                % espessura da linha
grid on                           % insere linhas de grade
xlabel('q (^o)')                  % rótulo do eixo x
ylabel('B(q) (^o)')               % rótulo do eixo y
title('Posição do elo de saída')        % título do gráfico
xlim([qmin*180/pi qmax*180/pi])
% terceiro gráfico
subplot(2,2,[3 4])                    % gráfico da direita (posição 2)
plot(vetq*180/pi,vetdetjaco,...
    'LineWidth',2)                % espessura da linha
grid on                           % linhas de grade
xlabel('q (^o)')                  % rótulo do eixo x
ylabel('Det J(q)')                % rótulo do eixo y
title('Determinante da Matriz Jacobiana')        % título do gráfico
xlim([qmin*180/pi qmax*180/pi])

%-------------------------------------------------------------
%% Gráfico do determinante da matriz jacobiana
%-------------------------------------------------------------
figure('Name','Determinante da Matriz Jacobiana')% título na moldura
plot(vetq*180/pi,vetdetjaco,...
    'LineWidth',2)                % espessura da linha
grid on                           % linhas de grade
xlabel('q (^o)')                  % rótulo do eixo x
ylabel('Det J(q)')                % rótulo do eixo y
title('Determinante da Matriz Jacobiana')        % título do gráfico
xlim([qmin*180/pi qmax*180/pi])



%-------------------------------------------------------------
% FIM
%-------------------------------------------------------------

