From cb46201987e5759eb23daddf59e4d5536b2c8339 Mon Sep 17 00:00:00 2001
From: Sebastian Guadalupe <sebastianguadalupe00@gmail.com>
Date: Wed, 15 May 2019 20:31:19 -0300
Subject: [PATCH] Add concrete minkowski sum for polygons

---
 docs/src/lib/representations.md |  1 +
 src/VPolygon.jl                 | 58 ++++++++++++++++++++++++++++++++-
 test/unit_Polygon.jl            | 20 ++++++++++++
 3 files changed, 78 insertions(+), 1 deletion(-)

diff --git a/docs/src/lib/representations.md b/docs/src/lib/representations.md
index 98fc5c25ed..687e6cf472 100644
--- a/docs/src/lib/representations.md
+++ b/docs/src/lib/representations.md
@@ -415,6 +415,7 @@ constraints_list(::VPolygon{N}) where {N<:Real}
 translate(::VPolygon{N}, ::AbstractVector{N}) where {N<:Real}
 remove_redundant_vertices(::VPolygon{N}; ::String="monotone_chain") where {N<:Real}
 remove_redundant_vertices!(::VPolygon{N}; ::String="monotone_chain") where {N<:Real}
+minkowski_sum(::VPolygon{N}, ::VPolygon{N}) where {N<:Real}
 ```
 Inherited from [`LazySet`](@ref):
 * [`norm`](@ref norm(::LazySet, ::Real))
diff --git a/src/VPolygon.jl b/src/VPolygon.jl
index 3fb20c7a13..9a18d6b804 100644
--- a/src/VPolygon.jl
+++ b/src/VPolygon.jl
@@ -5,7 +5,8 @@ export VPolygon,
        remove_redundant_vertices,
        remove_redundant_vertices!,
        convex_hull,
-       linear_map
+       linear_map,
+       minkowski_sum
 
 """
     VPolygon{N<:Real} <: AbstractPolygon{N}
@@ -553,3 +554,58 @@ function translate(P::VPolygon{N}, v::AbstractVector{N}) where {N<:Real}
                                 "set by a $(length(v))-dimensional vector"
     return VPolygon([x + v for x in vertices_list(P)])
 end
+
+"""
+    minkowski_sum(P::VPolygon{N}, Q::VPolygon{N}) where {N<:Real}
+
+The Minkowski Sum of two polygon in vertex representation.
+
+### Input
+
+- `P` -- polygon in vertex representation
+- `Q` -- another polygon in vertex representation
+
+### Output
+
+A polygon in vertex representation.
+
+### Algorithm
+
+We treat each edge of the polygons as a vector, attaching them in polar order
+(attaching the tail of the next vector to the head of the previous vector). The
+resulting polygonal chain will be a polygon, which is the Minkowski sum of the 
+given polygons. This algorithm assumes that the vertices of P and Q are sorted 
+in counter-clockwise fashion and has linear complexity O(m+n) where m and n are 
+the number of vertices of P and Q respectively.
+
+"""
+function minkowski_sum(P::VPolygon{N}, Q::VPolygon{N}) where {N<:Real}
+    vlistP = vertices_list(P)
+    vlistQ = vertices_list(Q)
+    mP = length(vlistP)
+    mQ = length(vlistQ)
+    i = 1
+    k = 1
+    j = 1
+    R = Vector{Vector{N}}(undef, mP+mQ)
+    fill!(R, N[0, 0])
+    while i <= size(R, 1)
+        P₁, P₂ = vlistP[(k-1)%mP+1], vlistP[(k%mP+1)]
+        P₁P₂ = P₂ - P₁
+        Q₁, Q₂ = vlistQ[(j-1)%mQ+1], vlistQ[(j%mQ+1)]
+        Q₁Q₂ = Q₂ - Q₁
+        R[i] = P₁ + Q₁
+        turn = right_turn(P₁P₂, Q₁Q₂, N[0, 0])
+        if turn > 0
+            k += 1
+        elseif turn < 0
+            j += 1
+        else
+            pop!(R)
+            k += 1
+            j += 1
+        end
+        i += 1
+    end
+    return VPolygon(R)
+end
diff --git a/test/unit_Polygon.jl b/test/unit_Polygon.jl
index 1f7ca86308..c2c089e329 100644
--- a/test/unit_Polygon.jl
+++ b/test/unit_Polygon.jl
@@ -169,6 +169,26 @@ for N in [Float64, Float32, Rational{Int}]
         @test translate(hp, N[1, 2]) == typeof(hp)(
             [HalfSpace(N[2, 2], N(18)), HalfSpace(N[-3, 3], N(9)),
              HalfSpace(N[-1, -1], N(-3)), HalfSpace(N[2, -4], N(-6))])
+        
+        # test for concrete minkowski sum
+        A = [N[4, 0], N[6, 2], N[4, 4]]
+        B = [N[-2, -2], N[2, 0], N[2, 2], N[-2, 4]]
+
+        P = VPolygon(A)
+        Q = VPolygon(B)
+        PQ = minkowski_sum(P, Q)
+        @test LazySets.ispermutation(PQ.vertices, [N[2, -2], N[6, 0], N[8, 2],
+                                                N[8, 4], N[6, 6], N[2, 8]])
+
+        #test for corner case of parallel edges in minkowski sum
+        C = [N[10, 5], N[10, 10], N[8, 10], N[5, 8], N[5, 5]]
+        D = [N[-1, -2], N[1, -2], N[-1, 2]]
+
+        R = VPolygon(C)
+        S = VPolygon(D)
+        RS = minkowski_sum(R, S)
+        @test LazySets.ispermutation(RS.vertices, [N[4, 3], N[11, 3], N[11, 8],
+                                                N[9,12], N[7, 12], N[4, 10]])
     end
 
     # concrete intersection of H-rep