我能知道我的Python代码LineOfSight的错误在哪里吗?

我想检查一下 视线 算法首先工作,直到应用到我的寻路算法。下面是 Matlab code 属于 *LineOfSight* . 下面我放了Matlab和python的代码。根据下面的图片 obstacle yellow square shape ,和 starting point in a green circle and the ending point in red circle , 如果两点之间的线是红色的,那意味着没有视线,但是如果这条线是绿色的,那就有视线 . 当我在python中应用相同的思想时,它不会检查障碍(3,2)。有什么帮助吗?

视线:

%Check if there is free line of sight between two cells with coordinates
%(x1,y1) and (x2,y2) on grid E. Returns 1 if there is line of sight, 0 otherwise

%From:
%Daniel, Nash - Theta star, any-angle path planning on grids
%Modification:
%Inverted x and y to account for our grid coordinate system
%Check if the evaluation of E remains within the grid limits

%Limitation:
%Allows a straight line to pass between diagonally touching blocked cells


function sight=line_sight(E,h,y1,x1,y2,x2,sizeE)


%Size of environment matrix
y_size=sizeE(1);
x_size=sizeE(2);


%Distance
dy=y2-y1;
dx=x2-x1;

if dy<0
    dy=-dy;
    sy=-1;
else
    sy=1;
end

if dx<0
    dx=-dx;
    sx=-1;
else
    sx=1;
end


%Initialize
sight=1;

f=0;
if dy>=dx
    while y1~=y2
        f=f+dx;
        if f>=dy && 0<y1+(sy-1)/2 && y1+(sy-1)/2<=y_size && 0<x1+(sx-1)/2 && x1+(sx-1)/2<=x_size
            if E(y1+(sy-1)/2,x1+(sx-1)/2)>=h
                sight=0;
                return
            end
            x1=x1+sx;
            f=f-dy;
        end
        if 0<y1+(sy-1)/2 && y1+(sy-1)/2<=y_size && 0<x1+(sx-1)/2 && x1+(sx-1)/2<=x_size && f~=0 && E(y1+(sy-1)/2,x1+(sx-1)/2)>=h
            sight=0;
            return
        end

        if 0<y1+(sy-1)/2 && y1+(sy-1)/2<=y_size && 1<x1 && x1<=x_size && dx==0 && E(y1+(sy-1)/2,x1)>=h && E(y1+(sy-1)/2,x1-1)>=h
            sight=0;
            return
        end

        y1=y1+sy;
    end
else
    while x1~=x2
        f=f+dy;
        if f>=dx && 0<y1+(sy-1)/2 && y1+(sy-1)/2<=y_size && 0<x1+(sx-1)/2 && x1+(sx-1)/2<=x_size
            if E(y1+(sy-1)/2,x1+(sx-1)/2)>=h
                sight=0;
                return
            end
            y1=y1+sy;
            f=f-dx;
        end
        if 0<y1+(sy-1)/2 && y1+(sy-1)/2<=y_size && 0<x1+(sx-1)/2 && x1+(sx-1)/2<=x_size && f~=0 && E(y1+(sy-1)/2,x1+(sx-1)/2)>=h
            sight=0;
            return
        end
        if 1<y1 && y1<=y_size && 0<x1+(sx-1)/2 && x1+(sx-1)/2<=x_size && dy==0 && E(y1,x1+(sx-1)/2)>=h && E(y1-1,x1+(sx-1)/2)>=h
            sight=0;
            return
        end
        x1=x1+sx;
    end
end

主要:

%Test line of sight function

clear 
clc

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Manual grid generation

%Grid size
x_size=5;
y_size=5;

%Grid resolution
d_grid=1;

%Obstacle elevation
elev=1000;

E=zeros(y_size,x_size);

sizeE=[y_size x_size];

%Grid node elevation, discrete height
E(3:3,3:3)= elev;


%Starting point
x0=1;
y0=4;

%Arrival point
xend=5;
yend=1;

h=10;

sight=line_sight(E,h,y0,x0,yend,xend,sizeE);


if sight==1
    col='g';
else
    col='r';
end

%Grid side vectors
x=1:d_grid:x_size;
y=1:d_grid:y_size;


figure(1)
surf(x,y,E)
hold on
plot3(x0,y0,elev,'go')
plot3(xend,yend,elev,'ro')    
plot3([x0 xend],[y0 yend],[elev elev],col) 
axis tight
axis equal
view(0, 90);
% colorbar

在Python的视线下面:

x0 = 1
y0 = 4
xend = 5
yend = 1

def check(y1, x1, y2, x2):
    #print(f"y1={y1}, x1={x1}, y2={y2}, x2={x2}")

    dy = y2 - y1
    dx = x2 - x1

    if dy < 0:
        dy = -dy
        sy = -1
    else:
        sy = 1
    if dx < 0:
        dx = -dx
        sx = -1
    else:
        sx = 1

    f = 0
    #print(f"dy={dy}, dx={dx}, sy={sy}, sx={sx}")
    result = []
    if dy >= dx:
        while y1 != y2:
            f = f + dx
            if f >= dy:
                result.append((y1+(sy-1)/2, x1+(sx-1)/2))
                x1 = x1 + sx
                f = f - dy
            if f != 0:
                result.append((y1+(sy-1)/2, x1+(sx-1)/2))
            if dx == 0:
                result.extend(((y1+(sy-1)/2,x1), (y1+(sy-1)/2,x1-1)))
            y1 = y1 + sy

    else:
        while x1 != x2:
            f = f + dy
            if f >= dy:
                result.append((y1+(sy-1)/2, x1+(sx-1)/2))
                y1 = y1 + sy
                f = f - dx
            if f != 0:
                result.append((y1+(sy-1)/2, x1+(sx-1)/2))
            if dy == 0:
                result.extend(((y1,x1+(sx-1)/2), (y1-1,x1+(sx-1)/2)))
            x1 = x1 + sx

    return result

print(check(y0, x0, yend, xend))

[(3.0, 1.0), (2.0, 1.0), (2.0, 2.0), (3.0, 2.0) (2.0, 3.0), (1.0, 3.0), (1.0, 4.0)]