function h = plot_dir(x,y,theta,lng,color)

% h = plot_car(x,y,theta,size)
%
% (x,y): position of car
% theta: counterclockwise angle from x-axis
% size: scalar factor for size of car (size<1 smaller, size>1 bigger, defoult size=1) 
% color: color of car
% h:  handle to plot 

% position of triangle vertices

if nargin <5
    color = 'r';
    if nargin <4 
        lng = 1;
    end
end

a = [x y]';
b = [x+lng*cos(theta) y+lng*sin(theta)]';


P = [a b]; %matrix with points of vertices and also scaled

h = line(P(1,:),P(2,:),'Color',color);


