rotation_between for general dimensions (#257)

* deprecate rotation_between(::AbstractVector, ::AbstractVector)

* move deprecated `rotation_between` to `src/deprecated.jl`

* move `rotation_between(::SVector{3}, ::SVector{3})` to `src/rotation_between.jl`

* update `src/rotation_between.jl`

* add a method for `rotation_between(::SVector{2}, ::SVector{2})`

* add a method for `rotation_between(::SVector{N}, ::SVector{N})`

* add tests for `rotation_between`

* fix `rotation_between` for general dimensions

* fix `rotation_between(::SVector{2}, ::SVector{2})`

* bump version to v1.5.0

Project.toml

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

src/Rotations.jl

@@ -26,6 +26,7 @@ include("rotation_generator.jl")
 include("logexp.jl")
 include("eigen.jl")
 include("rand.jl")
+include("rotation_between.jl")
 include("deprecated.jl")
 
 export

src/deprecated.jl

@@ -1,2 +1,4 @@
 # Deprecate UnitQuaternion => QuatRotation
 Base.@deprecate_binding UnitQuaternion QuatRotation true
+
+Base.@deprecate rotation_between(from::AbstractVector, to::AbstractVector) rotation_between(SVector{3}(from), SVector{3}(to))

src/rotation_between.jl

@@ -0,0 +1,38 @@
+"""
+ rotation_between(u, v)
+
+Compute the quaternion that rotates vector `u` so that it aligns with vector
+`v`, along the geodesic (shortest path).
+"""
+function rotation_between end
+
+function rotation_between(u::SVector{2}, v::SVector{2})
+ c = complex(v[1], v[2]) / complex(u[1], u[2])
+ theta = Base.angle(c)
+ return Angle2d(theta)
+end
+
+function rotation_between(u::SVector{3}, v::SVector{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)
+ normprod < eps(T) && throw(ArgumentError("Input vectors must be nonzero."))
+ w = normprod + dot(u, v)
+ v = abs(w) < 100 * eps(T) ? perpendicular_vector(u) : cross(u, v)
+ @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
+ e1 = normalize(u)
+ e2 = normalize(v - e1 * dot(e1, v))
+ c = dot(e1, normalize(v))
+ s = sqrt(1-c^2)
+ P = [e1 e2 svd([e1 e2]'; full=true).Vt[StaticArrays.SUnitRange(3,N),:]']
+ Q = one(MMatrix{N,N})
+ Q[1,1] = c
+ Q[1,2] = -s
+ Q[2,1] = s
+ Q[2,2] = c
+ R = RotMatrix(P*Q*P')
+ return R
+end

src/unitquaternion.jl

@@ -311,23 +311,6 @@ end
 Base.:\(q1::QuatRotation, q2::QuatRotation) = inv(q1)*q2
 Base.:/(q1::QuatRotation, q2::QuatRotation) = q1*inv(q2)
 
-"""
- rotation_between(from, to)
-
-Compute the quaternion that rotates vector `from` so that it aligns with vector
-`to`, along the geodesic (shortest path).
-"""
-rotation_between(from::AbstractVector, to::AbstractVector) = rotation_between(SVector{3}(from), SVector{3}(to))
-function rotation_between(from::SVector{3}, to::SVector{3})
- # Robustified version of implementation from https://www.gamedev.net/topic/429507-finding-the-quaternion-betwee-two-vectors/#entry3856228
- normprod = sqrt(dot(from, from) * dot(to, to))
- T = typeof(normprod)
- normprod < eps(T) && throw(ArgumentError("Input vectors must be nonzero."))
- w = normprod + dot(from, to)
- v = abs(w) < 100 * eps(T) ? perpendicular_vector(from) : cross(from, to)
- @inbounds return QuatRotation(w, v[1], v[2], v[3]) # relies on normalization in constructor
-end
-
 """
  slerp(R1::Rotaion{3}, R2::Rotaion{3}, t::Real)
 

test/rotation_tests.jl

@@ -395,6 +395,18 @@ all_types = (RotMatrix3, RotMatrix{3}, AngleAxis, RotationVec,
  end
  end
  end
+
+ @testset "$(N)-dimensional rotation_between" for N in 2:7
+ 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
+
 #########################################################################
  # Check that the eltype is inferred in Rot constructors
  @testset "Rot constructor eltype promotion" begin