Bresenham2d 算法 （matlab)

原理部分参考 布莱森汉姆算法原理介绍

matlab程序

clc;
clear;
close all;

%% 测试二维bresenham algorithm

%读取图像
ImpRgb = imread('map.png');
global Imp;
Imp = rgb2gray(ImpRgb);
%使得图像占满整个figure窗口
imshow(Imp,'border','tight','initialmagnification','fit');
%set(gcf,'position',[0,0,60,80]);
size(Imp,1)  %height
size(Imp,2)  %width
hold on;
start_pt = [1,10];
goal_pt = [620,460];
plot(start_pt(1,1),start_pt(1,2),'ro','MarkerSize',5,'MarkerFaceColor','r');
plot(goal_pt(1,1),goal_pt(1,2),'go','MarkerSize',5,'MarkerFaceColor','g');  %x,y取值 返回y,x
hold on;
%plot(start_pt,goal_pt);
hold on;
vPts = bresenham_2d(start_pt, goal_pt);  
plot(vPts(:,2), vPts(:,1),'b*');
%plot (pt(1,1), pt(1,2), 'r*');  %x,y
hold on;



function vPts = bresenham_2d(start_pt,goal_pt)
global Imp;
dx = abs(goal_pt(1,1) - start_pt(1,1));
dy = abs(goal_pt(1,2) - start_pt(1,2));
x = start_pt(1,1);
y = start_pt(1,2);

if (goal_pt(1,1) > start_pt(1,1))
    s1 = 1;
elseif (goal_pt(1,1) < start_pt(1,1))
    s1 = -1;
else
     s1 = 0;   
end

if (goal_pt(1,2) > start_pt(1,2))
    s2 = 1;
elseif (goal_pt(1,2) < start_pt(1,2))
    s2 = -1;
else
    s2 = 0;
end

if (dy > dx)
    temp = dx;
    dx = dy;
    dy = temp;
    interchange = 1;
else
    interchange = 0;
end
interchange;
p = 2*dy - dx;
vPts = [];

for i = 1:dx-1
    if (p >= 0)
        y = y + s2;
        x = x + s1;
        if (interchange == 0)
            p = p + 2*(dy - dx);
        else
            p = p + 2*(dy - dx);
        end
    else
        if (interchange == 0)
            x = x + s1;
            p =  p + 2*dy;
        else
            y = y + s2;
            p = p + 2*dy;
        end
        %p =  p + 2*dy;
    end
    
    if (Imp(y,x) == 0)
        disp('obs!');
        return;
    end
    
    vPts = [vPts; y,x];
end

end

图像（其实就是在win下二维画图工具里面随便生成一张图片)

 结果：