From 66b7852ae02b9178f198e066f4a02cf4421ed669 Mon Sep 17 00:00:00 2001
From: SebastianGuadalupe <sebastianguadalupe00@gmail.com>
Date: Tue, 14 Jul 2020 10:42:02 -0300
Subject: [PATCH] add quadratic map of a zonotope

---
 docs/src/lib/sets/Zonotope.md |  1 +
 src/Sets/Zonotope.jl          | 52 +++++++++++++++++++++++++++++++++++
 2 files changed, 53 insertions(+)

diff --git a/docs/src/lib/sets/Zonotope.md b/docs/src/lib/sets/Zonotope.md
index 5667ab9363..b638e1e93a 100644
--- a/docs/src/lib/sets/Zonotope.md
+++ b/docs/src/lib/sets/Zonotope.md
@@ -18,6 +18,7 @@ split(::AbstractZonotope, ::Int)
 split(::AbstractZonotope, ::AbstractVector{Int}, ::AbstractVector{Int})
 remove_zero_generators(::Zonotope)
 linear_map!(::Zonotope, ::AbstractMatrix, ::Zonotope)
+quadratic_map(::Zonotope{N}, ::Array{A}) where {N, A<:Array{N}}
 ```
 
 Inherited from [`LazySet`](@ref):
diff --git a/src/Sets/Zonotope.jl b/src/Sets/Zonotope.jl
index 132dc817db..fd6aae3a28 100644
--- a/src/Sets/Zonotope.jl
+++ b/src/Sets/Zonotope.jl
@@ -427,3 +427,55 @@ function linear_map!(Zout::Zonotope, M::AbstractMatrix, Z::Zonotope)
     mul!(Zout.generators, M, Z.generators)
     return Zout
 end
+
+"""
+    quadratic_map(Z::Zonotope{N}, Q::Array{A}) where {N, A<:Array{N}}
+
+Return the overapproximation of the quadratic map of the given zonotope.
+
+### Input
+
+- `Z` -- zonotope
+- `Q` -- array of square matrices
+
+### Output
+
+The overapproximation of the quadratic map of the given zonotope.
+
+### Algorithm
+
+This function implements [Lemma 1, 1]
+
+[1] *Matthias Althoff and Bruce H. Krogh. 2012. Avoiding geometric intersection
+operations in reachability analysis of hybrid systems. In Proceedings of the
+15th ACM international conference on Hybrid Systems: Computation and Control
+(HSCC ’12). Association for Computing Machinery, New York, NY, USA, 45–54.*
+"""
+function quadratic_map(Z::Zonotope{N}, Q::Array{A}) where {N, A<:Array{N}}
+    @assert length(Q) == dim(Z) "the length of Q needs to match the dimension of Z"
+    G = genmat(Z)
+    c = center(Z)
+    n, p = size(G)
+    h = Array{N}(undef, n, binomial(p+2, 2)-1)
+    d = Vector{N}(undef, n)
+    for i=1:n
+        aux = 0
+        g(x) = G[:, x]
+        for s=1:p
+            aux += g(s)' * Q[i] * g(s)
+        end
+        d[i] = c' * Q[i] * c + 0.5 * aux
+        for j=1:p
+            h[i, j] = c' * Q[i] * g(j) + g(j)' * Q[i] * c
+            h[i, p+j] = 0.5 * g(j)' * Q[i] * g(j)
+        end
+        l = 0
+        for j=1:p-1
+            for k=j+1:p
+                l += 1
+                h[i, 2p+l] = g(j)' * Q[i] * g(k) + g(k)' * Q[i] * g(j)
+            end
+        end
+    end
+    return Zonotope(d, h)
+end