#40 - Implement binary search for VPolygon support vector (#1475)

* add binary search for VPolygon support vector * wrap binary and brute force code in new functions * add N<:Real * move inlined functions * Update src/VPolygon.jl Co-Authored-By: Christian Schilling <schillic@informatik.uni-freiburg.de> * Update src/VPolygon.jl Co-Authored-By: Christian Schilling <schillic@informatik.uni-freiburg.de> * Update src/VPolygon.jl Co-Authored-By: Christian Schilling <schillic@informatik.uni-freiburg.de> * Update binary search * fix inline functions * update minkowski sum * solve merge error * Update src/Arrays/vector_operations.jl Co-Authored-By: Christian Schilling <schillic@informatik.uni-freiburg.de> * Update src/Arrays/vector_operations.jl Co-Authored-By: Christian Schilling <schillic@informatik.uni-freiburg.de> * Update src/VPolygon.jl Co-Authored-By: Christian Schilling <schillic@informatik.uni-freiburg.de> * Update src/VPolygon.jl Co-Authored-By: Christian Schilling <schillic@informatik.uni-freiburg.de> * solve problem in minkowski sum * Update src/VPolygon.jl Co-Authored-By: Christian Schilling <schillic@informatik.uni-freiburg.de> * rotate hexadecagon * rename variables in minkowski sum function * export inline functions * fix problem in unit test * solve critical error in binary search and tests * fix unit test * add docs for inline functions * fix comments and minkowski sum * Update src/Arrays/vector_operations.jl Co-Authored-By: Christian Schilling <schillic@informatik.uni-freiburg.de> * Update src/Arrays/vector_operations.jl Co-Authored-By: Christian Schilling <schillic@informatik.uni-freiburg.de> * Update src/Arrays/vector_operations.jl Co-Authored-By: Christian Schilling <schillic@informatik.uni-freiburg.de> * Update src/Arrays/vector_operations.jl Co-Authored-By: Christian Schilling <schillic@informatik.uni-freiburg.de> * Update src/Arrays/vector_operations.jl Co-Authored-By: Christian Schilling <schillic@informatik.uni-freiburg.de> * Update src/Arrays/vector_operations.jl Co-Authored-By: Christian Schilling <schillic@informatik.uni-freiburg.de> * Update src/VPolygon.jl Co-Authored-By: Christian Schilling <schillic@informatik.uni-freiburg.de> * Update src/VPolygon.jl Co-Authored-By: Christian Schilling <schillic@informatik.uni-freiburg.de>
JuliaReach · Jul 20, 2019 · 57c0ad7 · 57c0ad7
1 parent 03f0448
commit 57c0ad7
Show file tree

Hide file tree

Showing 4 changed files with 132 additions and 17 deletions.
diff --git a/docs/src/lib/utils.md b/docs/src/lib/utils.md
@@ -24,6 +24,9 @@ remove_duplicates_sorted!
 samedir
 SingleEntryVector
 to_negative_vector
+_up
+_dr
+_above
 ```
 
 ## Functions and Macros

diff --git a/src/Arrays/vector_operations.jl b/src/Arrays/vector_operations.jl
@@ -5,7 +5,10 @@ export dot_zero,
        nonzero_indices,
        rectify,
        is_cyclic_permutation,
-       to_negative_vector
+       to_negative_vector,
+       _above,
+       _dr,
+       _up
 
 """
     dot_zero(x::AbstractVector{N}, y::AbstractVector{N}) where{N<:Real}
@@ -190,6 +193,64 @@ function is_cyclic_permutation(candidate::AbstractVector,
     return any(candidate == circshift(paragon, i) for i in 0:m-1)
 end
 
+"""
+    _up(u::AbstractVector, v::AbstractVector)
+
+Checks if the given vector is pointing towards the given direction.
+
+### Input
+
+- `u` -- direction
+- `v` -- vector
+
+### Output
+
+A boolean indicating if the vector is pointing towards the direction.
+"""
+@inline function _up(u::AbstractVector, v::AbstractVector)
+    dot(u, v) > 0
+end
+
+"""
+    _dr(u::AbstractVector, Vi::AbstractVector, Vj::AbstractVector)
+
+Returns the direction of the difference of the given vectors.
+
+### Input
+
+- `u` -- direction
+- `Vi` -- first vector
+- `Vj` -- second vector
+
+### Output
+
+A number indicating the direction of the difference of the given vectors.
+"""
+@inline function _dr(u::AbstractVector, Vi::AbstractVector, Vj::AbstractVector)
+    dot(u, (Vi) - (Vj))
+end
+
+"""
+    _above(u::AbstractVector, Vi::AbstractVector, Vj::AbstractVector)
+
+Checks if the difference of the given vectors is pointing towards the given
+direction.
+
+### Input
+
+- `u` -- direction
+- `Vi` -- first vector
+- `Vj` -- second vector
+
+### Output
+
+A boolean indicating if the difference of the given vectors is pointing
+towards the given direction.
+"""
+@inline function _above(u::AbstractVector, Vi::AbstractVector, Vj::AbstractVector)
+    _dr(u, Vi, Vj) > 0
+end
+
 """
     to_negative_vector(v::AbstractVector{N}) where {N}
 

diff --git a/src/VPolygon.jl b/src/VPolygon.jl
@@ -241,27 +241,72 @@ If the direction has norm zero, the first vertex is returned.
 
 ### Algorithm
 
-This implementation performs a brute-force search, comparing the projection of
-each vector along the given direction.
-It runs in ``O(n)`` where ``n`` is the number of vertices.
-
-### Notes
-
-For arbitrary points without structure this is the best one can do.
-However, a more efficient approach can be used if the vertices of the polygon
-have been sorted in counter-clockwise fashion.
-In that case a binary search algorithm can be used that runs in ``O(\\log n)``.
-See issue [#40](https://github.com/JuliaReach/LazySets.jl/issues/40).
+This implementation uses a binary search algorithm when the polygon has more
+than ten vertices and a brute-force search when it has ten or less.
+For the brute-force search, it compares the projection of
+each vector along the given direction and runs in ``O(n)`` where 
+``n`` is the number of vertices.
+For the binary search the algorithm runs in ``O(log n)``.
+We follow [this implementation](http://geomalgorithms.com/a14-_extreme_pts.html#polyMax_2D())
+based on an algorithm described in [1].
+
+[1] Joseph O'Rourke, Computational Geometry in C (2nd Edition)
 """
 function σ(d::AbstractVector{N}, P::VPolygon{N}) where {N<:Real}
     @assert !isempty(P.vertices) "the polygon has no vertices"
+    binary_or_brute_force = 10;
+    if length(P.vertices) > binary_or_brute_force
+        P.vertices[_binary_support_vector(d, P)]
+    else
+        P.vertices[_brute_force_support_vector(d, P)]
+    end
+end
+
+function _brute_force_support_vector(d::AbstractVector{N}, P::VPolygon{N}) where {N <: Real}
     i_max = 1
     @inbounds for i in 2:length(P.vertices)
         if dot(d, P.vertices[i] - P.vertices[i_max]) > zero(N)
             i_max = i
         end
     end
-    return P.vertices[i_max]
+    return i_max
+end
+
+function _binary_support_vector(d::AbstractVector{N}, P::VPolygon{N}) where {N <: Real}
+    n = length(P.vertices)
+    @assert n > 2
+    push!(P.vertices, P.vertices[1]) # add extra vertice on the end equal to the first
+    a = 1; b = n + 1 # start chain = [1,n+1] with P.vertices[n+1]=P.vertices[1]
+    A = P.vertices[2] - P.vertices[1]
+    upA = _up(d, A)
+    # test if P.vertices[0] is a local maximum
+    if (!upA && !_above(d, P.vertices[n], P.vertices[1])) # P.vertices[1] is the maximum
+        pop!(P.vertices) # remove the extra point added
+        return 1
+    end
+    while true
+        c = round(Int, (a + b) / 2) # midpoint of [a,b], and 1<c<n+1
+        C = P.vertices[c + 1] - P.vertices[c]
+        upC = _up(d, C)
+        if (!upC && !_above(d, P.vertices[c - 1], P.vertices[c])) # P.vertices[c] is a local maximum
+            pop!(P.vertices) # remove the extra point added
+            return c # thus it is the maximum
+        end
+        # no max yet, so continue with the binary search
+        # pick one of the two subchains [a,c] or [c,b]
+        if (upA && upC && !_above(d, P.vertices[a], P.vertices[c])) ||
+        (!upA && (upC || (!upC && _above(d, P.vertices[a], P.vertices[c]))))
+            a = c
+            A = C
+            upA = upC
+        else
+            b = c
+        end
+        if (b <= a + 1) # the chain is impossibly small
+            pop!(P.vertices) # remove the extra point added
+            throw(ErrorException("something went wrong")) # return an error
+        end
+    end
 end
 
 """
@@ -585,10 +630,8 @@ function minkowski_sum(P::VPolygon{N}, Q::VPolygon{N}) where {N<:Real}
     mP = length(vlistP)
     mQ = length(vlistQ)
     i = 1
-    σP = σ(N[1, 0], P)
-    σQ = σ(N[1, 0], Q)
-    k = findfirst(==(σP), vlistP)
-    j = findfirst(==(σQ), vlistQ)
+    k = _binary_support_vector(N[1, 0], P)
+    j = _binary_support_vector(N[1, 0], Q)
     R = Vector{Vector{N}}(undef, mP+mQ)
     fill!(R, N[0, 0])
     while i <= size(R, 1)

diff --git a/test/unit_Polygon.jl b/test/unit_Polygon.jl
@@ -280,6 +280,14 @@ for N in [Float64, Float32, Rational{Int}]
     @test σ(d, vp) == points[1]
     d = N[0, -1]
     @test σ(d, vp) == points[2]
+    dirs = [[1, 0], [1, 0.5], [0.5, 0.5], [0.5, 1], [0, 1], [-0.5, 1],
+    [-0.5, 0.5], [-1, 0.5], [-1, 0], [1, -0.5], [0.5, -0.5],
+    [0.5, -1], [0, -1], [-0.5, -1], [-0.5, -0.5], [-1, -0.5]]
+    B = Ball2(zeros(2), 1.0)
+    P = HPolygon([HalfSpace(di, ρ(di, B)) for di in dirs])
+    vlistP = vertices_list(P)
+    V = VPolygon(vlistP)
+    all(x -> _isapprox(σ(x, V), x), vlistP)
 
     # test that #83 is fixed
     v = VPolygon([N[2, 3]])