diff --git a/src/concrete_intersection.jl b/src/concrete_intersection.jl index ad8a1ea3b5..2497d52f82 100644 --- a/src/concrete_intersection.jl +++ b/src/concrete_intersection.jl @@ -127,26 +127,46 @@ one. function intersection(P1::AbstractHPolygon{N}, P2::AbstractHPolygon{N} )::Union{HPolygon{N}, EmptySet{N}} where N<:Real - @inline function choose_first_diff_dir(i, i1, i2, c1, c2) + # all constraints of one polygon are processed; now add the other polygon's + # constraints + @inline function add_remaining_constraints!(c, i, c1, i1, duplicates) + c[i+1:length(c)-duplicates] = c1[i1:length(c1)] + return true + end + + # choose the constraint of c1; the directions are different + @inline function choose_first_diff_dir!(c, i, i1, i2, c1, c2, duplicates) c[i] = c1[i1] if i1 == length(c1) - c[i+1:length(c)] = c2[i2:length(c2)] - return true + return add_remaining_constraints!(c, i, c2, i2, duplicates) end return false end - @inline function choose_first_same_dir(i, i1, i2, c1, c2) + + # choose the constraint of c1; the directions are equivalent (i.e., linearly + # dependent) + @inline function choose_first_same_dir!(c, i, i1, i2, c1, c2, duplicates) c[i] = c1[i1] if i1 == length(c1) if i2 < length(c2) - c[i+1:length(c)] = c2[i2+1:length(c2)] + return add_remaining_constraints!(c, i, c2, i2+1, duplicates) end return true + elseif i2 == length(c2) + return add_remaining_constraints!(c, i, c1, i1+1, duplicates) end return false end - @inline function is_first_constraint_tighter(lc1, lc2) - return lc1.a[1]/lc1.b <= lc2.a[1]/lc2.b + + # check if the first of two constraint with equivalent direction is tighter + @inline function is_first_constraint_tighter(lc1::LinearConstraint{N}, + lc2::LinearConstraint{N} + ) where {N<:Real} + if lc1.a[1] == zero(N) + @assert lc2.a[1] == zero(N) + return lc1.b <= lc1.a[2]/lc2.a[2] * lc2.b + end + return lc1.b <= lc1.a[1]/lc2.a[1] * lc2.b end c1 = constraints_list(P1) @@ -167,12 +187,12 @@ function intersection(P1::AbstractHPolygon{N}, # constraints have the same normal vector: take the tighter one if is_first_constraint_tighter(c1[i1], c2[i2]) # first constraint is tighter - if choose_first_same_dir(i, i1, i2, c1, c2) + if choose_first_same_dir!(c, i, i1, i2, c1, c2, duplicates) break end else # second constraint is tighter - if choose_first_same_dir(i, i2, i1, c2, c1) + if choose_first_same_dir!(c, i, i2, i1, c2, c1, duplicates) break end end @@ -180,14 +200,14 @@ function intersection(P1::AbstractHPolygon{N}, i2 += 1 else # first constraint comes first - if choose_first_diff_dir(i, i1, i2, c1, c2) + if choose_first_diff_dir!(c, i, i1, i2, c1, c2, duplicates) break end i1 += 1 end else # second constraint comes first - if choose_first_diff_dir(i, i2, i1, c2, c1) + if choose_first_diff_dir!(c, i, i2, i1, c2, c1, duplicates) break end i2 += 1 diff --git a/test/unit_Polygon.jl b/test/unit_Polygon.jl index 0642521ca9..d8036f800f 100644 --- a/test/unit_Polygon.jl +++ b/test/unit_Polygon.jl @@ -122,6 +122,28 @@ for N in [Float64, Float32, Rational{Int}] p3 = intersection(p, p2) @test length(constraints_list(p3)) == 4 + # 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)) + h3 = HalfSpace([N(0), N(1)], N(7)) + h4 = HalfSpace([N(0), N(-1)], N(-5)) + h5 = HalfSpace([N(0), N(1)], N(6)) + h6 = HalfSpace([N(0), N(-1)], N(-4)) + p1 = HPolygon([h1, h3, h2, h4]) + p2 = HPolygon([h1, h5, h2, h6]) + c = intersection(p1, p2).constraints + @test c == [h1, h5, h2, h4] + h1 = HalfSpace([N(1), N(1)], N(1)) + h2 = HalfSpace([N(-1), N(1)], N(1)) + h3 = HalfSpace([N(0), N(-1)], N(0)) + p1 = HPolygon([h1, h2, h3]) + h4 = HalfSpace([N(0), N(1)], N(1)) + h5 = HalfSpace([N(-1), N(-1)], N(0)) + h6 = HalfSpace([N(1), N(-1)], N(0)) + p2 = HPolygon([h4, h5, h6]) + c = intersection(p1, p2).constraints + @test c == [h1, h4, h2, h5, h3, h6] + # is intersection empty p3 = convert(HPolygon, Hyperrectangle(low=N[-1, -1], high=N[1, 1])) I1 = Interval(N(0.9), N(1.1))