From 1a98b3ec6de71d29c30941aad94bc8a612881831 Mon Sep 17 00:00:00 2001
From: Twan Koolen <koolen.twan@gmail.com>
Date: Mon, 17 Jun 2019 16:37:40 -0400
Subject: [PATCH 1/3] Fix #15, error with latest StaticArrays due to dot
 change.

---
 src/gjk.jl                | 8 ++++----
 src/reference_distance.jl | 2 +-
 2 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/gjk.jl b/src/gjk.jl
index 9ff4959..eae4d9f 100644
--- a/src/gjk.jl
+++ b/src/gjk.jl
@@ -92,11 +92,11 @@ function gjk!(cache::CollisionCache,
             # in collision
             return GJKResult(
                 simplex,
-                dot(weights, cache.simplex_points),
+                transpose(weights) * cache.simplex_points,
                 penetration_distance(simplex)
             )
         end
-        best_point = dot(weights, simplex)
+        best_point = transpose(weights) * simplex
 
         direction = -best_point
         direction_in_A = rotAinv * direction
@@ -126,7 +126,7 @@ function gjk!(cache::CollisionCache,
         if score <= dot(best_point, direction) + atol || iter >= max_iter
             return GJKResult(
                 simplex,
-                dot(weights, cache.simplex_points),
+                transpose(weights) * cache.simplex_points,
                 norm(best_point)
             )
         else
@@ -142,7 +142,7 @@ function penetration_distance(simplex)
     _, penetration_distance = gt.argmax(1:length(simplex)) do i
         face = simplex_face(simplex, i)
         weights = projection_weights(face)
-        closest_point = dot(weights, face)
+        closest_point = transpose(weights) * face
         distance_to_face = norm(closest_point)
         -distance_to_face
     end
diff --git a/src/reference_distance.jl b/src/reference_distance.jl
index c2b1712..9ada508 100644
--- a/src/reference_distance.jl
+++ b/src/reference_distance.jl
@@ -33,7 +33,7 @@ end
 function exterior_distance(face_points, target)
     simplex = convert(SVector, Simplex(face_points)) .- SVector((target,))
     weights = projection_weights(simplex)
-    projected = dot(weights, simplex)
+    projected = transpose(weights) * simplex
     norm(projected)
 end
 

From b8fb57a37b71bb89682b3134001906782430d35d Mon Sep 17 00:00:00 2001
From: Twan Koolen <koolen.twan@gmail.com>
Date: Tue, 18 Jun 2019 16:51:21 -0400
Subject: [PATCH 2/3] Switch to generated function approach.

---
 src/gjk.jl                | 20 ++++++++++++++++----
 src/reference_distance.jl |  4 ++--
 2 files changed, 18 insertions(+), 6 deletions(-)

diff --git a/src/gjk.jl b/src/gjk.jl
index eae4d9f..f8e303b 100644
--- a/src/gjk.jl
+++ b/src/gjk.jl
@@ -63,6 +63,18 @@ end
     transform_simplex_impl(N, cache, poseA, poseB)
 end
 
+# Note: it looks like this can be replaced with transpose(weights) * points in Julia 1.3 (before that, it's a lot slower)
+@generated function linear_combination(weights::StaticVector{N}, points::StaticVector{N}) where {N}
+    expr = :(weights[1] * points[1])
+    for i = 2 : N
+        expr = :($expr + weights[$i] * points[$i])
+    end
+    return quote
+        Base.@_inline_meta
+        $expr
+    end
+end
+
 function transform_simplex_impl(N, cache, poseA, poseB)
     Expr(:call, :(SVector),
         [:((poseA(value(cache.simplex_points[$i].a)) -
@@ -92,11 +104,11 @@ function gjk!(cache::CollisionCache,
             # in collision
             return GJKResult(
                 simplex,
-                transpose(weights) * cache.simplex_points,
+                linear_combination(weights, cache.simplex_points),
                 penetration_distance(simplex)
             )
         end
-        best_point = transpose(weights) * simplex
+        best_point = linear_combination(weights, simplex)
 
         direction = -best_point
         direction_in_A = rotAinv * direction
@@ -126,7 +138,7 @@ function gjk!(cache::CollisionCache,
         if score <= dot(best_point, direction) + atol || iter >= max_iter
             return GJKResult(
                 simplex,
-                transpose(weights) * cache.simplex_points,
+                linear_combination(weights, cache.simplex_points),
                 norm(best_point)
             )
         else
@@ -142,7 +154,7 @@ function penetration_distance(simplex)
     _, penetration_distance = gt.argmax(1:length(simplex)) do i
         face = simplex_face(simplex, i)
         weights = projection_weights(face)
-        closest_point = transpose(weights) * face
+        closest_point = linear_combination(weights, face)
         distance_to_face = norm(closest_point)
         -distance_to_face
     end
diff --git a/src/reference_distance.jl b/src/reference_distance.jl
index 9ada508..7078931 100644
--- a/src/reference_distance.jl
+++ b/src/reference_distance.jl
@@ -1,7 +1,7 @@
 module ReferenceDistance
 
 using GeometryTypes
-import EnhancedGJK: projection_weights
+import EnhancedGJK: projection_weights, linear_combination
 import StaticArrays: SVector
 using LinearAlgebra
 
@@ -33,7 +33,7 @@ end
 function exterior_distance(face_points, target)
     simplex = convert(SVector, Simplex(face_points)) .- SVector((target,))
     weights = projection_weights(simplex)
-    projected = transpose(weights) * simplex
+    projected = linear_combination(weights, simplex)
     norm(projected)
 end
 

From 19489ae774cc744411fd20bb07e1cca8e5e8f45f Mon Sep 17 00:00:00 2001
From: Twan Koolen <koolen.twan@gmail.com>
Date: Thu, 20 Jun 2019 12:01:02 -0400
Subject: [PATCH 3/3] Allow failures on 1.0 for now.

Because of
https://github.com/JuliaRobotics/EnhancedGJK.jl/pull/16#issuecomment-503319584.
---
 .travis.yml | 1 +
 1 file changed, 1 insertion(+)

diff --git a/.travis.yml b/.travis.yml
index 5286fd9..ba5d457 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -11,6 +11,7 @@ notifications:
   email: false
 matrix:
   allow_failures:
+    - julia: 1.0 # because of https://github.com/JuliaRobotics/EnhancedGJK.jl/pull/16#issuecomment-503319584
     - julia: nightly
 branches:
   only: