From f00b07d9d809717bfb6b1d9fc3966e6be059af22 Mon Sep 17 00:00:00 2001
From: Yuto Horikawa <hyrodium@gmail.com>
Date: Tue, 16 May 2023 10:02:40 +0900
Subject: [PATCH] Fix `rotation_between` (#259)

* add more tests for `rotation_between`

* fix methods for `rotation_between`

* bump version to v1.5.1
---
 Project.toml            |  2 +-
 src/rotation_between.jl | 10 +++++++---
 test/rotation_tests.jl  | 22 +++++++++++++++++++---
 3 files changed, 27 insertions(+), 7 deletions(-)

diff --git a/Project.toml b/Project.toml
index 03a52dd..537dc04 100644
--- a/Project.toml
+++ b/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"
diff --git a/src/rotation_between.jl b/src/rotation_between.jl
index 00cde71..7f1210f 100644
--- a/src/rotation_between.jl
+++ b/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),:]']
diff --git a/test/rotation_tests.jl b/test/rotation_tests.jl
index 92acf13..c020844 100644
--- a/test/rotation_tests.jl
+++ b/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
 
 #########################################################################