How to locate a collide segment of a post-smoothed path?

I am working on motion planning problem, and so fare i have generated irregular path and then prun it using Ramer–Douglas–Peucker algorithm later by utilizing piecewise cubic B-spline we could produce smoothed path. However, collision-freeness is not quarantee so that I am trying to check for a collision condition for a post-smoothed path. I have mangage to identify if there is a collision using the following code:

        env = map;
        % J = im2uint8( map ); 
        % env = imnoise( J ,'salt & pepper');
        v = pathSmooth;
        for ii = 1:length(v)-1
            if env(round(v(ii,1)), round(v(ii,2))) ~= 0
                disp('There is no intersection')
            else
                disp('There is intersection')
                pause;
            end
               end

My question, how to locate the first and last locations where the collision are detected based on the ii index and return them?

I have attached mat files for the used map as well as the resulted smoothed path for your reference as well as an image for the map, reduced path, and smoothed path.

Answer 1

[KSSV]

env = map;
% J = im2uint8( map );
% env = imnoise( J ,'salt & pepper');
v = pathSmooth;
iwant = zeros([],2) ; 
count = 0 ;
for ii = 1:length(v)-1
    if env(round(v(ii,1)), round(v(ii,2))) ~= 0
        disp('There is no intersection')
    else
        count = count+1 ; 
        iwant(count,:) = [round(v(ii,1)), round(v(ii,2))] ; 
        disp('There is intersection')        
    end
end

[YAAQOB AL-JAMALI]

thank you so much @KSSV, it is working perfectly.