add binary search routine to H-polygons

JuliaReach · Jul 11, 2018 · b493e7f · b493e7f
1 parent 43c808d
commit b493e7f
Show file tree

Hide file tree

Showing 3 changed files with 117 additions and 33 deletions.
diff --git 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
@@ -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
@@ -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