I am having a path that is resulted from RRT-connect, then it is pruned using Ramer–Douglas–Peucker. Once we get the path with reduced waypoints we applied cubic B-spline that is a built-in MATLAB function [pathSmooth, qd, qdd, pp] = bsplinepolytraj(cpts,tpts,tvec); The smoothed path we check wherether it is a collisiono-free or not using the function [cc, iwant, computationTime3] = collisionChecking(map, pathSmooth); Once collsision is detected, we need to to refine the collide path. The refinment function is [pathReducedRefined] = reformCollideSegment(iwant, pathReduced);, and the process will continue while iwant is not empty. The program could not produced a path that is collision-free and goes for infinity. The issue is: refinement cannot achieved due to some reasons which are not clear to me. Can anyone suggest something to tackle such issue

I have attached the following:files

1. Initial path before pruning name as
2. Pruned path named as 'pathReduced.mat'
3. Smoothed path (that is colliding) named as 'smoothedPath.mat'
4. the map that we are working with name as 'environmentMatrix.mat'

The cod that we used as follow:

%% Path reduction
% reducepoly function Reduce density of points in ROI using Ramer–Douglas–Peucker
% algorithm.
% P_reduced = reducepoly(P,tolerance) reduces the density of points in array
% P, where tolerance specifies how much a point can deviate from a straight
% line.
tolerance = 0.03;
pathReduced = reducepoly(path,tolerance);
% Check the number of row for pathReduced (must be at least 4 rows)
[row, column] = size(pathReduced);
if row < 4
    pathReduced = path;
end

%% Path smoothing sing B-spline smoothing function
% Inputs:
% cpts = pathReduced';
cpts = path';
% Calling Smoothing Funtion
[pathSmooth, pathLengthS, qd, qdd, pp] = BSpline(cpts);

%% Collision detection for the smoothed path and refinment
% Collision detection is performed by discrete samples
[cc, iwant, computationTime3] = collisionChecking(map, pathSmooth);
keyboard;
while ~isempty(iwant)
    [pathReducedRefined] = reformCollideSegment(iwant, pathReduced);
    pathReduced = pathReducedRefined;
    cpts = pathReduced';
    % Calling Smoothing Funtion
    [pathSmooth, ComputationTime2, pathLengthS] = BSpline(cpts);
    [cc, iwant, computationTime3] = collisionChecking(map, pathSmooth);
end

function [cc, iwant, Time, collideIndices] = collisionChecking(map, pathSmooth)
% % FUNCTION COLLISIONCHECKING test for collision occurence.
% %
% % Inputs:
% %   map         - logical map of size 500x500
% %   pathSmooth  - smoothed path in cartesian form produced using
% %                   [pathSmooth, pathLengthS] = BSpline(cpts).
% %
% % Outputs:
% %   cc          - flage of collision occurence: logic 1 represents
% %   collision, while logic 0 represetns collision-free condition
% %

tic;

v = pathSmooth;
env = map;
cc = true;
% Initialize arrays for storing intersections and their indices
iwant = zeros([],2) ;           % To store the coordinates of intersections
collideIndices = zeros([]);   % To store the indices in v that correspond to iwant
count = 0;
% Loop through the path v
for ii = 1:length(v)-1
    % Check if the point in v is in an obstacle-free area (intersection)
    if env(round(v(ii,1)), round(v(ii,2)))  % Use (y,x) indexing for the environment
        cc = false;
        %         disp('There is no intersection');
    else
        count = count + 1;
        
        iwant(count,:) = [round(v(ii,2)), round(v(ii,1))];  % Store the point in iwant
        collideIndices(count) = ii;                         % Store the index from v
        cc = true;
        %         disp('There is intersection');
    end
end

Time = toc;
end