diff --git a/src/AbstractHPolygon.jl b/src/AbstractHPolygon.jl index 32af187489..a134e800d9 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} @@ -206,3 +213,41 @@ function addconstraint!(P::AbstractHPolygon{N}, insert!(P.constraints, i+1, constraint) return nothing end + +""" + binary_search_constraints(d::AbstractVector{<:Real}, + 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{<:Real}, + 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 + return upper +end diff --git a/src/HPolygon.jl b/src/HPolygon.jl index e4d694746e..c751e6bd65 100644 --- a/src/HPolygon.jl +++ b/src/HPolygon.jl @@ -47,8 +47,10 @@ 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,17 +62,31 @@ 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{<:Real}, P::HPolygon{N})::Vector{N} where {N<:Real} +function σ(d::AbstractVector{<:Real}, P::HPolygon{N}; + linear_search=(length(P.constraints) < BINARY_SEARCH_THRESHOLD)::Bool + )::Vector{N} where {N<:Real} n = length(P.constraints) if n == 0 error("this polygon is empty") end - 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 intersection(Line(P.constraints[1]), Line(P.constraints[n])) else diff --git a/src/HPolygonOpt.jl b/src/HPolygonOpt.jl index 205e52b232..20ded8f68e 100644 --- a/src/HPolygonOpt.jl +++ b/src/HPolygonOpt.jl @@ -65,8 +65,10 @@ 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,40 +80,61 @@ 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{<:Real}, - P::HPolygonOpt{N})::Vector{N} where {N<:Real} +function σ(d::AbstractVector{<:Real}, P::HPolygonOpt{N}; + linear_search=(length(P.constraints) < BINARY_SEARCH_THRESHOLD)::Bool + )::Vector{N} where {N<:Real} n = length(P.constraints) if n == 0 error("this polygon is empty") end - 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 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 intersection(Line(P.constraints[n]), + Line(P.constraints[1])) + else + P.ind = k + end else - P.ind = k - return 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 intersection(Line(P.constraints[n]), + Line(P.constraints[1])) + else + P.ind = k-1 + end end + return 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 - return intersection(Line(P.constraints[n]), - Line(P.constraints[1])) + # 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 intersection(Line(P.constraints[1]), + Line(P.constraints[n])) else - P.ind = k-1 - return intersection(Line(P.constraints[k-1]), - Line(P.constraints[k])) + return intersection(Line(P.constraints[k]), + Line(P.constraints[k-1])) end end end