Merge pull request #379 from JuliaReach/schillic/40

#111 - Binary search for HPolygon functions

docs/src/lib/utils.md

@@ -26,6 +26,7 @@ sign_cadlag
 get_radius!
 an_element_helper
 σ_helper
+binary_search_constraints
 ```
 
 ### Types

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}
 
@@ -180,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.
@@ -195,12 +205,83 @@ 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
+
+"""
+    binary_search_constraints(d::AbstractVector{N},
+                              constraints::Vector{LinearConstraint{N}},
+                              n::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
+- `choose_lower` -- (optional, default: `false`) flag for choosing the lower
+                    index (see the 'Output' section)
+
+### Output
+
+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;
+                                   choose_lower::Bool=false
+                                  )::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 choose_lower
+        return lower
+    else
+        if lower == 1 && !(constraints[1].a <= d)
+            # special case for index 1
+            return 1
+        end
+        return upper
+    end
+end

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

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

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)
@@ -217,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}