function [p,phi]=kindir3R(theta,a1,a2,a3,plotta)

% Programmino per il calcolo della cinematica diretta 
% del robot planare 3R
% se la variabile stringa plotta='plot'
% allora calcola anche le coordinate del gomito
% e disegna il robot
% 
%    axis([-1 1 -1 1]*(a1+a2+a3)*1.1), % axis equal
%    axis('square')

phi=theta(1)+theta(2)+theta(3);

c1=cos(theta(1)); c12=cos(theta(1)+theta(2));  c123=cos(phi);
s1=sin(theta(1)); s12=sin(theta(1)+theta(2));  s123=sin(phi);

px=a1*c1+a2*c12+a3*c123;
py=a1*s1+a2*s12+a3*s123;

p=[px; py]; 

if plotta=='plot'
    pg1x=a1*c1; pg2x=pg1x+a2*c12;
    pg1y=a1*s1; pg2y=pg1y+a2*s12;
    PPX=[0 pg1x pg2x px];
    PPY=[0 pg1y pg2y py];
    plot(PPX,PPY,'o-','linewidth',4)
end

