Segmenting Shortest Paths in Simple Virtual Reality Map Images

The goal is to segment the major paths by turns in a shortest path of a virtual reality map to know how much to score a person's navigation trajectory (blue line in image) based on how far they walked on the shortest path.

The problem right now is the algorithm function is plotting the 1st path segment good but the next segment plotted is the same sized bounding box but on the next x,y line segment position of the shortest path (right next to the first x, y line segment - in idealPath variable)

How can I fix my current code to detect the next path segment and plot the next bounding segmentation box (in orange)? idealPath which is the shortest path plotted in green in this case is 94666x2 where column 1 is the x position and 2 is the y position to plot as line segments to form the green path in the image.

Current code block:

function plotSegmentedCheckpoints(trajectory_raw, idealPath, checkpointInfo)
    % Clear and hold the plot
    cla;
    hold on;
    
    % Plot ideal path in green
    plot(idealPath(:,1), idealPath(:,2), 'g.', 'MarkerSize', 5, 'DisplayName', 'Ideal Path');
    
    % Plot trajectory in blue
    scatter(trajectory_raw(:,1), trajectory_raw(:,2), 15, 'b', 'filled', 'DisplayName', 'Trajectory');

    % Get path segments
    [segmentIndices, numSegments] = findPathSegments(idealPath);
    
    % Initialize checkpoint storage
    checkpoint_bounds = zeros(numSegments, 4);  % [x1, x2, y1, y2]
    reached = false(numSegments, 1);
    
    % Constants
    BOX_WIDTH = 86;  % Width of checkpoint boxes
    
    % Process each segment
    for i = 1:numSegments
        % Get current segment
        startIdx = segmentIndices(i);
        endIdx = segmentIndices(i+1);
        segmentPoints = idealPath(startIdx:endIdx, :);
        
        % Calculate segment bounds
        [bounds, orientation] = calculateSegmentBounds(segmentPoints, BOX_WIDTH);
        checkpoint_bounds(i,:) = bounds;
        
        % Check if trajectory reaches this checkpoint
        reached(i) = checkCheckpointReached(trajectory_raw, bounds);
        
        % Draw checkpoint box
        drawCheckpointBox(bounds, reached(i), i);
        
        % Draw direction arrow to next checkpoint if not last segment
        if i < numSegments
            drawDirectionArrow(bounds, idealPath(segmentIndices(i+1),:));
        end
    end
    
    % Calculate and display score
    score = calculateScore(reached, numSegments);
    
    % Configure plot
    configureplot();
    
    % Update title with progress
    title(sprintf('Checkpoint Progress: (%d/%d)\nScore: %.2f', ...
          sum(reached), numSegments, score));
    
    % Store checkpoint information
    checkpointInfo.bounds = checkpoint_bounds;
    checkpointInfo.reached = reached;
    checkpointInfo.score = score;
end

% Function to find path segments based on direction changes
function [segmentIndices, numSegments] = findPathSegments(idealPath)
    % Calculate path directions
    vectors = diff(idealPath);
    angles = atan2(vectors(:,2), vectors(:,1));
    
    % Find significant direction changes (> 30 degrees)
    angleChanges = [true; abs(diff(angles)) > pi/6];
    
    % Combine consecutive changes that are very close
    MIN_SEGMENT_LENGTH = 50;
    for i = 2:length(angleChanges)-1
        if angleChanges(i) && angleChanges(i+1) && ...
           (i - find(angleChanges(1:i-1), 1, 'last') < MIN_SEGMENT_LENGTH)
            angleChanges(i) = false;
        end
    end
    
    % Get indices where direction changes significantly
    segmentIndices = find(angleChanges);
    
    % Ensure first and last points are included
    if segmentIndices(1) ~= 1
        segmentIndices = [1; segmentIndices];
    end
    if segmentIndices(end) ~= size(idealPath,1)
        segmentIndices = [segmentIndices; size(idealPath,1)];
    end
    
    % Number of segments
    numSegments = length(segmentIndices) - 1;
end

% Function to calculate bounds for a segment
function [bounds, orientation] = calculateSegmentBounds(segmentPoints, BOX_WIDTH)
    % Determine if segment is more vertical or horizontal
    endpoint_diff = diff(segmentPoints([1 end],:));
    is_vertical = abs(endpoint_diff(2)) > abs(endpoint_diff(1));
    
    if is_vertical
        % Vertical segment
        x_center = mean(segmentPoints(:,1));
        y_range = [min(segmentPoints(:,2)), max(segmentPoints(:,2))];
        
        bounds = [
            x_center ,  % x1
            x_center + BOX_WIDTH,  % x2
            y_range(1),              % y1
            y_range(2)               % y2
        ];
        orientation = 'vertical';
    else
        % Horizontal segment
        y_center = mean(segmentPoints(:,2));
        x_range = [min(segmentPoints(:,1)), max(segmentPoints(:,1))];
        
        bounds = [
            x_range(1),              % x1
            x_range(2),              % x2
            y_center ,  % y1
            y_center + BOX_WIDTH   % y2
        ];
        orientation = 'horizontal';
    end
end

% Function to check if trajectory reaches checkpoint
function reached = checkCheckpointReached(trajectory, bounds)
    in_x = trajectory(:,1) >= bounds(1) & trajectory(:,1) <= bounds(2);
    in_y = trajectory(:,2) >= bounds(3) & trajectory(:,2) <= bounds(4);
    reached = any(in_x & in_y);
end

% Function to draw checkpoint box
function drawCheckpointBox(bounds, is_reached, checkpoint_number)
    x = [bounds(1), bounds(2), bounds(2), bounds(1), bounds(1)];
    y = [bounds(3), bounds(3), bounds(4), bounds(4), bounds(3)];
    
    if is_reached
        fill(x(1:4), y(1:4), [0.8 1 0.8], 'FaceAlpha', 0.3, 'EdgeColor', 'k', 'LineWidth', 1);
    else
        fill(x(1:4), y(1:4), [1 0.8 0.8], 'FaceAlpha', 0.3, 'EdgeColor', 'k', 'LineWidth', 1);
    end
    
    % Add checkpoint number
    text(mean([bounds(1), bounds(2)]), mean([bounds(3), bounds(4)]), ...
         num2str(checkpoint_number), 'HorizontalAlignment', 'center', ...
         'VerticalAlignment', 'middle', 'FontSize', 12, ...
         'FontWeight', 'bold', 'Color', 'k');
end

% Function to draw direction arrow
function drawDirectionArrow(current_bounds, next_point)
    % Calculate arrow start point (center of current box)
    arrow_start = [
        mean([current_bounds(1), current_bounds(2)]), 
        mean([current_bounds(3), current_bounds(4)])
    ];
    
    % Calculate direction to next checkpoint
    dir_vec = next_point - arrow_start;
    dir_vec = dir_vec / norm(dir_vec) * 50;  % Fixed length arrows
    
    % Draw arrow
    quiver(arrow_start(1), arrow_start(2), dir_vec(1), dir_vec(2), 0, ...
           'Color', [1 0.5 0], 'LineWidth', 2, 'MaxHeadSize', 0.5);
end

% Function to calculate score
function score = calculateScore(reached, numSegments)
    weights = linspace(1, 2, numSegments);
    score = sum(reached .* weights') / sum(weights);
end

% Function to configure plot
function configureplot()
    grid on;
    axis equal;
    set(gca, 'YDir', 'reverse');
    xlabel('X Position (cm)');
    ylabel('Y Position (cm)');
    legend('Location', 'northwest');
    xlim([0 600]);
    ylim([0 600]);
end

Edit: With Till's suggestion, I was able to implement it and plot the segmented paths onto the figure.

If you already have the green path and just want to divide it in segments, I'd transform the coordinates of the green paths into a logical matrix. Then you can easily find the borders of the segment and display it as you want.

matrixSize = 600;

%Your green path 2 column vector
xPositions = greenPath(:,1);
yPositions = greenPath(:,2);

%Transform to logical matrix
greenPathMat = false(matrixSize);
idx = sub2ind([matrixSize, matrixSize], yPositions, xPositions);
greenPathMat(idx) = true;

%Get border in x and y direction
xBorders = diff(m,1,2);
yBorders = diff(m,1,1);

%Get Start and End
xStart = find(any(xBorders==1)) + 1;
xEnd = find(any(xBorders==-1));

yStart = find(any(yBorders==1,2)) + 1;
yEnd = find(any(yBorders==-1,2));

%Check if there are any values at matrix borders to add them to start/end
if any(greenPathMat(:,1));    xStart = [1 xStart]; end

if any(greenPathMat(:,end));    xEnd = [xEnd, matrixSize];  end

if any(greenPathMat(1,:));    yStart = [1; yStart];    end

if any(greenPathMat(end,:));    yEnd = [yEnd; matrixSize];  end

%Calc number of segments
nbSegments = length(xStart) * 2;

if length(xStart) == length(xEnd)
    nbSegments = nbSegments - 1;
end

%Display path in black and white
imagesc(greenPathMat)
set(gca,"YDir","normal")
hold on
colormap gray

for ii = 1:floor(nbSegments/2)
    %Vertical segments
    plot(polyshape([xStart(ii) xStart(ii) xEnd(ii) xEnd(ii)],...
        [yStart(ii) yEnd(ii) yEnd(ii) yStart(ii)]));

    %Horizontal segments
    plot(polyshape([xStart(ii) xStart(ii) xEnd(ii+1) xEnd(ii+1)],...
        [yStart(ii+1) yEnd(ii) yEnd(ii) yStart(ii+1)]));
end

%If last segment is vertical
if length(xStart) == length(xEnd)
    plot(polyshape([xStart(ii+1) xStart(ii+1) xEnd(ii+1) xEnd(ii+1)],...
        [yStart(ii+1) yEnd(ii+1) yEnd(ii+1) yStart(ii+1)]));
end

If you plot them one by one it looks like this. Is this what you want?