How to find nearest point then add midpoint between nearest and consecutive point for two different pointsets?

I am trying to test for collision occurence and trying to overcome it by locating the collide segements then adding midepoint between P_i and P_i+1 so that the post-smoothed path is collision-free. I have so fare locate the collision location with the hlep from KSSV, and to find the nearest point for points with collision i have utilized built-in function MATLAB [k,dist] = dsearchn(pathReduced,iwant) to return the indeces of the nearest points. Code is shown bellow:

%% Collision Checking
env = map;
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 collision')
    else
        count = count+1 ; 
        iwant(count,:) = [round(v(ii,1)), round(v(ii,2))] ; 
        disp('There is collision')        
    end
end

%% Find the nearest point for each point in 'iwant'relavent to 'pathReduced'
PQ = iwant;
P = pathReduced;
[k,dist] = dsearchn(P,PQ)

What i am trying to do now is adding midepoints between the nearest point in P and the consecutive point, so that when i check for collision supposedly no collision will occure.

**I have attached the map file, pathSmooth, iwant, and pathReduced.

[KSSV]

If you know the points, you can find mid point right? Just mean of x and y.

[YAAQOB AL-JAMALI]

yep i do know, but i am stuck on how to update the array pathReduced, and shall i test again for collision and repeate the midepoitn insertion untill there is no collision?