function [P_ptu P_cam] = DirectKinematics( Q,L )
%UNTITLED Summary of this function goes here
%   Detailed explanation goes here
l1 = L(1); l2 = L(2); l3 = L(3);
q1 = Q(1); q2 = Q(2);
% Pan-tilt unit ___________________________________________________________
P_ptu = [ 0 0  -l2*cos(q1)*sin(q2)  l3*cos(q1)*cos(q2)-l2*cos(q1)*sin(q2)
                  0 0  -l2*sin(q1)*sin(q2)  l3*sin(q1)*cos(q2)-l2*sin(q1)*sin(q2)
                  0 l1 l2*cos(q2)+l1          l3*sin(q2)+l2*cos(q2)+l1                    ];
% Video camera _________________________________________________________     
L1 = l1; L2 = l2+0.4*l2; L3 = l3-0.01*l3; d3 = 0.4*l3;
P0x = L3*cos(q1)*cos(q2)-L2*cos(q1)*sin(q2);
P0y = L3*sin(q1)*cos(q2)-L2*sin(q1)*sin(q2);
P0z = L3*sin(q2)+L2*cos(q2)+L1;

P1x = P0x+sin(q1)*d3;
P1y = P0y-cos(q1)*d3;
P1z = P0z;

P2x = P0x-sin(q1)*d3;
P2y = P0y+cos(q1)*d3;
P2z = P0z;


L2 = l2-0.4*l2; 
P0x = L3*cos(q1)*cos(q2)-L2*cos(q1)*sin(q2);
P0y = L3*sin(q1)*cos(q2)-L2*sin(q1)*sin(q2);
P0z = L3*sin(q2)+L2*cos(q2)+L1;

P3x = P0x+sin(q1)*d3;
P3y = P0y-cos(q1)*d3;
P3z = P0z;

P4x = P0x-sin(q1)*d3;
P4y = P0y+cos(q1)*d3;
P4z = P0z;

P_cam = [P1x P2x P4x P3x; P1y P2y P4y P3y; P1z P2z P4z P3z];

end