function optimizedPath = optimizePath(path, L1, L2)
    % Shortens the path by connecting non-consecutive nodes directly if there's no obstacle
    optimizedPath = path(:, 1);
    skip = 0;
    for i = 1:size(path, 2)-1
        for j = size(path, 2):-1:i+1+skip
            if ~isLineIntersectingObstacle(path(:, i), path(:, j), L1, L2)
                optimizedPath = [optimizedPath, path(:, j)];
                skip = j - i - 1;
                break;
            end
        end
    end
end