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 #########################################################################