From 074423cd13cb7a91d947eaf98066026ec35285f2 Mon Sep 17 00:00:00 2001 From: Shaoshuaizyk Date: Fri, 22 Oct 2021 14:27:23 -0700 Subject: [PATCH 01/15] star convex to convexhull --- Motion_Planning/6Star_Convex/StarConvex.m | 132 ++++++++++++++++++++++ 1 file changed, 132 insertions(+) create mode 100644 Motion_Planning/6Star_Convex/StarConvex.m diff --git a/Motion_Planning/6Star_Convex/StarConvex.m b/Motion_Planning/6Star_Convex/StarConvex.m new file mode 100644 index 0000000..78ef3a6 --- /dev/null +++ b/Motion_Planning/6Star_Convex/StarConvex.m @@ -0,0 +1,132 @@ +classdef StarConvex < handle + properties + % All verticals are in CCW + xs_; + ys_; + zs_; + vertices_; + Po_; + epsilon_ = 1e-5; + % Output convex region in original space as Ax <= b + A; + b; + end + + properties %(Access = private) + dim; + + % Constructed convex hull from star convex polytope, 3*n + Edge_; + + convex_hull; + + % Points inside the constructed convex hull, 3*k + P_in_; + end + + methods + function obj = StarConvex(xs, ys, Po, zs) + assert(all(size(xs) == size(ys))); + obj.xs_ = xs; + obj.ys_ = ys; + obj.Po_ = Po; + if nargin > 3 + obj.zs_ = zs; + obj.dim = 3; + else + obj.zs_ = zeros(size(xs)); + obj.dim = 2; + end + obj.vertices_ = [obj.xs_; obj.ys_; obj.zs_]; + end + + function ConstructConvexFromStar(obj) + % Graham Scan, reference + % http://www.cosy.sbg.ac.at/~held/teaching/compgeo/cg_study.pdf + + % Find the bottommost point + ymin = obj.vertices_(2, 1); + min_ind = 1; + for i = 2:size(obj.vertices_,2) + y = obj.vertices_(2, i); + % Pick the bottom-most or choose the left most point in + % case of tie + if y < ymin || (y == ymin && obj.vertices_(1, i) < ... + obj.vertices_(1, min_ind)) + ymin = obj.vertices_(2, i); + min_ind = i; + end + end + obj.vertices_ = circshift(obj.vertices_, [0, min_ind]); + + obj.convex_hull = obj.vertices_; + obj.P_in_ = []; + + cur_ind = 1; + init_pt = obj.convex_hull(:, cur_ind); + done = false; + while done == false + if cur_ind == 1 + pre_ind = size(obj.convex_hull, 2); + next_ind = cur_ind + 1; + elseif cur_ind == size(obj.convex_hull, 2) + pre_ind = cur_ind - 1; + next_ind = 1; + else + pre_ind = cur_ind - 1; + next_ind = cur_ind + 1; + end + pre_pt = obj.convex_hull(:, pre_ind); + cur_pt = obj.convex_hull(:, cur_ind); + next_pt = obj.convex_hull(:, next_ind); + cross_turn = cross(cur_pt - pre_pt, next_pt - cur_pt); +% fprintf("Pre index is %d, current index is %d, next index is %d \n", pre_ind, cur_ind, next_ind); +% fprintf("Pre pt is [%.2f, %.2f, %.2f], cur pt is [%.2f, %.2f, %.2f]," + ... +% " next pt is [%.2f, %.2f, %.2f]", pre_pt(1), pre_pt(2), pre_pt(3),... +% cur_pt(1), cur_pt(2), cur_pt(3), next_pt(1), next_pt(2), next_pt(3)); +% fprintf("crossturn is %f \n", cross_turn(3)); + if cross_turn(3) > obj.epsilon_ % left turn if CCW + cur_ind = mod(cur_ind, size(obj.convex_hull, 2)) + 1; + if norm(obj.convex_hull(:, cur_ind) - init_pt) < obj.epsilon_ + done = true; + end + elseif cross_turn(3) < -obj.epsilon_ % right turn if CCW + obj.P_in_(:, end+1) = obj.convex_hull(:, cur_ind); + obj.convex_hull(:, cur_ind) = []; + % start from bottom most and left point guarantee + % cur_ind is not 1 + cur_ind = cur_ind - 1; + else % collinear + obj.convex_hull(:, cur_ind) = []; + if norm(obj.convex_hull(:, cur_ind)-init_pt) < obj.epsilon_ + done = true; + end + end + end + end + + function PlotConvexHull(obj) + plot([obj.convex_hull(1, :), obj.convex_hull(1, 1)], ... + [obj.convex_hull(2, :), obj.convex_hull(2, 1)], 'k*--'); + end + + function PlotVertices(obj) + plot([obj.vertices_(1, :), obj.vertices_(1, 1)], ... + [obj.vertices_(2, :), obj.vertices_(2, 1)], 'b*-'); + end + + function PlotInterPoint(obj) + plot(obj.P_in_(1, :), obj.P_in_(2, :), 'ro'); + end + + function VisualizeResult(obj) + figure; + hold on + obj.PlotVertices(); + obj.PlotConvexHull(); + obj.PlotInterPoint(); + hold off; + end + end + +end From b2039d866cf8f1efd715125b0cf74635633e6a06 Mon Sep 17 00:00:00 2001 From: Shaoshuaizyk Date: Tue, 26 Oct 2021 23:42:48 -0700 Subject: [PATCH 02/15] accept pointclouds 3d coordinates and construct star convex hull with hyperedges --- Motion_Planning/6Star_Convex/HyperEdge.m | 21 +++ .../6Star_Convex/LargeConvexPolytopes.m | 13 ++ Motion_Planning/6Star_Convex/SphereFlipping.m | 4 + Motion_Planning/6Star_Convex/StarConvex.m | 155 +++++++++++------- 4 files changed, 130 insertions(+), 63 deletions(-) create mode 100644 Motion_Planning/6Star_Convex/HyperEdge.m create mode 100644 Motion_Planning/6Star_Convex/LargeConvexPolytopes.m create mode 100644 Motion_Planning/6Star_Convex/SphereFlipping.m diff --git a/Motion_Planning/6Star_Convex/HyperEdge.m b/Motion_Planning/6Star_Convex/HyperEdge.m new file mode 100644 index 0000000..f864303 --- /dev/null +++ b/Motion_Planning/6Star_Convex/HyperEdge.m @@ -0,0 +1,21 @@ +classdef HyperEdge < handle + properties + % In 2d, it has 2 points as an edge; in 3d, it has 3 points as a facet. + p1_; + p2_; + p3_; + n_; + end + + methods + function obj = HyperEdge(p1, p2, p3) + obj.p1_ = p1; + obj.p2_ = p2; + if argmin > 2 + obj.p3_ = p3; + end + + end + end + +end \ No newline at end of file diff --git a/Motion_Planning/6Star_Convex/LargeConvexPolytopes.m b/Motion_Planning/6Star_Convex/LargeConvexPolytopes.m new file mode 100644 index 0000000..8e6089e --- /dev/null +++ b/Motion_Planning/6Star_Convex/LargeConvexPolytopes.m @@ -0,0 +1,13 @@ +function [A, b] = LargeConvexPolytopes(pointclouds_xyz, pos, R) + % Given a set of pointclouds coordinates n*3 in 3d, and the current position. + % With the user defined radius R as well. + % Return a convex region denoted by Ax <= b + flipping_xyz = zeros(size(pointclouds_xyz)); + for i = 1: size(pointclouds_xyz, 2) + flipping_xyz(i, :) = SphereFlipping(pos, pointclouds_xyz(i, :), R); + end + k = convhull(flipping_xyz); + sc = StarConvex(pointclouds_xyz(k, 1), pointclouds_xyz(k, 2), pos, pointclouds_xyz(k, 3)); + sc.ConstructConvexFromStar(); + +end \ No newline at end of file diff --git a/Motion_Planning/6Star_Convex/SphereFlipping.m b/Motion_Planning/6Star_Convex/SphereFlipping.m new file mode 100644 index 0000000..cfbe7ae --- /dev/null +++ b/Motion_Planning/6Star_Convex/SphereFlipping.m @@ -0,0 +1,4 @@ +function p2 = SphereFlipping(pos, p1, R) + % Sphere flipping p1 from pos by a radius R to p2 + p2 = pos + (p1 - pos) * (2 * R - norm(p1 - pos)) / norm(p1 - pos); +end \ No newline at end of file diff --git a/Motion_Planning/6Star_Convex/StarConvex.m b/Motion_Planning/6Star_Convex/StarConvex.m index 78ef3a6..30e5cab 100644 --- a/Motion_Planning/6Star_Convex/StarConvex.m +++ b/Motion_Planning/6Star_Convex/StarConvex.m @@ -15,12 +15,13 @@ properties %(Access = private) dim; - % Constructed convex hull from star convex polytope, 3*n - Edge_; - + % Constructed convex hull from star convex polytope, 2/3*n convex_hull; + + % Hyperedges + Edges; - % Points inside the constructed convex hull, 3*k + % Points inside the constructed convex hull, 2/3*k P_in_; end @@ -37,74 +38,102 @@ obj.zs_ = zeros(size(xs)); obj.dim = 2; end - obj.vertices_ = [obj.xs_; obj.ys_; obj.zs_]; + obj.vertices_ = [obj.xs_, obj.ys_, obj.zs_]; end - function ConstructConvexFromStar(obj) - % Graham Scan, reference - % http://www.cosy.sbg.ac.at/~held/teaching/compgeo/cg_study.pdf - - % Find the bottommost point - ymin = obj.vertices_(2, 1); - min_ind = 1; - for i = 2:size(obj.vertices_,2) - y = obj.vertices_(2, i); - % Pick the bottom-most or choose the left most point in - % case of tie - if y < ymin || (y == ymin && obj.vertices_(1, i) < ... - obj.vertices_(1, min_ind)) - ymin = obj.vertices_(2, i); - min_ind = i; - end - end - obj.vertices_ = circshift(obj.vertices_, [0, min_ind]); - - obj.convex_hull = obj.vertices_; - obj.P_in_ = []; - - cur_ind = 1; - init_pt = obj.convex_hull(:, cur_ind); - done = false; - while done == false - if cur_ind == 1 - pre_ind = size(obj.convex_hull, 2); - next_ind = cur_ind + 1; - elseif cur_ind == size(obj.convex_hull, 2) - pre_ind = cur_ind - 1; - next_ind = 1; - else - pre_ind = cur_ind - 1; - next_ind = cur_ind + 1; + function ConstructConvexFromStar(obj) + if obj.dim == 2 + % Graham Scan, reference + % http://www.cosy.sbg.ac.at/~held/teaching/compgeo/cg_study.pdf + + % Find the bottommost point + ymin = obj.vertices_(1, 2); + min_ind = 1; + for i = 2:size(obj.vertices_,1) + y = obj.vertices_(i, 2); + % Pick the bottom-most or choose the left most point in + % case of tie + if y < ymin || (y == ymin && obj.vertices_(i, 1) < ... + obj.vertices_(min_ind, 1)) + ymin = obj.vertices_(i, 2); + min_ind = i; + end end - pre_pt = obj.convex_hull(:, pre_ind); - cur_pt = obj.convex_hull(:, cur_ind); - next_pt = obj.convex_hull(:, next_ind); - cross_turn = cross(cur_pt - pre_pt, next_pt - cur_pt); -% fprintf("Pre index is %d, current index is %d, next index is %d \n", pre_ind, cur_ind, next_ind); -% fprintf("Pre pt is [%.2f, %.2f, %.2f], cur pt is [%.2f, %.2f, %.2f]," + ... -% " next pt is [%.2f, %.2f, %.2f]", pre_pt(1), pre_pt(2), pre_pt(3),... -% cur_pt(1), cur_pt(2), cur_pt(3), next_pt(1), next_pt(2), next_pt(3)); -% fprintf("crossturn is %f \n", cross_turn(3)); - if cross_turn(3) > obj.epsilon_ % left turn if CCW - cur_ind = mod(cur_ind, size(obj.convex_hull, 2)) + 1; - if norm(obj.convex_hull(:, cur_ind) - init_pt) < obj.epsilon_ - done = true; + obj.vertices_ = circshift(obj.vertices_, [min_ind, 0]); + + obj.convex_hull = obj.vertices_; + + cur_ind = 1; + obj.P_in_ = []; + init_pt = obj.convex_hull(cur_ind, :); + done = false; + while done == false + if cur_ind == 1 + pre_ind = size(obj.convex_hull, 1); + next_ind = cur_ind + 1; + elseif cur_ind == size(obj.convex_hull, 1) + pre_ind = cur_ind - 1; + next_ind = 1; + else + pre_ind = cur_ind - 1; + next_ind = cur_ind + 1; end - elseif cross_turn(3) < -obj.epsilon_ % right turn if CCW - obj.P_in_(:, end+1) = obj.convex_hull(:, cur_ind); - obj.convex_hull(:, cur_ind) = []; - % start from bottom most and left point guarantee - % cur_ind is not 1 - cur_ind = cur_ind - 1; - else % collinear - obj.convex_hull(:, cur_ind) = []; - if norm(obj.convex_hull(:, cur_ind)-init_pt) < obj.epsilon_ - done = true; + pre_pt = obj.convex_hull(pre_ind, :); + cur_pt = obj.convex_hull(cur_ind, :); + next_pt = obj.convex_hull(next_ind, :); + cross_turn = cross(cur_pt - pre_pt, next_pt - cur_pt); + % fprintf("Pre index is %d, current index is %d, next index is %d \n", pre_ind, cur_ind, next_ind); + % fprintf("Pre pt is [%.2f, %.2f, %.2f], cur pt is [%.2f, %.2f, %.2f]," + ... + % " next pt is [%.2f, %.2f, %.2f]", pre_pt(1), pre_pt(2), pre_pt(3),... + % cur_pt(1), cur_pt(2), cur_pt(3), next_pt(1), next_pt(2), next_pt(3)); + % fprintf("crossturn is %f \n", cross_turn(3)); + if cross_turn(3) > obj.epsilon_ % left turn if CCW + cur_ind = mod(cur_ind, size(obj.convex_hull, 1)) + 1; + if norm(obj.convex_hull(cur_ind, :) - init_pt) < obj.epsilon_ + done = true; + end + elseif cross_turn(3) < -obj.epsilon_ % right turn if CCW + obj.P_in_(end+1, :) = obj.convex_hull(cur_ind, :); + obj.convex_hull(cur_ind, :) = []; + % start from bottom most and left point guarantee + % cur_ind is not 1 + cur_ind = cur_ind - 1; + else % collinear + obj.convex_hull(cur_ind, :) = []; + if norm(obj.convex_hull(cur_ind, :)-init_pt) < obj.epsilon_ + done = true; + end end end + obj.convex_hull(end+1, :) = obj.convex_hull(1, :); + + obj.Edges = repmat(struct, 1, size(obj.convex_hull, 1)-1); + for i = 1: size(obj.convex_hull, 1)-1 + obj.Edges(i).p1 = obj.convex_hull(i, :); + obj.Edges(i).p2 = obj.convex_hull(i+1, :); + vec = obj.Edges(i).p2 - obj.Edges(i).p1; + n_vec = [-vec(2), vec(1)]; + obj.Edges(i).n = n_vec / norm(n_vec); + end + end + + k = convhull(obj.vertices_, 'Simplify',true); + ind_in = setdiff(1:size(obj.vertices_, 1), reshape(k, 1, 3 * size(k, 1))); + obj.P_in_ = obj.vertices_(ind_in, :); + obj.convex_hull = repmat(struct, 1, size(k, 1)); + for i = 1: size(k, 1) + obj.convex_hull(i).p1 = obj.vertices_(k(i, 1), :); + obj.convex_hull(i).p2 = obj.vertices_(k(i, 2), :); + obj.convex_hull(i).p3 = obj.vertices_(k(i, 3), :); + %%%%%% TODO implement normal vector of this hyperedge. end + end +% function ShrinkToConvex(obj) +% +% end + function PlotConvexHull(obj) plot([obj.convex_hull(1, :), obj.convex_hull(1, 1)], ... [obj.convex_hull(2, :), obj.convex_hull(2, 1)], 'k*--'); From b308d65caf85ba93ef99134981cdda23bb521adc Mon Sep 17 00:00:00 2001 From: Shaoshuaizyk Date: Wed, 27 Oct 2021 23:59:20 -0700 Subject: [PATCH 03/15] implement normal vector for 3d convex hull facets --- Motion_Planning/6Star_Convex/StarConvex.m | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/Motion_Planning/6Star_Convex/StarConvex.m b/Motion_Planning/6Star_Convex/StarConvex.m index 30e5cab..82d2c5e 100644 --- a/Motion_Planning/6Star_Convex/StarConvex.m +++ b/Motion_Planning/6Star_Convex/StarConvex.m @@ -125,9 +125,13 @@ function ConstructConvexFromStar(obj) obj.convex_hull(i).p1 = obj.vertices_(k(i, 1), :); obj.convex_hull(i).p2 = obj.vertices_(k(i, 2), :); obj.convex_hull(i).p3 = obj.vertices_(k(i, 3), :); - %%%%%% TODO implement normal vector of this hyperedge. + % Normal vector of this hyperedge. It points inside if + % facet vertices detemined by k are CCW + n_vec = cross(obj.convex_hull(i).p3 - obj.convex_hull(i).p1, ... + obj.convex_hull(i).p2 - obj.convex_hull(i).p1); + obj.convex_hull(i).n = n_vec / norm(n_vec); end - + obj.Edges = obj.convex_hull; end % function ShrinkToConvex(obj) From ca54385895cd5f489c381ef002984b68ddbe2211 Mon Sep 17 00:00:00 2001 From: Shaoshuaizyk Date: Sun, 31 Oct 2021 21:53:08 -0700 Subject: [PATCH 04/15] return A and b to form an obstacle-free convex from star convex --- .../3Safe_Flight_Corridors/Polyhedron.m | 1 + Motion_Planning/6Star_Convex/HyperEdge.m | 21 ------ .../6Star_Convex/LargeConvexPolytopes.m | 4 +- Motion_Planning/6Star_Convex/StarConvex.m | 72 +++++++++++++++++-- 4 files changed, 72 insertions(+), 26 deletions(-) delete mode 100644 Motion_Planning/6Star_Convex/HyperEdge.m diff --git a/Motion_Planning/3Safe_Flight_Corridors/Polyhedron.m b/Motion_Planning/3Safe_Flight_Corridors/Polyhedron.m index 8533202..4a42a1a 100644 --- a/Motion_Planning/3Safe_Flight_Corridors/Polyhedron.m +++ b/Motion_Planning/3Safe_Flight_Corridors/Polyhedron.m @@ -11,6 +11,7 @@ % Append Hyperplane function add(obj, plane) + % Does normal vector of the plane point inside or outside? obj.polys_{end+1} = plane; end % Check if the point is inside polyhedron, diff --git a/Motion_Planning/6Star_Convex/HyperEdge.m b/Motion_Planning/6Star_Convex/HyperEdge.m deleted file mode 100644 index f864303..0000000 --- a/Motion_Planning/6Star_Convex/HyperEdge.m +++ /dev/null @@ -1,21 +0,0 @@ -classdef HyperEdge < handle - properties - % In 2d, it has 2 points as an edge; in 3d, it has 3 points as a facet. - p1_; - p2_; - p3_; - n_; - end - - methods - function obj = HyperEdge(p1, p2, p3) - obj.p1_ = p1; - obj.p2_ = p2; - if argmin > 2 - obj.p3_ = p3; - end - - end - end - -end \ No newline at end of file diff --git a/Motion_Planning/6Star_Convex/LargeConvexPolytopes.m b/Motion_Planning/6Star_Convex/LargeConvexPolytopes.m index 8e6089e..8cbd43a 100644 --- a/Motion_Planning/6Star_Convex/LargeConvexPolytopes.m +++ b/Motion_Planning/6Star_Convex/LargeConvexPolytopes.m @@ -9,5 +9,7 @@ k = convhull(flipping_xyz); sc = StarConvex(pointclouds_xyz(k, 1), pointclouds_xyz(k, 2), pos, pointclouds_xyz(k, 3)); sc.ConstructConvexFromStar(); - + sc.ShrinkToConvex(); + A = sc.A; + b = sc.b; end \ No newline at end of file diff --git a/Motion_Planning/6Star_Convex/StarConvex.m b/Motion_Planning/6Star_Convex/StarConvex.m index 82d2c5e..e00424d 100644 --- a/Motion_Planning/6Star_Convex/StarConvex.m +++ b/Motion_Planning/6Star_Convex/StarConvex.m @@ -112,7 +112,7 @@ function ConstructConvexFromStar(obj) obj.Edges(i).p1 = obj.convex_hull(i, :); obj.Edges(i).p2 = obj.convex_hull(i+1, :); vec = obj.Edges(i).p2 - obj.Edges(i).p1; - n_vec = [-vec(2), vec(1)]; + n_vec = [-vec(2), vec(1)]; % Left is the inside part obj.Edges(i).n = n_vec / norm(n_vec); end end @@ -129,14 +129,78 @@ function ConstructConvexFromStar(obj) % facet vertices detemined by k are CCW n_vec = cross(obj.convex_hull(i).p3 - obj.convex_hull(i).p1, ... obj.convex_hull(i).p2 - obj.convex_hull(i).p1); + % Inverse if the vector point to the other side + if dot(n_vec, obj.Po_ - obj.convex_hull(i).p1) < -obj.epsilon_ + n_vec = -n_vec; + end obj.convex_hull(i).n = n_vec / norm(n_vec); end obj.Edges = obj.convex_hull; end -% function ShrinkToConvex(obj) -% -% end + function ShrinkToConvex(obj) + % Shrink the convex hull to pointcloud-free convex region + obj.A = zeros(size(obj.Edges, 2), obj.dim); + obj.b = zeros(size(obj.Edges, 2), 1); + for i = 1: size(obj.Edges, 2) + polyhedron = Polyhedron(); + base = Hyperplane(obj.Edges(i).p1,... + obj.Edges(i).n); + polyhedron.add(base); + if obj.dim == 2 % 2d dimension + % Add first edge + vec1 = obj.Edges(i).p1 - obj.Po_; + n_vec1 = [-vec1(2), vec1(1)]; % Left is the inside part + n1 = n_vec1 / norm(n_vec1); + polyhedron.add(Hyperplane(obj.Po_, n1)); + % Add second edge + vec2 = obj.Po_ - obj.Edges(i).p2; + n_vec2 = [-vec2(2), vec2(1)]; + n2 = n_vec2 / norm(n_vec2); + polyhedron.add(Hyperplane(obj.Po_, n2)); + else % 3d dimension + % Add first edge + n1 = cross(obj.Edges(i).p2 - obj.Po_, ... + obj.Edges(i).p1 - obj.Po_); + n1 = n1 / norm(n1); + if dot(n1, obj.Edges(i).p3 - obj.Po_) < -obj.epsilon_ + n1 = -n1; + end + polyhedron.add(Hyperplane(obj.Po_, n1)); + % Add second edge + n2 = cross(obj.Edges(i).p3 - obj.Po_, ... + obj.Edges(i).p2 - obj.Po_); + n2 = n2 / norm(n2); + if dot(n2, obj.Edges(i).p1 - obj.Po_) < -obj.epsilon_ + n2 = -n2; + end + polyhedron.add(Hyperplane(obj.Po_, n2)); + % Add third edge + n3 = cross(obj.Edges(i).p1 - obj.Po_, ... + obj.Edges(i).p3 - obj.Po_); + n3 = n3 / norm(n3); + if dot(n3, obj.Edges(i).p2 - obj.Po_) < -obj.epsilon_ + n3 = -n3; + end + polyhedron.add(Hyperplane(obj.Po_, n3)); + end + % Iterate every point inside, find the furthest to base + furthest_dis = -1; + pi = obj.Edges(i).p1; + for j = 1: size(obj.P_in_, 1) + p_in = obj.P_in_(j,:); + if (polyhedron.inside(p_in)) % CHECK IF IT NEEDS TO BE REVERSED + dis = base.signed_dist(p_in); + if (dis >= furthest_dis) + pi = p_in; + furthest_dis = dis; + end + end + end + obj.A(i, :) = base.n_; + obj.b(i) = dot(base.n_, pi); + end + end function PlotConvexHull(obj) plot([obj.convex_hull(1, :), obj.convex_hull(1, 1)], ... From cddbfd431ea74826d6579856c8d0e1b4a4d27970 Mon Sep 17 00:00:00 2001 From: Shaoshuaizyk Date: Mon, 1 Nov 2021 13:49:27 -0700 Subject: [PATCH 05/15] debugging input --- .../6Star_Convex/LargeConvexPolytopes.m | 43 ++++++++++++++++++- Motion_Planning/6Star_Convex/SphereFlipping.m | 4 -- Motion_Planning/6Star_Convex/StarConvex.m | 13 +++--- 3 files changed, 49 insertions(+), 11 deletions(-) delete mode 100644 Motion_Planning/6Star_Convex/SphereFlipping.m diff --git a/Motion_Planning/6Star_Convex/LargeConvexPolytopes.m b/Motion_Planning/6Star_Convex/LargeConvexPolytopes.m index 8cbd43a..903e891 100644 --- a/Motion_Planning/6Star_Convex/LargeConvexPolytopes.m +++ b/Motion_Planning/6Star_Convex/LargeConvexPolytopes.m @@ -2,14 +2,53 @@ % Given a set of pointclouds coordinates n*3 in 3d, and the current position. % With the user defined radius R as well. % Return a convex region denoted by Ax <= b + dim = length(pos); + assert(dim == 2 || dim == 3, "Wrong dimension! Check input data!"); + + pointclouds_xyz = AddSurroundingPoints(pointclouds_xyz, pos, R); flipping_xyz = zeros(size(pointclouds_xyz)); for i = 1: size(pointclouds_xyz, 2) flipping_xyz(i, :) = SphereFlipping(pos, pointclouds_xyz(i, :), R); end k = convhull(flipping_xyz); - sc = StarConvex(pointclouds_xyz(k, 1), pointclouds_xyz(k, 2), pos, pointclouds_xyz(k, 3)); + if dim == 2 + sc = StarConvex(pointclouds_xyz(k, 1), pointclouds_xyz(k, 2), pos); + else + sc = StarConvex(pointclouds_xyz(k, 1), pointclouds_xyz(k, 2), pos, pointclouds_xyz(k, 3)); + end sc.ConstructConvexFromStar(); sc.ShrinkToConvex(); A = sc.A; b = sc.b; -end \ No newline at end of file +end + +function p_xyz = AddSurroundingPoints(pointclouds_xyz, pos, R) + % If there are not enough surrounding points, add surrounding points + % manually. For 2d, add them as a square, for 3d, add them as a cube + dim = length(pos); + if dim == 2 + % Add four points as a square. + rc = R / sqrt(2); + pointclouds_xyz(end+1, :) = [pos(1)+rc, pos(2)+rc]; + pointclouds_xyz(end+1, :) = [pos(1)+rc, pos(2)-rc]; + pointclouds_xyz(end+1, :) = [pos(1)-rc, pos(2)-rc]; + pointclouds_xyz(end+1, :) = [pos(1)-rc, pos(2)+rc]; + else + % Add eight points as a cube. + rc = R / sqrt(3); + pointclouds_xyz(end+1, :) = [pos(1)+rc, pos(2)+rc, pos(3)-rc]; + pointclouds_xyz(end+1, :) = [pos(1)+rc, pos(2)-rc, pos(3)+rc]; + pointclouds_xyz(end+1, :) = [pos(1)-rc, pos(2)+rc, pos(3)+rc]; + pointclouds_xyz(end+1, :) = [pos(1)+rc, pos(2)-rc, pos(3)-rc]; + pointclouds_xyz(end+1, :) = [pos(1)-rc, pos(2)+rc, pos(3)-rc]; + pointclouds_xyz(end+1, :) = [pos(1)-rc, pos(2)-rc, pos(3)+rc]; + pointclouds_xyz(end+1, :) = [pos(1)-rc, pos(2)-rc, pos(3)-rc]; + pointclouds_xyz(end+1, :) = [pos(1)+rc, pos(2)+rc, pos(3)+rc]; + end + p_xyz = pointclouds_xyz; +end + +function p2 = SphereFlipping(pos, p1, R) + % Sphere flipping p1 from pos by a radius R to p2 + p2 = pos + (p1 - pos) * (2 * R - norm(p1 - pos)) / norm(p1 - pos); +end diff --git a/Motion_Planning/6Star_Convex/SphereFlipping.m b/Motion_Planning/6Star_Convex/SphereFlipping.m deleted file mode 100644 index cfbe7ae..0000000 --- a/Motion_Planning/6Star_Convex/SphereFlipping.m +++ /dev/null @@ -1,4 +0,0 @@ -function p2 = SphereFlipping(pos, p1, R) - % Sphere flipping p1 from pos by a radius R to p2 - p2 = pos + (p1 - pos) * (2 * R - norm(p1 - pos)) / norm(p1 - pos); -end \ No newline at end of file diff --git a/Motion_Planning/6Star_Convex/StarConvex.m b/Motion_Planning/6Star_Convex/StarConvex.m index e00424d..98da7e4 100644 --- a/Motion_Planning/6Star_Convex/StarConvex.m +++ b/Motion_Planning/6Star_Convex/StarConvex.m @@ -34,11 +34,11 @@ if nargin > 3 obj.zs_ = zs; obj.dim = 3; + obj.vertices_ = [obj.xs_, obj.ys_, obj.zs_]; else - obj.zs_ = zeros(size(xs)); obj.dim = 2; + obj.vertices_ = [obj.xs_, obj.ys_]; end - obj.vertices_ = [obj.xs_, obj.ys_, obj.zs_]; end function ConstructConvexFromStar(obj) @@ -81,18 +81,20 @@ function ConstructConvexFromStar(obj) pre_pt = obj.convex_hull(pre_ind, :); cur_pt = obj.convex_hull(cur_ind, :); next_pt = obj.convex_hull(next_ind, :); - cross_turn = cross(cur_pt - pre_pt, next_pt - cur_pt); + v1 = cur_pt - pre_pt; + v2 = next_pt - cur_pt; + cross_turn = v1(1)*v2(2) - v1(2)*v2(1); % fprintf("Pre index is %d, current index is %d, next index is %d \n", pre_ind, cur_ind, next_ind); % fprintf("Pre pt is [%.2f, %.2f, %.2f], cur pt is [%.2f, %.2f, %.2f]," + ... % " next pt is [%.2f, %.2f, %.2f]", pre_pt(1), pre_pt(2), pre_pt(3),... % cur_pt(1), cur_pt(2), cur_pt(3), next_pt(1), next_pt(2), next_pt(3)); % fprintf("crossturn is %f \n", cross_turn(3)); - if cross_turn(3) > obj.epsilon_ % left turn if CCW + if cross_turn > obj.epsilon_ % left turn if CCW cur_ind = mod(cur_ind, size(obj.convex_hull, 1)) + 1; if norm(obj.convex_hull(cur_ind, :) - init_pt) < obj.epsilon_ done = true; end - elseif cross_turn(3) < -obj.epsilon_ % right turn if CCW + elseif cross_turn < -obj.epsilon_ % right turn if CCW obj.P_in_(end+1, :) = obj.convex_hull(cur_ind, :); obj.convex_hull(cur_ind, :) = []; % start from bottom most and left point guarantee @@ -116,6 +118,7 @@ function ConstructConvexFromStar(obj) obj.Edges(i).n = n_vec / norm(n_vec); end end + disp(obj.vertices_); k = convhull(obj.vertices_, 'Simplify',true); ind_in = setdiff(1:size(obj.vertices_, 1), reshape(k, 1, 3 * size(k, 1))); From 90fa7b78b5e92446be06a50141fd29a77a5d9046 Mon Sep 17 00:00:00 2001 From: Shaoshuaizyk Date: Mon, 1 Nov 2021 18:01:14 -0700 Subject: [PATCH 06/15] no error, need to be verified with data --- .../6Star_Convex/LargeConvexPolytopes.m | 9 +- Motion_Planning/6Star_Convex/StarConvex.m | 174 +++++++++--------- 2 files changed, 93 insertions(+), 90 deletions(-) diff --git a/Motion_Planning/6Star_Convex/LargeConvexPolytopes.m b/Motion_Planning/6Star_Convex/LargeConvexPolytopes.m index 903e891..1864945 100644 --- a/Motion_Planning/6Star_Convex/LargeConvexPolytopes.m +++ b/Motion_Planning/6Star_Convex/LargeConvexPolytopes.m @@ -7,14 +7,15 @@ pointclouds_xyz = AddSurroundingPoints(pointclouds_xyz, pos, R); flipping_xyz = zeros(size(pointclouds_xyz)); - for i = 1: size(pointclouds_xyz, 2) + for i = 1: size(pointclouds_xyz, 1) flipping_xyz(i, :) = SphereFlipping(pos, pointclouds_xyz(i, :), R); end k = convhull(flipping_xyz); if dim == 2 sc = StarConvex(pointclouds_xyz(k, 1), pointclouds_xyz(k, 2), pos); else - sc = StarConvex(pointclouds_xyz(k, 1), pointclouds_xyz(k, 2), pos, pointclouds_xyz(k, 3)); + sc = StarConvex(pointclouds_xyz(k, 1), pointclouds_xyz(k, 2),... + pos, pointclouds_xyz(k, 3)); end sc.ConstructConvexFromStar(); sc.ShrinkToConvex(); @@ -30,9 +31,9 @@ % Add four points as a square. rc = R / sqrt(2); pointclouds_xyz(end+1, :) = [pos(1)+rc, pos(2)+rc]; - pointclouds_xyz(end+1, :) = [pos(1)+rc, pos(2)-rc]; - pointclouds_xyz(end+1, :) = [pos(1)-rc, pos(2)-rc]; pointclouds_xyz(end+1, :) = [pos(1)-rc, pos(2)+rc]; + pointclouds_xyz(end+1, :) = [pos(1)-rc, pos(2)-rc]; + pointclouds_xyz(end+1, :) = [pos(1)+rc, pos(2)-rc]; else % Add eight points as a cube. rc = R / sqrt(3); diff --git a/Motion_Planning/6Star_Convex/StarConvex.m b/Motion_Planning/6Star_Convex/StarConvex.m index 98da7e4..eb04e8c 100644 --- a/Motion_Planning/6Star_Convex/StarConvex.m +++ b/Motion_Planning/6Star_Convex/StarConvex.m @@ -43,72 +43,75 @@ function ConstructConvexFromStar(obj) if obj.dim == 2 - % Graham Scan, reference - % http://www.cosy.sbg.ac.at/~held/teaching/compgeo/cg_study.pdf - - % Find the bottommost point - ymin = obj.vertices_(1, 2); - min_ind = 1; - for i = 2:size(obj.vertices_,1) - y = obj.vertices_(i, 2); - % Pick the bottom-most or choose the left most point in - % case of tie - if y < ymin || (y == ymin && obj.vertices_(i, 1) < ... - obj.vertices_(min_ind, 1)) - ymin = obj.vertices_(i, 2); - min_ind = i; - end - end - obj.vertices_ = circshift(obj.vertices_, [min_ind, 0]); - - obj.convex_hull = obj.vertices_; - - cur_ind = 1; - obj.P_in_ = []; - init_pt = obj.convex_hull(cur_ind, :); - done = false; - while done == false - if cur_ind == 1 - pre_ind = size(obj.convex_hull, 1); - next_ind = cur_ind + 1; - elseif cur_ind == size(obj.convex_hull, 1) - pre_ind = cur_ind - 1; - next_ind = 1; - else - pre_ind = cur_ind - 1; - next_ind = cur_ind + 1; - end - pre_pt = obj.convex_hull(pre_ind, :); - cur_pt = obj.convex_hull(cur_ind, :); - next_pt = obj.convex_hull(next_ind, :); - v1 = cur_pt - pre_pt; - v2 = next_pt - cur_pt; - cross_turn = v1(1)*v2(2) - v1(2)*v2(1); - % fprintf("Pre index is %d, current index is %d, next index is %d \n", pre_ind, cur_ind, next_ind); - % fprintf("Pre pt is [%.2f, %.2f, %.2f], cur pt is [%.2f, %.2f, %.2f]," + ... - % " next pt is [%.2f, %.2f, %.2f]", pre_pt(1), pre_pt(2), pre_pt(3),... - % cur_pt(1), cur_pt(2), cur_pt(3), next_pt(1), next_pt(2), next_pt(3)); - % fprintf("crossturn is %f \n", cross_turn(3)); - if cross_turn > obj.epsilon_ % left turn if CCW - cur_ind = mod(cur_ind, size(obj.convex_hull, 1)) + 1; - if norm(obj.convex_hull(cur_ind, :) - init_pt) < obj.epsilon_ - done = true; - end - elseif cross_turn < -obj.epsilon_ % right turn if CCW - obj.P_in_(end+1, :) = obj.convex_hull(cur_ind, :); - obj.convex_hull(cur_ind, :) = []; - % start from bottom most and left point guarantee - % cur_ind is not 1 - cur_ind = cur_ind - 1; - else % collinear - obj.convex_hull(cur_ind, :) = []; - if norm(obj.convex_hull(cur_ind, :)-init_pt) < obj.epsilon_ - done = true; - end - end - end - obj.convex_hull(end+1, :) = obj.convex_hull(1, :); - +% % Graham Scan, reference +% % http://www.cosy.sbg.ac.at/~held/teaching/compgeo/cg_study.pdf +% +% % Find the bottommost point +% ymin = obj.vertices_(1, 2); +% min_ind = 1; +% for i = 2:size(obj.vertices_,1) +% y = obj.vertices_(i, 2); +% % Pick the bottom-most or choose the left most point in +% % case of tie +% if y < ymin || (y == ymin && obj.vertices_(i, 1) < ... +% obj.vertices_(min_ind, 1)) +% ymin = obj.vertices_(i, 2); +% min_ind = i; +% end +% end +% obj.vertices_ = circshift(obj.vertices_, [min_ind, 0]); +% +% obj.convex_hull = obj.vertices_; +% +% cur_ind = 1; +% obj.P_in_ = []; +% init_pt = obj.convex_hull(cur_ind, :); +% done = false; +% while done == false +% if cur_ind == 1 +% pre_ind = size(obj.convex_hull, 1); +% next_ind = cur_ind + 1; +% elseif cur_ind == size(obj.convex_hull, 1) +% pre_ind = cur_ind - 1; +% next_ind = 1; +% else +% pre_ind = cur_ind - 1; +% next_ind = cur_ind + 1; +% end +% pre_pt = obj.convex_hull(pre_ind, :); +% cur_pt = obj.convex_hull(cur_ind, :); +% next_pt = obj.convex_hull(next_ind, :); +% v1 = cur_pt - pre_pt; +% v2 = next_pt - cur_pt; +% cross_turn = v1(1)*v2(2) - v1(2)*v2(1); +% % fprintf("Pre index is %d, current index is %d, next index is %d \n", pre_ind, cur_ind, next_ind); +% % fprintf("Pre pt is [%.2f, %.2f, %.2f], cur pt is [%.2f, %.2f, %.2f]," + ... +% % " next pt is [%.2f, %.2f, %.2f]", pre_pt(1), pre_pt(2), pre_pt(3),... +% % cur_pt(1), cur_pt(2), cur_pt(3), next_pt(1), next_pt(2), next_pt(3)); +% % fprintf("crossturn is %f \n", cross_turn(3)); +% if cross_turn > obj.epsilon_ % left turn if CCW +% cur_ind = mod(cur_ind, size(obj.convex_hull, 1)) + 1; +% if norm(obj.convex_hull(cur_ind, :) - init_pt) < obj.epsilon_ +% done = true; +% end +% elseif cross_turn < -obj.epsilon_ % right turn if CCW +% obj.P_in_(end+1, :) = obj.convex_hull(cur_ind, :); +% obj.convex_hull(cur_ind, :) = []; +% % start from bottom most and left point guarantee +% % cur_ind is not 1 +% cur_ind = cur_ind - 1; +% else % collinear +% obj.convex_hull(cur_ind, :) = []; +% if norm(obj.convex_hull(cur_ind, :)-init_pt) < obj.epsilon_ +% done = true; +% end +% end +% end +% obj.convex_hull(end+1, :) = obj.convex_hull(1, :); + k = convhull(obj.vertices_, 'Simplify', true); + ind_in = setdiff(1:size(obj.vertices_, 1), k); + obj.P_in_ = obj.vertices_(ind_in, :); + obj.convex_hull = obj.vertices_(k, :); obj.Edges = repmat(struct, 1, size(obj.convex_hull, 1)-1); for i = 1: size(obj.convex_hull, 1)-1 obj.Edges(i).p1 = obj.convex_hull(i, :); @@ -117,28 +120,27 @@ function ConstructConvexFromStar(obj) n_vec = [-vec(2), vec(1)]; % Left is the inside part obj.Edges(i).n = n_vec / norm(n_vec); end - end - disp(obj.vertices_); - - k = convhull(obj.vertices_, 'Simplify',true); - ind_in = setdiff(1:size(obj.vertices_, 1), reshape(k, 1, 3 * size(k, 1))); - obj.P_in_ = obj.vertices_(ind_in, :); - obj.convex_hull = repmat(struct, 1, size(k, 1)); - for i = 1: size(k, 1) - obj.convex_hull(i).p1 = obj.vertices_(k(i, 1), :); - obj.convex_hull(i).p2 = obj.vertices_(k(i, 2), :); - obj.convex_hull(i).p3 = obj.vertices_(k(i, 3), :); - % Normal vector of this hyperedge. It points inside if - % facet vertices detemined by k are CCW - n_vec = cross(obj.convex_hull(i).p3 - obj.convex_hull(i).p1, ... - obj.convex_hull(i).p2 - obj.convex_hull(i).p1); - % Inverse if the vector point to the other side - if dot(n_vec, obj.Po_ - obj.convex_hull(i).p1) < -obj.epsilon_ - n_vec = -n_vec; + else + k = convhull(obj.vertices_, 'Simplify',true); + ind_in = setdiff(1:size(obj.vertices_, 1), reshape(k, 1, 3 * size(k, 1))); + obj.P_in_ = obj.vertices_(ind_in, :); + obj.convex_hull = repmat(struct, 1, size(k, 1)); + for i = 1: size(k, 1) + obj.convex_hull(i).p1 = obj.vertices_(k(i, 1), :); + obj.convex_hull(i).p2 = obj.vertices_(k(i, 2), :); + obj.convex_hull(i).p3 = obj.vertices_(k(i, 3), :); + % Normal vector of this hyperedge. It points inside if + % facet vertices detemined by k are CCW + n_vec = cross(obj.convex_hull(i).p3 - obj.convex_hull(i).p1, ... + obj.convex_hull(i).p2 - obj.convex_hull(i).p1); + % Inverse if the vector point to the other side + if dot(n_vec, obj.Po_ - obj.convex_hull(i).p1) < -obj.epsilon_ + n_vec = -n_vec; + end + obj.convex_hull(i).n = n_vec / norm(n_vec); end - obj.convex_hull(i).n = n_vec / norm(n_vec); + obj.Edges = obj.convex_hull; end - obj.Edges = obj.convex_hull; end function ShrinkToConvex(obj) From f3b940f46705751bbc641f0d5bb0265d48808af7 Mon Sep 17 00:00:00 2001 From: Shaoshuaizyk Date: Tue, 2 Nov 2021 12:58:56 -0700 Subject: [PATCH 07/15] add testing data --- Motion_Planning/xyzPoints.mat | Bin 0 -> 44766 bytes 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 Motion_Planning/xyzPoints.mat diff --git a/Motion_Planning/xyzPoints.mat b/Motion_Planning/xyzPoints.mat new file mode 100644 index 0000000000000000000000000000000000000000..d4f8b5b8c28db9e1f016647b55131ea8297b36db GIT binary patch literal 44766 zcma&NcUV)|_dPr_j=e->6s1K}WDtb`q(d@_hz+75y-5=lY0@DiW1|-Zr6&Z|fXr562OI_{6|KGn~{`dFmHB%7q-m>(UUqZ5r)8jC(Kq)-ooXwd_T@S1m ztn50@+vZ+1$@QultSS9N_U-qRZ=XzlF*J^T6leOFboI%K`e6yui8Q}X_rF&Cr+j)2 z^wfY@{xeC)drsH|X67iFX+R+J42b@SYBqOfUMDg9LZ_0#`4XY^98S6v@2!AWk~bYd z9_NGP)Gy>AX_8K|zSENA?7VBeLOMC{Dw_Rwas3YWcB$pFPwSMzn_`9uo}W*Sls%w4 z`5a&|#B&Rb#?ELBrm2t4I@M|Dj}$zL7;^FtJ5QBU8oM-wQ4EIg9>fc|@by_mgw~^eqk|!tpdpahNB|-41NP^gGW3^{; zeyCgstEc|T>UTD8-THvM$e(Eo<#^*>kA)j@5Aw}6pjQzcHY3K^Vk3#hNg4Sj#$XG_ z)}alolIf<0P@$Zk3iw0V@yCS@*>*j4|2{k6RMp(F>uO_x)mpn6DZb+4OU=Gk^2{~X zQWE5%;MNNwpAm53W+>HT6Sm8&U)wqfYQR+-)HCe6jIe|>J@j1DZ@Hd_sJ1BH>)oqz z0_r)3>_2k|fxK5&QqRzh^W0jwP}jP3+xGy{<*U72FrBNhjGacnSB+q~vqkRxKickg z<^Lq@ov1RFn0wNpUqVjD2C-D9(m>LAF$)@Wu7%z^=TA|3OVvjQR3EIRJBXJIKdJS7UY;yqT(N`q_7`%RXCD?oh5G_=1rs)#k~mwp(X<=W;r=Hm zpJkl+CI&B5U$e`(pVCYk*F#6Dy)*nDIBLTYMcWrVM`jj~{ez^1!iCA1E_j^(d7N_{ zlUjogBUM{;R~KV@yc`ejr7y9bWkY%&B#!r`JgjX`LHOFPKqSnIIv9F_``5XIGwqgD9JmidHq&T1pfr9nBCqnn zz-3CW1kbCtXMZtr<_V?$tIXi~hqc#7WY$u*wm54OxJ!W-z6*7eB2@OU@{F1$Od`x!jwIfy2`U!Fa>POFmw3tbdmRj%6(h8RO(dMV zWS3p%c$0J(q$y!*sJgG9Q)dVZ3E%5yX>p+;aOD1a&&*}i-a=|8>~Z4(rxp3}Zhgy! z)KH}(G5nYLsgwN0$hx$o$X29NY@MX~LV1%*`n9G|^PzA~pLVOFS(e~BQVl%d1n+>f zMJ*e_I}mMA_Gz4hN5CXWFi8SD!S9)`bA^0n+eOqn(&eeGQLNhKhin+ULys;$dJ5v! z#Cg@gd8JD-a-qvdv_-X&j5cFHHj0q63 zd!NL46$-cmEGmnRFSkW)91>v$JYIF8%ZG~F+5Zno?u~9RL!RFjRSd+%{)T|7fh$0O z%hsrQG4PQ%kd3Oy(tJ}is4xzcp$vJC;T*(?fdY6aB%{0Vt5CZ9S%@1^{2nZR4@3gs z04^cvISx)L%ye=5z%YI+h#n(q06-BsfYg7{of9ztG*J{7F?(>5Q9Vpyg>5T#KnFg7 z7K}EdMImoUfOEyZ0eHZnC`SPGT$OfJHE^;d_%IN@RXY&M9Z69Gx&o&DiXHe_Y|mJi7rp{~AdbSuJmeUp*&cDqeG;#oATO8J6=Z&^5u% zRRk0`Eix)jqlk9_Nz@C7Y2NcVtsNd6E8V2S z1OVCqPVx?mqAvxcDQJxxqMUBKtp~0I&f;0}XHl zv~tm;5@7EVQpDq4a^3I}pC0PRk`pO7}e z9yf_t=A8|E%|(>^Ux49;b=le zf_g{ZFB?;;&ZJ{k206K&gT=&H`a*b^Cd7w%5&q2+wH&;eukr;q%TUR&TUC?gYCt@E zwoj6MKw}YHJ6=~YmNx5uT}7R^3R?;0%<9L|S9tp{8b#-u3Y^0yiEoL=tl)-uoXtR| zFSXm3RUfb#>ECu5Vt>%8yWrdTk5~W6ryC<1-M0FIG$GYg^{mO_SD`ahr@vJDh2upV zybF>}7nYRZF%-~5(}?f^yNdZ3KKl-~_EPBB+fdM^5ria+)Lt=IfO2x1?H;dwhHuvV zG+YRcQ|}%v+urnb<-0+(ebcnColy|wFnEiNPFk945PWtZetEoE({=P#u)5-8)0yu4 zB#nl*)9j<+zp&&SL6V`3LPk^DGP8me zEInp6y2{Tiys10p$@zJo(x3a%s?%!fRy?kzOJQq2>ozPtwDmJXeX|CW_!**#B4rQM zUWlxlq-t*dzH=ro$-HDo#FpyLJ8Tg6b+W|KdUZPxlqOciK z+2n$|O*Ab|x?f))B)qjm`l$O)eXH)n+cEwb_R#tQNeENN7X_S(XizjuW~Yrq8kV?2 z(}dtVAU)PIT+8RwpvI^y4aGWxXx(q$&vetnv8QNh51!ZQ9%4T5K~JYmMoNzcpS@^! zuI{z2^SI)2*y;l}!mmEKOEaMjeIwWm7v4U}G5>2u;icEy;;VSN9+4{=wb>{MSp9vb zl1Yc+M^4o@%czY)JK?23cLs+YKHIrF1`JZD}^132k)B0Hb7G3G$V! zaW1(rYPnX^aF|70QnjQZ8afu7kWDhf-KEH>qv7bx$|3YNt0^zSTj^z8?E;6kUL?%m zA)&HO$r38eH1r{wxAb~oU(QPmkEjz-(}TULiu)=U_-~3UQR%#pG8z$B3)-UVgZZ@GbhK~D?^D-2cYa<>ig$sxmUeyg!|KV!rjp;tf^2*i@ z(MGzHQ%*g4f;hxyLiBM3W3hoVy}>Klrz&@n!Trg@t%yz62WywM>sJ>~NBX~g@W`vz zId{LpO2d<(#-N!BKYgxmyJ?V$d1X>tURSQ!#n5$u7iN;{iujfM_bLxOQ9Hkvyu9Vd zxU%k8dp29Sw)dRAlT_Ay@+(Y4<<3UsGDhLYVDe$`*gthd2U0`7q1Hri^#GatZbjMK z9g~76W0ZgAt-y;tD^2I2VZPfKcGKk}tKscZ@X&gz991WqYk~mGxh*@@^R2@1$nA@Z zL$vO+&GVx?Wqup{!Kik0sG4}G0hS*2MR*4@8Z@3S-pT-L6|kNGAI__BoL9gK0xW&N zlH(67TEOnW3jW`xI&c7d+yg7xI>~4x26P_U=?HWLdI4({Fcw&efD0^n(H_FBRJ68K zfa$Xp!e4*+MJO!J2>%BbxQS2l>$>MK;>GEFc*(7#{z2PC>)2h!trsmV&Ak-dJtICG zJd$pRgHuRKyiaL|!r&8g}EBBzQwjpAQ2iMv8r>Mg=!N2nBuusx) zy0Q`H=pQs%!H!Q3;g$!-{wm1m;;8C(g6l2N4T!fTX=VE<&U4_qS7@N{#D;Q@So<6V zu`TVC^aZCRm2BNTsFcr^bzN|hy+A2F6}a-isi8Z0$}wQ}R| zX6D)tZlA$$A}T*n4gPSjuK64If^%-V&uekZ3*X;n!oDcUqwi+2S?j+&4!bdajohzm z^1XIW!HV%iiGIpkJ&D%lgkcN-#}$8CL$7hZKjSpjMb!V*1!EnMSy*&O)KU_ zQWgfB8itVtXqA|U5(P-;~T{IlTx z2Xgj8rQ>@`#xF*!Oyx$b)co%7*8+I)S(4Tr)yEn)dErdKrL96t?q2L&^Xcun^YriB zA;u`{O{t%BfFBmjY^czk5H57Z?BqXkZcMH0m}^kGOP$0$H<*bWZbf0qpRnV~opTM- zj=Y%#DY|uu^BCu3386^SDuSkT7k%hBh1~RaC}KJvVY}}RdXYGV&D%3+%(^vF(xKOd zt2ty7{4i##V~23c$iTjSL2K^FfOjsAd->N*B3NP7Y~}1^@V3wf-IO=vHviH7BR3UR z_k&H*AuQg!-=is0^cd7hK~6t3xu*JF*{0HDv_W~V@B|Eg%P*jJuT?Z&Lish`sUaTG zLJrb^+LN4J=d_ds0((jb*5XtWt`S9w%T$J?$Pdw#-=0Z={x~;RzMs+}JQ-D!7M32; zz+UrQhLwD=rFBAYDK@A@>d|}77f48Rx6PI3qRM8PUWJ65zQIf0Vqsf(e<;Y!zx`-- z>;69@=J~q=r-B__rk0S#jB0{8=G!AudAn+!F9R(}MeTMjD=ouyt zAu+45Q2q@xWwh=RsCdy;S!jhmUms8c>+L#)w zA$N2~e7$BzhLzPnJZvR#F zt$$)D8`&Mg3A|lg+4Q@g2fL^hH9J1QhRaZcCy;V>e&aR^rp3Iytj?XsldK*P41{ap z9_d2nJ4_6AG7?OsAJY;%4qd2a>Y>g+a5L8)+p2BM%#Xz5gJtg-SaY%@#_ICsVM@;f zpLD6o-7xF<=xyeGlmX@FH?fzo@}cZymnl^{=AU!DF2zFU^N-iKIApU{tj8-+pgS)-;cULsm3xIS&ihN0GkZE za%62)20bpX=_0QB-u_L(prvd6>5^oXhuf)qC%$P|aBV5_*U;$=m!d$2pj*(?VK<3D zs?G+P0Ik`MB!2T+oIF#BULz{$XIObmsBC)9m{L{QF|0gKl@U~BYQ&zMX5+9XEr~G# zXIyp}tG|#atWS)pf)d@;A=K7GD@5z96F+a1ra0BI)gBks+B^C5T_~*{%zx#l?YVu< zY_k$yzGCK!c|qGSk{rPn(vh;mA%YR}vAxCtIPZ+!nylLy^dBFh0-OY;JJ?wLij)mn z)ev9b^#r?8J7ha5JmT4KMZ+aX}Hm^~Si#M`LQ@<>L8q#T1Q z%NvH0f|pc2^%`hb86RL>)@y6*zny_7i%LzTvjz#Vq#TU)e;b0}&fYdut<$;-A9V;r_rerb=ac#v z#r34#GJ8XEq}!hY_z`rzqc@mTUM_7ReC-vBK}3Z#%p72ndxy8?B%<+F$oK4#!pgAH zYU}r0O{pdBL>1Hz?^(hY(n(?Hs4#DRhU!{FVzB9%)zdfUA|&Ds-*H%J%xu`qJ2p|^ z!-RY<%Gs2?K{ZT%&ramE>X24*E;S;F--p6Kt@9|Abc@EF)ru7U*t<6~=GuohX2xf( z(7wHI>rN6fFE(P8OYdOlqZ^3HK!=6_mSbBu?Jk(OdIvqB`8m-L6XSDuA{wLuvH%-r z7A4J07D}*S2SHWqnz-Ssji5B|BOb+}$$GP#lccwU<8|Ik)b&7z9S;p>BRi=nUBQke@eZBdIKMRXmg#$o$n__0(%!@&wr>Jvz&W zppaBAyzS@r{Upk+Lio|Y4R(z*Q@taam_K?#4ik_>QAj+5hbiUyH#;?5CS!}icANdIkWNS_Fnl!o`Ie)scV=`-=y$pS*>hP;ArR?H2Ae5oM}qg{?fmXTko-C z&+QEV9&AJY4+z#(0K=EZ=E^wUK-JMGu@L?y)L#s$L71ND1` zxT`(01Jhg3-f}ZcrJji@unmBCqI!KKZd_$g%k<89k7o_2La~hXCRiXuu9Wr*0&{3F zJE~}L%5Ysqk9;L?#mAm3xcAs8p!~*gzpm5yK5mtjQImEnChKeP??P+Ir3L%D)BVP8 zv1Vt8l3V5${kn0<{h^A)0j^=NK2erLbDRr*89ZvWndrA-LaCoU6A_ip`;(}atts2T z-Ky)|Ui8-U&6~x9;FPia1dht#MAX%pwYRbGq+Q_={uf7)14<5Q_ZO$bp{IRnyyQaX z+&9r*k?-i&NW-2~-uF3B$|zF7f@q!$>1g)}TGe6Fdfy-hFK?_)aMaf4XF76e4Z)Rf zahlxclSw(C8~N_)Xlc2N+Ah#A;?z7wh^?|`d@KenDFWG&+L8;(0%AF*t|(L*$A zq8g@|lU;*fvdZr%AQno7IdgK{NIhW@bk2w(4IaSK<@v|KcSeC*a|xhAA9ys-W;n07 z?NQwtkoQ2R5m(WPgN=0gusNgk<6w0H=M^VizX`ZO5f3`94cyQTCp&!luXqyh!fSmRgN8ihN zfEk&1~YmZtKTaOWY2LOnCjEE5rsX^w$#4(TW6ng}M z%@s#~*#llc*9wD4fs=_MtfB^ZN47_`CV(;k)-@rgadi33-y_Jtmj?hrfH>C83nJ>= za0wtFc7QKRog1|dAh~YeKv+BoK#)ujn{vo)5*mvm7l=>a!He+#BY+P7nUkV@q`Jbd zHrYocB>>K0i$RH;zbb$`tJ2^rS|p=ly1d_9OUD&>G(bU(IHG@#I}8A(kN=mXe^UkY z7x&cszvLu{z}x|H@LyhU$oNN9#zm78K&Pa|Z1bf=QYZxE^smI=;>cs5?`bZF#8LcT ziUD?k#HowZ(V-`f9N@?%2}s9Zkki1+zxnx>kO3e|jQ>dtNa0n8n0Fu;Aa|z$c>{6+ zkf~#nn()TrYy$unx0s_t|ku z+*kI0-hu8SiUBA<116U*3pA(W^jS+Sx)38JXt~pO&I{%otN%q>&#wbV(=U`7B6@2WQ_p zho=!Bo2fUy8(r{;9YWNwB?Q~O$sY*$d9Ac0P_t_=Ol!Xd+M+AasloTlE@oQorem4U z+Yt@lqoEC(MxUW8pHlF4WfvJV&s=Jm=blg(FP1W_i}zL$X+ZZKzv>W`yz-X(dVQX5 zvefjXVo!`dGfD}T$27ix9q-vM+%Ed6e>W}EKJnos9y{P7jXrs@%YtR7c1>MnJR`T_ z?Kg)G+iPypgI=NMVrHuvj!(L$DbU_Le%%EzxO*vRH9EK4?O;n$uwE0NA?PP>1%UB< zLnQtQn(U_>o2xDNz~j$Y7Xx3lDkZbc33Gs9lcQ9IR{Glu2Y~o=Me#liN3}OAZ zJ;biVe}{}~w`G$P;mAzBk?%s(!BIk9ffdJPGuQUFu&JQkCY726qbo+ORV}DGJo+2~ zR^a=Yd#VHWC%vfa%PrOFKh~EyJN-&(_mM4}){ET+p-;^_V5_%g7i;l-Jcb%cDjdD_ zV&-7tMC~rws5526b;;~31^;O3{0UO#)+;@kMpq-j>LBz`=;Cpw7PElI_Sd#7ln*1P zp3D}g@%#M0Ic^2i`?UUKKi!f!do)qONTObERVVCz)gMrI_R)=iRWn3tSTjAB5D!Vr zwX5}3AWv`nX}kP={I`v4>0KW7XSuuANZC7J_n>Jf*2g~^X7`B{J8W!9eO`y=zl5^C?SFCF_^E?g&vJm;{H%5rVI>2O{?X{lxLOzYcJg&_YpHyzpB zr6jZcFY~^X+*if0QE{amaYL@nJ7|f-y+s#?QL1vV*v(^yJsFdQE>5zJuPAUVr z2Y(fNj(Mw6w%?rcl&EF59&pk|$JPfl`@qb+&09Ali$mCoLl_MHVwV4XN*aG94&zw9 zKQz9nxiO4rr<1xzdN$T38fMJeq5W^R7z#VJ{-&QHDU zJi%@$Hf>hb>AmCbaJna0Ugjt6`j2_m2S)`^Tj|Tdl;NrS=lEmINsK{Sp6{D#k1>y* zmu43ctc}1-jPHgD@>_=7?~1;9+3iN1up#}8&)wf>Gmj{0%})`HYS9q;t%MqCf=iDu zgJFH_8M>^x`S$thPS=?*9m1pa-Xq%&T7_`5YqfcfO^6qZN$m=f$xx-oKETL)G5#M?4H$Jy+MB9AGOJfv_(aIrnQ)fnp64C;=2Z{&s`=nq3!sKFGlh5Z!L#7pPP zK%y~USf`k_Z(g8n6taV({w&XGq$Xo}Ru`}J%)z~NZbM)W2ci6`%BjJHvkjw<)*LpA z`XuXpe&so={N4BjyRM>J^1;c}vmCkF9Db}$t~XVpxnE8vC*SHU{cTvPf8rD5{`eE6 z9?tXh^*=Z4um58nE)iz$_$t4|Z@#izo7*4ctIA`_mzK-9Xq#$l;9Ae0CKMs%q1vz< z-alZO>e3Cuh*PLgR;nLt>eXj%N;Yh`5GAOUj6^3=f7st|8bSve5uSw(-KZvQb-SPk zLFmhb!H2DZAHfk&zq5#oG+o6O`0UN3A1Kb|n{ZdFv#d3{2F3yNbPpL_Xe1Wnc9CYSqaZdn{Y_ zE*dp@3$G$nR&odx&Hd&qn{lUKWElOAeC0;uq5VZaLel(C@_!gi?9JcCemlZZ=F1$t z-bm5AgU#CCH~7eq*Cvr>*$saez>)1WU4>O?`oGe0KIbRv|4r~oGXfeI%*~!C+G1bX z#SC+K$2-8tcl_3DF#Oz?d!!zg*g62kMjo_cZ(mgU^gz+G88j3z=F;0H zy>oq(t;w`*jNO7u;th}Lz7>4J)1>)R?B?6ab=LI8{Ts84#QBo4(y`&}KPf*y|6$l; z+2@4T?(p~3$qsbfV8lOf9lr3pVL!jkyL;mlRN2ZL3*p6t2PlQ^{u7tqf>R_s_2{om z((S+MmS}#;HW`df@GZvPB6OCtdDC(2l-X~(`x{Ifb6E$26Q0leR2q^EJruLuUuI)R zzQKitjJLLpvdpmCmxVX$uh>)N4zue$)*TTTsGL={qmEM=cccX68G9d zFg7S}4~@4L*WB-)*awxg7h)XeuK!8PcG^p+imEm7?4a!)gu`YgU9TppW^-7mpTpGz`ow0ySw^Nr+A%9#cAWy8< zI&d?BD@F`9#`sZN)=}SI8~NsNBNjN!x2`!~m3-Dv1^>rF^}Pw*Iy zs{El822almP~LB-wB>y}arLX5mYm^{bn3=gjqk({_s2j@Dww@p-*hoYAKbCNJ#8-yO2ixsrS&Y{z5CdZq6N z3{A582McAxntT*w7+o&QEVX7eSfxZ_Xb5_&%IC(5ywMQ5iyZ!^mg5gz%Hh6~JU%x9 z9j9(LmtQcny2kQ@oDFgx)S3M=*#AXds&xyUXS6{vXN=*u>V3~C7{*Y)Uzkk~KKCr; z$2Q<0j_=Kd<^gYa9#1uLF7=Y5LFyipgvXI`k{+|x2S6u~s88Q)YVYbS2-05U!B(!z z?pe5<&y-*6{34M^Kao)5RrDI1wbMoWXToXdw%}d(Y<^?|hjldZ(eRzEeR#O?I6D*P zd+vE@PH?N-x)silYhwu0dDLvVxnOOvk*(~EITPs{zm3qR|3EjQQX{h|!|~!)K$LTH zc{VG5CPR0=ZSL9Fc_*zsCR9C`&h;x$*3#T_^hE99>f|{y`>yMSDbqf_Uy7erx~2Md zJD^^=%Gn?9eHz=kQU%kQ=m8e>Wsn4$q2_xfUST!G=w;p&*?hB%jCov_EUWM)PN-3h zkm!ANm4uj-!v(17&{5Hz%wv>|bFU*1dVe=+T-jv;tJ~*MrJo(~WvDXp6rb6>-EfZb z3is}suZv`h&497a#~QcU#o(r7;pMoow=KYaQ2p@>*Kzi%2=9&4s%>+HWIe0(V#Pi9 zp)|&OZnk8-C48sZ2i*MBp9wXJ-TRl6D?LtRhYff754*QoWcET-ls1g`U9Aztf9p_U zO{XN+!ppDoSp>yDH_LrxD@~DcaLb ziYx2)Dhy!&!goj}e&|u@?ShpKRWAHO!rJbh_Bp!8L?7|G1}le0Xxvy{Kni2fnUYK9 z(y)#Bq!Umh&lc{ca@d)pWl8M7jg7AS13gW?&=Sr~d!xw1NS-wb_V5(q(Q1&+uV%|+ zR-(bAZH{u49^Zd7is#vQeA@FcJ$x9m)omIjF%Lt&Nj5oErB-KXx64F`*v_}l2ulyj z&LMak9~5@!4=MW|A$Q@RbtU|9!eRn?>$gp=|DkM{Pg!`0MB>Pr&-yYVttOTKY_-R~ zbFhP+jlIIxrG?wQtA8uaMFeFU)LnSRD0SCX5PUJ|8+_{jz%PajSzfF6y?IJ8hj%}; zDMj$!F*wYZFq*9-GuW@an6VknQC7%PEGV5Sq7-cNO;!B5yZDCTGetkJt;%cs%Hl|| z?Bcd0nbqOSy)W__BaER((NFygKy?yUyYYAHwP?Mqe{GHM?o>~MYe0mfzJ#6CulXC> z!DAlA`yxK8>^GEdu0hU8%ufB3vvOUu>iRUZjyRsA=&Rb~bE#=i`%s!tR%aXQPEw8X z6sS@wB_D+b_VwDkO&S|s8qDl^*TnMB#a`K+O(D$Mb2{{w!qAW<$!IhY%|J)mKlg{9 zsjD}9s*#tR4PHsoYe);WU(+B-+E1mBx;0&ZC!bA>Ai%Pkh5catQ&#yRtS!dxJe;hj!yqM^4(|)jh7d~L% zCBi=BjUavHrqj;9Mr~5#4<(|tcJ(@%u;LOWd!g){d6O(eEayx7r;|LX6gP{b_tiZ3 zq_(Xz2N*=#G+n;PvQ1XlFzg$Dmu}wheBR$x-QW~O#*g6DZi~%@ms+-ZcVhRmr|l^- zdbhWRcyjBt-jrs<{%SRXS*3?pdX&@ltvAw>S zqA>XICS11Alnm^|M)eQllAjvVPV;zLvy9di&P#P4>Y?zm@T9M}h~13GSeGqFlL{vx zeay8q1($oPmmrvEWfnmZ5;r=#3MDp-2@dPHC#^9w@O?S0pK4LIM~4(j{daf{VSHDR zEi-{yVX?o>TAH@dob1-J?2j0lYb;^DxN>p#A`idCr&_RgA?h$4Ti zef%8Uw9;8vJh2Gs{M)44v)8ZHOu#v6S;Y*4$;9s^B=2T5nSRbb)hr|OYew$scRGhzC} zdo)y0N^SJ42cHYj*)vFr**pPZZLbCB0+FgG0-@Pp@sexiDxUaFKf z@&m^@-=O&t?$aB#OngMbWPhVQQ`|Ds_@FQs4BS&T|4!^2wc!R@?CSA7S!(5T>W>%H z*|q4mJbtMQ+=)z|0I%`oaSaVBG%}Tmz;uzzU5`aNkt<25@>=xemZyPcs;$)n+RoF) z<(Zh1jr;rctf&&NhevvMQ4 zhDH(mJwr{7X$^iB)TvYSm%Q;JJ%@87%aoB%{+!a@;U}*qs-+JH7YiPlH&;P6vU@8l z-&3%$wVQPbmfNDagg-4VJKJ@V61O2!%NMD zoI!*0}KP381M`#^wFZ z!TI?QaKiY(x`*t$B>C_{(dV*4$91gZ&5M?J73h#O%`b+31Ss`gIb zl+Pd5nkr&$J69a2bd%ZTG_dspY51&nLY9)h@DR5{mvf(}-N93BjMbch-9e4I+^x>} zKxaojoQ|!trrtk}f_|21^1q+Dkn*t*gM=})?-wxfo-^YgxUK!42Fa!?jEgT=+-ZE+ zHJoR}U=YQ`VKl;Y<`bRV1&u|$&7)>+4A=4sN^xa-LW|r3i*I>{hf#f8r<+zf z>SK(n5ED8mG97b4NVIHCxJD~5L&Q>u*nHMaT~hC>CTo3mXt6djbp{kX7>If3pb5!O z8j1Me&wGenVjgG2Q;-^0$ss?FtniQm|H%Mf+uwyTjy$h>J!NG|Ppd|&v;MHAGCCf% z;^4@$0s@1)W5h>20uuOB8^3)-+R;mkE+r8Y9*)A;R5!AdIMW!@+$Oep%tl-x?#*WM zp{~v}skt2^JzXDrO()A=Feg<$zVhL9?jbRoqr)|mC0+BHZWw#a&uexE69qAlN!!zd zie7~4q)uJj?ciL)K(pfK=rpZY0<%u5PHB0n!Ipm-B1f3IIluC{&6*7TqwY{+q@+eV znd)S9yLy9hciJRsKOobaO{BY&BO{oml=4fKW0m3^ZSFVFk8F}8kt8MUveg^ypwDF`EY9I&T5T0g z`iXiox;v+@D=v|J8oudrY}jM@<8al8>?Dcc!V)5$;Jy z7Cg&rzT{AKp|+!JPpE!eW-b}?skfm$pSGHZxB}`Ou3w9FBpT;$Z0T2@rZuFSHu@yE z;qJG`EoMj~2*cFr2T-bsmb;n9=>A4C&($Z0pmCE<{X<6M7@@ObX_g`Vq~7YMLv=Je ztHMfQQ*@%*Y0DDiRYG8hdT6ENH5U1W<3)VUNB`Ong3EZcKIxZ;>-+BE+I#93tydy_ zWwSHRgibC-B~CUflWBq{SFmI!FAL;z7^+goicnJfQqxHF3 zD;;(k`L}5wM@K@_fpi7&xHd))Knx#>R(79#=Wv7jv@a@Yw%s`KY zM@4lMgqKxhmVae&ET^PT&BJ)*E5yK@S^cq0UigT8-mS z7ItQUne>q&$g^5$_nyup2XpsPdz^y2zY6@GF#?XdFA3~)hxC;PtZ5^ee>>%G1=n{# z)2^d*SAXn4L5E$@7$V7FTEGZg)j~d5y~5lzQvW>b0Z}W0DKlwe?$Pb|H4H0UVYW`{ zW#>iYYxt+*Eh(Wtz55YNjY4fgs=LselQML?=Wt*IyolJBb+Cw+qBr6AGV`0h?>x{pJn!Vn?r^R9aJ-yQVX7XlZw71KvoP-ZhcopqBD)q?J= zE6oSjgIdeBbDYWK+i8<8!h$<@Z%W^gJomwSVVp)M(sZI~U2f!glzysA-+HZI8RG-j zP2^s`iF#>f><{5CuS0KP^dLK+Prm+1VXb1W5;K+a7eg2HH@H!P?eJ*ynmqp{Xh{{` z(a;>#`VvHc3Cg$uJbs%sS~v_jtpbk*t^fz{StAP2O%Z;zaZNsS)~MHrb5I#5h@UlD z|KG6k=BUM8V0Fxze8daT(kb{=d$E~S$yuY3y9hfyLUX!nDj+%c70w4h)^&j~!kgH%IszZ=gNuYv$ zO@326(B-Ab4<*9fg5Ur=uUwP2$R58=9g6_GOF1U#JH zB{F;2MC_k;7}B~&bnMn7zZot8IH>NN-v+r11D?cbqLhD#$Q%H=Hmu15 z0-OUF1nB=)3L;Vg5dm%C-%tVkKo@{*Kt>|99TM~E^lz7=kjqY-vX?Wo&EVH-@_J$_ z0kYJq#fU2at`Yys4j>;O?LhZ~vj3AQpkW|W0P0(0*f1_iEb7Al%Sy2X#LZi*+ipP4 z_96fxQ3G-S+cN(Vr$jUw>lrx&0MtN-c4576~)Q3^#l0N?;B z%mqvV3|WeK@!#{`#eutUTX5XdN?$qhi$ham59Yw}dF#sSZw=>-4;Y>0FxhNYe+o{9fzRx}>41RVS~ z3jvrBFF{NL1~Z%p-5{vev~x2WKdv_t4jli$=q_vyHVXe1!bkj})wnjLxKw*2-p$Fo zF*{^I7k)>Wi1mL;U0guTHeHacCh66fDzDi~bv>f`SJc03s+LQiN@Jl_nc=Q?vRqeu*JBX^9~wsn;S9Loo*HC}=->i8Znbk3c-< z>v-G%D`F&rezyzGMX+NYwD~(j^z1NkZ`kmWr1LsA;Xe;X2xUq>&`>ITgyQPQTORJX zOHO%4ztWDdADBiUT;~LT+MA#lfzbkV!&WW=TU5^ZL-3kA#65!0X}2HTFkJb7UEr#n z>DEtJ1$Qk`P4I(+!Y39)4dXk6!%jmfdPPL=hK7{S2%GUVafrqT5XRCzt(%96Jif4>e&&{&@i~jkp{a6 zSrxwrEE{Pu!@XA2Y$f@{y@ns2&A^5~$+$Qj!V+8W2M+UxZF0yQ`xtn{>PHIAnyI{) z-nOHt_s6wG9b}I&{&DI*iAPZJiNha`k{`U;V}joYEyiWJJALeR_;M3FS@K*Dw&)&E z{M{}P{fXB{Pi6O0%LUQ_%Pycd(x0k7_UcoYBAzUr)tiz0P(x_Q&dxgi)C(1CShi4x zklf}O56*;brR9F74xXnhM-xkTjF=iA^~y%xpA2pFEICPgJQ1JBbWjY)PQMz^-RHR( z40F_7u?p7G6YTZuqbn}RoP^5T28JaX;g9)Jf7?`3r5ZrN~w9{QY3zBRb^NUxTv$#e$?4E(}f zRDsXMz?gFQqz}j1j%aK4uejB!T~VT1=y=~wO}^9l!d1yGxun9ugcMnG)*-XNRmt;0 zhkEm&!Gks?`$%KPHu#{|JORW>@S+LV046g+kux;52NE&kg!jw#$6y@Xn~{_hiQUKT zkxEzO#v3x$gHi|z@9Q7-6ut;RpF^CMpNkkpT=7|Pp*)vV&CY3>RK%Tedo$t#B{I(V zToOJyfM{Mr5tWd*@6O9XwcGWbr%|!44ts(~E3!!p!`V&MMF-@fqFbTF5D)tk^?thI zNmSYB`mF;M-wsgnq>W-mm92gp+r(*3$4w8O(!Q6^i5+}1T5N=CR1V#_h{sA)yXKD; z4b?YpZe6KwKx|ARadY0w`2@Qp;b)Z%#jVM~R_V`K?VL6rtEnrz`U8dhE{in@S?g;J z-`*$&J7d@1JpMlV^A6auDC`fui!6&i?Y4E4A`SVwkeo8+k z9M%bbZFJ)=q%-l;&X~D;ZTj58$|*-$;p!nGS7mVKc-=pt0e2klp{HytkhhSQ8jdq$ zuSK7RwPVz8rl#Z3=lTP6!)7!-CtVpHsQEyYNwW-=zz#mXz#z^4@d09R2Te~B49h@Q zqvCKM1b3xNs>in#f*xsl$UQ&o^aD?0oSr+l5#gas-#V2b2-EZFDKmMtijFk9m&|&?{!(+Stvo zIov7q2Upy%o|K+6cdZ>3lBj(1PFbag~Ctsjy;K;Wstge1fJuHWXd+N2Bq18|-W}wOBiN_7@U^^|AOv zLm-UxTv}zIPEc-JIzkQ{xlvP1y56R}+Se1L=fg`K47IHq&?A-n#fxT5{;@&2gQ*no zlRO_dc&H4ANi(;TjYIz2hV#R~=#pXx= zcP4zsoY#YX!A;e&y@8uMFb)foK> zG!AvoT5`Fj(4+bmnrFog_dOXz+L`LCC|Btx$;{jAFvGLU=Qiqn5A#|+okiPZjQO1{ z(M#H}yN3^be5+W)CO_^@Mj+1Ld1QktR@c z__IA~OcU}RC`twDJAt9=Uck=&f1@XWB0`{O^C3JM2^1Aha}Gw#87%|Fk@$aY1J$oU zNn=nvs2?c61!^JzlVgB0pf(h6&Jvp&04j8S=8OPCxgPLn!#ShxK#?R+69^Ol5`mIF z0BV}}9RO_^s6PejEdl3(1dtL?gljPJF5(&Jb$irjZLxV^000oMb9G7_%l~8QOW=~) zw*TL|-SRbBdCM%7+ikEi^_H4BL0v5?Q?E_VlBt=ck|`o8a2wSe%S_FIa!Ac7hn$hh zITdmqfgDj$5Rpj){u_Gl_xXQ5eBf}--fPCyz4dD zwQ?gg8}t~lJGu{$m{oyH0I3B%h}bChey+U##7Ss2STAfWm*WapISGrFaRcU<0;_q0 z!D39X9yO>X{1t#9VAV0$0@|6&?U9H4JDLX=fJKyGcNkb*J1E#L0?QPEDZ%nvU~8b- z1ZXPM5Z8D2N6>UA5vMcNESC0}I0tQdAZ?7Us=H$}_2hTHg0D*y ze#5$C6u)jVZIH?(6<%e9dwu7O_#W0qEIEzX5Tzu;AN5Ng2VTI!k-HTUIpQhje!~%j z`l@Y;P!N}Xx8MkT>4@kXT>LA8DjeKXG-}&E!gASHhNyQjG@?EC>y_N8OBe63A>g(y zexns*7Sy_-qns@v5(@1*S@3D!!erjy&2|G9%DPb}O(<$R!{|are!#n1mg#jRcHM^8 z7p+?k9OG)!ZKm+urrix0E1Sqo@cU>Z&g3hC72R`w4<&)PZ~)3L(AZP*u_=Vqxb)&u z+a1=rT7^;HOuX&9`Qjk2RvJcMM={1%9e@(O)hG&fMvOGLzrx9HltV-dTsF@3#^!;u zE9N_xiFw~>-2pG@-)PMpE3m?9S3W#fz3o2JZrRwth2PShRjVMq(`-tI&mRo!IIz~t z`QYukjrtX{mnb+C!tVSJj?J%C&OEt3WpGJ|>fU^BeNSD_7Mx%Ddm}RE68y}2trr=@ z-*Eoil;}_sp5j8&MbktJrsU-bv0?%JKJ;98 z@As%@Fl~a8LoM8x;mpU=&R~`Zaa(lyZ@@yC^oHjwl@>!)vUcD`1s-TfIix)MQD{${m#JW)q4%`c*-Cud+?mLYb`-LkF7bV+yo!`x6Gm|_@#1sMN z!LfPujGl{(hNV;U;tjf`lRk!8uEz$>1%)`Hpy;Pq9lP%owApvW^D zKOo?0bDe+#tF!k$=1>0C|M&oLIm5m|e!_S>Ye97zAGh&<)x4g1qp<=V!CXg| zvk$zo2xdQ`+F6u*fJMD#Q0xQ>)Th{@L9Y5Y?T1@5Ox_23t@Mh5CI6z9e(CcJX|)=; zOsG2+g|%qb(9}T{MYmwqLZ>jUlqpo-c`1~``+KT^0g$`v}IO|Jn0~e+NIH^ zWUiH161d6dM#Ch@0F6I?4hZ%s@f+%PtSyv%W-rTABQ$sUsA2y~F(on6w_3WV6$>&z#jcMg2yrH%K-)+sE-I80(s?ZBRLM zh@g;YF$8~vT%f>W*WR*|mWf$R+l=Y%0!MmGRWHKeL#r_rQEDmj=WJ?Dae0AWw*Jle z3H6_3LT^A!?p}UEn)aB^FR;GR@F~!Cxa89_Nt32Asosz_H8^%meR@1m zZEE@!OJ`~ap@aNV`rzxmFp>V=utvV7(3_+B>fX`}57s#L{+qwh~bY{hkYkgI=9zpGtz!yFRCvu zYFqwz8Fwf=zGSn(?T>L31!I3#!SJUPVf`}q6NYL^cr`nsqo#QgO|01hTnx}Eq7Ukqz) zti!$pKh2FjT9UZ@ocU>)18&Cf z(^h=X$EDAu;>T~2)*Bkd%~y022o>)Zxa0xTSFf{$;BHN{QfMzlzOvB_JHsE_*Z2yy z2;Y}{C!sM< zmTZI`q<$#Mt`hk9n_W6ol-r!%WIJ6Mi3{oMJDp~vwe5ag8$ejJr{^t6ce4GD^_ekx zWvJ;wjT{OH06H48o?QhYEc=zhlH)f*FSh}%AZSDnY7SzF05CGiTm4mrT>$(5NC=>% z!yb&M-LJUMOw5a++(O$Q1;&dq}_G@=76Y5Uw$T`sMc!QbfWRbJ;by-Z$ zjodc-8wEbM0t_z{yKFdNoNUOfD%F1MA8E*pzJ2IbM;n$b(v`AcT`>G0e>n_KPoo)< zIZ&FvYU^AX_ea)YY;K73TqyZ?wEj2B-`npE`83*A_?2yw{x?TwO6Q zj69bj>Mq}_eAzqBXmKzjx?fF?`R640AR=4x?a|e~#oFZRs}J^ZsuRPf#&tsRYdW`W zVc%3y(Y4{B>R*Zb5n8-r8pMlA_!Ma|t!Un<%XQXfKcM6>UL7_1i_&wnu+f*_2c4)9 z-xx%(i-Jz#qs11ef(57 zB}bIhfz@gtW0#IKYLZwcl5}oC-vjq+m^p2o}u(TG`vIN#rr3{6q@eb zn(KEvlzV*B1=jc*}XZ634kMi$q3?aMSpGoo%+Pd+Mt@{jrT=Z0J-U(;^#YagvyJ%gl#O}MG2?Gr9ais$JPEPdaCfXJedx=m zmOgeVKVV9E?V!uTB#Nr_Iq3Yhb4s+zbB&p_yK6oEL)}pZ$5<)Nu=pyNd6p?^l0Re9 z60~99?Bf#4cWy*s9_-!gmVRc)>)@nmsi(8p<<9B)$7?Rew=^!j-&ICBrM;IGKDAzJ z3ehJD4WyHFr%;A5&rp`r(Yn(L>w?#O5>K##4I)Q7RzjIB1fifz@+cBJi$4$yUtJ~^^59D3V zXP}z-UNB^MwdnhF0`Cec6aff1v<@hu5@3sc+H1O0i>wx^{>37bcY;K_c~-r#(zp_-~O5u$Fmc^2Cj9bzNIQz z5Whghk3rTbj!}{)6hZrQW+N}roRYn+bCPc0*oAaS{u0vIbKRCz&UHjkTzdA;IalF7 zp%OBxLTag7VfV#uqvggcuDpJ+rRir(Kb zZX0CPcxe-J;j4LA@>|$mE_xqQV{k?o@mg<2xODQ_yl@ZgUop_&H+0Q~`WX7R_En{M zzD^GZ$?qDX!aL^Crlx|3jH;;UDA&>G!>d&@HF3<;5A=p9Y5vltc8u2IssM!`WTp=|{ynan*d+4CT0-CgrM2Kw4~vp4 znqW2!xkd`4Hk!-YM5luW>5c1o)oU}}{hC?zH<2%9oK_8*y?l0Ce3B5BUt(?3&n8{> zvJ9BZ?4HO*PW*`ra`7ksaB$?$X4N+6v0phIxQO%($M)@jwCv^Vzbj8#H`F3aR~O+c z3!{{!uXB$P%HmLwAi5LW6~XQdwp-x|$SvTm0C!D!Ea?TzqxN(9QPm*9eh@$;( z7$%4q1)dlXD(W+7*6$Fh#@UaIR@(o7lf8Z z5tBfoK7a*&iUNQFAP+)Tf&T5964zL{i5RD3bmQ4_DS&-IkgmDBGTS-55oxnBVgO?VvwLv z-W8ZdrngWYTnjh?)5-romx(Kr1sq8MKR{SYocf(k8L1(8&j|zQ6c&U?gHa^*z&peH z3_uP;z_bwrwC;sGaFH7t1kp}`C}+w00-Kv*dGw-;V&E$RCJNCJ%}YRRFo=F-oPc@& zJAokAPT=|ivje~;Fw`IdU3L`Ao1a5JzX!5h2LKV^0#xe+ikixeE3-7%GDRq>D0B8# zvSlAI$bI^9d&t71!N7sp1e643l}QIw0rbJh%Kif=3Pw>jc0frm`k>#>LHh4(0&GE0 zy{rw85%l>#c>w%eu#Gsqpp&npz8G1ZxYH+X4vQb{cf0 zG=r3tz`}r9wyVl?vpf#^rmOU0a#gsY2>_uTy{^F|KwYi4VBV;#>mizVbrIptvtci`^SVIn&?*nzS2EkR>9LZFk zj6X0oK^Qk#e5hq zu-8`rLmv+pI#J7Q`Gm<0%gg){GdQ7ggH&ML4Feva&hL!EAQ(vryvS zG}dds`AIER66?+EIOHYnIhF_#1!>nm??r28?i9~T(a3=QIBT0(w5qr$qa+EM6Q>+R z{D5_19pru@CTG$09WcC`&sOKpk}cZ!t-J}e&6*#cn)SM|A8$-AfCz44=Ii){KL~8Q z<$&Xc`g&^3Lo(N zzcwx^Q)coJyfd#QkmdG&5eC~+zKLuxAls&uH1}dje#6k}#(9>OZ=PgmR(c1wa_X2) z)nncl1K15X0*zcm;ciGZPvpMhHLBYjE5W#@Yu-EMOtlT|M6KWNM2h7Y!PgNg_xO2kYidgf^%m#MB?8 zgr-=99B&$9Y_&LZ0Nv|GNwPEYhpy+E_@B+cJiUnjfE`xi&#Ic7zDggeKV{UqH>hW| znDSO?i8am56Vx^<8k6SJ?ye%ZT`m0n8k0NtWzx2udNX`4TG+QM-Q9P9;fytM;+*QQ znGr>{&bLJHzZJ&Od z`(4us_)OA^kf;8-=CkwD_~oW*pSl{IZ|$B34=4GjZVcFPpeAOlHD{QNhDj){;_fxh ziis?FKAFEpGNe@{OrG}dt$^Gxid70R)j2d!5LkG+6k9Gl6d4mT@Fhg+u#0se=Mph$ zwyFqb6E=NonUs4qU&+e#f|^&8sq#l}dYb}r)xEJZy^lSAImf!brfGgl=OKvHsLaSK zz$aq{Q)sD!Tem1R)6bRmiSOwOgNI*2BJ75PH;Qw@wckp+ z)R&n<+ir&zX2%+MzUoIa+t7i^Eid^Sw!E{pU)qgKb2PZ3AsDordc01dEwmR>g+8YL z&icCq-?vfZ{FH>|{U-c#I%Hl@4s)Xl=0Ju|dhpQAp}QSK{X{6W!{F~$II3z|H`O71 zbhW3I*zgN-%{4qyLDP4ebkco_pi7X&UTsni@6m9kq(Q&DEs0R@ z9C0J0C5P1;(~2!AJG|OaI%ZHKmxoZ>X4gvmYE^eDMvR^y9mw?&*Lt7wsz?s|9yqHK zm`*EY7@}TxrCf=GcCYO0u+cooyuGC|jL{P0f-)hgO6;iK14*joNc8oO>?!utWanJ& zVu;@F#iB$BJEe3gYdEycqKJpjw#mJgN({B=fCbG~a7zN7L$jPlBpN<$Py~}cm42Og zh~=|F^lEsf-_}awLVfvgWCwoVU(|d42iVmI|6mTRzLZeY8m7eUW>NP1Ww(j(AC*JZ zH`F?4V~JsUTIAJTv0rOF=^)TXi+>s`bhofaaYCCWZ8~nIlRI9ZP7%zhDu3TxCApMh z>wGOrufq&GYhP^L<_2j<2)3VA9}w12*J8PNp78bB^W$r8dmj%iwSOxN*jH(M0h%@b z?}e7KSweSFMELM%q(yl#CC+#0mC*gzt@KPrgaXp*x9@y=TPTAjQE1>#4JX&8mPd*b z?}CgVoh_@ZzAxt?V){Oxgg=u)P-{UN*e~Y`t7gA$Tc~ocEHCc;s_K(;%qfM_NUnTa zQi;;7&CoK~fgiekK+MXQ=D6@ljPKL*qGpM3`R;^9hU0TAE27uy2d`XkQY_SnkJNz zDtQYuNJ?T1al*J59(G)fVaI4eZYfD2#2}TF?~BJO28C*m3?5TfLIb_k8Si0Ln<>R9 zYZ=|2Oh2;g1F?4e?wcjeunHQ&!;ZO`xU9#Fs_y#-BdD5r8wW)$VpR!k%#w!*n<8O> zI|zJx{c|KJD*V+qpG(u$u^SyDaZ zV~tD0qRdtws)B7fxw}~jUq&}Vrp#Hpda}+{wH^&+GTthd9ex|N+hAHe6(<>?#z~Ui zcg%UPz7XplYdgD+Qc+}+yR1nE&qa)2_VN4Bw>9aS1%soa2Q8|hto{NIWbeMA(0k@+ zW4K9gC>--pd&{;SgS)OT_@PUT_V@Q_WNPoB-XiJ;{^dcP4$_N4OwhJ1bAI&pLB-*) z12X{`)lQDdYREoP5hc-=RIquWb}fH1{p!$=f%cKQ&Or3lp>ROeSb-nrK_6P+1;y$w z-r~4u$7rwd3oY$|1}Vb2mz7ZF`(x(53o zG2+kFlYLtUP^%{u14)4@}nuFZ)AY;C^Y@qB z`CE|AC~F_{8e40;HeWre{-?HjTBPoEDw@~xL+bH;O3~5+oky=MpIK_YCMV_c5zvbXyv>EDP+l-|3*rce=D08zUbSrWq z$u$Rl?d1A&qIZwVf(YSANa(nnf)Ni>s({( zMxm*%&MH@6`d2GcBecVi|IBHNmk%;(AuSs?FbidC$A=_{11S)1CCm_CtMynFT6EVK zLf%Gg=rnxTAUYFBBJ|JgQ#5)+sy^%ppX(b9 z#m~J@``BRi&7QD|wrE!#I79asI7e3t-kR=I!Tb|RJ&k%I(C6gsOB213_|3+!$rmK$E(i|MFUe4}~`Hg_O@AJLw>r?R7G68M9TRnVq`R+v_-zYR!l z&Qm_#iW3V^dZuR@ZWA7N_H$k5S9M5dSUC+Y|KLO~Fe}6pPvC28 zh_*MmPPVYs@v|%bPh-$cQ4^He4Sr@Ue(hGmzE9MLOhLNwFr2XB97u@8K88tHp-ako zL(gFt=WPtJ|7c3Unz%54lY9D3Z9mP}?=d-{JKDBGAfhSz+iFf6kcPIpHVyWrFlt zqb4q`jz22qhu8%AXN~NZ^3|i1OSCFP?|$PW>TjF)8&vIO&kE}zLaPQQph$&Jy5dx= zYMsfAgqk=)tMXD<@TlWbSgjIXG{DRPQ79|c8457IvWWp3Vrj2jT+&|q)_((Ig3w?p z)u(-;>z5y9jHz^?nPXyuA<=CNE@Lup^GK*$$!;v}tx&}f zH7(YPP8r{s=G+0hN+FGAGI>-h>DN{1{*Gk@conB)JS3HzewV@DssYi)4wA@U8-up# zlPkEN-o(cloBTy(JGvOMipv82fO@dV(UU>j0`n}XM@>njx|PM(aQ-90Afs5%Z>TaY z?0)TQF2d3;2%}82V;wUb*2V z{c^+FqH@;#xL1c+l`FoOj~kq>8zH@=XEa zPpD&AY10IMl-2w81oCU9vuK;c>h6LpgB40F+wGQ;?D~876bPV@6*u{_bR#5FUU&gq zlEI}q(h4|KhFh99LS91r>eE2j3VZ*Ju;ig9ib|;8vF9h9j66(@Fr z!W@>REHvoylW-Vtb39ip1=UMHWs~Oc+2-&*Ifs|Tj)tg1z-3ka{3JWiiwl#E!^i1FFer0dX5Z1j@fe+v1n@3qu#{Zh}w-aK8!Xu_RAqoTuo+ z{7w+~1L|UA0PhmWwFn|BWZB1ot;%2^GdbV@GS-2zFR}8HFn|vz)iSeFb|VX-=}**M z{2IPsA(IpA_`j>0+#;Z0ON6}eiifsbX}}kV4@?K_0t|p`3h&f{R;(5+h%)7*<;>%2 z<#)sGNX22iTo_PV zb9-k8#07%|CWoxF3^KKyl^~x9F!nps)kcLcm<2!~ z@K>e^7*#OOWHSud5R{w&VuQ5uz#!0+pI4E?a-+$oB1k7c|6Ep`=jWJ$hJY0K$ZF(( z+A2Tkc1J08FP5|3ZffB#~s5GM_Q%{z}UZY<^ybeNHK$(-XG6rBuY$ph| zgAuY0yTjHq)&RDU_XQn6BVceqM5Y0#BLfQgz(*Q5;3W8;K~Cn%y0&q5|JQoFy5ELa zhoKd;oVdlG(;u8J4IpgiIYw$JV-zox7j*$A9xxmrBRiesc6-;%s-l4|gg->;;9TWt zNQ49`1~(lyZ1dnG?s-g|v$51-F}~Om`RH_gky- z#+_$v-J^G|*EzvX$;;oRmq|E$44S!o=fth81p@0$j(R8dwkQ66jC=0Phf}kaJJ8Sb zj=ZfzIr*$F%)IvS`M2FW4*aUODf7-1DaUve+d~n(!(V14;bziO0&d^-!Ui~!IS+_Q z;bQNIc>=Xk`=Xf6FVQjA&B6}FXG{2-p-0Wja~hoz z`btvm;%}!qg_vLEWOr7DQjapMM~O_#W#Kk&TDj#_vc-sSzUNiIn;>f4CkA#c{4NA` z+&oL6wfIusiN{oJbMdrq)K2b9jn1x2Q*HqJ(#va{W9F3Ao!3RzB5K9fSNN1oS1{6p zNP6Dy{picr(iwmCQQ{6F`Gwn&eEo|Qf{wWw>}8*S-gcx3W20490;pDk&Mh?dmDci4 zOdDro$l6w>5N~>ci@<#aQK(p zvy;h(DiE&hH@)`*yKgPf2Z+;poPZR0E>C!vlW!7pzM z5nX9{s-VnKM=yrH>DG5gqaI^m#qSBZN<>)pOOzhUN@~L<6DQU!DBWFB$Q^qI1zBZT`ALtW_Bq%L ztr?)pym+G6;5uwJwp}tNMAws@W^ouPBC;H*-P3@AP9sUn;^@qd!2uY`1 zxYw9Tjsv@_O&^~gSJF+kskBKiVvy<_>?+)MFgC@SdWgy^UWz$;nd2trqa``6TU^!L zXmxI{7Rk2FQypPkCGi8BxRD6y;4s_`;c~IzPR}Ni)y(@$E}L+Z+RZ%aB*ly7=q}Vd z^n$!-v_+TAr(v`Sq5xCH9*eu17f) zA3WQ~j<1w`Z?BVmf6t%Xk7aKs2466d?TK(PF>f|on0R-!)$E!{la6}NZpCyX^^=OS zCg5PUmikEuJHC42XQ#HXCbN-={p==CiY@#OE{YZ0R@-DYvtNBe8Pv&=QH9E=UT~6; zI;q2sub^o|M-DmJ#qPwt;=26g53i~dkQiIY?1R-?izLg!II-G%k3gQx=;$aw?R5J>uh;yu~?VIm_Yy53(GMf%uX*{Exm%~xj9wi)qK6#XjVwqo7O)f3Lu z36d`#4{0tbjzyRnOtkYOs!CKtD^OYe`RB~?EyPC)3X8*TzwM9f{ezn0jNUA{FW5%i z2kMS6ydkg=>bo}yk&N_HkBkhIwdv_x1RTMgRI?c;Z=Lf98*12$3;d!QS~AJkbF%Yg zdr|Ko*=gt3B!>%hbS{B1q_yw~om5scdfBUG#CU#d#wBqw zy81a6-C8--?o|3X1FA?thk3&3HNYx7y z+x7$jA&KsQS!(`X&n-C^hk}2aHZ=Wc6v#8?d%9fk!T}jCH4|?zZ35h>>?EX2YYj3Zuf3fkb z=L^inC+oI3d7t_F^^>mnjqzvVb9~NcfBv}nkKa5?n?DQ5EG!YPGt+^gkO@$7=NKo| zAa=5v@!m%}NLAz5sQq3`>1w5K=@v)i@mu1S$awCwepRNyY5UzB=5Z_JTPioS#-4zZ zol_SJ7tUw=Y4F%&yj4Yj&M|G9oFSI2m%8)gTNi^v)-6K|=#=idUqy(Mc}W-0zrdtz zfoqvhg*V9etV~g(8`kP~i3Z*9SC9u7z6F1i3zR36O<;Rr()(I#Ycrk#VQ!f(&Z1*! zbGe^p6Oo?ysWeb>`X9;THS~aV>HQ7VOtF4pF72xhdrcIi&UJEX7#qCNZDYN4ZH@em zz4`o&zYY8)B@2ypp`6?{o6y3;tRK?9Y2L||`_Uvw7F^T*;o95g4^xK9+YOF3&c_** zJ`n^dy)Jd_IDoiy<{x5j!|E>!jHKh)C2r&&tiJhwh$3|J1CLJ+%U_s=bb7)9TvKhN z*A=(Zxf#&c^+gS;aTpJt$8uMDTFdG_Z{0T0Rk6MwwAdxg&%((%I;U!G{+0dtoOQWb zB||XH*M33$hHaJCe;Hsr-Fz#hO_lH;DcK@9W7PeGut9_mTQjx`i^kmHA1YmOs_(2F z%pz^_h^oLu=QIY~5qf$5Aq2JTohPCHsV5;E{XWB-OCtr?>HTF=`vok;sVO|1 zbTdvJf8#H)Fl*qY*V*D}oYY}qykh49wc(-DQo*V1b;ZWqbashb+y}^Krd7i&HmX9p zB?W#NQx&QnluL@gvf?&5jOTQe-J0KKQko)wZ)s(wJNt-qE`7q7Ts@n9lf*7+sxrFv z+Qe)A;Qa9%Yx{eO$jl)3kH%=-Qc);lSE}^p{At9LWBP_Kr`B@ouX|v|{)n6T4f%C! z-_*rduN~5ZaKGD_C`R_0G{HIX5jt3MfftM|ms?5~~KFL@I#xToHO zpV@y!-mI_RpaqmL-RyJh+ECcZ8G z=Dv=E7uM<}?N_J#0~8kj!jHdbMdxgh-swL#LD5fGR2?w!V`$x;R7Pg_sxDPIES;*!3Ap>)BI#e>So)$(-r!Tkcm^4&Wi+Zt|%ig`!d7e9gI`Zhh#j(OtRoo-F_3$%`qpU`i2;h|S>>ED z)ji~{u%7Co5chc_46RhOc>gBNoRUuOmCeu|7H^Z*8)x}Tm z9Wxv>r(A5wN#sP+Lx>wogi`(LJw&|L2?^#_>!tbYhPUc@v)fmfH`S`dFcLmK*reAJy%CAJ6YLKi%b&e%=+qQdt`#s%Oz|e%`R)hA$-UAkJbZ3H5F* z;S`#~6Dn`bLa^u?gIi*sm^bcF{(_-)mPnNeUR8!GT(_;uHBaM?H)92>#|I1ybg6X# zceT-1Tm;`oZ9~;1>)ivsmp_aBijaQU*oO(`{$@;`gd(w-#J9b@uMur6ig~n3Eo8lw z%i$hYfy4cLR95w-kd^UW7!TGKk|46!D zk**7~&8cWQ$5_fY=gu|?%-_*at71D8jwBW3CX@Rpiqk6QxLGZ!wJ zC`6`GY5C;@HD_iar4{qqzA`~{*l3_XBS{gn{PZ3zy8+*`sV?D$2Mt1R5I{@JS zm#Fx6`&unPF>Al@eV3AKYG9P$a?BY42%7wh#;m)g7SDz7x=UvMUsdp!5NNA=M60BIO z>k3@B2E$kc98}QHxg6j@LM%>Md$m)mf{TI{zN0+etOBeicTGwtq&!9DgbnT4}5B zohut2-G#-srW)_0Mle_cTE-Tm3>-|`)27;mVM?yKstxp+VPBL$? zwd9Y~e7d+NPT%w|bpzdwg{s%7($4&;i&G;3KVB=AGU6SSila!z3xg^vg{C$+23q7z zG{J1M)&1z+>xv}IxN1r1N>T%D_DQB-HwC0z`!*GoI=2X1e{4Ff!xyh)(OuxDp@A@Cysuaw*s7pV+sISCez0Y}x(j`CJAHzFvTiAj3KT2>t|U+fxC#`l0lqo# z)(pr198ACsW)l|i8{{P%6o&!jT0r!z{JRc#HBYp&ya1~RnQ#K&76iqXS4Z1f=AdLo zlwv8^0X%-7!%=7x=!^x@fufCaO#nNYvcMC0RL0~5a5I9E!obx;1VYLG4*=(Jq>L{3 zmdOAJ00$i?ft@Ya6Z8DP$3P3<LrvtV&I>uocP^|zDp-LF1|RrEW%bhmH89n{oc91@_H*Wlx0ds* zI>UKm+KuWQnX!g+oZw*9?oYQX;DH7917*nNV=kKlzz*^=L1t0lHU*Pe8(L9+wOxq= zp54Lu5Fac*1pvpNCrTMP-MHkp*0RQTtXp#poYklHM(*JdFL8o#^kA;W5~mi&E3x0O znS_t@yfv{HZjZPYL7|l;nnbcyrq}^xZIh>Jd}yZO%+ioJ(jPW`H%-x*8;yxgP``+2 zG@cUKhd847TCsuX^EY>kw{yqQe7bjX7bmqm?sjd%yWxTa| z$}J_lNT*4Xi08>gCXe8rS4HUyRL&6L-*!W&kI=<~!jQBn<4$kf_RwI>RIHLWDy>=i z*C6gCzsJ5|bZqarw5TM>uOjO;(Wb#KFoTYUeG1%G=C-|Gxv|AtJZaSAB;p@9>L!u; ze0nqXQ3&~a2@SQMu1pYq6uY8n7o%{J9rI6G4Z2HU!~=u9_e^LKoqF+h-jAxTg@$}d zkL`+f?;@%%ckfNqo8sHBanTEk!8*hc|E3#Eq_)HySs9NuHd8au_fO{1qZZbJVAf&Z zVNb=sChn`Yb{#n~%8!CAr!S=EMNxdAPmIpS^o|)w#vOT~9zj;c@TH{N8nf<$`}+J# zR6o(*_2L)!J)_v)sqL=K$iTha3fA1(ZC?6d#S_k@PDUd(tKVzUxa8sikvMZkbB#HB zuyzL|_MSKWb&^RE{mP<9GRJmSrhldkJ?>TXmyFjDt5{1%pRQ$0Ixo2MN3V!TJSPbi zYnLbNZkhOBB$@~x2TIr?!f*L_47Ys{Q=i71y=Dz}8mqkCU2)}D%F>A=;fbDC3@exy zk3TQDWO=ft8+&e_vDvLBPThOxo*N!lr^j`j-css5(7>cm+uXs{sIwTg)IlVH48qf5 zClB^OXrBiBRi&Q{q`Nw1aaRqRdn~ue*g^=Qvub<5&Nby#NYz9`dU;Y)dz5W07bT3Fs83 ztp6{B=YUA{57&x#uC~v!ngW2>Qf%Va3oS5A? z$+o!;iy(ZYV5h`aBk0iH6XXH(%%-*1$f#M^J0HkuPF5b-IKEyT6Zg*IyF$kj=ai|r zt42<=c&F-}>X!M`pq1OiHlCAd%eVoe;zh{5<3`q@Qs=cyN4(^W3#9TOJh}WUZ* z9Z4#Tf_WYkwL_Mt&b2tX8wnrje7lMYJXL|YUVf(_U1YHoprL;=j8r3xhMfrgg#o#i z7Z4d#0&{HGg(Sz9m|Y*}Qu`juy`cyx;LYt(lYx@JAreKQ$)Blp zIM&HOyII9B@wDXA<%w!V@*lBDlJ)-e>g}Cb34MvD{RS~j{`XF`>e$y04Jbqb=%5QVp$lUe9$_Oc4@-=YUM&R)Q~cG3b=&9jO#YPr&X#!Odq z_TENoV?+Ke)qonAVLWxVx564Q#;eqnQ@GIv&974S{BHUWE=5oTkEP%wW&OspS-n$M zYW@+tiB4ZZ(oBNJf2eCa#7*y8BaDCSp7=Fxq15Qq;nB6l`OxRQ=rYKX=Do^Y4va0l zrjA3=#`X@ZBkWqWE{mWepVg^tZ(U1VpC$87^U ztBcy*m7640w zH~XuR;QfH&BKA(FQES)*@E-q)47h$LS?brt$uTu(Hgi#i8D-4Wy8Ab4v!f{;N399e_EYJ3wm(GeEALVhZbotm_I`=K&T?t81NCuW36yc5`fldI@f~G(+pa}r;O*dBg{J8S26rcs3V15og z=nNn(K>WbqU<3Rek$=FLMQTE`x5?+nMC8Gz>$GOa)mhn8wA*C#$uArj+m zLbG;5%8UJG{L7Q3oX>7+aN`nBedCSpC=U&#{wWN!jM5z%Xm-kQ^Z(}Mj@oAZhxK#VTQt&3X45H*MCCr?|C=A4XT9eWT2Du9=qPUgC@HtGNzv>FbAS2C%t* zPdrH~843=Elq0>DYtwV5o>rDrk^c8rRXyq-{UaR7 zX|zf!|1o5-n`q5QHIXQcN=27qqTbtlL`?`gg0R-FMuJ}#l?Av6LrIuSF5ZVoA2_o? zSG_VsIr^gr(SC$^6i2;eoLgSyjvDm4Ip)T_JXy>@4cFU{b^VH5dIeRBd+Ye$t_+1s zqLoXH8DZr~qEt$#SKWhcjsLH%bB|~G{r~^FR6?KU1s^Qj%yN#77N&>r<&FnWrR;5d}&qYRqsz-7TSBehCYBkf@3;LEi{yzJQ@ zgu!8bzyJqrHrN-cB_DFiw`!e+HVzg%zAlr&pVn>#oRYwwdHF#K`qfApt~0ugUW{vJ ziQA9)uiMS+WSIq!f?L-u1{cRmMt{+>RAIkC!Zn;#@h4sSB%^Dd-*7}m?GJ-};!%C{ zG|WG>CrqG#+9#MdV>R0SjB-Q=_pD5D}Fu;!KleeS{_E>yk>Td5x` zTV_3;@vLJ>_E2fEWL26eu!nVgSPQ94Nc=&`Kq(_-CZn}Cq{w-5fUKE)&x%a+FVWqc z(pmc@JSh6Rq^(BInppzfONKOtjKsf^<6Cg^R+u)QWsH(oz)Cd})>gUN{UVaiP>X8c z+~d7nl@llbc6+*Eig>)}LyG9!o2a#G(KFgi?&5GAoqm+(4o`AyUH<|& z%4N;PH;WcK6jEshc_?%4IEvy!>0KUMG?*2rk@OB{#6`beOTv-jby|OD8;pcGa{Xw7 zzt=X{i2H7oR}bYDYZj7nX<ylhS7)XDC-pYXm)+_gTl5kleoDW$_+!Ar`sX*h{4(H*pH6`^$w-UPs2v4Z|wYl-@ z`;h?)r;(ll|F_Flg`V}&uE%)l@po%}29sQ}(_x>_I`^n69~BDLL-d=@#(! zvhkIEZ~s+i=jS0w@52&Zp47n^L>o7nyWodiYrIMbg8V*2kA*4*9P+)1U|LwB127BN zL_8SCK0yj<$R4W!pRyLyTP;|A;4FWJM_hq}phc-tV|W8_eJy%1SkKT>{&z}nrUe$2 ze4<|~ef^4--q+&gYtzjv0#2X64!`3OuE?Hx8F>D#bmsayR@7vr#p|eu_#f@g@YVhm zEy}A#3z{PsUK`l{VZ2r4Q2GelKiJ99vvC(CXJ)L+e||h{AI%EKjOL{DI>+<%V4Y!-hK#C0U-0O30OsDE?Rf zgCAMl68*OHrGFXIrgS%X17Pk2@k(+CkjI*{r2zsWU-Jzha~lwT02ly}Ik_bT0yqP7 z8D+qI82-Po09JE=S6;*_L3vAL8+%Lh04M_l0rx1M69F*>;Oa@}2LOTq8~_vpSOAP@ z0LSA2(L;Sp0|RZ54S*X^D1aqRY74q5fBN#rAy&K94rmILl`Wk7AvS4*sx|Xn!0=VVoCAUZiL=ZrP1B@1U4!|*>{Fw}(A<)Unt$qT`1|%O~`2SY? zN1OxtJqgH^fLydyjsPOomhkjv5`nsZ!fyCcTEfUp`0jYtKuk!JwUCSW{u*;3Ph z8{F2ffW88AkZ2Qm@6WPyHL@*yYf@UaLLQ*TM%5}$`wZPuiMU?Wo(oX?Ir|6FfQxt1YRdpnxHjhk9}B&x|O zrtQN636`aIV|@H0G494k{~@p(sA*l=KA?|EHye{&6XhxuUHn4G+J*gaC*pN|fiP9C z^31avS^xfNL=CDlN}!_NPk(ol46iOb*DO?|LlM9+qAHu zU~nZ6s%Z)On_NvXZmf1tnw~huJhWZzo5EN&U%u9F1AK?z5b&tx;aZJs3vaqhyzh{Y zW@^OzuXOZSYwcA0b8%*Z-nm$w+|I;gKXc=dpZ{>xvQ91e+K-I+>Jr~^i9AE?CaCN^>DgI8>z=77%Yx)gd zllWB`19lV@{uv9rj%-@D=?AZ$n_OM|axGQD|j=Cka6a1w;G6K*b;_?+N! zG4gh9IC;r?>BS=&#!UBe3tbl9E;xCphE*AtzE1_DcK0wt1hIz^DpUjWs)@~2^}mfm zY-6n#{6y-`4jp*^9Gjk>Ay|yP-Qn12IC0OB`kUNpKa;7yHYs%;Gk9)-am(d(&QeVh zg&lf=wR!&_1CiQrI3jT!?56}O&Xr|kK+?(NjjFM1=fy90wWnmFYbV17OJ%&=)k6Ih z<&-}lpX8RE;0scE`@K_b?agvc_KPEqQC*x;MuQVaEHrNIC_1bD3#pf>7if4biz?J~ z82mfG;Gk5z+yRFa$1nwU-7Ot-c9bQgwM)^7dd8sK@3z8x6*RnTP&1 z)}(t(IF8UK1k zEYf}tE)?k9uA-hs{F1O~Mtk>F&j%girV8)N*=8-bsU9EL6IGM#$lINN|1_&Jstu`& zYs7ZJe&v|2{UGR9{T8kn=Dgb&2^um-#Vthsyo7Z;JdF(klFlGsY5+ zL*-^;=EYe;y*1L@t8LD-IV}a`Po29keRh~ZKGC2vo!C77E~Ye}X3ycZ|8pi& zXjPF>bT*SJk-8YDF@N6m{P196ZB&aVUnQKU)lGC4M$F3M895(D*0B+|Zs6VdBLiaY zdbZceQI}jT0W}ep$0&icwP-BmUeB)AnnSh%DN0vlc&6yG&yeNjo9D9?T13?NEWX-$ z*l>S6v>HEzeO>A)vUcbPw1@y{Kz2G#y}M81-X>R@Dt^={zL&KjPIIIj(_mW2ON<__h-)}%7eqWk6EE~NKJ(rpc^VDe6 zvo&&|jUG%eD)tw}pN)E)dwooUEPR=-Gcj8P-j@TH$HvOmPCi^ZEBQD#)Z!2t`hC*U zDa>$RedoLPV!i7pS!~D7^bq0A*xW=pGkJnUq$1I$&rI6JSP~s^?g|85ryUOq^lXno zfNwyYIlCujNh9&(H!`!TiF&_By{e)F=0>V8t|#O{AYYdjI2;etOn*%IHgI9nq3-8C^pR9iYWMkx z6NO;J?C_<@{ExN!(brxpf);Q;B#k?Q-vod>Yk&PfNVn%}!?h6#5mu0y^5+4&zFvfx z47X6N%^!eLc9_e>Yn*j?i#@r_o>tIU?mhL{R60L|IsNIz-}2y;IMT{@zgFkY%?Wo(p;_?(h>)`Eg1Wwphkzt|w5eK90hvCe zRV0o=_$$JMrLV&5gyw8eU%OW~jbkaZznkkk);?;WuQ$j~Yli46OIfeh%&slTa*{(G z=lcTBX4C}+yzi^g+0@oY3eItR@}|;f1H{+re#&N@Ir+q?Lx2wbrm|zqEL35Z{~zN zr+2rdXdI#04d+rq-O8$=rL1Ra|cw*nsfhQE!(5@;v6dphN?8sG!M~($T~4W;J3LJuy** zNdYgHUZhG;uI{8E_L)RG*~LR7(9xGRL$7+8zG<-7Jc8VHoUBTm2q-atm>0`DHY#>& zPf~Q|~vR{N)pQ zdd-d~L8v&-%6Z4_#-mvTD52Wg$oVuvJEE_{B-x2pL#W#zuf8jC(9fPU=vnS{;Fx`l znbK9WT`5SMK-t)#rXw@E>TZ|d)@JXcGo+iU*-UvhR9Y{G7Wqg+G)NC@4*QK%Kl8k{ zeh#yyW4=5eXOs<$HFE4}*r>#0GM}SIg{|%f5nVZ!&4W7VK=^K!;ipTuJ~?4kg!TqM zCAh4tGYc8pH8xBxzbs$h)eVQ@vtRLsu>NDn`AF<6aSyl&yL6LosiAt6Ve}}cwkyy# zxh4CF%dCH|(i~M+It5=)qLH`ikgnHMnwps_CZUFJwFpH~c@s_5TviPYEYaNJXC9?H zF^j@K&b11o8!SWP$jA*=qo@i(B946C?a^*T7nZEj)kJVdawD$m+oTN3RA zS0Sfs*nL`c$$>^^=IT5cm!46%nr?dP3-K%cLM^ZMDUoly;m03{^ExZzAK0fvV-W9wCe0 zd$)88&`EG@>{GfCb~$Qxy0xwr3*x$oMNf_zp^+bh$iIYtsz=@GwV{mkt;*PR$DO4h zIlaa3CQYrHh9kj`xCu7B5KsH{NUx5;6Z9i}@1M`2Ts3(??5nKhShx69LR<^JScNS1 zPK}0Hn&~5cQwew$VF~JQuw%|;nzZDbYOeccH_wrf{vrgOh_R6++nIAWr=3 zKd^SKYIB$VIDXI=v9*+wehlgoR)%G7UXCuPL-RFDv;+B;rbLl+J>%)lvi{g%m2~K< z-b{}EbzwAVTY()fAegzD!j)XSz8e9g{yQ*$!vdf4+MB=`w z$_A(K3O3AK1e&`rrZ5p0u&L!ThICQTRJY$12Jy!2hInO1;uE{RtT#so8W()PHCLZh zIe?5F?yBZs-!y3!g$L)H^V5tnQ-@P#t2Uw&QC>#mFyK6G_>b6taZNSKptRR)$FfWm z+UwrJDrYS(peb_B=DOlg*vR7mVv0Sw7U??$t?5;s%->03$$d>h-u}usG+*9$vd)u) zrf`+6JbYpBjDJoq0V$H(R+;0Re*F3RURw;@Q(MWVW8LFg^3GXir7rI#aKsjJ=8bPA zs>FbS$JG|BI%HxH^DX>rN^Ox?oXH#{tqt|0td_i+1W1{jeYC7HG3 z;~5yJgDnWL6-G<`Wo~Ju$oH7Irw?jAfYxLf!be>&|5{aOuLU*^*AF)&Ye_mh$y9fH z;}&8&<*eqa_bl}5*Z!W^q_bCOio=i93q06M}tsT6g$)W172` zx};BDaPHVo=P#q)QBKUH0iL0c0Z$|(IwF*SAW#v6RYR&?nB&Q5g0HN=w&U`Ne%k=oc)cP3I?3wFalU^AAOtE(8ws!xOk|Gts+gT2=4GIWF< zr)%#M#De1MAfcl>!N(HP%Ngq-L&Lp$!`>z@AJRUq+MS4rm8%Ldj#xH?I2Lre%^M8*(-5OE$I|7wC|V;TWvwNF z=D)vQCi@H|v?Fwx@)xIiikKY?uDy4R)kDgte;&yZt~?C#QP^1ZL#C~Trf<~d%|?N4 z;be{1*w)Ot=usv$c#4Tc#dI7bZ?abBX-f9Or+$F=wcp<(-(4 z`XyfR&04rBC8#>tDDN&t>(mkBQ-QUdBdqHUfybLl6W-*Bvx4e|kK?h&qa8p7sEYkH zVIov}DEGzVU?^yv2ESXU)3EFlYbS2*(cBH2wv1{5w#|kMSYU!?K|XA1G$vqlHwd|( zdQB-651w}2Ia5tDmCQLeNeh!ysB)XcoZ5I+@kQV;dC$#nrsI3HC{L4~AM$W2>MUYV zt3bi6QVe&RiMdjaV(#r{+g_>d#be53jSW6&`DE&>9vXXAFP0abbWPP8 zx_&+>5R55ku}KeN^gmcd={beauxb3V?Se7bG%BR{HtH)z^&)v~m)aimn?~+C@CWRT z=pHN&MXwpJmEkEssh>0oeFcWBGUB}dZIs1`=oR+g}8Lx8`w{0^#(1n zeexI%snI@rc~^%e-hp}T`j^S}MTK2ll-Ne86rzpP9+z#Cv!1`?KC8vFIKYQU@nD=-fZR)DIdrZJJ`CEeH1mN%&f5FGayOH|sz-vC0ZMN(dh` z5w5|gmzl0<+31`4WOg2Ywnf3wv)--$NeJoscn ziEwjC^z@{t(+0p3c6Nt1@vjC=KtNsS~p8!tDo?jH?MxnDId;$`0!SvZl2ju&b9;# zrKh21bfA0QA8<%t+|ey+O?8O5_XM$Q*DzVT&GzPUwk)@i`AIK#Ch?7yH{p|>5)t&* J`IMs1{{w28_7eaA literal 0 HcmV?d00001 From 7733ff67cc73c7739af7cad83f17437d06c94af5 Mon Sep 17 00:00:00 2001 From: Shaoshuaizyk Date: Tue, 2 Nov 2021 17:22:59 -0700 Subject: [PATCH 08/15] change names to avoid conflicts --- Motion_Planning/3Safe_Flight_Corridors/LineSegment.m | 2 +- .../3Safe_Flight_Corridors/{Polyhedron.m => Polyhedron_.m} | 4 ++-- Motion_Planning/6Star_Convex/StarConvex.m | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) rename Motion_Planning/3Safe_Flight_Corridors/{Polyhedron.m => Polyhedron_.m} (94%) diff --git a/Motion_Planning/3Safe_Flight_Corridors/LineSegment.m b/Motion_Planning/3Safe_Flight_Corridors/LineSegment.m index 664688d..c13c694 100644 --- a/Motion_Planning/3Safe_Flight_Corridors/LineSegment.m +++ b/Motion_Planning/3Safe_Flight_Corridors/LineSegment.m @@ -27,7 +27,7 @@ function set_local_bbox(obj, local_bbox) end function set_obs(obj, obs) - Vs = Polyhedron(); + Vs = Polyhedron_(); obj.add_local_bbox(Vs); obj.obs_ = Vs.points_inside(obs); end diff --git a/Motion_Planning/3Safe_Flight_Corridors/Polyhedron.m b/Motion_Planning/3Safe_Flight_Corridors/Polyhedron_.m similarity index 94% rename from Motion_Planning/3Safe_Flight_Corridors/Polyhedron.m rename to Motion_Planning/3Safe_Flight_Corridors/Polyhedron_.m index 4a42a1a..de53b1d 100644 --- a/Motion_Planning/3Safe_Flight_Corridors/Polyhedron.m +++ b/Motion_Planning/3Safe_Flight_Corridors/Polyhedron_.m @@ -1,10 +1,10 @@ -classdef Polyhedron < handle +classdef Polyhedron_ < handle properties polys_; epsilon_; end methods - function obj = Polyhedron() + function obj = Polyhedron_() obj.polys_ = cell(0); obj.epsilon_ = 1e-10; end diff --git a/Motion_Planning/6Star_Convex/StarConvex.m b/Motion_Planning/6Star_Convex/StarConvex.m index eb04e8c..6590afc 100644 --- a/Motion_Planning/6Star_Convex/StarConvex.m +++ b/Motion_Planning/6Star_Convex/StarConvex.m @@ -148,7 +148,7 @@ function ShrinkToConvex(obj) obj.A = zeros(size(obj.Edges, 2), obj.dim); obj.b = zeros(size(obj.Edges, 2), 1); for i = 1: size(obj.Edges, 2) - polyhedron = Polyhedron(); + polyhedron = Polyhedron_(); base = Hyperplane(obj.Edges(i).p1,... obj.Edges(i).n); polyhedron.add(base); From 9b15afe3e219fed0b01b7d9ef7ec689f52520806 Mon Sep 17 00:00:00 2001 From: Shaoshuaizyk Date: Sun, 7 Nov 2021 16:42:50 -0800 Subject: [PATCH 09/15] might need to reset normal vectors to outside --- .../3Safe_Flight_Corridors/Polyhedron_.m | 4 +- .../6Star_Convex/LargeConvexPolytopes.m | 6 +- Motion_Planning/6Star_Convex/StarConvex.m | 68 +++++++++++++++---- 3 files changed, 62 insertions(+), 16 deletions(-) diff --git a/Motion_Planning/3Safe_Flight_Corridors/Polyhedron_.m b/Motion_Planning/3Safe_Flight_Corridors/Polyhedron_.m index de53b1d..3f642e0 100644 --- a/Motion_Planning/3Safe_Flight_Corridors/Polyhedron_.m +++ b/Motion_Planning/3Safe_Flight_Corridors/Polyhedron_.m @@ -17,9 +17,9 @@ function add(obj, plane) % Check if the point is inside polyhedron, function isinside = inside(obj, pt) isinside = false; - [len, ~]=size(obj.polys_); + [~, len]=size(obj.polys_); for i = 1 : len - if(obj.polys_{i}.signed_dist(pt) > obj.epsilon_) + if(obj.polys_{i}.signed_dist(pt) < -obj.epsilon_) return; end end diff --git a/Motion_Planning/6Star_Convex/LargeConvexPolytopes.m b/Motion_Planning/6Star_Convex/LargeConvexPolytopes.m index 1864945..8c7ba67 100644 --- a/Motion_Planning/6Star_Convex/LargeConvexPolytopes.m +++ b/Motion_Planning/6Star_Convex/LargeConvexPolytopes.m @@ -12,8 +12,11 @@ end k = convhull(flipping_xyz); if dim == 2 - sc = StarConvex(pointclouds_xyz(k, 1), pointclouds_xyz(k, 2), pos); + k = k(1:end-1); + sc = StarConvex(pointclouds_xyz(k, 1),... + pointclouds_xyz(k, 2), pos); else + k = unique(k); sc = StarConvex(pointclouds_xyz(k, 1), pointclouds_xyz(k, 2),... pos, pointclouds_xyz(k, 3)); end @@ -21,6 +24,7 @@ sc.ShrinkToConvex(); A = sc.A; b = sc.b; + sc.VisualizeResult(); end function p_xyz = AddSurroundingPoints(pointclouds_xyz, pos, R) diff --git a/Motion_Planning/6Star_Convex/StarConvex.m b/Motion_Planning/6Star_Convex/StarConvex.m index 6590afc..8da752a 100644 --- a/Motion_Planning/6Star_Convex/StarConvex.m +++ b/Motion_Planning/6Star_Convex/StarConvex.m @@ -122,7 +122,7 @@ function ConstructConvexFromStar(obj) end else k = convhull(obj.vertices_, 'Simplify',true); - ind_in = setdiff(1:size(obj.vertices_, 1), reshape(k, 1, 3 * size(k, 1))); + ind_in = setdiff(1:size(obj.vertices_, 1), unique(k)); obj.P_in_ = obj.vertices_(ind_in, :); obj.convex_hull = repmat(struct, 1, size(k, 1)); for i = 1: size(k, 1) @@ -191,42 +191,84 @@ function ShrinkToConvex(obj) end % Iterate every point inside, find the furthest to base furthest_dis = -1; - pi = obj.Edges(i).p1; + p_i = obj.Edges(i).p1; for j = 1: size(obj.P_in_, 1) p_in = obj.P_in_(j,:); if (polyhedron.inside(p_in)) % CHECK IF IT NEEDS TO BE REVERSED dis = base.signed_dist(p_in); if (dis >= furthest_dis) - pi = p_in; + p_i = p_in; furthest_dis = dis; end end end - obj.A(i, :) = base.n_; - obj.b(i) = dot(base.n_, pi); + % The normal vector in the paper is pointing to outside. + obj.A(i, :) = -base.n_; + obj.b(i) = dot(-base.n_, p_i); end end function PlotConvexHull(obj) - plot([obj.convex_hull(1, :), obj.convex_hull(1, 1)], ... - [obj.convex_hull(2, :), obj.convex_hull(2, 1)], 'k*--'); + plot(obj.convex_hull(:, 1), obj.convex_hull(:, 2), 'k*--'); end function PlotVertices(obj) - plot([obj.vertices_(1, :), obj.vertices_(1, 1)], ... - [obj.vertices_(2, :), obj.vertices_(2, 1)], 'b*-'); + plot([obj.vertices_(:, 1); obj.vertices_(1, 1)], ... + [obj.vertices_(:, 2); obj.vertices_(1, 2)], 'b*-'); end function PlotInterPoint(obj) - plot(obj.P_in_(1, :), obj.P_in_(2, :), 'ro'); + plot(obj.P_in_(:, 1), obj.P_in_(:, 2), 'ro'); + end + + function PlotResults(obj) + upper = max(obj.vertices_); + lower = min(obj.vertices_); + range = upper - lower; + upper = upper + abs(range * 0.1); + lower = lower - abs(range * 0.1); + if obj.dim == 2 + [xx, yy] = meshgrid(linspace(lower(1), upper(1), 13),... + linspace(lower(2), upper(2), 13)); + region = ones(size(xx)); + for i=1:size(obj.b, 1) + sub_region = obj.A(i, 1).*xx + obj.A(i, 2).*yy <= obj.b(i); + region = region & sub_region; + end + surf(xx,yy,double(region)); + colorbar; + view(0,90); + else + [xx, yy, zz] = meshgrid(linspace(lower(1), upper(1), 13),... + linspace(lower(2), upper(2), 13),... + linspace(lower(3), upper(3), 13)); + region = ones(size(xx)); + for i=1:size(obj.b, 1) + sub_region = obj.A(i, 1).*xx + obj.A(i, 2).*yy +... + obj.A(i, 3).*zz <= obj.b(i); + region = region & sub_region; + end + + scatter3(xx(region), yy(region), zz(region)); +% p=patch(isosurface(region, 0)); +% set(p,'FaceColor','red','EdgeColor','none'); + daspect([1,1,1]) + view(3); axis equal + camlight + lighting gouraud + grid on + end end function VisualizeResult(obj) figure; hold on - obj.PlotVertices(); - obj.PlotConvexHull(); - obj.PlotInterPoint(); + if obj.dim == 2 + obj.PlotVertices(); + obj.PlotConvexHull(); + obj.PlotInterPoint(); + end + obj.PlotResults(); hold off; end end From 757fdf463db48991e9e7d4dc4195d5bc4dada1fd Mon Sep 17 00:00:00 2001 From: Shaoshuaizyk Date: Sun, 5 Dec 2021 00:54:53 -0800 Subject: [PATCH 10/15] Connecting convex region for star convex method from path --- .../3Safe_Flight_Corridors/LineSegment.m | 2 +- .../6Star_Convex/LargeConvexPolytopes.m | 2 +- Motion_Planning/6Star_Convex/SCMFromPath.m | 38 +++++++++++++++++++ Motion_Planning/runsim.m | 6 +++ 4 files changed, 46 insertions(+), 2 deletions(-) create mode 100644 Motion_Planning/6Star_Convex/SCMFromPath.m diff --git a/Motion_Planning/3Safe_Flight_Corridors/LineSegment.m b/Motion_Planning/3Safe_Flight_Corridors/LineSegment.m index c13c694..7f07c66 100644 --- a/Motion_Planning/3Safe_Flight_Corridors/LineSegment.m +++ b/Motion_Planning/3Safe_Flight_Corridors/LineSegment.m @@ -144,7 +144,7 @@ function find_ellipsoid(obj, offset_x) end function find_polyhedron(obj) - Vs = Polyhedron(); + Vs = Polyhedron_(); obs_remain = obj.obs_; while(length(obs_remain)) plane = obj.ellipsoid_.closest_hyperplane(obs_remain); diff --git a/Motion_Planning/6Star_Convex/LargeConvexPolytopes.m b/Motion_Planning/6Star_Convex/LargeConvexPolytopes.m index 8c7ba67..4783867 100644 --- a/Motion_Planning/6Star_Convex/LargeConvexPolytopes.m +++ b/Motion_Planning/6Star_Convex/LargeConvexPolytopes.m @@ -24,7 +24,7 @@ sc.ShrinkToConvex(); A = sc.A; b = sc.b; - sc.VisualizeResult(); +% sc.VisualizeResult(); end function p_xyz = AddSurroundingPoints(pointclouds_xyz, pos, R) diff --git a/Motion_Planning/6Star_Convex/SCMFromPath.m b/Motion_Planning/6Star_Convex/SCMFromPath.m new file mode 100644 index 0000000..35dbf12 --- /dev/null +++ b/Motion_Planning/6Star_Convex/SCMFromPath.m @@ -0,0 +1,38 @@ +function [A, b] = SCMFromPath(path, point_cloud_obs, map_boundary) +% LargeConvexPolytopes generates A and b from a single point, SCMFrom path +% generate A and b from a path. + map_size = min(map_boundary.ld - map_boundary.ru); + R = map_size; + tolerance = 0.1; + A = {}; + b = {}; + ieq = 1; + n_node = size(path, 1) - 1; + pos = path(1, :); + [cur_A, cur_b] = LargeConvexPolytopes(point_cloud_obs, pos, R); + A{1} = cur_A; + b{1} = cur_b; + for i = 1:n_node + cur_pt = path(i, :); + next_pt = path(i+1, :); + while any(cur_A * next_pt' >= cur_b) + start = 0; + goal = 1; + distance = norm(next_pt - cur_pt); + while distance*(goal - start) > tolerance + mid = start + (goal - start) / 2; + mid_pt = cur_pt + (next_pt - cur_pt) * mid; + if any(cur_A * mid_pt' >= cur_b) + goal = mid; + else + start = mid; + end + end + cur_pt = cur_pt + (next_pt - cur_pt) * goal; + [cur_A, cur_b] = LargeConvexPolytopes(point_cloud_obs, cur_pt, R); + A{ieq+1} = cur_A; + b{ieq+1} = cur_b; + ieq = ieq + 1; + end + end +end \ No newline at end of file diff --git a/Motion_Planning/runsim.m b/Motion_Planning/runsim.m index 75e33be..08309a3 100644 --- a/Motion_Planning/runsim.m +++ b/Motion_Planning/runsim.m @@ -39,6 +39,12 @@ decomps{1} = SFC_3D(path{2}, obps, map.boundary); % call SFC toc +disp('JPS -> StarConvexMethod time is : '); +tic +[A, b] = SCMFromPath(path{2}, obps, map.boundary); +toc + + path{path_id} From 92aaf60cd48b1211fcca2e8ee5509a2a7d91805e Mon Sep 17 00:00:00 2001 From: Prabz25 <95543036+Prabz25@users.noreply.github.com> Date: Sun, 5 Dec 2021 14:46:29 -0800 Subject: [PATCH 11/15] Add files via upload --- forest_environment.txt | 13 +++++++++++++ urban_environment.txt | 15 +++++++++++++++ 2 files changed, 28 insertions(+) create mode 100644 forest_environment.txt create mode 100644 urban_environment.txt diff --git a/forest_environment.txt b/forest_environment.txt new file mode 100644 index 0000000..d9f3b76 --- /dev/null +++ b/forest_environment.txt @@ -0,0 +1,13 @@ +# map forest environment +boundary 0.0 0.0 0.0 5 5 3.0 + +block 1.0 0.5 0.1 1.3 0.8 3.0 0.000000 255.000000 0.000000 +block 0 1.5 0 0.3 1.8 3.0 0.000000 255.000000 0.000000 +block 2.0 1.5 0.1 2.3 1.8 3.0 0.000000 255.000000 0.000000 +block 1.0 3 0.1 1.3 3.3 3.0 0.000000 255.000000 0.000000 +block 3 3 0.1 3.3 3.3 3.0 0.000000 255.000000 0.000000 +block 0 4.5 0.1 0.3 4.8 3.0 0.000000 255.000000 0.000000 +block 2 4.5 0.1 2.3 4.8 3.0 0.000000 255.000000 0.000000 + + + diff --git a/urban_environment.txt b/urban_environment.txt new file mode 100644 index 0000000..0ab75b1 --- /dev/null +++ b/urban_environment.txt @@ -0,0 +1,15 @@ +# map urban environment +boundary 0.0 0.0 0.0 40 40 3.0 + +block 32 5 0 35 8 3.0 0 0 255 +block 28 8 0 35 9.5 3.0 0 0 255 +block 25 5 0 28 9.5 3.0 0 0 255 +block 5 13.5 0 15 29.5 3.0 255 0 0 +block 5 2 0 12.5 9.5 3.0 0 0 255 +block 25 13.5 0 35 25.5 3.0 0 0 255 + + + + + + From 13ef62c8e86a6a1fc383951826874a2f9315d497 Mon Sep 17 00:00:00 2001 From: Prabz25 <95543036+Prabz25@users.noreply.github.com> Date: Sun, 5 Dec 2021 14:48:39 -0800 Subject: [PATCH 12/15] Add files via upload --- Motion_Planning/0Maps/forest_environment.txt | 13 +++++++++++++ Motion_Planning/0Maps/urban_environment.txt | 15 +++++++++++++++ 2 files changed, 28 insertions(+) create mode 100644 Motion_Planning/0Maps/forest_environment.txt create mode 100644 Motion_Planning/0Maps/urban_environment.txt diff --git a/Motion_Planning/0Maps/forest_environment.txt b/Motion_Planning/0Maps/forest_environment.txt new file mode 100644 index 0000000..d9f3b76 --- /dev/null +++ b/Motion_Planning/0Maps/forest_environment.txt @@ -0,0 +1,13 @@ +# map forest environment +boundary 0.0 0.0 0.0 5 5 3.0 + +block 1.0 0.5 0.1 1.3 0.8 3.0 0.000000 255.000000 0.000000 +block 0 1.5 0 0.3 1.8 3.0 0.000000 255.000000 0.000000 +block 2.0 1.5 0.1 2.3 1.8 3.0 0.000000 255.000000 0.000000 +block 1.0 3 0.1 1.3 3.3 3.0 0.000000 255.000000 0.000000 +block 3 3 0.1 3.3 3.3 3.0 0.000000 255.000000 0.000000 +block 0 4.5 0.1 0.3 4.8 3.0 0.000000 255.000000 0.000000 +block 2 4.5 0.1 2.3 4.8 3.0 0.000000 255.000000 0.000000 + + + diff --git a/Motion_Planning/0Maps/urban_environment.txt b/Motion_Planning/0Maps/urban_environment.txt new file mode 100644 index 0000000..0ab75b1 --- /dev/null +++ b/Motion_Planning/0Maps/urban_environment.txt @@ -0,0 +1,15 @@ +# map urban environment +boundary 0.0 0.0 0.0 40 40 3.0 + +block 32 5 0 35 8 3.0 0 0 255 +block 28 8 0 35 9.5 3.0 0 0 255 +block 25 5 0 28 9.5 3.0 0 0 255 +block 5 13.5 0 15 29.5 3.0 255 0 0 +block 5 2 0 12.5 9.5 3.0 0 0 255 +block 25 13.5 0 35 25.5 3.0 0 0 255 + + + + + + From bbf4605ff689ef343c2facaf45e4df3d7fd11d36 Mon Sep 17 00:00:00 2001 From: Prabz25 <95543036+Prabz25@users.noreply.github.com> Date: Sun, 5 Dec 2021 14:49:18 -0800 Subject: [PATCH 13/15] Add files via upload --- Motion_Planning/demo/demo_forest_environment.m | 9 +++++++++ Motion_Planning/demo/demo_urban_environment.m | 9 +++++++++ 2 files changed, 18 insertions(+) create mode 100644 Motion_Planning/demo/demo_forest_environment.m create mode 100644 Motion_Planning/demo/demo_urban_environment.m diff --git a/Motion_Planning/demo/demo_forest_environment.m b/Motion_Planning/demo/demo_forest_environment.m new file mode 100644 index 0000000..3ad3c27 --- /dev/null +++ b/Motion_Planning/demo/demo_forest_environment.m @@ -0,0 +1,9 @@ +close all; +clear all; +clc; + +demoActivate = true; +choose_map = 1; +path_id = 2; + +runsim \ No newline at end of file diff --git a/Motion_Planning/demo/demo_urban_environment.m b/Motion_Planning/demo/demo_urban_environment.m new file mode 100644 index 0000000..3ad3c27 --- /dev/null +++ b/Motion_Planning/demo/demo_urban_environment.m @@ -0,0 +1,9 @@ +close all; +clear all; +clc; + +demoActivate = true; +choose_map = 1; +path_id = 2; + +runsim \ No newline at end of file From 79480f0ccfff3b3acd4b83db9f9007c5a98d75c9 Mon Sep 17 00:00:00 2001 From: Prabz25 <95543036+Prabz25@users.noreply.github.com> Date: Sun, 5 Dec 2021 14:50:50 -0800 Subject: [PATCH 14/15] Add files via upload --- Motion_Planning/init_data.m | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/Motion_Planning/init_data.m b/Motion_Planning/init_data.m index b671059..731e482 100644 --- a/Motion_Planning/init_data.m +++ b/Motion_Planning/init_data.m @@ -2,7 +2,7 @@ stop = {[]}; if (~exist('demoActivate')) || (demoActivate == false) - choose_map = 2; + choose_map = 1; end demoActivate = false; if (~exist('path_id')) @@ -17,11 +17,11 @@ switch choose_map case 1 %% 3 blocks - map.load_map('0Maps/ellipsoid.txt', 0.2, 0.2, 0.1); - start = {[1.5 0.2 0.2]}; - stop = {[1.5 2.0 1.9]}; + map.load_map('0Maps/forest_environment.txt', 0.2, 0.2, 0.1); + start = {[1.2 0 0]}; + stop = {[1.5 5 2]}; speed = 1; - acc = 2; + acc = 1; case 2 map.load_map('0Maps/3dCorner.txt', 0.1, 0.1, 0.1); start = {[2.6 1.0 1.0]}; @@ -37,7 +37,7 @@ start = {[-1 1.5 2]}; stop = {[11 1.5 1]}; acc = 2; - if path_id == 4 + if path_id == 2 speed = 1.4; %for path_id = 4, time = 12.9343, snap = 58.376, fly use 12.45s else speed = 1.10423; %for path_id = 2, time = 12.9343, snap = 300.3111, fly use 13.9s From d6a5c5ba2ab9458b1cd45ce3a28f9f0e13d35e20 Mon Sep 17 00:00:00 2001 From: Shaoshuaizyk Date: Sat, 1 Jan 2022 16:55:09 -0800 Subject: [PATCH 15/15] fix bugs --- .../6Star_Convex/LargeConvexPolytopes.m | 27 +++++++++++++------ Motion_Planning/6Star_Convex/SCMFromPath.m | 24 ++++++++++++++--- Motion_Planning/6Star_Convex/StarConvex.m | 9 +++++++ 3 files changed, 49 insertions(+), 11 deletions(-) diff --git a/Motion_Planning/6Star_Convex/LargeConvexPolytopes.m b/Motion_Planning/6Star_Convex/LargeConvexPolytopes.m index 4783867..3a3c3e4 100644 --- a/Motion_Planning/6Star_Convex/LargeConvexPolytopes.m +++ b/Motion_Planning/6Star_Convex/LargeConvexPolytopes.m @@ -5,20 +5,22 @@ dim = length(pos); assert(dim == 2 || dim == 3, "Wrong dimension! Check input data!"); - pointclouds_xyz = AddSurroundingPoints(pointclouds_xyz, pos, R); - flipping_xyz = zeros(size(pointclouds_xyz)); - for i = 1: size(pointclouds_xyz, 1) - flipping_xyz(i, :) = SphereFlipping(pos, pointclouds_xyz(i, :), R); + points_inside = FindInsidePoints(pointclouds_xyz, pos, R); + +% pointclouds_xyz = AddSurroundingPoints(pointclouds_xyz, pos, R); + flipping_xyz = zeros(size(points_inside)); + for i = 1: size(points_inside, 1) + flipping_xyz(i, :) = SphereFlipping(pos, points_inside(i, :), R); end k = convhull(flipping_xyz); if dim == 2 k = k(1:end-1); - sc = StarConvex(pointclouds_xyz(k, 1),... - pointclouds_xyz(k, 2), pos); + sc = StarConvex(points_inside(k, 1),... + points_inside(k, 2), pos); else k = unique(k); - sc = StarConvex(pointclouds_xyz(k, 1), pointclouds_xyz(k, 2),... - pos, pointclouds_xyz(k, 3)); + sc = StarConvex(points_inside(k, 1), points_inside(k, 2),... + pos, points_inside(k, 3)); end sc.ConstructConvexFromStar(); sc.ShrinkToConvex(); @@ -27,6 +29,14 @@ % sc.VisualizeResult(); end +function points_inside = FindInsidePoints(pointclouds_xyz, pos, R) + % Find all the points inside the circle with R as radius and pos as the + % center. + distance = vecnorm(pointclouds_xyz - pos, 2, 2); + points_inside = pointclouds_xyz(distance <= R, :); + points_inside = AddSurroundingPoints(points_inside, pos, R); +end + function p_xyz = AddSurroundingPoints(pointclouds_xyz, pos, R) % If there are not enough surrounding points, add surrounding points % manually. For 2d, add them as a square, for 3d, add them as a cube @@ -49,6 +59,7 @@ pointclouds_xyz(end+1, :) = [pos(1)-rc, pos(2)-rc, pos(3)+rc]; pointclouds_xyz(end+1, :) = [pos(1)-rc, pos(2)-rc, pos(3)-rc]; pointclouds_xyz(end+1, :) = [pos(1)+rc, pos(2)+rc, pos(3)+rc]; +% disp(pointclouds_xyz(end-7:end, :)); end p_xyz = pointclouds_xyz; end diff --git a/Motion_Planning/6Star_Convex/SCMFromPath.m b/Motion_Planning/6Star_Convex/SCMFromPath.m index 35dbf12..490c785 100644 --- a/Motion_Planning/6Star_Convex/SCMFromPath.m +++ b/Motion_Planning/6Star_Convex/SCMFromPath.m @@ -1,9 +1,10 @@ function [A, b] = SCMFromPath(path, point_cloud_obs, map_boundary) % LargeConvexPolytopes generates A and b from a single point, SCMFrom path % generate A and b from a path. - map_size = min(map_boundary.ld - map_boundary.ru); - R = map_size; - tolerance = 0.1; + map_size = min(abs(map_boundary.ld - map_boundary.ru)); +% fprintf("Map size is: %f \n", map_size); + R = map_size/2; + tolerance = 0.01; A = {}; b = {}; ieq = 1; @@ -11,17 +12,30 @@ pos = path(1, :); [cur_A, cur_b] = LargeConvexPolytopes(point_cloud_obs, pos, R); A{1} = cur_A; +% cur_b = cur_A * pos' + cur_b; b{1} = cur_b; for i = 1:n_node +% fprintf("Node number %d \n", i); +% if i == 7 +% ; +% end cur_pt = path(i, :); next_pt = path(i+1, :); +% disp(cur_pt); +% disp(next_pt); while any(cur_A * next_pt' >= cur_b) + if any(cur_A * cur_pt' >= cur_b) + disp("Wrong! Current point is not in the polytope!"); + end +% fprintf("Checking node number %d \n", i); start = 0; goal = 1; distance = norm(next_pt - cur_pt); +% fprintf("Distance between start and goal is %f .\n", distance); while distance*(goal - start) > tolerance mid = start + (goal - start) / 2; mid_pt = cur_pt + (next_pt - cur_pt) * mid; +% fprintf("Start is %f, middle is %f, goal is %f.\n", start, mid, goal); if any(cur_A * mid_pt' >= cur_b) goal = mid; else @@ -30,7 +44,11 @@ end cur_pt = cur_pt + (next_pt - cur_pt) * goal; [cur_A, cur_b] = LargeConvexPolytopes(point_cloud_obs, cur_pt, R); + if any(cur_A * cur_pt' >= cur_b) + disp("Wrong! Current point is not in the polytope!"); + end A{ieq+1} = cur_A; +% cur_b = cur_A * cur_pt' + cur_b; b{ieq+1} = cur_b; ieq = ieq + 1; end diff --git a/Motion_Planning/6Star_Convex/StarConvex.m b/Motion_Planning/6Star_Convex/StarConvex.m index 8da752a..8c5fdbf 100644 --- a/Motion_Planning/6Star_Convex/StarConvex.m +++ b/Motion_Planning/6Star_Convex/StarConvex.m @@ -205,6 +205,15 @@ function ShrinkToConvex(obj) % The normal vector in the paper is pointing to outside. obj.A(i, :) = -base.n_; obj.b(i) = dot(-base.n_, p_i); +% if i == 12 +% ; +% end +% disp("Current point Po is: "); +% disp(obj.Po_); +% if dot(obj.A(i, :), obj.Po_) >= obj.b(i) +% disp("Shrink the edge inside the pos."); +% end + assert(dot(obj.A(i, :), obj.Po_) < obj.b(i), "Shrink the edge inside the pos."); end end