HyRRTConnect/checksolution.m at main · HybridSystemsLab/HyRRTConnect · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
function [flag, ifw, ibw] = checksolution(Gfw, Gbw, reachedsetfw, reachedsetbw, Iextendedfw, Iextendedbw, isextendedfw, isextendedbw)

% ifw = [];
% ibw = [];
% for i1 = 1:numnodes(Gfw)
%     for i2 = 1:numnodes(Gbw)
%         if norm(getstate(Gfw, i1) -  getstate(Gbw, i2)) < tol
%             ifw = i1;
%             ibw = i2;
%             return;
%         end
%     end
% end
ifw = [];
ibw = [];
flag = 0;
if isextendedfw
    N = size(Iextendedfw, 2);
    for i = 1:N
        x_cur = getstate(Gfw, Iextendedfw(i));
        [flag, idx] = isInreachedset(x_cur, reachedsetbw);
        if flag
            ibw = idx;
            ifw = Iextendedfw(i);
            return;
        end
    end
end
if isextendedbw && (~flag)
    N = size(Iextendedbw, 2);
    for i = 1:N
        x_cur = getstate(Gbw, Iextendedbw(i));
        [flag, idx] = isInreachedset(x_cur, reachedsetfw);
        if flag
            ifw = idx;
            ibw = Iextendedbw(i);
            return;
        end
    end
end