function draw_path(map, x_path, y_path, start, goal,c)
% Michael Otte
% University of Colorado at Boulder
%
% this fucntion is used to draw a path in the map from start to goal
%
% map = 2D matrix
% x_path = vector of path columns
% y_path = vector of path rows
% start = [row_start column_start]
% goal = [row_goal column_goal]
% c = color, i.e. 'r' for red

if ~isempty(map)
    c_map = repmat([1:-1/128:0]',1,3);
    colormap(c_map)
    imagesc(map)
end
hold on
plot(start(1,1),start(1,2),'o','MarkerFaceColor','g','MarkerSize',10)
plot(goal(1,1),goal(1,2),'x','LineWidth', 2,'MarkerEdgeColor','r','MarkerSize',12)
plot(x_path,y_path,'.','Color',c)
hold off