障碍物图的凸多边形分解

1.障碍物图的构造

%% 1.绘制地图大小
map_size = [0,300];          %地图大小             
obstacle_polys=[];            %障碍物多边形
obstacle_verts=[];
figure;
xlim([0 300]);
ylim([0 300]);
hold on;

%% 2.创建障碍物
obstacle_shape=[0,0;30,5;
    40,50;5,25];

obstacle_1=obstacle_shape+30;

obstacle_2=obstacle_shape+130;
obstacle_3=obstacle_shape;
obstacle_3(:,1)=obstacle_shape(:,1)+200;
obstacle_3(:,2)=obstacle_shape(:,2)+30;

obstacle_4=obstacle_shape;
obstacle_4(:,1)=obstacle_shape(:,1)+30;
obstacle_4(:,2)=obstacle_shape(:,2)+200;
obstacle_5=obstacle_shape+230;

for i=1:5
    obstacle_vert=eval(strcat('obstacle_',int2str(i)));
    obstacle_poly=polyshape(eval(strcat('obstacle_',int2str(i))));
    obstacle_verts=[obstacle_verts;obstacle_vert];     %所有障碍物顶点的集合
    obstacle_polys=[obstacle_polys;obstacle_poly];     %所有障碍物多边形的集合
end

2.将图形多次分割，将任意两个障碍物连接最短边，并判断是否穿过障碍物，如果未穿过则保留，连接障碍物与边框的最短边，如果未穿过障碍物则保留。

% verts_count=1;
min_edges=[];
% vert_nums=[];

for i=1:size(obstacle_polys,1)
    for j=i+1:size(obstacle_polys,1)
        min_edge=twopoly_min_edge(obstacle_polys(i,:),obstacle_polys(j,:));
        if edge_collision_check(min_edge,obstacle_polys)
            min_edges=[min_edges;min_edge];
%             if ~ismember(j,vert_nums)
%                 vert_nums=[vert_nums;j];
%             end
        end 
    end
end

%连接障碍物与最近边框
for i=1:size(obstacle_verts,1)
    min_edges=ex_boundary(obstacle_verts(i,:),min_edges);
end

final_min_edges=[];
for i=1:size(min_edges,1)
    min_edge=min_edges(i,:);
    if edge_collision_check(min_edge,obstacle_polys)
        final_min_edges=[final_min_edges;min_edge];
    end
end

3检测相交的边

i =1;
while(i<=size(final_min_edges,1))
    j = i+1;
    while(j<=size(final_min_edges,1))
        edge1=final_min_edges(i,:);
        edge2=final_min_edges(j,:);
        if iscro_edge(edge1,edge2) && ~endpoint(edge1,edge2)%相交是否在端点
            if distance_Cal(edge1(1,1:2),edge1(1,3:4))>distance_Cal(edge2(1,1:2),edge2(1,3:4)) 
                final_min_edges(i,:)=[];
                i=i-1;
                break;
            else
                final_min_edges(j,:)=[];
                j=j-1;
            end
            
        end
        j=j+1;
    end
    i=i+1;
end

4.将边缘的点用边连接

%将边缘改造为凸多边形,后续可改为再生成边的时候，就添加进去
boundary_polys=[];
boundary_poly1=[];
boundary_poly2=[];
boundary_poly3=[];
boundary_poly4=[];
for i=1:size(final_min_edges,1)
    edge1 = final_min_edges(i,:);
    if edge1(1)==0
        boundary_poly1 = [boundary_poly1;edge1(1:2)];
        continue;
    end
    if edge1(3)==0
        boundary_poly1 = [boundary_poly1;edge1(3:4)];
        continue;
    end
    
    if edge1(1)==300
        boundary_poly3 = [boundary_poly3;edge1(1:2)];
        continue;
    end
    if edge1(3)==300
        boundary_poly3 = [boundary_poly3;edge1(3:4)];
        continue;
    end
    
    if edge1(2)==0
        boundary_poly4 = [boundary_poly4;edge1(1:2)];
        continue;
    end
    if edge1(4)==0
        boundary_poly4 = [boundary_poly4;edge1(3:4)];
        continue;
    end
    
    if edge1(2)==300
        boundary_poly2 = [boundary_poly2;edge1(1:2)];
        continue;
    end
    if edge1(4)==300
        boundary_poly2 = [boundary_poly2;edge1(3:4)];
        continue;
    end
end

boundary_poly1 = [boundary_poly1;0,300;0,0]
boundary_poly2 = [boundary_poly2;300,300;0,300]
boundary_poly3 = [boundary_poly3;300,300;300,0]
boundary_poly4 = [boundary_poly4;300,0;0,0]

boundary_poly1 = arange(boundary_poly1);
boundary_poly2 = arange(boundary_poly2);
boundary_poly3 = arange(boundary_poly3);
boundary_poly4 = arange(boundary_poly4);

% for i=1:4
%     boundary_vert=eval(strcat('boundary_poly',int2str(i)));
%     boundary_poly=polyshape(boundary_vert);报错polyshape不能对两条首尾相连的直线
%     boundary_polys=[obstacle_polys;boudary_poly];     %所有障碍物多边形的集合
% end
% plot(boundary_polys);

5新建Edge类，并将边都录入Edge类

classdef Edge
    %UNTITLED 此处显示有关此类的摘要
    %   此处显示详细说明
    
    properties
        A;
        B;
        l;
        r
    end
    
    methods
        function obj = Edge(edge,lflag,rflag)
            %UNTITLED 构造此类的实例
            %   此处显示详细说明
            obj.A = edge(1:2);
            obj.B = edge(3:4);
            obj.l = lflag;
            obj.r = rflag;
        end
        
    end
end

%将所有的边重新录入Edge类
%1障碍物的边
allEdges = [];
for i=1:5
    obstacle_vert=eval(strcat('obstacle_',int2str(i)));
    for j =1:size(obstacle_vert,1)
        if j<size(obstacle_vert,1)
            edge1 = [obstacle_vert(j,:),obstacle_vert(j+1,:)];
        else
            edge1 = [obstacle_vert(j,:),obstacle_vert(1,:)];
        end
        Edge1 = Edge(edge1,1,0);
        allEdges = [allEdges ; Edge1];     %所有障碍物边的集合
    end
end

%2minedges
for i=1:size(final_min_edges,1)
    edge1 = final_min_edges(i,:);
    Edge1 = Edge(edge1,0,0);
    allEdges = [allEdges ; Edge1];     %所有新增边的集合
end

%3边框
for i=1:2
    boundary_poly=eval(strcat('boundary_poly',int2str(i)));
    for j =1:size(boundary_poly,1)-1
        edge1 = [boundary_poly(j,:),boundary_poly(j+1,:)];
        Edge1 = Edge(edge1,1,0);
        allEdges = [allEdges ; Edge1];     %所有边缘边的集合
    end
end

for i=3:4
    boundary_poly=eval(strcat('boundary_poly',int2str(i)));
    for j =1:size(boundary_poly,1)-1
        edge1 = [boundary_poly(j,:),boundary_poly(j+1,:)];
        Edge1 = Edge(edge1,0,1);
        allEdges = [allEdges ; Edge1];     %%所有边缘边的集合
    end
end

6.画出左旋多边形。

%无障碍物多边形
free_polys = [];
for i =1:size(allEdges,1)
    Edge1 = allEdges(i);
    if Edge1.l == 0
        free_poly = [Edge1.B]
        [number,angel,lr] = nextEdge(Edge1.A,Edge1.B,allEdges)
%         free_poly_angles = [];
%         free_poly_angles = [free_poly_angles;angle];
%         if(lr == 'l')
%             allEdges(number).l = 1; 
%         elseif(lr == 'r')
%             allEdges(number).r = 1; 
%         end

        while(number ~= i)
            if(lr == 'l')
                allEdges(number).l = 1
                free_poly = [free_poly;allEdges(number).B]
                [number,angel,lr] = nextEdge(allEdges(number).A,allEdges(number).B,allEdges)
            elseif(lr == 'r')
                allEdges(number).r = 1
                free_poly = [free_poly;allEdges(number).A]
                [number,angel,lr] = nextEdge(allEdges(number).B,allEdges(number).A,allEdges)
            end
%             free_poly_angles = [free_poly_angles;angle];
            
        end
        
        allEdges(number).l = 1
        free_polys = [free_polys;polyshape(free_poly)]
        
        
    end
    if Edge1.r == 0
        free_poly = [Edge1.A]
        [number,angel,lr] = nextEdge(Edge1.B,Edge1.A,allEdges)
        while(number ~= i )
            if(lr == 'l')
                allEdges(number).l = 1
                free_poly = [free_poly;allEdges(number).B]
                [number,angel,lr] = nextEdge(allEdges(number).A,allEdges(number).B,allEdges)
            elseif(lr == 'r')
                allEdges(number).r = 1
                free_poly = [free_poly;allEdges(number).A]
                [number,angel,lr] = nextEdge(allEdges(number).B,allEdges(number).A,allEdges)
            end
           
        end
        
        allEdges(number).r = 1
        free_polys = [free_polys;polyshape(free_poly)]
        
        
    end
end

7.拆分凹多边形

%拆分凹多边形
i=1;
while (i <=size(free_polys,1))
    s = size(free_polys(i).Vertices,1);
    for j = 1:s
        if j == 1
            A = free_polys(i).Vertices(s,:)-free_polys(i).Vertices(j,:);
            B = free_polys(i).Vertices(j+1,:)-free_polys(i).Vertices(j,:);
        elseif j == s
            A = free_polys(i).Vertices(j-1,:)-free_polys(i).Vertices(j,:);
            B = free_polys(i).Vertices(1,:)-free_polys(i).Vertices(j,:);
        else
            A = free_polys(i).Vertices(j-1,:)-free_polys(i).Vertices(j,:);
            B = free_polys(i).Vertices(j+1,:)-free_polys(i).Vertices(j,:);
        end
        if Angle(A,B)>=180
            %生成两个新多边形
            poly1 = [];
            poly2 = [];
            for k = 1:j
                poly1 = [poly1;free_polys(i).Vertices(k,:)];
            end
            if j<s-1
                for k = j+2:s
                    poly1 = [poly1;free_polys(i).Vertices(k,:)];
                end
                poly2 = [free_polys(i).Vertices(j,:);free_polys(i).Vertices(j+1,:);free_polys(i).Vertices(j+2,:)];
            elseif j==s
                poly1 = [poly1;free_polys(i).Vertices(2,:)];
                poly2 = [free_polys(i).Vertices(j,:);free_polys(i).Vertices(1,:);free_polys(i).Vertices(2,:)]
            elseif j==s-1
                poly2 = [free_polys(i).Vertices(j,:);free_polys(i).Vertices(j+1,:);free_polys(i).Vertices(1,:)]
            end
            
            free_polys = [free_polys;polyshape(poly1);polyshape(poly2)];
%             free_polys = [free_polys;polyshape(poly2)];
            free_polys(i) = [];
            i=i-1;
            break;
        end
    end
    i=i+1;
end

plot(free_polys)

8.取中心点，若两点所在的多边形有公共边，连接

%中心点
centers = [];
for i =1:size(free_polys,1)
    x=0;
    y=0;
    for j=1:size(free_polys(i).Vertices,1)
        x = x + free_polys(i).Vertices(j,1);
        y = y + free_polys(i).Vertices(j,2);
    end
    x = x/size(free_polys(i).Vertices,1);
    y = y/size(free_polys(i).Vertices,1);
    centers = [centers;x,y];
end

scatter(centers(:,1),centers(:,2),10,'r');

links = []
for i =1:size(free_polys,1)
    for j=i:size(free_polys,1)
        if comedge(free_polys(i),free_polys(j))
            edge=[centers(i,:),centers(j,:)];
            links = [links;edge];
        end
    end
    
end

links

for i = 1 :size(links,1)
    line([links(i,1),links(i,3)],[links(i,2),links(i,4)]);
end;