From b04d039385ab927e10c2c019491dc43a3a0de513 Mon Sep 17 00:00:00 2001
From: Yuto Horikawa <hyrodium@gmail.com>
Date: Mon, 15 May 2023 01:44:38 +0900
Subject: [PATCH] `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            |  2 +-
 src/Rotations.jl        |  1 +
 src/deprecated.jl       |  2 ++
 src/rotation_between.jl | 38 ++++++++++++++++++++++++++++++++++++++
 src/unitquaternion.jl   | 17 -----------------
 test/rotation_tests.jl  | 12 ++++++++++++
 6 files changed, 54 insertions(+), 18 deletions(-)
 create mode 100644 src/rotation_between.jl

diff --git a/Project.toml b/Project.toml
index 4ddda94f..03a52dd2 100644
--- a/Project.toml
+++ b/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"
diff --git a/src/Rotations.jl b/src/Rotations.jl
index 965a9b4f..22cfd9bd 100644
--- a/src/Rotations.jl
+++ b/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
diff --git a/src/deprecated.jl b/src/deprecated.jl
index 81d7639f..4fe23780 100644
--- a/src/deprecated.jl
+++ b/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))
diff --git a/src/rotation_between.jl b/src/rotation_between.jl
new file mode 100644
index 00000000..00cde71a
--- /dev/null
+++ b/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
diff --git a/src/unitquaternion.jl b/src/unitquaternion.jl
index a14f1ccc..7e706c38 100644
--- a/src/unitquaternion.jl
+++ b/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)
 
diff --git a/test/rotation_tests.jl b/test/rotation_tests.jl
index ca317930..92acf136 100644
--- a/test/rotation_tests.jl
+++ b/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