Switch to generated function approach.

JuliaRobotics · Jun 18, 2019 · b0d1a39 · b0d1a39
1 parent 74ceba7
commit b0d1a39
Show file tree

Hide file tree

Showing 2 changed files with 18 additions and 6 deletions.
diff --git 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
@@ -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