function h = plot_dist(x1,y1,x2,y2,color)

%  h = plot_dist(x1,y1,x2,y2,color)

%
% (x,y): position of car
% color: color of car
% h:  handle to plot 

% position of triangle vertices

if nargin <5
    color = 'y';
end

a = [x1 y1]';
b = [x2 y2]';

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

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


