From 8a9d86b0ecef295108f7da0f338c55d4f4a5b475 Mon Sep 17 00:00:00 2001 From: schillic Date: Wed, 11 Jul 2018 18:11:13 +0200 Subject: [PATCH 1/5] add binary search routine to H-polygons --- src/AbstractHPolygon.jl | 49 ++++++++++++++++++++++++++++ src/HPolygon.jl | 32 ++++++++++++++---- src/HPolygonOpt.jl | 72 ++++++++++++++++++++++++++++------------- 3 files changed, 123 insertions(+), 30 deletions(-) diff --git a/src/AbstractHPolygon.jl b/src/AbstractHPolygon.jl index 2611eaf40e..e143837581 100644 --- a/src/AbstractHPolygon.jl +++ b/src/AbstractHPolygon.jl @@ -6,6 +6,13 @@ export AbstractHPolygon, vertices_list, constraints_list +# This constant marks the threshold for the number of constraints of a polygon +# above which we use a binary search to find the relevant constraint in a +# support vector query. +# +# NOTE: The value must be strictly greater than 2. +const BINARY_SEARCH_THRESHOLD = 10 + """ AbstractHPolygon{N<:Real} <: AbstractPolygon{N} @@ -204,3 +211,45 @@ function addconstraint!(P::AbstractHPolygon{N}, insert!(P.constraints, i+1, constraint) return nothing end + +""" + binary_search_constraints(d::AbstractVector{N}, + constraints::Vector{LinearConstraint{N}}, + n::Int, + k::Int + )::Int where {N<:Real} + +Performs a binary search in the constraints. + +### Input + +- `d` -- direction +- `constraints` -- constraints +- `n` -- number of constraints +- `k` -- start index + +### Output + +The largest index `k` such that `constraints[k] <= d`. +""" +function binary_search_constraints(d::AbstractVector{N}, + constraints::Vector{LinearConstraint{N}}, + n::Int, + k::Int + )::Int where {N} + lower = 1 + upper = n+1 + while lower + 1 < upper + if constraints[k].a <= d + lower = k + else + upper = k + end + k = lower + div(upper - lower, 2) + end + if lower == 1 && !(constraints[1].a <= d) + # special case for index 1 + return 1 + end + return upper +end diff --git a/src/HPolygon.jl b/src/HPolygon.jl index 7f72e09aad..89a7b50153 100644 --- a/src/HPolygon.jl +++ b/src/HPolygon.jl @@ -41,14 +41,18 @@ HPolygon(S::LazySet) = convert(HPolygon, S) """ - σ(d::AbstractVector{N}, P::HPolygon{N}) where {N<:Real} + σ(d::AbstractVector{N}, P::HPolygon{N}; + [linear_search]::Bool=(length(P.constraints) < BINARY_SEARCH_THRESHOLD) + ) where {N<:Real} Return the support vector of a polygon in a given direction. ### Input -- `d` -- direction -- `P` -- polygon in constraint representation +- `d` -- direction +- `P` -- polygon in constraint representation +- `linear_search` -- (optional, default: see below) flag for controlling whether + to perform a linear search or a binary search ### Output @@ -60,15 +64,29 @@ norm zero, any vertex is returned. Comparison of directions is performed using polar angles; see the overload of `<=` for two-dimensional vectors. + +For polygons with `BINARY_SEARCH_THRESHOLD = 10` or more constraints we use a +binary search by default. """ -function σ(d::AbstractVector{N}, P::HPolygon{N}) where {N<:Real} +function σ(d::AbstractVector{N}, P::HPolygon{N}; + linear_search::Bool=(length(P.constraints) < BINARY_SEARCH_THRESHOLD) + ) where {N<:Real} n = length(P.constraints) @assert n > 0 "the polygon has no constraints" - k = 1 - while k <= n && P.constraints[k].a <= d - k += 1 + + if linear_search + # linear search + k = 1 + while k <= n && P.constraints[k].a <= d + k += 1 + end + else + # binary search + k = binary_search_constraints(d, P.constraints, n, 1 + div(n, 2)) end + if k == 1 || k == n+1 + # corner cases: wrap-around in constraints list return element(intersection(Line(P.constraints[1]), Line(P.constraints[n]))) else diff --git a/src/HPolygonOpt.jl b/src/HPolygonOpt.jl index c2a8e11478..c4f0a25d15 100644 --- a/src/HPolygonOpt.jl +++ b/src/HPolygonOpt.jl @@ -59,14 +59,18 @@ HPolygonOpt(S::LazySet) = convert(HPolygonOpt, S) """ - σ(d::AbstractVector{N}, P::HPolygonOpt{N}) where {N<:Real} + σ(d::AbstractVector{N}, P::HPolygonOpt{N}; + [linear_search]::Bool=(length(P.constraints) < BINARY_SEARCH_THRESHOLD) + ) where {N<:Real} Return the support vector of an optimized polygon in a given direction. ### Input -- `d` -- direction -- `P` -- optimized polygon in constraint representation +- `d` -- direction +- `P` -- optimized polygon in constraint representation +- `linear_search` -- (optional, default: see below) flag for controlling whether + to perform a linear search or a binary search ### Output @@ -78,35 +82,57 @@ norm zero, any vertex is returned. Comparison of directions is performed using polar angles; see the overload of `<=` for two-dimensional vectors. + +For polygons with `BINARY_SEARCH_THRESHOLD = 10` or more constraints we use a +binary search by default. """ -function σ(d::AbstractVector{N}, P::HPolygonOpt{N}) where {N<:Real} +function σ(d::AbstractVector{N}, P::HPolygonOpt{N}; + linear_search::Bool=(length(P.constraints) < BINARY_SEARCH_THRESHOLD) + ) where {N<:Real} n = length(P.constraints) @assert n > 0 "the polygon has no constraints" - if (d <= P.constraints[P.ind].a) - k = P.ind-1 - while (k >= 1 && d <= P.constraints[k].a) - k -= 1 - end - if (k == 0) - P.ind = n - return element(intersection(Line(P.constraints[n]), - Line(P.constraints[1]))) + if linear_search + # linear search + if (d <= P.constraints[P.ind].a) + # search backward + k = P.ind-1 + while (k >= 1 && d <= P.constraints[k].a) + k -= 1 + end + if (k == 0) + P.ind = n + # corner case: wrap-around in constraints list + return element(intersection(Line(P.constraints[n]), + Line(P.constraints[1]))) + else + P.ind = k + end else - P.ind = k - return element(intersection(Line(P.constraints[k]), - Line(P.constraints[k+1]))) + # search forward + k = P.ind+1 + while (k <= n && P.constraints[k].a <= d) + k += 1 + end + if (k == n+1) + P.ind = n + # corner case: wrap-around in constraints list + return element(intersection(Line(P.constraints[n]), + Line(P.constraints[1]))) + else + P.ind = k-1 + end end + return element(intersection(Line(P.constraints[P.ind]), + Line(P.constraints[P.ind + 1]))) else - k = P.ind+1 - while (k <= n && P.constraints[k].a <= d) - k += 1 - end - if (k == n+1) - P.ind = n + # binary search + k = binary_search_constraints(d, P.constraints, n, P.ind) + P.ind = k + if k == 1 || k == n+1 + # corner cases: wrap-around in constraints list return element(intersection(Line(P.constraints[n]), Line(P.constraints[1]))) else - P.ind = k-1 return element(intersection(Line(P.constraints[k-1]), Line(P.constraints[k]))) end From 8b373d5058c99156def1ef6af48dd059be5bfb34 Mon Sep 17 00:00:00 2001 From: schillic Date: Wed, 11 Jul 2018 18:15:35 +0200 Subject: [PATCH 2/5] add unit test --- test/unit_Polygon.jl | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/test/unit_Polygon.jl b/test/unit_Polygon.jl index 54eaf64743..69b0ccf90c 100644 --- a/test/unit_Polygon.jl +++ b/test/unit_Polygon.jl @@ -57,17 +57,22 @@ for N in [Float64, Float32, Rational{Int}] # Test Dimension @test dim(p) == 2 - # Test Support Vector + # test support vector, with linear and binary search d = N[1., 0.] @test σ(d, p) == N[4., 2.] + @test σ(d, p, linear_search=true) == σ(d, p, linear_search=false) d = N[0., 1.] @test σ(d, p) == N[2., 4.] + @test σ(d, p, linear_search=true) == σ(d, p, linear_search=false) d = N[-1., 0.] @test σ(d, p) == N[-1., 1.] + @test σ(d, p, linear_search=true) == σ(d, p, linear_search=false) d = N[0., -1.] @test σ(d, p) == N[0., 0.] + @test σ(d, p, linear_search=true) == σ(d, p, linear_search=false) d = N[1., -1.] @test σ(d, p) == N[4., 2.] + @test σ(d, p, linear_search=true) == σ(d, p, linear_search=false) # membership @test ∈(N[0., 0.], p) From 6ab899eb39cf36b778ef2700b42aa23fee2cf16d Mon Sep 17 00:00:00 2001 From: schillic Date: Thu, 12 Jul 2018 19:02:18 +0200 Subject: [PATCH 3/5] add binary search to H-polygons' addconstraint! method --- src/AbstractHPolygon.jl | 68 ++++++++++++++++++++++++++++++----------- 1 file changed, 50 insertions(+), 18 deletions(-) diff --git a/src/AbstractHPolygon.jl b/src/AbstractHPolygon.jl index e143837581..7436d399ad 100644 --- a/src/AbstractHPolygon.jl +++ b/src/AbstractHPolygon.jl @@ -187,7 +187,10 @@ end """ addconstraint!(P::AbstractHPolygon{N}, - constraint::LinearConstraint{N})::Void where {N<:Real} + constraint::LinearConstraint{N}; + [linear_search]::Bool=( + length(P.constraints) < BINARY_SEARCH_THRESHOLD) + )::Void where {N<:Real} Add a linear constraint to a polygon in constraint representation, keeping the constraints sorted by their normal directions. @@ -202,13 +205,30 @@ constraints sorted by their normal directions. Nothing. """ function addconstraint!(P::AbstractHPolygon{N}, - constraint::LinearConstraint{N})::Void where {N<:Real} - i = length(P.constraints) - while i > 0 && constraint.a <= P.constraints[i].a - i -= 1 + constraint::LinearConstraint{N}; + linear_search::Bool=( + length(P.constraints) < BINARY_SEARCH_THRESHOLD) + )::Void where {N<:Real} + k = length(P.constraints) + if k > 0 + d = constraint.a + if d <= P.constraints[1].a + k = 0 + else + if linear_search + # linear search + while d <= P.constraints[k].a + k -= 1 + end + else + # binary search + k = binary_search_constraints( + d, P.constraints, k, 1 + div(k, 2), choose_lower=true) + end + end end - # here P.constraints[i] < constraint - insert!(P.constraints, i+1, constraint) + # here P.constraints[k] < constraint + insert!(P.constraints, k+1, constraint) return nothing end @@ -216,26 +236,34 @@ end binary_search_constraints(d::AbstractVector{N}, constraints::Vector{LinearConstraint{N}}, n::Int, - k::Int + k::Int; + [choose_lower]::Bool=false )::Int where {N<:Real} Performs a binary search in the constraints. ### Input -- `d` -- direction -- `constraints` -- constraints -- `n` -- number of constraints -- `k` -- start index +- `d` -- direction +- `constraints` -- constraints +- `n` -- number of constraints +- `k` -- start index +- `choose_lower` -- (optional, default: `false`) flag for choosing the lower + index (see the 'Output' section) ### Output -The largest index `k` such that `constraints[k] <= d`. +In the default setting, the result is the smallest index `k` such that +`d <= constraints[k]`, or `n+1` if no such `k` exists. +If the `choose_lower` flag is set, the result is the largest index `k` such +that `constraints[k] < d`, which is equivalent to being `k-1` in the normal +setting. """ function binary_search_constraints(d::AbstractVector{N}, constraints::Vector{LinearConstraint{N}}, n::Int, - k::Int + k::Int; + choose_lower::Bool=false )::Int where {N} lower = 1 upper = n+1 @@ -247,9 +275,13 @@ function binary_search_constraints(d::AbstractVector{N}, end k = lower + div(upper - lower, 2) end - if lower == 1 && !(constraints[1].a <= d) - # special case for index 1 - return 1 + if choose_lower + return lower + else + if lower == 1 && !(constraints[1].a <= d) + # special case for index 1 + return 1 + end + return upper end - return upper end From 605c2027d97e2f41ae6ffc05f46b106ff21bd397 Mon Sep 17 00:00:00 2001 From: schillic Date: Thu, 12 Jul 2018 19:02:40 +0200 Subject: [PATCH 4/5] unit tests --- test/unit_Polygon.jl | 31 +++++++++++++++++++++++++++++++ 1 file changed, 31 insertions(+) diff --git a/test/unit_Polygon.jl b/test/unit_Polygon.jl index 69b0ccf90c..8f178793ce 100644 --- a/test/unit_Polygon.jl +++ b/test/unit_Polygon.jl @@ -222,6 +222,37 @@ for N in [Float64, Float32, Rational{Int}] end end +function same_constraints(v::Vector{LinearConstraint{N}})::Bool where N<:Real + c1 = v[1] + for k = 2:length(v) + c2 = v[2] + if c1.a != c2.a || c1.b != c2.b + return false + end + end + return true +end + +for N in [Float64, Float32] + # test adding constraints, with linear and binary search + p1 = HPolygon{N}() + p2 = HPolygon{N}() + po1 = HPolygonOpt{N}() + po2 = HPolygonOpt{N}() + n = 10 + for i in 1:n + constraint = LinearConstraint(rand(N, 2), rand(N)) + addconstraint!(p1, constraint, linear_search=true) + addconstraint!(p2, constraint, linear_search=i<=2) + addconstraint!(po1, constraint, linear_search=true) + addconstraint!(po2, constraint, linear_search=i<=2) + end + for i in 1:n + @test same_constraints([p1.constraints[i], p2.constraints[i], + po1.constraints[i], po2.constraints[i]]) + end +end + # default Float64 constructors @test HPolygon() isa LazySets.HPolygon{Float64} @test HPolygonOpt() isa LazySets.HPolygonOpt{Float64} From f10d4725df9025c0841b905e36fa72ef4c5ef4f5 Mon Sep 17 00:00:00 2001 From: schillic Date: Mon, 6 Aug 2018 14:49:57 +0200 Subject: [PATCH 5/5] add docs --- docs/src/lib/utils.md | 1 + 1 file changed, 1 insertion(+) diff --git a/docs/src/lib/utils.md b/docs/src/lib/utils.md index ce4a212efc..e01accd572 100644 --- a/docs/src/lib/utils.md +++ b/docs/src/lib/utils.md @@ -26,6 +26,7 @@ sign_cadlag get_radius! an_element_helper σ_helper +binary_search_constraints ``` ### Types