function h = plot_car(x,y,theta,size,color)

% modifica

% 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 
        size = 1;
    end
end

a = [-1 -1]';
b = [2 0]';
c = [-1 1]';

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

R = [ cos(theta) -sin(theta); sin(theta) cos(theta)]; % rotation matrix to rotate car.

Prot = R*P; %rotated car

Prot_trasl = Prot + [ones(1,3)*x; ones(1,3)*y]; % add offset of car's center 

h = patch(Prot_trasl(1,:),Prot_trasl(2,:),color);


