JPS_search_matlab-main.zip

function nodes = single_ori_expand(node,map,MAX_X,MAX_Y,dir) %右上角 1 ，右下角 2， 左下角 3，左上角 4 nodes = []; % |x|y|force neighbor flag | force neigbor1 x | force neighbor1 y| force neighbour2 x| force neighbour2 y % | parent_dir| nodes_count = 0; flag_dia_plus = 0; %表明dia递增没有 flag_dia_reach_bound = 0; %表明dia有没有边界 x_idx = round(node(1,1)); y_idx = round(node(1,2)); x0 = round(node(1,1)); y0 = round(node(1,2)); flag_dia_stop = 0; %% %右上角 if(dir == 1) while(1) %x+ expansion 拓展停止条件优先级：存在force；到达目标；遇到障碍物；到达边界。 for x_exp = x_idx : MAX_X %情况 -1 到达目标点 if(obs_map(x_exp,y_idx,map,MAX_X,MAX_Y) == 0 ) new_node= []; new_node(1,1) = x_exp; new_node(1,2) = y_idx; new_node(1,3) = 0; new_node(1,4) = 0; new_node(1,5) = 0; new_node(1,6) = 0; new_node(1,7) = 0; new_node(1,8) = 1; if(flag_dia_plus ==1 ) parent_dia_node = []; parent_dia_node(1,1) = x_idx; parent_dia_node(1,2) = y_idx; parent_dia_node(1,3) = 0;%在is extingsing函数里更改 parent_dia_node(1,4) = 0; parent_dia_node(1,5) = 0; parent_dia_node(1,6) = 0; parent_dia_node(1,7) = 0; parent_dia_node(1,8) = 1; flag_dia_stop = 1; [sign,nodes] = is_extising(nodes,parent_dia_node); if(sign==0) nodes_count = nodes_count + 1; nodes(nodes_count,:) = parent_dia_node; end else %说明还未对角移动，如果发生对角移动，就不加入，不然会重复拓展，而且也无法直接到达啊 nodes_count = nodes_count + 1; nodes(nodes_count,:) = new_node; end break; end %情况 0 x+已经到达边界，或者下一步被堵住，无论是上面什么情况，都无法拓展 if(x_exp == MAX_X || obs_map(x_exp+1,y_idx,map,MAX_X,MAX_Y)==-1) break; end %情况1 有force neighbour，表现：x_exp的上下存在障碍物，但对应force %neighbor没障碍物，且原本的有效neighbor也没有障碍物(情况0已讨论) if(obs_map(x_exp,y_idx+1,map,MAX_X,MAX_Y)== -1 && obs_map(x_exp+1,y_idx+1,map,MAX_X,MAX_Y)~= -1 &&(x_exp ~= x_idx)) new_node= []; new_node(1,1) = x_exp; new_node(1,2) = y_idx; new_node(1,3) = 1; new_node(1,4) = x_exp+1; new_node(1,5) = y_idx+1; new_node(1,6) = 0; new_node(1,7) = 0; new_node(1,8) = 1; if(flag_dia_plus ==1 ) %父对角拓展节点也需要加入,此时的对角节点 parent_dia_node = []; parent_dia_node(1,1) = x_idx; parent_dia_node(1,2) = y_idx; parent_dia_node(1,3) = 0;%在is extingsing函数里更改 parent_dia_node(1,4) = 0; parent_dia_node(1,5) = 0; parent_dia_node(1,6) = 0; parent_dia_node(1,7) = 0; parent_dia_node(1,8) = 1; flag_dia_stop = 1; [sign,nodes] = is_extising(nodes,parent_dia_node); if(sign==0) nodes_count = nodes_count + 1; nodes(nodes_count,:) = parent_dia_node; end else if(obs_map(x_exp,y_idx-1,map,MAX_X,MAX_Y)== -1 && obs_map(x_exp+1,y_idx-1,map,MAX_X,MAX_Y)~= -1) new_node(1,3) = 2; new_node(1,6) = x_exp+1; new_node(1,7) = y_idx-1; end nodes_count = nodes_count + 1; nodes(nodes_count,:) = new_node; end break; %x 拓展结束? end if(obs_map(x_exp,y_idx-1,map,MAX_X,MAX_Y)== -1 && obs_map(x_exp+1,y_idx-1,map,MAX_X,MAX_Y)~= -1&&(x_exp ~= x_idx)) new_node= []; new_node(1,1) = x_exp; new_node(1,2) = y_idx; new_node(1,3) = 1; new_node(1,4) = x_exp+1; new_node(1,5) = y_idx-1; new_node(1,6) = 0; new_node(1,7) = 0; new_node(1,8) = 1; if(flag_dia_plus ==1 ) parent_dia_node = []; parent_dia_node(1,1) = x_idx; parent_dia_node(1,2) = y_idx; parent_dia_node(1,3) = 0;%在is extingsing函数里更改 parent_dia_node(1,4) = 0; parent_dia_node(1,5) = 0; parent_dia_node(1,6) = 0; parent_dia_node(1,7) = 0; parent_dia_node(1,8) = 1; flag_dia_stop = 1; [sign,nodes] = is_extising(nodes,parent_dia_node); if(sign==0) %当xy方向都有force，这个点会被重复加 nodes_count = nodes_count + 1; nodes(nodes_count,:) = parent_dia_node; end else if(obs_map(x_exp,y_idx+1,map,MAX_X,MAX_Y)== -1 && obs_map(x_exp+1,y_idx+1,map,MAX_X,MAX_Y)~= -1) new_node(1,3) = 2; new_node(1,6) = x_exp+1; new_node(1,7) = y_idx+1; end nodes_count = nodes_count + 1; nodes(nodes_count,:) = new_node; end break; end end %y+ expansion for y_exp = y_idx : MAX_Y %情况 -1 到达目标点 if(obs_map(x_idx,y_exp,map,MAX_X,MAX_Y) == 0) new_node= []; new_node(1,1) = x_idx; new_node(1,2) = y_exp; new_node(1,3) = 0; new_node(1,4) = 0; new_node(1,5) = 0; new_node(1,6) = 0; new_node(1,7) = 0; new_node(1,8) = 1; if(flag_dia_plus ==1 ) parent_dia_node = []; parent_dia_node(1,1) = x_idx; parent_dia_node(1,2) = y_idx; parent_dia_node(1,3) = 0;%在is extingsing函数里更改 parent_dia_node(1,4) = 0; parent_dia_node(1,5) = 0; parent_dia_node(1,6) = 0; parent_dia_node(1,7) = 0; parent_dia_node(1,8) = 1; flag_dia_stop = 1; [sign,nodes] = is_extising(nodes,parent_dia_node); if(sign==0) nodes_count = nodes_count + 1; nodes(nodes_count,:) = parent_dia_node; end else nodes_count = nodes_count + 1; nodes(nodes_count,:) = new_node; end break; end %情况 0 x+已经到达边界，或者下一步被堵住，无论是上面什么情况，都无法拓展 if(y_exp == MAX_Y || obs_map(x_idx,y_exp+1,map,MAX_X,MAX_Y)==-1) break; end %情况1 有force neighbour，表现：y_exp的左右存在障碍物，但对应force %neighbor没障碍物，且原本的有效ne