Fix rotation_between (#259)

* add more tests for `rotation_between`

* fix methods for `rotation_between`

* bump version to v1.5.1

Author: hyrodium

Project.toml

@@ -1,6 +1,6 @@
 name = "Rotations"
 uuid = "6038ab10-8711-5258-84ad-4b1120ba62dc"
-version = "1.5.0"
+version = "1.5.1"
 
 [deps]
 LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"

src/rotation_between.jl

@@ -6,13 +6,15 @@ Compute the quaternion that rotates vector `u` so that it aligns with vector
 """
 function rotation_between end
 
-function rotation_between(u::SVector{2}, v::SVector{2})
+function rotation_between(u::StaticVector{2}, v::StaticVector{2})
     c = complex(v[1], v[2]) / complex(u[1], u[2])
+    iszero(c) && throw(ArgumentError("Input vectors must be nonzero and finite."))
+    isfinite(c) || throw(ArgumentError("Input vectors must be nonzero and finite."))
     theta = Base.angle(c)
     return Angle2d(theta)
 end
 
-function rotation_between(u::SVector{3}, v::SVector{3})
+function rotation_between(u::StaticVector{3}, v::StaticVector{3})
     # Robustified version of implementation from https://www.gamedev.net/topic/429507-finding-the-quaternion-betwee-two-vectors/#entry3856228
     normprod = sqrt(dot(u, u) * dot(v, v))
     T = typeof(normprod)
@@ -22,9 +24,11 @@ function rotation_between(u::SVector{3}, v::SVector{3})
     @inbounds return QuatRotation(w, v[1], v[2], v[3]) # relies on normalization in constructor
 end
 
-function rotation_between(u::SVector{N}, v::SVector{N}) where N
+function rotation_between(u::StaticVector{N}, v::StaticVector{N}) where N
     e1 = normalize(u)
     e2 = normalize(v - e1 * dot(e1, v))
+    any(isnan, e1) && throw(ArgumentError("Input vectors must be nonzero and finite."))
+    any(isnan, e2) && throw(ArgumentError("Input vectors must be nonzero and finite."))
     c = dot(e1, normalize(v))
     s = sqrt(1-c^2)
     P = [e1 e2 svd([e1 e2]'; full=true).Vt[StaticArrays.SUnitRange(3,N),:]']

test/rotation_tests.jl

@@ -397,14 +397,30 @@ all_types = (RotMatrix3, RotMatrix{3}, AngleAxis, RotationVec,
     end
 
     @testset "$(N)-dimensional rotation_between" for N in 2:7
-        for _ in 1:100
-            u = randn(SVector{N})
-            v = randn(SVector{N})
+        @testset "random check" begin
+            for _ in 1:100
+                u = randn(SVector{N})
+                v = randn(SVector{N})
+                R = rotation_between(u,v)
+                @test isrotation(R)
+                @test R isa Rotation
+                @test normalize(v) ≈ R * normalize(u)
+            end
+        end
+
+        @testset "mixed types" begin
+            u = randn(MVector{N})
+            v = randn(SizedVector{N})
             R = rotation_between(u,v)
             @test isrotation(R)
             @test R isa Rotation
             @test normalize(v) ≈ R * normalize(u)
         end
+
+        @testset "zero-vector" begin
+            @test_throws ArgumentError rotation_between(zero(SVector{N}), rand(SVector{N}))
+            @test_throws ArgumentError rotation_between(rand(SVector{N}), zero(SVector{N}))
+        end
     end
 
 #########################################################################