diff --git a/src/Approximations/iterative_refinement.jl b/src/Approximations/iterative_refinement.jl index 726e0d1ebd..5f3cac9276 100644 --- a/src/Approximations/iterative_refinement.jl +++ b/src/Approximations/iterative_refinement.jl @@ -5,17 +5,17 @@ Type that represents a local approximation in 2D. ### Fields -- `p1` -- first inner point -- `d1` -- first direction -- `p2` -- second inner point -- `d2` -- second direction -- `q` -- intersection of the lines l1 ⟂ d1 at p1 and l2 ⟂ d2 at p2 -- `refinable` -- states if this approximation is refinable -- `err` -- error upper bound +- `p1` -- first inner point +- `d1` -- first direction +- `p2` -- second inner point +- `d2` -- second direction +- `q` -- intersection of the lines l1 ⟂ d1 at p1 and l2 ⟂ d2 at p2 +- `refinable` -- states if this approximation is refinable +- `err` -- error upper bound ### Notes -The criteria for refinable are determined in the method `new_approx`. +The criteria for being refinable are determined in the method `new_approx`. """ struct LocalApproximation{N<:Real} p1::Vector{N} @@ -34,36 +34,41 @@ Type that represents the polygonal approximation of a convex set. ### Fields -- `S` -- convex set -- `approx_list` -- vector of local approximations +- `S` -- convex set +- `approx_stack` -- stack of local approximations that still need to be examined +- `constraints` -- vector of linear constraints that are already finalized + (i.e., they satisfy the given error bound) """ struct PolygonalOverapproximation{N<:Real} - S::LazySet - approx_list::Vector{LocalApproximation{N}} -end + S::LazySet{N} + approx_stack::Vector{LocalApproximation{N}} + constraints::Vector{LinearConstraint{N}} -# initialize a polygonal overapproximation with an empty list -PolygonalOverapproximation{N}(S::LazySet) where {N<:Real} = - PolygonalOverapproximation(S::LazySet, LocalApproximation{N}[]) + PolygonalOverapproximation(S::LazySet{N}) where N<:Real = new{N}( + S, Vector{LocalApproximation{N}}(), Vector{LinearConstraint{N}}()) +end """ new_approx(S::LazySet, p1::Vector{N}, d1::Vector{N}, p2::Vector{N}, - d2::Vector{N}) where {N<:Real} + d2::Vector{N}) where N<:Real + +Create a `LocalApproximation` instance for the given excerpt of a polygonal +approximation. ### Input -- `S` -- convex set -- `p1` -- first inner point -- `d1` -- first direction -- `p2` -- second inner point -- `d2` -- second direction +- `S` -- convex set +- `p1` -- first inner point +- `d1` -- first direction +- `p2` -- second inner point +- `d2` -- second direction ### Output A local approximation of `S` in the given directions. """ function new_approx(S::LazySet, p1::Vector{N}, d1::Vector{N}, p2::Vector{N}, - d2::Vector{N}) where {N<:Real} + d2::Vector{N}) where N<:Real if norm(p1-p2, 2) <= TOL(N) # this approximation cannot be refined and we set q = p1 by convention ap = LocalApproximation{N}(p1, d1, p2, d2, p1, false, zero(N)) @@ -71,24 +76,24 @@ function new_approx(S::LazySet, p1::Vector{N}, d1::Vector{N}, p2::Vector{N}, ndir = normalize([p2[2]-p1[2], p1[1]-p2[1]]) q = element(intersection(Line(d1, dot(d1, p1)), Line(d2, dot(d2, p2)))) approx_error = min(norm(q - σ(ndir, S)), dot(ndir, q - p1)) - refinable = (approx_error > TOL(N)) && - !(norm(p1-q, 2) <= TOL(N) || norm(q-p2, 2) <= TOL(N)) + refinable = (approx_error > TOL(N)) && (norm(p1-q, 2) > TOL(N)) && + (norm(q-p2, 2) > TOL(N)) ap = LocalApproximation{N}(p1, d1, p2, d2, q, refinable, approx_error) end return ap end """ - addapproximation!(Ω::PolygonalOverapproximation, - p1::Vector{N}, d1::Vector{N}, p2::Vector{N}, d2::Vector{N}) where {N<:Real} + addapproximation!(Ω::PolygonalOverapproximation, p1::Vector{N}, + d1::Vector{N}, p2::Vector{N}, d2::Vector{N}) where N<:Real ### Input -- `Ω` -- polygonal overapproximation of a convex set -- `p1` -- first inner point -- `d1` -- first direction -- `p2` -- second inner point -- `d2` -- second direction +- `Ω` -- polygonal overapproximation of a convex set +- `p1` -- first inner point +- `d1` -- first direction +- `p2` -- second inner point +- `d2` -- second direction ### Output @@ -97,63 +102,67 @@ the new approximation is returned by this function. """ function addapproximation!(Ω::PolygonalOverapproximation, p1::Vector{N}, d1::Vector{N}, p2::Vector{N}, - d2::Vector{N})::LocalApproximation{N} where {N<:Real} + d2::Vector{N})::LocalApproximation{N} where N<:Real approx = new_approx(Ω.S, p1, d1, p2, d2) - push!(Ω.approx_list, approx) + push!(Ω.approx_stack, approx) return approx end """ - refine(Ω::PolygonalOverapproximation, i::Int)::Tuple{LocalApproximation, LocalApproximation} + refine(approx::LocalApproximation, S::LazySet + )::Tuple{LocalApproximation, LocalApproximation} -Refine a given local approximation of the polygonal approximation of a convex set, -by splitting along the normal direction to the approximation. +Refine a given local approximation of the polygonal approximation of a convex +set by splitting along the normal direction of the approximation. ### Input -- `Ω` -- polygonal overapproximation of a convex set -- `i` -- integer index for the local approximation to be refined +- `approx` -- local approximation to be refined +- `S` -- 2D convex set ### Output The tuple consisting of the refined right and left local approximations. """ -function refine(Ω::PolygonalOverapproximation, i::Int)::Tuple{LocalApproximation, LocalApproximation} - R = Ω.approx_list[i] - @assert R.refinable - - ndir = normalize([R.p2[2]-R.p1[2], R.p1[1]-R.p2[1]]) - s = σ(ndir, Ω.S) - ap1 = new_approx(Ω.S, R.p1, R.d1, s, ndir) - ap2 = new_approx(Ω.S, s, ndir, R.p2, R.d2) +function refine(approx::LocalApproximation, + S::LazySet + )::Tuple{LocalApproximation, LocalApproximation} + @assert approx.refinable + + ndir = normalize([approx.p2[2]-approx.p1[2], approx.p1[1]-approx.p2[1]]) + s = σ(ndir, S) + ap1 = new_approx(S, approx.p1, approx.d1, s, ndir) + ap2 = new_approx(S, s, ndir, approx.p2, approx.d2) return (ap1, ap2) end """ - tohrep(Ω::PolygonalOverapproximation{N})::AbstractHPolygon where {N<:Real} + tohrep(Ω::PolygonalOverapproximation{N})::AbstractHPolygon{N} where N<:Real Convert a polygonal overapproximation into a concrete polygon. ### Input -- `Ω` -- polygonal overapproximation of a convex set +- `Ω` -- polygonal overapproximation of a convex set ### Output A polygon in constraint representation. + +### Algorithm + +Internally we keep the constraints sorted. +Hence we do not need to use `addconstraint!` when creating the `HPolygon`. """ -function tohrep(Ω::PolygonalOverapproximation{N})::AbstractHPolygon where {N<:Real} - p = HPolygon{N}() - for ai in Ω.approx_list - addconstraint!(p, LinearConstraint(ai.d1, dot(ai.d1, ai.p1))) - end - return p +function tohrep(Ω::PolygonalOverapproximation{N} + )::AbstractHPolygon{N} where N<:Real + return HPolygon{N}(Ω.constraints) end """ approximate(S::LazySet{N}, - ε::N)::PolygonalOverapproximation{N} where {N<:Real} + ε::N)::PolygonalOverapproximation{N} where N<:Real Return an ε-close approximation of the given 2D convex set (in terms of Hausdorff distance) as an inner and an outer approximation composed by sorted @@ -169,7 +178,7 @@ local `Approximation2D`. An ε-close approximation of the given 2D convex set. """ function approximate(S::LazySet{N}, - ε::N)::PolygonalOverapproximation{N} where {N<:Real} + ε::N)::PolygonalOverapproximation{N} where N<:Real # initialize box directions pe = σ(DIR_EAST(N), S) @@ -177,38 +186,44 @@ function approximate(S::LazySet{N}, pw = σ(DIR_WEST(N), S) ps = σ(DIR_SOUTH(N), S) - Ω = PolygonalOverapproximation{N}(S) + Ω = PolygonalOverapproximation(S) - addapproximation!(Ω, pe, DIR_EAST(N), pn, DIR_NORTH(N)) - addapproximation!(Ω, pn, DIR_NORTH(N), pw, DIR_WEST(N)) - addapproximation!(Ω, pw, DIR_WEST(N), ps, DIR_SOUTH(N)) + # add constraints in reverse (i.e., clockwise) order to the stack addapproximation!(Ω, ps, DIR_SOUTH(N), pe, DIR_EAST(N)) + addapproximation!(Ω, pw, DIR_WEST(N), ps, DIR_SOUTH(N)) + addapproximation!(Ω, pn, DIR_NORTH(N), pw, DIR_WEST(N)) + addapproximation!(Ω, pe, DIR_EAST(N), pn, DIR_NORTH(N)) + + approx_stack = Ω.approx_stack + while !isempty(approx_stack) + approx = pop!(approx_stack) - i = 1 - while i <= length(Ω.approx_list) - approx = Ω.approx_list[i] if !approx.refinable || approx.err <= ε - # if this approximation doesn't need to be refined, consider the next - # one in the queue (counter-clockwise order wrt d1) # if the approximation is not refinable => continue - i += 1 - else - inext = i + 1 - - (la1, la2) = refine(Ω, i) + push!(Ω.constraints, + LinearConstraint(approx.d1, dot(approx.d1, approx.p1))) + continue + end - Ω.approx_list[i] = la1 + # refine + (la1, la2) = refine(approx, Ω.S) - redundant = inext > length(Ω.approx_list) ? false : - (norm(la2.p1-Ω.approx_list[inext].p1) <= TOL(N)) && - (norm(la2.q-Ω.approx_list[inext].q) <= TOL(N)) - if redundant - # if it is redundant, keep the refined approximation - Ω.approx_list[inext] = la2 - else - insert!(Ω.approx_list, inext, la2) - end + if isempty(approx_stack) + redundant = false + else + # check if the next local approximation became redundant + next = approx_stack[end] + redundant = (norm(la2.p1-next.p1) <= TOL(N)) && + (norm(la2.q-next.q) <= TOL(N)) + end + if redundant + # replace redundant old constraint + approx_stack[end] = la2 + else + # add new constraint + push!(approx_stack, la2) end + push!(approx_stack, la1) end return Ω end