Algorithm.m

%% 
clc;
clear;
close all
%% сохранение окружения
save('workspace5');
%% загрузка окружения
load('workspace1');
%%
max_vertices = 200;
%% Параметры алгоритма по умолчанию
T = -0.1; % ограничение на время локального перехода
T_max = 0.5; % ограничение на время, хранимое в сеточной функции
N = 50; % размер сеточной функции
max_vertices = 200; % максимальное число вершин в дереве
K_start = 20; % начальный ресурс
x_init = [8;10]; % начальная точка
A = [1,1;0,1];%
B = [2,3;3,2];% параметры в системе уравнений
f = [1;0];%
eta = 2;% радиус ближайшей
dimension = 2;
m = 2;
n_corput = 50;
mode = 1;
%% Параметры системы
dimension = input('Введите размерность пространства ');
%dimension = 2;
%m = 2;
m = input('Введите размерность вектора управления ');
if numel(m) ~= 1 || m < 0 || m ~= floor(m)
    error('Размерность вектора управления должна быть натуральным числом');
end
T = input('Введите время локального перехода(отрицательное): ');
T_max = input('Введите ограничение на время в сетке: ');
N = input('Введите размер сетки: ');
eta = input('Введите радиус: ');
max_vertices = input('Введите максимальное количество вершин в дереве: ');
K_start = input('Введите начальный ресурс системы: ');
n_corput = input('Введите количество элементов в последовательности ван дер Корпута: ');
x_init = zeros(dimension, 1);
disp('Введите начальную точку:')
for i=1:dimension
    x_init(i) = input('');
end
disp('Введите матрицу A(построчно):');
A = zeros(dimension);
for i = 1:dimension
    for j = 1:dimension
        A(i,j) = input('');
    end
end
disp('Введите матрицу B(построчно):');
B = zeros(dimension, m);
for i = 1:dimension
    for j = 1:m
        B(i,j) = input('');
    end
end
disp('Введите матрицу f(построчно):');
f = zeros(dimension, 1);
for i = 1:dimension
    f(i) = input('');
end
mode = input(['Введите режим подбора лучшего вектора энергии:\n0 - по наибольшей энергии для наименьшего времени;\n' ...
    '1 - в среднем(интегральном смысле).\n']);
mode = mode > 0;
%% ввод конфигурации системы
space = space_input(dimension);
goal = space_input(dimension);
obs = obs_input();
r = space{1};
side = r{4};
V_space = prod(side);% объем X
V_unit_sphere = pi^(dimension/2) / gamma(dimension/2 + 1); % объем единичной сферы
opt_const = 2 * ((1 + 1/dimension) * V_space / V_unit_sphere) ^ (1/dimension);
% оптимальная константа в определении радиуса ближайшей
%% моя реализация
rng(16);
[tr, leaf, ans_T, t_span, times, iters] = rrt(x_init, K_start, ...
                                    space, ...
                                    obs, goal, T, ...
                                    T_max, N, eta, opt_const, max_vertices, ...
                                    A, B, f, n_corput, 0);
disp(['Всего проведено ', num2str(iters), ' итераций.']);
disp(['Число вершин в построенном дереве: ', num2str(length(tr))]);
if ~isempty(leaf)
    disp('Целевое множество достигнуто');
    disp('Время быстродействия');
    disp(ans_T);
else
    disp('Целевое множество не достигнуто');
end
disp(['Суммарное время работы функции: ', num2str(sum(times))]);
%% реализация базового алгоритма
rng(16);
[tr_def, leaf_def, times_def, iters_def]=default_rrt(x_init, space, ...
                                    obs, goal, eta, opt_const, max_vertices, n_corput);
disp(['Всего проведено ', num2str(iters_def), ' итераций.']);
disp(['Число вершин в построенном дереве: ', num2str(length(tr_def))]);
if ~isempty(leaf_def)
    disp('Целевое множество достигнуто');
    disp('Время быстродействия');
    disp(tr_def(leaf_def).cost);
else
    disp('Целевое множество не достигнуто');
end
disp(['Суммарное время работы функции: ', num2str(sum(times))]);

%% построение графиков
clc;
close all;
hold on;
visualization(space, obs, goal, tr, leaf, ans_T, t_span, A, B, f, x_init);
%% построение графика для базового алгоритма
clc;
close all;
hold on;
visualization_base(space, obs, goal, tr_def, leaf_def);
%% functions
%% проверка введенных параметров системы
function check_params(x_init, K_start, space, obs, T, T_max, N, eta, max_iters, ...
                      n_corput, mode)
    if ~in_space(x_init', space)
        error('Начальная точка не лежит в указанном пространстве.');
    end
    if ~obs_free(x_init, obs)
        error('Начальная точка не лежит в препятствии.');
    end
    if K_start < 0
        error('Начальный свободный ресурс должен быть положительным.');
    end
    if T > 0
        error('Параметр T должен быть отрицательным.');
    end
    if T_max <= 0
        error('Параметр T_max должен быть положительным.');
    end
    if N <= 0 || N ~= round(N)
        error('Параметр N должен быть натуральным числом.');
    end
    if eta <= 0
        error('Параметр eta должен быть положительным.');
    end
    if max_iters <= 0 || max_iters ~= round(max_iters)
        error('Параметр max_iters должен быть натуральным числом.');
    end
    if n_corput <= 0 || n_corput ~= round(n_corput)
        error('Параметр n_corput должен быть натуральным числом.');
    end
    if mode ~= 0 && mode ~= 1
        error('Параметр mode должен быть равен 0 или 1.');
    end
end
function check_controlability(A,B)
% проверка вполне управляемости системы
    n = size(A,1);
    C = zeros(n, n * size(B,2));
    mult = eye(size(A));
    for i=1:n
        C(:,(i-1) * size(B,2) + 1:i*size(B,2)) = mult * B;
        mult = mult * A;
    end
    if rank(C) < n
        error('Система не является вполне управляемой.');
    end
end

%% конфигурация системы
function space = space_input(d)
    % Предполагается ввод прямоугольника
    % Функция также используется для ввода целевого множества
    space = cell(1,1);
    left = zeros(d,1);
    disp('Введите левую нижнюю вершину(d * 1): ')
    for i=1:d
        left(i) = input('');
    end
    side = -1.*ones(d,1);
    while 1
        disp('Введите стороны');
        for i=1:d
            side(i) = input('');
        end
        if all(side > 0)
            break;
        else
            disp('Стороны должны быть положительными');
        end
    end
    pts = [left, left + [0;side(2);zeros(d - 2,1)],left + side, left + [side(1); 0;zeros(d - 2,1)]];
    [left, pts, angles_arr] = polar_sort(pts);
    space{1} = {left, pts, angles_arr, side};
end
function obs = obs_input()
    % каждое препятствие предполагается выпуклым
    while 1
        N = input('Введите количество препятствий: ');
        N = floor(N);
        if numel(N) ~= 1
            disp('Введите одно число: ');
        elseif N <= 0
            disp('Введите одно натуральное число: ');
        else
            break;
        end
    end
    obs = cell(1, N);
    for i=1:N
        n = input(['Введите количество вершин у ', num2str(i), '-го препятствия: ']);
        pts = zeros(2,n);
        disp('Введите вершины многоугольника (каждую координату на новой строке): ');
        for j = 1:n
            point = zeros(2,1);
            for k=1:2
                point(k) = input('');
            end
            pts(:,j) = point;
        end
        [center, pts, angles_arr] = polar_sort(pts);
        obs{i} = {center, pts, angles_arr};
    end
end

function [center, pts, angles]=polar_sort(pts)
% сортируем точки по углу относительно самой левой нижней вершины
    [~, I] = min(pts(1,:));
    [~, idx] = min(pts(2,I));
    center = pts(:, idx);% самая нижняя из самых левых точек
    dirs = [pts(:, 1:idx - 1), pts(:, idx + 1:end)] - center;
    angles = zeros(1, size(dirs, 2));
    mask = dirs(1,:) == 0;
    angles(mask) = pi / 2;
    angles(mask ~= 1) = atan(dirs(2, mask ~= 1) ./ dirs(1, mask ~= 1));
    [angles, I] = sort(angles);
    pts = dirs(:, I) + center;
end


function idx = bin_search(angles_arr, angle_)
    % бинарный поиск индекса i такого, что angles(i)<=angle<angles(i+1)
    % точно знаем, что angles(1)<=angle<=angles(end)
    left = 1;
    right = length(angles_arr);
    while left < right
        mid = floor((left + right + 1) / 2);
        if angle_ >= angles_arr(mid)
            left = mid;
        else
            right = mid - 1;
        end
    end
    idx = left;
end


%% Отрисовка
function visualization(space, obs, goal, tr, leaf, ans_T, t_span, A, B, f, x_init, is_energy)
    if ~isempty(leaf) && is_energy
        subplot(1,2,1);
    end
    
    for i=1:length(obs)
        a = obs{i};
        center = a{1};
        pts = a{2};
        pts = [center, pts];
        fill(pts(1,:),pts(2,:),'y');
        hold on;
    end
    title('Конфигурация системы');
    a = goal{1};
    center = a{1};
    pts = a{2};
    pts = [center, pts];
    fill(pts(1,:),pts(2,:),'g');
    plot_tree(tr, leaf, ans_T, t_span, A, B, f, is_energy);
    if ~isempty(leaf) && is_energy
        subplot(1,2,1);
    end
    plot(x_init(1), x_init(2), 'MarkerFaceColor', 'r', 'Marker', 'p', 'MarkerSize', 12);
    grid on;
    axis equal;
    a = space{1};
    center = a{1};
    side = a{4};
    xlim([center(1), center(1) + side(1)]);
    ylim([center(2), center(2) + side(2)]);
    if ~isempty(leaf) && is_energy
        subplot(1,2,2);
        grid on;
    end
end
function plot_tree(tr, leaf, ans_T, t_span, ...
                    A, B, f, is_energy)
    [~, Q_mat] = Q(A, B, t_span);
    [~, R_matrix] = R(A, f, t_span);
    [~, expon_A] = exp_A(A, t_span);
    if ~isempty(leaf)
        plot_one_branch(tr, leaf, ans_T, ...
                    A, B, f, Q_mat, R_matrix, expon_A, t_span, 1, 'k', 2, is_energy);
    end
    for i=1:length(tr)
        if ~isempty(leaf) && (i == leaf)
            continue
        end
        if isempty(tr(i).children)            
            idx = find(tr(i).K_t >= 0, 1);
            plot_one_branch(tr, i, tr(i).t(idx), ...
                A, B, f, Q_mat, R_matrix, expon_A, t_span, 0,'b', 0.5, is_energy);
        end
    end
end

function plot_one_branch(tr, leaf, ans_T, ...
                 A, B, f, Q_mat, R_matrix, expon_A, t_span, isopt, color, width, is_energy)
% Рисует траекторию от вершины tr(leaf) до корня дерева
    i = leaf;
    t_cur = ans_T;
    t_arr = tr(1).t;
    while i ~= 1
        idx_v = tr(i).parent;
        tr_v = tr(idx_v);
        tr_y = tr(i);
        K_y = tr_y.K_t;
        K_v = tr_v.K_t;
        K_t = tr_y.K_loc;
        t = t_cur + t_span; 
        idx_star = -1;
        K_s  = K_y(t_arr == t_cur);
        for r=1:length(t)
            k = find(t_arr == t(r), 1);
            if ~isempty(k)
                if K_v(k) - K_t(r)-K_s == 0
                    idx_star = r;
                    break;
                end
            end
        end
        t_star = t_span(idx_star);
        K_star = K_t(idx_star);
        [~, q] = numerator(tr_v.x, tr_y.x, expon_A, R_matrix, t_span);
        c = q(idx_star,:);
        Q_matrix = Q_mat(:,:,idx_star);
        [~, l_star] = find_maximum_k(c, Q_matrix);
        [~, x] = ode45(@(t,x) eq_4(t, x, A, B, f, ...
                         K_star, l_star, Q_matrix, idx_star, ...
                         t_span), [t_span(end), t_star], tr_y.x);
        x(end, :) = tr_v.x';
        if is_energy
            subplot(1,2,1);
        end
        hold on;
        plot(x(:, 1)', x(:, 2)', color, 'Marker', 's', 'MarkerFaceColor', 'm', ...
            'MarkerIndices', [1, length(x(:, 1))], 'LineWidth', width, ...
            'MarkerSize', 3);
        if is_energy
            if isopt
                subplot(1,2,2);
                hold on;
                K_last = K_y(t_arr == t_cur);
                K_start = K_v(t_arr == t_cur + t_star);
                plot([t_cur + t_star, t_cur],[K_start, K_last],'b');
                title('Расход энергии');
            end
        end
        i = idx_v;
        t_cur = t_cur + t_star;
    end
end

function visualization_base(space, obs, goal, tr, leaf)
    hold on;
    title('Конфигурация системы');
    for i=1:length(obs)
        a = obs{i};
        center = a{1};
        pts = a{2};
        pts = [center, pts];
        fill(pts(1,:),pts(2,:),'y');
    end
    a = goal{1};
    center = a{1};
    pts = a{2};
    pts = [center, pts];
    fill(pts(1,:),pts(2,:),'g');
    for i=1:length(tr)
        if isempty(tr(i).children)
            plot_tree_base(tr, leaf == i, i);
        elseif ~isempty(leaf)
            if leaf == i
                plot_tree_base(tr, leaf == i, i);
            end
        end
    end
    plot(tr(1).x(1), tr(1).x(2), 'MarkerFaceColor', 'r', 'Marker', 'p', 'MarkerSize', 12);
    grid on;
    axis equal;
    a = space{1};
    center = a{1};
    side = a{4};
    xlim([center(1), center(1) + side(1)]);
    ylim([center(2), center(2) + side(2)]);
end
function plot_tree_base(tr, isopt, idx)
    while idx ~= 1
        parent = tr(idx).parent;
        x_1 = tr(idx).x;
        x_2 = tr(parent).x;
        x = [x_1,x_2];
        hold on;
        if isopt
            plot(x(1,:), x(2,:), 'k', 'Marker', 's', 'MarkerFaceColor', 'm', ...
            'MarkerIndices', [1, length(x(:, 1))], 'LineWidth', 2, ...
            'MarkerSize', 3);
        else
            plot(x(1,:), x(2,:), 'b', 'Marker', 's', 'MarkerFaceColor', 'm', ...
            'MarkerIndices', [1, length(x(:, 1))], 'LineWidth', 0.5, ...
            'MarkerSize', 3);
        end
        idx = parent;
    end
end


%% Основная функция

function [tr, leaf, T_ans, t_span, times, i]=rrt(x_init, K_start, space, ...
                                    obs, goal, T, T_max, ...
                                    N, eta, opt_const, max_iters, A, B, f, n_corput, mode)
    %tr - полученное дерево вершин;
    %leaf - вершина, для которой достигнуто лучшее время быстродействия;
    %если целевое множество не достигнуто, то пустой массив;
    %T_ans - время быстродействия
    %times - массив времен, за которые присоединяются новые вершины
    check_params(x_init, K_start, space, obs, T, T_max, N, eta, max_iters, ...
                      n_corput, mode);
    check_controlability(A,B);
    v_writer = VideoWriter("myKRRT.mp4", 'MPEG-4');
    v_writer.FrameRate = 20;
    v_writer.Quality = 100;
    open(v_writer);
    step = T_max / N;% Чтобы шаг был постоянным
    t_span = 0:step:T_max;
    if t_span(end) < T_max
        t_span = [t_span, t_span(end) + step];
    end
    N = length(t_span);
    p = struct('x', x_init, 't', t_span, ...
                'K_t', K_start * ones(1,N), 'parent', [], 'children', [], 'K_loc', []);
    leaf = [];
    
    T_ans = T_max;
    tr = p;
    t_span = 0:-step:T; % Согласованность шагов
    if t_span(end > T)
        t_span = [t_span, t_span(end) - step];
        T = t_span(end) - step;
    end
    t_span = t_span(end:-1:1);
    [~, Q_mat] = Q(A, B, t_span);
    [~, expon_A] = exp_A(A, t_span);
    [~, R_matrix] = R(A, f, t_span);
    times = zeros(1, max_iters);
    i = 1;
    limit_iters = 2 * max_iters;
    while(length(tr) < max_iters)
        disp(['текущая итерация', num2str(i)]);

        tic();
        x_new = Sampling(space, obs);
        idx_nearest = Nearest(tr, x_new);
        x_new = closerPoint(tr(idx_nearest).x, x_new, eta);
        [K_v, K_loc_v] = Steer(space, obs, tr, idx_nearest, x_new, Q_mat, ...
            expon_A, R_matrix, A, B, f, t_span, K_start, n_corput);
        if all(K_v < 0)
            disp(['Нет соединения на итерации', num2str(i)]);
        else
            dim = length(tr(1).x);
            radius = min(opt_const * (log(length(tr))/length(tr)) ^ (1/dim), eta);
            idx_near = Near(x_new, tr, radius);
            if ~isempty(idx_near)
                if all(idx_near - idx_nearest)
                    idx_near = [idx_near, idx_nearest];
                end
                K_list = zeros(length(idx_near), length(K_v));
                K_loc = zeros(length(idx_near), length(t_span));
                for j=1:length(idx_near)
                    if idx_near(j) == idx_nearest
                        K_list(j, :) = K_v;
                        K_loc(j, :) = K_loc_v;
                        continue
                    else
                        
                        [K_list(j, :), K_loc(j, :)] = Steer(space, obs, tr, idx_near(j), ...
                                        x_new, Q_mat, expon_A, R_matrix, ...
                                        A, B, f, t_span, K_start, n_corput);
                    end
                end
                idx_best = compare_K_list(K_list, mode);
                K_v = K_list(idx_best, :);
                K_loc_v = K_loc(idx_best, :);
                idx_nearest = idx_near(idx_best);
            end
            v = struct('x', x_new, 't', tr(1).t, 'K_t', K_v, ...
                'parent', idx_nearest, 'children', [], 'K_loc', K_loc_v);
            tr = [tr, v];
            disp('before');
            disp(tr(idx_nearest).children);
            tr(idx_nearest).children = [tr(idx_nearest).children, length(tr)];
            
            disp('after');
            disp(tr(idx_nearest).children);
            disp(tr(length(tr)).parent);
            if in_space(x_new', goal)
                if isempty(leaf)
                    leaf = length(tr);
                    T_ans = v.t(find(v.K_t >= 0, 1));
                else
                    new_T = v.t(find(v.K_t >= 0, 1));
                    if new_T < T_ans
                        leaf = length(tr);
                        T_ans = new_T;
                    end
                end
            end
            %подцепили вершину
            %теперь рассмотрим ближайшие к новой точке вершины на предмет
            %переподвязывания
%             [tr, leaf, T_ans] = Rewiring(tr, idx_near, space, ...
%                                     obs, goal, t_span, A, B, f, ...
%                             Q_mat, R_matrix,expon_A, K_start, leaf, T_ans, n_corput);
            
            visualization(space, obs, goal, tr, leaf, T_ans, t_span, A, B, f, x_init, 0);
            writeVideo(v_writer,getframe(gcf));
            close(gcf());
        end
        times(length(tr)) = times(length(tr)) + toc();
        i = i + 1;
        
        if i > limit_iters
            break;
        end
    end
    i = i - 1;
end
%% Базовый алгоритм RRT*
function [tr, leaf, times, i]=default_rrt(x_init, space, ...
                                    obs, goal, eta, opt_const, max_iters, n_corput)
    check_params(x_init, 1, space, obs, -1, 1, 1, eta, max_iters, ...
                      n_corput, 0);
    p = struct('x', x_init, 'cost', 0, 'parent', [], 'children', []);
    leaf = [];
    tr = p;
    times = zeros(1, max_iters);
    i = 1;
    limit_iters = 2 * max_iters;
    degr = get_deg(n_corput);
    corput_seq = corput(n_corput);
    v_writer = VideoWriter('rrt_star.mp4', 'MPEG-4');
    open(v_writer);
    corput_idx = corput_seq .* degr;
    while(length(tr) < max_iters)
        disp(['текущая итерация', num2str(i)]);
        tic();
        x_new = Sampling(space, obs);
        idx_nearest = Nearest(tr, x_new);
        x_new = closerPoint(tr(idx_nearest).x, x_new, eta);
        x_tr = (x_new - tr(idx_nearest).x).* linspace(0,1,degr + 1) + tr(idx_nearest).x;
        if collision_free(x_tr(:, corput_idx)', obs)
            idx_min = idx_nearest;
            c_min = vecnorm(x_new - tr(idx_nearest).x) + tr(idx_nearest).cost;
            radius = min(opt_const * (log(length(tr))/length(tr))^(1/length(x_init)), eta);
            idx_near = Near(x_new, tr, radius);
            for j=1:length(idx_near)
                x_tr = (x_new - tr(idx_near(j)).x).* linspace(0,1,degr + 1) + tr(idx_near(j)).x;
                if collision_free(x_tr(:, corput_idx)', obs)
                    cost = vecnorm(x_new - tr(idx_near(j)).x) + tr(idx_near(j)).cost;
                    if cost < c_min
                        c_min = cost;
                        idx_min = idx_near(j);
                    end
                end 
            end
            v = struct('x', x_new, 'cost', c_min, 'parent', idx_min, 'children', []);
            tr = [tr, v];
            tr(idx_min).children = [tr(idx_min).children, length(tr)];
            if in_space(x_new', goal)
                if isempty(leaf)
                    leaf = length(tr);
                else
                    if tr(leaf).cost > tr(length(tr)).cost
                        leaf = length(tr);
                    end
                end
            end
            for j=1:length(idx_near)
                if (idx_near(j) == 1) || (idx_near(j) == tr(length(tr)).parent)
                    continue
                end
                x_tr = (x_new - tr(idx_near(j)).x).* linspace(0,1,degr + 1) + tr(idx_near(j)).x;
                if collision_free(x_tr(:, corput_idx)', obs)
                    cost = vecnorm(x_new - tr(idx_near(j)).x) + tr(length(tr)).cost;
                    if cost < tr(idx_near(j)).cost
                        parent = tr(idx_near(j)).parent;
                        tr(parent).children = tr(parent).children(tr(parent).children ~= idx_near(j));
                        tr(length(tr)).children = [tr(length(tr)).children, idx_near(j)];
                        tr(idx_near(j)).parent = length(tr);
                        old_cost = tr(idx_near(j)).cost;
                        tr(idx_near(j)).cost = cost;
                        tr = upd_children(tr,idx_near(j), old_cost);
                    end
                end
            end
            visualization_base(space, obs, goal, tr, leaf);
            writeVideo(v_writer,getframe(gcf));
            close(gcf());
        else
            disp(['Нет соединения на итерации', num2str(i)]);
        end
        times(length(tr)) = times(length(tr)) + toc();
        i = i + 1;
        if i > limit_iters
            break;
        end
    end
    i = i - 1;
end
function tr = upd_children(tr, idx, old_cost)
    for i=1:length(tr(idx).children)
        child = tr(idx).children(i);
        new_cost = tr(child).cost;
        tr(child).cost = tr(idx).cost + tr(child).cost - old_cost;
        if ~isempty(tr(child).children)
            tr = upd_children(tr, child, new_cost);
        end
    end
end


%% Составные части
%% Генерация точек и поиск ближайших в дереве

function x_new = Sampling(space, obs)
    % генерация новой точки
    f = space{1};
    center = f{1};
    side = f{4};
    x_new = center + rand(numel(side),1) .* side;
    while ~obs_free(x_new, obs)
        x_new = center + rand(numel(side),1) .* side;
    end
end

function x_new = closerPoint(x_tr, x_new, eta)
    v = x_new - x_tr;
    if vecnorm(v) > eta
        x_new = x_tr + (v) * eta / vecnorm(v);
    end
end

function idx_near = Near(x_new, tr, radius)
    x_pts = zeros(length(x_new), length(tr));
    for i=1:length(tr)
        x_pts(:,i) = tr(i).x;
    end
    idx_near = find(vecnorm(x_pts-x_new) <= radius);
end

function idx_nearest=Nearest(tr, x_new)
    % Поиск ближайшей к x_new точки дерева 
    X = zeros(length(x_new), length(tr));
    for i=1:length(tr)
        X(:,i) = tr(i).x;
    end
    dists = vecnorm(X - x_new);
    [~, idx_nearest] = min(dists);
end

%% Проверка столкновений и принадлежности пространству
function flag = obs_free(x, obs)
    for i = 1:length(obs)
        f = obs{i};
        center = f{1};
        pts = f{2};
        angles_arr = f{3};
        dir = x(1:2) - center;
        if dir(1) == 0
            if dir(2) < 0
                continue;
            elseif dir(2) == 0
                flag = false;
                return;
            else
                angle_ = pi / 2;
            end
        elseif dir(1) < 0
            continue;
        else
            angle_ = atan(dir(2)/dir(1));
            if angle_ < angles_arr(1) || angle_ > angles_arr(end)
                continue;
            end
        end
        idx = bin_search(angles_arr, angle_);
        if collision_check(x(1:2) - center, pts(:,idx) - center, ...
                pts(:, idx + 1) - center)
            flag = false;
            return;
        end
    end
    flag = true;
end
function flag = collision_check(x, a, b)
    % знаем, что полярный угол x лежит между полярными углами a и b
    % хотим понять, как расположены относительно друг друга
    d1 = b - a;
    d2 = x - a;
    m = [d1,d2];
    if det(m) >= 0
        flag = true;
    else
        flag = false;
    end
end

function flag=collision_free(x, obs)
% Здесь x - точки траектории, взятые по последовательности ван дер
% Корпута для времен
    for i=1:size(x,1)
        if ~obs_free(x(i,:)', obs)
            flag = false;
            return;
        end
    end
    flag = true;
end

function flag = in_space(x, space)
% Здесь x - точки траектории, взятые по последовательности ван дер
% Корпута для времен
% space имеет структуру {center, pts, angles, sides}
    for i = 1:size(x,1)
        a = space{1};
        center = a{1};
        sides = a{4};
        if ~(all(x(i,:)' - center >= 0) && all(x(i,:)' - center <= sides))
            flag = false;
            return;
        end
    end
    flag = true;
end
%% Функции рулевого управления(Steer и подсчет K(t))
function [K_y, K_t] = Steer(space, obs, tr, idx_nearest, x_new, Q_mat, ...
                        expon_A, R_matrix, A, B, f, t_span, K_start, n_corput)
    v = tr(idx_nearest);
    K_t = K(t_span, v.x, x_new, K_start, expon_A, R_matrix, Q_mat);
    t_arr = v.t; % linspace(0, T_max) в этих точках посчитана K_v(t)
    K_y = zeros(size(t_arr));
    K_v = v.K_t;
    for j=1:length(t_arr)
        t = t_arr(j) + t_span;
        max_diff = -1;
        for r=1:length(t)
            k = find(t_arr == t(r),1);
            if ~isempty(k)
                max_diff = max(K_v(k) - K_t(r), max_diff);
            end
        end
        K_y(j) = max_diff;
    end
    if all(K_y < 0)
        disp('недостаточно энергии');
        return;
    else
        idx_arr = find(K_y >= 0);
        for s=1:length(idx_arr)
            t = t_arr(idx_arr(s)) + t_span;
            max_diff = -1;
            idx_star = -1;
            for r=1:length(t)
                k = find(t_arr == t(r),1);
                if ~isempty(k)
                    if K_v(k) - K_t(r) > max_diff
                        max_diff = K_v(k) - K_t(r);
                        idx_star = r;
                    end
                end
            end
            [~, q] = numerator(v.x, x_new, expon_A, R_matrix, t_span);
            c = q(idx_star,:);
            Q_matrix = Q_mat(:,:,idx_star);
            [~, l_star] = find_maximum_k(c, Q_matrix);
            num = get_deg(n_corput);
            corput_seq = corput(n_corput);
            corput_seq_idx = [1; num; corput_seq .* num];
            step = (t_span(idx_star) - t_span(end)) / (num-1);
            t_sp = t_span(end):step:t_span(idx_star);
            [~, x] = ode45(@(t,x) eq_4(t, x, A, B, f, ...
                K_t(idx_star), l_star, Q_matrix, idx_star, t_span), t_sp, x_new);
            x_corput = x(corput_seq_idx,:);
            if ~in_space(x_corput, space)
                K_y(idx_arr(s)) = -1;
            end
            if ~collision_free(x_corput, obs)
                K_y(idx_arr(s)) = -1;
            end
        end
        if all(K_y < 0)
            disp('недостаточно энергии/траектории с врезанием в препятствие');
            return;
        end
    end
end

function idx_best = compare_K_list(K_list, mode)
    % Сравниваем функции max(0,K_v_iy(t))
    % mode == 0 => наибольшая энергия для наименьшего момента времени
    % mode == 1 => наибольшая сумма энергий
    idx_K_list = [];
    for i = 1:size(K_list, 1)
        if all(K_list(i, :) < 0)%Поскольку ранее не проверяли
            continue;
        end
        flag = true;
        for j = 1:size(K_list, 1)
            if all(K_list(i, :) < 0)%Поскольку ранее не проверяли
                continue;
            end
            if i == j
                continue;
            else
                if all(max(0,K_list(i,:)) <= max(0, K_list(j,:)))
                    flag = false;
                    break;
                end
            end
        end
        if flag
            idx_K_list = [idx_K_list; i];
        end
    end
    
    if length(idx_K_list) == 1
        idx_best = idx_K_list(1);
    else
        if mode
            % в среднем, то есть лучше тот родитель, при связи с которым
            % получается наибольшая сумма энергий
            sums = sum(max(K_list(idx_K_list,:), 0), 2);
            [~, idx] = max(sums);
            K_list_new = K_list(idx_K_list,:);
            if length(idx) ~= 1
                [m, idx_new] = max(K_list_new(idx,:),[],1);
                idx_best = idx_new(find(m >= 0, 1));
                idx_best = idx(idx_best);
                idx_best = idx_K_list(idx_best);
            else
                idx_best = idx_K_list(idx);
            end
        else
            % лучше тот родитель, при связи с которым получается наибольшая
            % энергия в наименьший момент времени
            [m, idx] = max(K_list(idx_K_list,:));
            idx_best = idx_K_list(idx(find(m >= 0, 1)));
        end
    end
end
function x = corput(n)
% генерирует первые n чисел последовательности ван дер Корпута
    b = dec2bin(1:n)-'0';   
    l = size(b,2);           
    w = (1:l)-l-1;          
    x = b * ( 2.^w');        
end
function deg=get_deg(n)
    %возвращает минимальную степень двойки, не меньшую заданного числа
    deg = 2^(ceil(log2(n)));
end

function K_t = K(t_span, v, y, K_start, expon_A, R_matrix, h)
    % t_span = linspace(T, 0)
    % v - вершина в дереве, y - координата
    % считаем K(t)
    K_t = zeros(size(t_span));
    [~, q] = numerator(v,y,expon_A,R_matrix,t_span);
    for i=1:length(t_span) - 1
        Q_matrix = h(:,:,i);
        c = q(i,:);
        if ~any(Q_matrix)
            continue
        end
        [k_max, ~] = find_maximum_k(c,Q_matrix);
        if k_max > K_start
            k_max = K_start + 1;
        end
        K_t(i) = k_max;
    end
    K_t(end) = K_start + 1;
end
function [k_max,l] = find_maximum_k(c,Q_matrix)
    % находит максимальное K и соответствующее ему направление l
    options = optimoptions(@fmincon, "Display", "none");
    [l, k_max] = fmincon(@(l) -K_max(c, Q_matrix, l), ...
        [1; zeros(size(Q_matrix,1) - 1,1)], [], [], [], [], [], [], @(l)nonlcon(l), options);
    k_max = -k_max;
end

function [c, ceq]=nonlcon(x)
    c = [];
    ceq = (x(1)^2 + x(2)^2 - 1);%достаточно производить поиск по единичной окружности.
end

function k_max=K_max(c,Q_matrix,l)
    k_max = dot(l,c) / sqrt(dot(l, Q_matrix * l));
end


%% Функции перевязывания вершин
function [tr, leaf, T_ans] = Rewiring(tr, idx_near, ...
                                                space, obs, goal, t_span, ...
                                                A, B, f, Q_mat, ...
                                                R_matrix,expon_A, K, leaf, T_ans, n_corput)
    for i=1:length(idx_near)
        idx_cur = idx_near(i);
        if idx_cur == 1 % пользуемся тем, что нельзя перевязывать корень
            continue
        end
        [K_y, K_loc_y] = Steer(space, obs, tr, length(tr), tr(idx_cur).x, Q_mat, ...
            expon_A, R_matrix, A, B, f, t_span, K, n_corput);
        if all(max(K_y, 0) >= max(tr(idx_cur).K_t, 0))
            [flag, min_time] = check_children(tr, idx_cur, space, obs, goal, t_span, A, B, ...
                    f, Q_mat, R_matrix, expon_A, K, leaf, T_ans, n_corput, K_y);
            flag_1 = ~isempty(leaf) && min_time < T_ans;
            flag = flag && (isempty(leaf) || flag_1);
            if flag 
                disp('есть перевязывание');
                tr(idx_cur).K_t = K_y;
                tr(idx_cur).K_loc = K_loc_y;
                %отвязываем от родителя
                tr(tr(idx_cur).parent).children = tr(tr(idx_cur).parent).children(tr(tr(idx_cur).parent).children ~= idx_cur);
                % привязываем к новому
                tr(idx_cur).parent = length(tr);
                tr(end).children = [tr(end).children, idx_cur];
                [tr, leaf, T_ans] = update_children(tr, ...
                                    idx_cur, space, obs, goal, t_span, ...
                                    A, B, f,Q_mat, R_matrix, expon_A, K, leaf, T_ans, n_corput);
            end
        end
    end
end

function [flag, min_time] = check_children(tr, idx_cur, space, obs, goal, t_span, A, B, ...
                f, Q_mat, R_matrix, expon_A, K, leaf, T_ans, n_corput, K_new)
    % проверка детей вершины после перевязывания
    K_save = tr(idx_cur).K_t;
    tr(idx_cur).K_t = K_new;
    min_time = tr(1).t(end);
    for i=1:length(tr(idx_cur).children)
        idx_new = tr(idx_cur).children(i);
        x_cur = tr(idx_new).x;
        [K_y, ~] = Steer(space, obs, tr, idx_cur, x_cur, Q_mat, ...
            expon_A, R_matrix, A, B, f, t_span, K, n_corput);
        if all(K_y < 0)
            flag = false;
            tr(idx_cur).K_t = K_save;
            return;
        end
        if ~isempty(tr(idx_new).children)
            [fl, min_t] = check_children(tr, idx_new, space, obs, goal, t_span, A, B, ...
                                f, Q_mat, R_matrix, expon_A, K, leaf, T_ans, ...
                                n_corput, K_y);
            if ~fl
                flag = false;
                tr(idx_cur).K_t = K_save;
                return;
            end
            if ~isempty(min_time)
                min_time = min(min_time, min_t);
            end
        end
        if in_space(tr(idx_new).x, goal)
            t_min = find(K_y >= 0, 1);
            min_time = min(min_time, t_min);
        end
    end
    tr(idx_cur).K_t = K_save;
    flag = true;
end

function [tr, leaf, T_ans] = update_children(tr, ...
                            idx_cur, space, obs, goal, t_span, A, B, f, ...
                            Q_mat, R_matrix, expon_A, K, leaf, T_ans, n_corput)
    for i=1:length(tr(idx_cur).children)
        idx_new = tr(idx_cur).children(i);
        x_cur = tr(idx_new).x;
        [K_y, K_loc_y] = Steer(space, obs, tr, idx_cur, x_cur, Q_mat, ...
            expon_A, R_matrix, A, B, f, t_span, K, n_corput);
        tr(idx_new).K_t = max(K_y,tr(idx_new).K_t);        
        tr(idx_new).K_loc = K_loc_y;
        if ~isempty(tr(idx_new).children) 
            [tr, leaf, T_ans] = update_children(tr, idx_new, ... 
                        space, obs, goal, t_span, A, B, f, ...
                        Q_mat, R_matrix, expon_A, K, leaf, T_ans, n_corput);
        end
        if in_space(tr(idx_new).x, goal)
            t_min = find(K_y >= 0, 1);
            if t_min < T_ans
                leaf = idx_new;
                T_ans = t_min;
            end
        end

    end
end

%% Основные дифференциальные уравнения и подсчет матриц

function [t_span,Q] = Q(A, B, t_span)
    % Возвращает numel(t_span) матриц Q
    % Для доступа к iй матрице необходимо написать Q(:,:,i)
    t_span = t_span(end:-1:1);
    [~,Q] = ode45(@(t, Q) eq_1(t, Q, A, B), t_span, zeros(size(A)));
    Q = Q(end:-1:1, :);
    Q = reshape(transpose(Q), [size(A,1),size(A,1),numel(t_span)]);
end
function dydt = eq_1(~, Q, A, B)
    Q = reshape(Q, size(A));
    dydt = A * Q + Q * transpose(A) - B * transpose(B);
    dydt = reshape(dydt, [numel(Q),1]);
end
function [t,R] = R(A, f, t_span)
    % Возвращает numel(t_span) векторов R
    t_span = t_span(end:-1:1);
    [t,R] = ode45(@(t,R) eq_2(t, R, A, f), t_span, zeros(size(f)));
    R = R(end:-1:1,:);
end
function dydt = eq_2(~, R, A, f)
    dydt = A * R - f;
end

function [t,x] = exp_A(A, t_span)
    % Подсчитывает матричную экспоненту
    t_span = t_span(end:-1:1);
    [t,x] = ode45(@(t,x) eq_3(t,x,A), t_span, eye(size(A)));
    x = x(end:-1:1, :);
    if length(t_span) == 2
        x = [x(1,:); x(end,:)];
    end
    x = reshape(transpose(x), [size(A,1),size(A,1),numel(t_span)]);
end

function dydt = eq_3(~,x,A)
    x = reshape(x, size(A));
    dydt = A * x;
    dydt = reshape(dydt, [numel(x),1]);
end

function [t_span, q] = numerator(v, y, X, r, t_span) %->[numel(t_span),2]
    % Подсчитывает числитель в формуле для K(t^*)
    q = zeros(numel(t_span), size(y,1));
    for i = 1:numel(t_span)
        q(i,:) = v - X(:,:,i) * y + transpose(r(i,:));
    end
end

function dydt = eq_4(t, x, A, B, f, K_t, l_star, Q_matrix, idx_star, t_span)
    u = -K_t * transpose(B) * transpose(expm(A * (t_span(idx_star) - t))) *...
    l_star / sqrt(transpose(l_star) * Q_matrix * l_star);
    dydt = A * x + B * u + f;
end