From 1cd0fed3ba4772f043493a523a7522a40bf63806 Mon Sep 17 00:00:00 2001 From: Marcelo Forets Date: Tue, 25 Aug 2020 14:57:44 -0300 Subject: [PATCH] =?UTF-8?q?Add=20Sutherland=E2=80=93Hodgman=20VPolygon=20i?= =?UTF-8?q?ntersection=20algorithm=20(#2345)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Add SH vpolygon intersection * Update src/ConcreteOperations/intersection.jl Co-authored-by: Christian Schilling * Update intersection.jl * return empty set if intersection is empty * add area for empty set * add tests * Update src/ConcreteOperations/intersection.jl Co-authored-by: Christian Schilling * test fixes Co-authored-by: Christian Schilling --- docs/src/lib/binary_functions.md | 3 +- docs/src/lib/sets/EmptySet.md | 1 + src/ConcreteOperations/intersection.jl | 85 +++++++++++++++++++++----- src/Sets/EmptySet.jl | 17 ++++++ src/Sets/VPolygon.jl | 50 +++++++++++++++ test/unit_Polygon.jl | 12 ++++ test/unit_Polytope.jl | 21 +++++++ 7 files changed, 174 insertions(+), 15 deletions(-) diff --git a/docs/src/lib/binary_functions.md b/docs/src/lib/binary_functions.md index 3cf5ae15c3..1599944836 100644 --- a/docs/src/lib/binary_functions.md +++ b/docs/src/lib/binary_functions.md @@ -69,7 +69,8 @@ intersection(::Interval{N}, ::Hyperplane{N}) where {N<:Real} intersection(::Interval{N}, ::LazySet{N}) where {N<:Real} intersection(::AbstractHPolygon{N}, ::AbstractHPolygon{N}, ::Bool=true) where {N<:Real} intersection(::AbstractPolyhedron{N}, ::AbstractPolyhedron{N}) where {N<:Real} -intersection(::Union{VPolytope{N}, VPolygon{N}}, ::Union{VPolytope{N}, VPolygon{N}}) where {N<:Real} +intersection(::Union{VPolytope{N}, VPolygon{N}}, ::Union{VPolytope{N}, VPolygon{N}}) where {N} +intersection(::VPolygon{N}, ::VPolygon{N}; ::Bool=true) where {N} intersection(::UnionSet{N}, ::LazySet{N}) where {N<:Real} intersection(::UnionSetArray{N}, ::LazySet{N}) where {N<:Real} intersection(::Universe{N}, ::LazySet{N}) where {N<:Real} diff --git a/docs/src/lib/sets/EmptySet.md b/docs/src/lib/sets/EmptySet.md index b480edaa64..c72b86289c 100644 --- a/docs/src/lib/sets/EmptySet.md +++ b/docs/src/lib/sets/EmptySet.md @@ -25,6 +25,7 @@ linear_map(::AbstractMatrix{N}, ::EmptySet{N}) where {N} translate(::EmptySet{N}, ::AbstractVector{N}) where {N<:Real} plot_recipe(::EmptySet{N}, ::N=zero(N)) where {N<:Real} RecipesBase.apply_recipe(::AbstractDict{Symbol,Any}, ::EmptySet{N}, ::N=zero(N)) where {N<:Real} +area(::EmptySet{N}) where {N} ``` Inherited from [`LazySet`](@ref): * [`norm`](@ref norm(::LazySet, ::Real)) diff --git a/src/ConcreteOperations/intersection.jl b/src/ConcreteOperations/intersection.jl index 46614e5dde..5b3940bb2d 100644 --- a/src/ConcreteOperations/intersection.jl +++ b/src/ConcreteOperations/intersection.jl @@ -658,10 +658,10 @@ function intersection(P::AbstractPolyhedron{N}, X::Interval{N} end """ - intersection(P1::Union{VPolytope{N}, VPolygon{N}}, - P2::Union{VPolytope{N}, VPolygon{N}}; + intersection(P1::VPolytope{N}, + P2::VPolytope{N}; [backend]=default_polyhedra_backend(P1, N), - [prunefunc]=removevredundancy!) where {N<:Real} + [prunefunc]=removevredundancy!) where {N} Compute the intersection of two polytopes in vertex representation. @@ -676,21 +676,78 @@ Compute the intersection of two polytopes in vertex representation. ### Output -A `VPolygon` if both arguments are `VPolygon`s, and a `VPolytope` otherwise. +A `VPolytope`. """ -function intersection(P1::Union{VPolytope{N}, VPolygon{N}}, - P2::Union{VPolytope{N}, VPolygon{N}}; - backend=default_polyhedra_backend(P1, N), - prunefunc=removevredundancy!) where {N<:Real} - Q1 = polyhedron(convert(VPolytope, P1); backend=backend) - Q2 = polyhedron(convert(VPolytope, P2); backend=backend) +function intersection(P1::Union{VPolygon{N}, VPolytope{N}}, + P2::Union{VPolygon{N}, VPolytope{N}}; + backend=nothing, + prunefunc=nothing) where {N} + n = dim(P1) + @assert n == dim(P2) "expected polytopes with equal dimensions but they " * + "are $(dim(P1)) and $(dim(P2)) respectively" + + # fast path for one and two-dimensional sets + if n == 1 + Q1 = overapproximate(P1, Interval) + Q2 = overapproximate(P2, Interval) + Pint = intersection(Q1, Q2) + return convert(VPolytope, Pint) + elseif n == 2 + Q1 = convert(VPolygon, P1) + Q2 = convert(VPolygon, P2) + Pint = intersection(Q1, Q2) + return convert(VPolytope, Pint) + end + + if isnothing(backend) + backend = default_polyhedra_backend(P1, N) + end + if isnothing(prunefunc) + prunefunc = removevredundancy! + end + + # general case: convert to half-space representation + Q1 = polyhedron(P1; backend=backend) + Q2 = polyhedron(P2; backend=backend) Pint = Polyhedra.intersect(Q1, Q2) prunefunc(Pint) - res = VPolytope(Pint) - if P1 isa VPolygon && P2 isa VPolygon - return convert(VPolygon, res) + return VPolytope(Pint) +end + +""" + intersection(P1::VPolygon{N}, P2::VPolygon{N}; + apply_convex_hull::Bool=true) where {N} + +Compute the intersection of two polygons in vertex representation. + +### Input + +- `P1` -- polygon in vertex representation +- `P2` -- polygon in vertex representation +- `apply_convex_hull` -- (default, optional: `true`) use the flag to skip the + computation of the convex hull in the resulting `VPolygon` + +### Output + +A `VPolygon` or an `EmptySet` if the intersection is empty. + +### Algorithm + +This function applies the [Sutherland–Hodgman polygon +clipping algorithm](https://en.wikipedia.org/wiki/Sutherland%E2%80%93Hodgman_algorithm). +The implementation is based on the one found in +[rosetta code](http://www.rosettacode.org/wiki/Sutherland-Hodgman_polygon_clipping#Julia). +""" +function intersection(P1::VPolygon{N}, P2::VPolygon{N}; + apply_convex_hull::Bool=true) where {N} + v1 = vertices_list(P1) + v2 = vertices_list(P2) + v12 = _intersection_vrep(v1, v2) + if isempty(v12) + return EmptySet{N}(2) + else + return VPolygon(v12, apply_convex_hull=apply_convex_hull) end - return res end """ diff --git a/src/Sets/EmptySet.jl b/src/Sets/EmptySet.jl index c2a4903e2d..efbbfd63d5 100644 --- a/src/Sets/EmptySet.jl +++ b/src/Sets/EmptySet.jl @@ -359,3 +359,20 @@ In the special case of an empty set, we define the sequence as `nothing`. function plot_recipe(∅::EmptySet{N}, ε::N=zero(N)) where {N<:Real} return [] end + +""" + area(∅::EmptySet{N}) where {N} + +Return the area of an empty set. + +### Input + +- `∅` -- empty set + +### Output + +The zero element of type `N`. +""" +function area(∅::EmptySet{N}) where {N} + return zero(N) +end diff --git a/src/Sets/VPolygon.jl b/src/Sets/VPolygon.jl index 98335ce319..25461ebe36 100644 --- a/src/Sets/VPolygon.jl +++ b/src/Sets/VPolygon.jl @@ -699,3 +699,53 @@ function minkowski_sum(P::VPolygon{N}, Q::VPolygon{N}) where {N<:Real} end return VPolygon(R) end + +# ======================= +# Concrete intersection +# ======================= + +function _isinside(p, a, b) + @inbounds begin + α = (b[1] - a[1]) * (p[2] - a[2]) + β = (b[2] - a[2]) * (p[1] - a[1]) + end + return !_leq(α, β) +end + +function _intersection_line_segments(a, b, s, f) + @inbounds begin + dc = [a[1] - b[1], a[2] - b[2]] + dp = [s[1] - f[1], s[2] - f[2]] + n1 = a[1] * b[2] - a[2] * b[1] + n2 = s[1] * f[2] - s[2] * f[1] + n3 = 1.0 / (dc[1] * dp[2] - dc[2] * dp[1]) + + α = (n1 * dp[1] - n2 * dc[1]) * n3 + β = (n1 * dp[2] - n2 * dc[2]) * n3 + end + return [α, β] +end + +function _intersection_vrep(spoly::Vector{VT}, cpoly::Vector{VT}) where {N, VT<:AbstractVector{N}} + outarr = spoly + q = cpoly[end] + for p in cpoly + inarr = outarr + outarr = Vector{VT}() + isempty(inarr) && break + s = inarr[end] + for vtx in inarr + if _isinside(vtx, q, p) + if !_isinside(s, q, p) + push!(outarr, _intersection_line_segments(q, p, s, vtx)) + end + push!(outarr, vtx) + elseif _isinside(s, q, p) + push!(outarr, _intersection_line_segments(q, p, s, vtx)) + end + s = vtx + end + q = p + end + return outarr +end diff --git a/test/unit_Polygon.jl b/test/unit_Polygon.jl index 8321936e07..b0f3ff3c5d 100644 --- a/test/unit_Polygon.jl +++ b/test/unit_Polygon.jl @@ -199,6 +199,10 @@ for N in [Float64, Float32, Rational{Int}] N[9,12], N[7, 12], N[4, 10]]) end + # =================================== + # Concrete intersection + # =================================== + # concrete intersection of H-rep p2 = HPolygon{N}() c1 = LinearConstraint(N[2, 2], N(14)) @@ -212,6 +216,14 @@ for N in [Float64, Float32, Rational{Int}] p3 = intersection(p, p2) @test length(constraints_list(p3)) == 4 + # concrete intersection of V-rep + paux = VPolygon([N[0, 0], N[1, 0], N[0, 1], N[1, 1]]) + qaux = VPolygon([N[1, -1/2], N[-1/2, 1], N[-1/2, -1/2]]) + xaux = intersection(paux, qaux) + oaux = VPolygon([N[0, 0], N[1/2, 0], N[0, 1/2]]) + @test xaux ⊆ oaux && oaux ⊆ xaux # TODO use isequivalent + @test LazySets._intersection_vrep(paux.vertices, qaux.vertices) == xaux.vertices + # check that tighter constraints are used in intersection (#883) h1 = HalfSpace([N(1), N(0)], N(3)) h2 = HalfSpace([N(-1), N(0)], N(-3)) diff --git a/test/unit_Polytope.jl b/test/unit_Polytope.jl index 7cb9318199..8efcb8897e 100644 --- a/test/unit_Polytope.jl +++ b/test/unit_Polytope.jl @@ -398,6 +398,27 @@ for N in [Float64] cap = intersection(p1, p3) @test ispermutation(vertices_list(cap), vlist) + # 2D intersection + paux = VPolytope([N[0, 0], N[1, 0], N[0, 1], N[1, 1]]) + qaux = VPolytope([N[1, -1/2], N[-1/2, 1], N[-1/2, -1/2]]) + xaux = intersection(paux, qaux) + oaux = VPolytope([N[0, 0], N[1/2, 0], N[0, 1/2]]) + @test xaux ⊆ oaux && oaux ⊆ xaux # TODO use isequivalent + + # mixed types + paux = VPolygon([N[0, 0], N[1, 0], N[0, 1], N[1, 1]]) + qaux = VPolytope([N[1, -1/2], N[-1/2, 1], N[-1/2, -1/2]]) + xaux = intersection(paux, qaux) + oaux = VPolytope([N[0, 0], N[1/2, 0], N[0, 1/2]]) + @test xaux ⊆ oaux && oaux ⊆ xaux # TODO use isequivalent + + # 1D set + paux = VPolytope([N[0], N[1]]) + qaux = VPolytope([N[-1/2], N[1/2]]) + xaux = intersection(paux, qaux) + oaux = VPolytope([N[0], N[1/2]]) + @test xaux ⊆ oaux && oaux ⊆ xaux # TODO use isequivalent + # isuniversal answer, w = isuniversal(p1, true) @test !isuniversal(p1) && !answer && w ∉ p1