diff --git a/src/Flowpipes/clustering.jl b/src/Flowpipes/clustering.jl index 73a6c0d62..f1e39eaa3 100644 --- a/src/Flowpipes/clustering.jl +++ b/src/Flowpipes/clustering.jl @@ -117,8 +117,12 @@ end # for Taylor model flowpipes we preprocess it with a zonotopic overapproximation function cluster(F::Flowpipe{N, TaylorModelReachSet{N}}, idx, method::LazyClustering{P, Val{true}}) where {N, P} Fz = overapproximate(Flowpipe(view(F, idx)), Zonotope) + return cluster(Fz, 1:length(idx), method) # Fx is now indexed from 1 ... length(idx) +end - # Fx is now indexed from 1 ... length(idx) +# ambiguity fix +function cluster(F::Flowpipe{N, TaylorModelReachSet{N}}, idx, method::LazyClustering{P, Val{false}}) where {N, P} + Fz = overapproximate(Flowpipe(view(F, idx)), Zonotope) return cluster(Fz, 1:length(idx), method) end diff --git a/src/Flowpipes/setops.jl b/src/Flowpipes/setops.jl index c5be60384..05a38232c 100644 --- a/src/Flowpipes/setops.jl +++ b/src/Flowpipes/setops.jl @@ -660,13 +660,30 @@ end return true end -# ================================== +# ==================================================================== # Concrete intersection -# ================================== +# +# Methods to compute the intersection between two or more sets +# ==================================================================== -# ------------------------------------------------------------- -# Methods to compute the intersection between two or more sets -# ------------------------------------------------------------ +# ----------------------- +# Auxiliary functions +# ----------------------- + +# converts the normal vector of a list of half-spaces to be a Vector +const VECH{N, VT} = Vector{HalfSpace{N, VT}} +_to_vec(c::HalfSpace{N, Vector{N}}) where {N} = c +_to_vec(c::HalfSpace{N, VT}) where {N, VT<:AbstractVector{N}} = HalfSpace(Vector(c.a), c.b) +_to_vec(x::VECH{N, Vector{N}}) where {N} = x +_to_vec(x::VECH{N, VT}) where {N, VT<:AbstractVector{N}} = broadcast(_to_vec, x) + +# concatenates lists of half-spaces such that the normal vectors of the final list +# are all Vector +_vcat(args::VECH...) = vcat(map(_to_vec, args)...) + +# ------------------------ +# FallbackIntersection +# ------------------------ abstract type AbstractIntersectionMethod end @@ -690,63 +707,23 @@ function _intersection(X::AbstractPolyhedron{N}, Y::AbstractPolyhedron{N}, alg:: end end -#= -TODO annotate normal vector types? -struct HRepIntersection{SX, SY} <: AbstractIntersectionMethod end -setrep(int_method::HRepIntersection{SX<:AbstractPolytope}, SY<:AbstractPolyhedron}) = SX -=# +# ------------------------ +# HRepIntersection +# ------------------------ # evaluate X ∩ Y exactly using the constraint representation of X and Y # evaluate X₁ ∩ ⋯ ∩ Xₖ using the constraint representation of each Xᵢ -struct HRepIntersection <: AbstractIntersectionMethod # -end - -setrep(::HRepIntersection) = HPolytope{Float64, Vector{Float64}} - -struct BoxIntersection <: AbstractIntersectionMethod +# TODO Annotate normal vector types? +# struct HRepIntersection{SX, SY} <: AbstractIntersectionMethod end +# setrep(int_method::HRepIntersection{SX<:AbstractPolytope}, SY<:AbstractPolyhedron}) = SX # +struct HRepIntersection <: AbstractIntersectionMethod + # end -setrep(::BoxIntersection) = Hyperrectangle{Float64, Vector{Float64}, Vector{Float64}} - -# evaluate X ∩ Y approximately using support function evaluations, with the -# property that the support function of X ∩ Y along direction d is not greater -# ρ(d, X ∩ Y) <= min(ρ(d, X), ρ(d, Y)) -# by the same token, compute X₁ ∩ ⋯ ∩ Xₖ approximately using the same property -# if the template is provided, we have TN<:AbstractDirections{N, VN} -# otherwise, the constraints of X and Y are used and TN is Missing -struct TemplateHullIntersection{N, VN, TN} <: AbstractIntersectionMethod - dirs::TN -end - -# constructor with template directions provided -function TemplateHullIntersection(dirs::TN) where {N, VN, TN<:AbstractDirections{N, VN}} - TemplateHullIntersection{N, VN, TN}(dirs) -end - -# constructor without template directions => directions are missing until evaluated -function TemplateHullIntersection{N, VN}() where {N, VN<:AbstractVector{N}} - TemplateHullIntersection{N, VN, Missing}(missing) -end -TemplateHullIntersection() = TemplateHullIntersection{Float64, Vector{Float64}}() - -setrep(::TemplateHullIntersection{N, VN}) where {N, VN} = HPolytope{N, VN} -setrep(::TemplateHullIntersection{N, SEV}) where {N, SEV<:SingleEntryVector{N}} = Union{HPolytope{N, SEV}, HPolytope{N, Vector{N}}} -setrep(::TemplateHullIntersection{N, SP}) where {N, SP<:SparseVector{N}} = Union{HPolytope{N, SP}, HPolytope{N, Vector{N}}} - -# converts the normal vector of a list of half-spaces to be a Vector -const VECH{N, VT} = Vector{HalfSpace{N, VT}} -_to_vec(c::HalfSpace{N, Vector{N}}) where {N} = c -_to_vec(c::HalfSpace{N, VT}) where {N, VT<:AbstractVector{N}} = HalfSpace(Vector(c.a), c.b) -_to_vec(x::VECH{N, Vector{N}}) where {N} = x -_to_vec(x::VECH{N, VT}) where {N, VT<:AbstractVector{N}} = broadcast(_to_vec, x) - -# concatenates lists of half-spaces such that the normal vectors of the final list -# are all Vector -_vcat(args::VECH...) = vcat(map(_to_vec, args)...) +setrep(::HRepIntersection) = HPolytope{Float64, Vector{Float64}} -# TODO use LazySets intersection function _intersection(X::AbstractPolyhedron, Y::AbstractPolyhedron, ::HRepIntersection) clist_X = constraints_list(X) clist_Y = constraints_list(Y) @@ -774,9 +751,59 @@ function _intersection(X::LazySet, Y::AbstractPolyhedron, Z::AbstractPolyhedron, return (success, out) end +# ------------------------ +# BoxIntersection +# ------------------------ + +struct BoxIntersection <: AbstractIntersectionMethod + # +end + +setrep(::BoxIntersection) = Hyperrectangle{Float64, Vector{Float64}, Vector{Float64}} + +# ------------------------ +# TemplateHullIntersection +# ------------------------ + +# evaluate X ∩ Y approximately using support function evaluations +# +# if lazy = false (default) use the property that the support function +# of X ∩ Y along direction d is not greater +# ρ(d, X ∩ Y) <= min(ρ(d, X), ρ(d, Y)) +# by the same token, compute X₁ ∩ ⋯ ∩ Xₖ approximately using the same property +# if the template is provided, we have TN<:AbstractDirections{N, VN} +# otherwise, the constraints of X and Y are used and TN is Missing +# +# if lazy = true, use specialized approximation of lazy intersections +# assuming that Y is polyhedral +struct TemplateHullIntersection{N, VN, TN, L} <: AbstractIntersectionMethod + dirs::TN + lazy::L +end + +# constructor with template directions provided +function TemplateHullIntersection(dirs::TN; lazy=false) where {N, VN, TN<:AbstractDirections{N, VN}} + lazy_val = Val(lazy) + TemplateHullIntersection{N, VN, TN, typeof(lazy_val)}(dirs, lazy_val) +end + +# constructor without template directions => directions are missing until evaluated +function TemplateHullIntersection{N, VN}(; lazy=false) where {N, VN<:AbstractVector{N}} + lazy_val = Val(lazy) + TemplateHullIntersection{N, VN, Missing, typeof(lazy_val)}(missing, lazy_val) +end + +function TemplateHullIntersection(; lazy=false) + TemplateHullIntersection{Float64, Vector{Float64}}(; lazy=lazy) +end + +setrep(::TemplateHullIntersection{N, VN}) where {N, VN} = HPolytope{N, VN} +setrep(::TemplateHullIntersection{N, SEV}) where {N, SEV<:SingleEntryVector{N}} = Union{HPolytope{N, SEV}, HPolytope{N, Vector{N}}} +setrep(::TemplateHullIntersection{N, SP}) where {N, SP<:SparseVector{N}} = Union{HPolytope{N, SP}, HPolytope{N, Vector{N}}} + # if the template directions is missing => use the constraints of both X and Y -# TODO remove redundant constraints? -function _intersection(X::LazySet, Y::LazySet, method::TemplateHullIntersection{N, VN, Missing}) where {N, VN} +# doesn't remove redundant constraints +function _intersection(X::LazySet, Y::LazySet, method::TemplateHullIntersection{N, VN, Missing, Val{false}}) where {N, VN} clist_X = constraints_list(X) clist_Y = constraints_list(Y) @@ -799,7 +826,7 @@ function _intersection(X::LazySet, Y::LazySet, method::TemplateHullIntersection{ end # use ρ(d, X∩Y) ≤ min(ρ(d, X), ρ(d, Y)) for each d in the template -function _intersection(X::LazySet, Y::LazySet, method::TemplateHullIntersection{N, VN, TN}) where {N, VN, TN<:AbstractDirections{N, VN}} +function _intersection(X::LazySet, Y::LazySet, method::TemplateHullIntersection{N, VN, TN, Val{false}}) where {N, VN, TN<:AbstractDirections{N, VN}} dirs = method.dirs out = Vector{HalfSpace{N, VN}}(undef, length(dirs)) @inbounds for (i, d) in enumerate(dirs) @@ -810,6 +837,21 @@ function _intersection(X::LazySet, Y::LazySet, method::TemplateHullIntersection{ return HPolytope(out) end +# compute the min of X ∩ Hi for each Hi in Y (assuming the second set is polyhedral, +# requires the list of constraints of Y) for each template direction d +function _intersection(X::LazySet, Y::LazySet, method::TemplateHullIntersection{N, VN, TN, Val{true}}) where {N, VN, TN<:AbstractDirections{N, VN}} + dirs = method.dirs + out = Vector{HalfSpace{N, VN}}(undef, length(dirs)) + H = constraints_list(Y) + + @inbounds for (i, d) in enumerate(dirs) + d = convert(VN, d) + b = minimum(ρ(d, X ∩ H[j]) for j in 1:length(H)) + out[i] = HalfSpace(d, b) + end + return HPolytope(out) +end + # ===================================== # Methods for checking inclusion # ===================================== diff --git a/src/Hybrid/transitions.jl b/src/Hybrid/transitions.jl index 2e4abea15..09d5066fd 100644 --- a/src/Hybrid/transitions.jl +++ b/src/Hybrid/transitions.jl @@ -392,9 +392,14 @@ function apply(tr::DiscreteTransition{<:IdentityMap, <:ZeroSet, GT, IT⁻, IT⁺ return K end -# ====== Template hulls with set unions ============= -# TODO dispatch on UnionSetArray - +# ====================================== +# Template hulls with set unions +# +# TODO: now X is a vector of reach-sets, +# we may dispatch on UnionSetArray later +# ============================== + +# general case, requires constraints list of each Xi function apply(tr::DiscreteTransition{<:AbstractMatrix, <:LazySet, GT, IT⁻, IT⁺}, X::AbstractVector{RT}, method::TemplateHullIntersection) where {N, @@ -426,6 +431,8 @@ function apply(tr::DiscreteTransition{<:AbstractMatrix, <:LazySet, GT, IT⁻, IT return overapproximate(ConvexHullArray(Y), method.dirs) end +# case with R generic and W = 0 +# requires constraints list of X function apply(tr::DiscreteTransition{<:AbstractMatrix, <:ZeroSet, GT, IT⁻, IT⁺}, X::AbstractVector{RT}, method::TemplateHullIntersection) where {N, @@ -459,7 +466,7 @@ function apply(tr::DiscreteTransition{<:AbstractMatrix, <:ZeroSet, GT, IT⁻, IT return overapproximate(ConvexHullArray(Y), method.dirs) end -# the reset map is the identity, the affine term is zero +# the R = Id and W = 0 function apply(tr::DiscreteTransition{<:IdentityMap, <:ZeroSet, GT, IT⁻, IT⁺}, X::AbstractVector{RT}, method::TemplateHullIntersection) where {N, @@ -469,15 +476,52 @@ function apply(tr::DiscreteTransition{<:IdentityMap, <:ZeroSet, GT, IT⁻, IT⁺ IT⁺<:AbstractPolyhedron{N}} @unpack R, W, G, I⁻, I⁺ = tr + + # precompute L = G ∩ I⁻ ∩ I⁺ + success, L = _intersection(G, I⁻, I⁺, HRepIntersection()) + !success && return EmptySet(dim(X)) + m = length(X) Y = Vector{HPolytope{N, Vector{N}}}(undef, m) + for i in 1:m Xi = set(X[i]) - # concrete Xi ∩ G ∩ I⁻ ∩ I⁺ - success, Qi = _intersection(Xi, G, I⁻, I⁺, HRepIntersection()) + # concrete intersection of Xi ∩ L + success, Qi = _intersection(Xi, L, HRepIntersection()) !success && return EmptySet(dim(Xi)) Y[i] = HPolytope(Qi) end return overapproximate(ConvexHullArray(Y), method.dirs) end + +# the R = Id and W = 0 +# and AbstractZonotope => use template directions for the intersection +function apply(tr::DiscreteTransition{<:IdentityMap, <:ZeroSet, GT, IT⁻, IT⁺}, + X::AbstractVector{RT}, + method::TemplateHullIntersection) where {N, + ZT<:AbstractZonotope{N}, + RT<:ReachSet{N, ZT}, + GT<:AbstractPolyhedron{N}, + IT⁻<:AbstractPolyhedron{N}, + IT⁺<:AbstractPolyhedron{N}} + + @unpack R, W, G, I⁻, I⁺ = tr + + # precompute L = G ∩ I⁻ ∩ I⁺ + success, L = _intersection(G, I⁻, I⁺, HRepIntersection()) + !success && return EmptySet(dim(X)) + L = HPolyhedron(L) + + m = length(X) + Y = Vector{HPolytope{N, Vector{N}}}(undef, m) + + for i in 1:m + Xi = set(X[i]) + + # concrete intersection of Xi ∩ L + Y[i] = _intersection(Xi, L, method) + end + + return overapproximate(ConvexHullArray(Y), method.dirs) +end diff --git a/test/setops.jl b/test/setops.jl index fbf7d27b6..73a83f23a 100644 --- a/test/setops.jl +++ b/test/setops.jl @@ -1,11 +1,13 @@ -@testset "Intersection strategies" begin - # default constructors - @test TemplateHullIntersection() == TemplateHullIntersection{Float64,Array{Float64,1},Missing}(missing) - @test TemplateHullIntersection{Float64, Vector{Float64}}() == TemplateHullIntersection{Float64,Array{Float64,1},Missing}(missing) - @test TemplateHullIntersection{Float64, SVector{2, Float64}}() == TemplateHullIntersection{Float64,SArray{Tuple{2},Float64,1,2},Missing}(missing) +@testset "Intersection methods: TemplateHullIntersection" begin - # constructor passing the template + # default constructor without types + @test TemplateHullIntersection() == TemplateHullIntersection{Float64, Vector{Float64}, Missing, Val{false}}(missing, Val{false}()) + + # default constructor passing normal vector types + @test TemplateHullIntersection{Float64, Vector{Float64}}() == TemplateHullIntersection{Float64, Vector{Float64}, Missing, Val{false}}(missing, Val{false}()) + @test TemplateHullIntersection{Float64, SVector{2, Float64}}() == TemplateHullIntersection{Float64, SArray{Tuple{2}, Float64, 1, 2}, Missing, Val{false}}(missing, Val{false}()) + + # constructor with a given template td = TemplateHullIntersection(BoxDirections(2)) - @test setrep(td) == Union{HPolytope{Float64,LazySets.Arrays.SingleEntryVector{Float64}}, - HPolytope{Float64,Vector{Float64}}} + @test setrep(td) == Union{HPolytope{Float64,LazySets.Arrays.SingleEntryVector{Float64}}, HPolytope{Float64,Vector{Float64}}} end