5. Алгоритм навигации робота

cd( @__DIR__ )
include( "room_functions.jl" );
engee.script.@include( "2.6 Алгоритм навигации 3.ngscript" );
gr();

карта = проезд .* ones(14, 14)

изученная_карта = здесь_стена .* ones( size(карта) )
изученная_карта[ 2:end-1, 2:end-1] .= можно_ехать
изученная_карта[ 9, 10 ] = здесь_стена
изученная_карта[ 10, 10 ] = здесь_стена
изученная_карта[ 10, 9 ] = здесь_стена

пройденная_карта = посетили .* ones( size(карта) )
пройденная_карта[ 12, 11 ] = не_посещали

положение = CartesianIndex( 8, 8 )
направление = CartesianIndex( 1, 0 )

# поле отмечаем цифрой 0
wavefront = zeros( size(карта) )

# все точки, куда мы хотим попасть, отмечаем цифрой 1
wavefront[изученная_карта .== здесь_стена] .= -1
wavefront[(изученная_карта .!= здесь_стена) .& (пройденная_карта .== не_посещали)] .= 1

# точку, где мы находимся, отметим цифрой 2 и будем считать от неё
wavefront[ положение ] = 2
heatmap( wavefront', yflip=:true, c=:tab20c, clim=(-1,8), aspect_ratio=:equal, leg=:false )

точка_впереди = положение + направление
scatter!( [точка_впереди[1]], [точка_впереди[2]], shape=:xcross, mc=:white, markersize=3 )

# нанесем обозначения
vec_coords = CartesianIndices( size(wavefront) )
ax = first.( Tuple.(vec_coords[:]) )
ay = last.( Tuple.(vec_coords[:]) )
az = reshape( wavefront, 1, : )
for (x,y,z) in zip(ax,ay,az)
    annotate!( x, y, text("$(convert(Int64, round(z)))",7,:white) );
    # Знаете почему график не выводится? ну так вот...
end

plot!( )

периметр = vec_coords[ wavefront .== maximum(wavefront) ]
маршрут = []

for точка in периметр
    
    набор_направлений = [ направление,
                          поворот(направление, 90),
                          поворот(направление, -90),
                          поворот(направление, 180) ]
    
    for направление in набор_направлений
        
        if wavefront[ точка + направление ] == 1
            # Волшебно, мы нашли первую интересующую нас точку, можно заканчивать поиск
            push!( маршрут, CartesianIndex( точка + направление ))
            push!( маршрут, CartesianIndex( точка ))
            # и выйдем
            break;
        end;
        
        # Шагаем в ту точку, до которой еще не расчитали маршрут
        if wavefront[ точка + направление ] == 0
            if изученная_карта[ точка + направление ] != здесь_стена
                wavefront[ точка + направление ] = wavefront[ точка ] + 1
            end
        end
    end
    
end

heatmap( wavefront', c=:tab20c, clim=(-1,9), yflip=:true,
                        aspect_ratio=:equal, leg=:false )
scatter!( [точка_впереди[1]], [точка_впереди[2]], shape=:xcross, mc=:white, markersize=3 )
for (x,y,z) in zip(ax,ay,az)
    annotate!( x, y, text("$(convert(Int64, round(z)))",7,:white) );
end
plot!( )

маршрут = []
wavefront = zeros( size(карта) )
wavefront[изученная_карта .== здесь_стена] .= -1
wavefront[(изученная_карта .!= здесь_стена) .& (пройденная_карта .== не_посещали)] .= 1
wavefront[ положение ] = 2

for x in 1:100
    if length(маршрут) > 0 break; end;
    периметр = vec_coords[ wavefront .== maximum(wavefront) ]
    
    for точка in периметр
        if length(маршрут) > 0 break; end;
        
        набор_направлений = [ направление, поворот(направление, 90), поворот(направление, -90), поворот(направление, 180) ]
        
        for направление in набор_направлений
            
            if wavefront[ точка + направление ] == 1
                # Волшебно, мы нашли первую интересующую нас точку, можно заканчивать поиск
                push!( маршрут, точка + направление )
                push!( маршрут, точка )
                # и выйдем
                break;
            end;
            
            # Шагаем в ту точку, до которой еще не расчитали маршрут
            if wavefront[ точка + направление ] == 0
                if изученная_карта[ точка + направление ] != здесь_стена
                    wavefront[ точка + направление ] = wavefront[ точка ] + 1
                end
            end
        end
        
    end
end

for p in 1:500
    
    # первой точкой отсчета маршрута является точка, куда мы дожлны попасть
    последняя_точка = маршрут[end];
    
    соседние_точки = repeat( [последняя_точка], inner=4) .+ CartesianIndex.([(1,0), (0,1), (-1,0), (0,-1) ])
    соседние_точки = [ x for x in соседние_точки if x in vec_coords ]
    соседние_точки = [ x for x in соседние_точки if wavefront[x] >= 2 ]
    
    (_,i) = findmin( wavefront[ соседние_точки ] )
    
    # Если мы находимся вблизи точки старта робота, просто закончим составление маршрута. Не добавляем точку старта в маршрут
    if wavefront[соседние_точки[i]] == 2 break;
    else push!( маршрут, соседние_точки[i] ); end
    
end

маршрут

heatmap( wavefront', yflip=:true, c=:tab20c, aspect_ratio=:equal, leg=:false )
scatter!( [точка_впереди[1]], [точка_впереди[2]], shape=:xcross, mc=:white, markersize=3 )
scatter!( [Tuple(положение)[1]], [Tuple(положение)[2]], ms=15, c=:white )
xx = first.(Tuple.(маршрут))
yy = last.(Tuple.(маршрут))
plot!( xx, yy, c=:cyan, lw=3, size=(500,500) )
for (x,y,z) in zip(ax,ay,az)
    annotate!( x, y, text("$(convert(Int64, round(z)))",9,:limegreen) );
end
plot!()