14 lines
494 B
Matlab
14 lines
494 B
Matlab
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 |