Add support for higher dimensional rotation (#219)

* update nearest_rotation tests for 4-dimensional matrix * update indents in rotation_tests.jl * update isrotation tests * updatge RotMatrix constructor * update isrotation methods * add isrotationgenerator * add tests for isrotationgenerator * cleanup logexp.jl and add support for general dimensions * Update src/rotation_generator.jl Co-authored-by: Chris Foster <[email protected]> * update around isrotationgenerator * update tests for general dimensional rotation generator * update general dimensional log(::RotMatrix{N}) * update log(::RotMatix{N}) for Julia 1.6 * add tests for general dimensional rotation_angle * add general dimensional rotation_angle * revert general dimensional rotation_angle * add rand method for higher dimensional rotation * add tests for higher dimensional RotMatrix * add rand.jl * update multiple dispatch of Random.rand for lower dimensional RotMatrix * update comments * fix rng in rand(::RotMatrix{N}) * move rand methods to rand.jl * fix typos * add documentation for general dimensional rotations * update reference * Update docs/src/reference.md Co-authored-by: Chris Foster <[email protected]> * Update src/logexp.jl Co-authored-by: Chris Foster <[email protected]> * remove duplicated end * remove Base.log(R::Rotation{2}) etc. and update some comments * update comments suggested by @c42f Co-authored-by: Chris Foster <[email protected]>
JuliaGeometry · Feb 17, 2022 · 345d0d9 · 345d0d9
1 parent 4f76c5a
commit 345d0d9
Show file tree

Hide file tree

Showing 18 changed files with 362 additions and 143 deletions.
diff --git a/docs/make.jl b/docs/make.jl
@@ -30,9 +30,11 @@ makedocs(;
             "2D Rotation Generators" => "2d_rotation_generator.md",
             "3D Rotation Generators" => "3d_rotation_generator.md",
             ],
+        "General Dimensional Rotations" => "general_dimensional_rotations.md",
         "Common Methods for Rotations" => "functions.md",
         "Visualizing Rotations" => "visualizing.md",
         "Function Reference" => "functionreference.md",
+        "References" => "reference.md",
     ],
 )
 

diff --git a/docs/src/general_dimensional_rotations.md b/docs/src/general_dimensional_rotations.md
@@ -0,0 +1,20 @@
+# General dimensional Rotation
+
+Our main goal is to efficiently represent rotations in lower dimensions, such as 2D and 3D, but some operations on rotations in general dimensions are also supported.
+
+```@setup rotmatrixN
+using Rotations
+using StaticArrays
+```
+
+**example**
+
+```@repl rotmatrixN
+r1 = one(RotMatrix{4})  # generate identity rotation matrix
+m = @SMatrix rand(4,4)
+r2 = nearest_rotation(m)  # nearest rotation matrix from given matrix
+r3 = rand(RotMatrix{4})  # random rotation in SO(4)
+r1*r2/r3  # multiplication and division
+s = log(r2)  # logarithm of RotMatrix is a RotMatrixGenerator
+exp(s)  # exponential of RotMatrixGenerator is a RotMatrix
+```
diff --git a/docs/src/reference.md b/docs/src/reference.md
@@ -1,4 +1,22 @@
 # Reference for rotations
 
-* [Quaternion (Wikipedia)](https://en.wikipedia.org/wiki/Quaternion)
-* [Quaternions and 3d rotation, explained interactively (by 3Blue1Brown)](https://www.youtube.com/watch?v=zjMuIxRvygQ)
+## Published articles
+* ["Fundamentals of Spacecraft Attitude Determination and Control" by Markley and Crassidis](https://link.springer.com/book/10.1007/978-1-4939-0802-8)
+    * See sections 2.1-2.9 for 3d rotation parametrization.
+    * See sections 3.1-3.2 for kinematics.
+* ["Derivation of the Euler-Rodrigues formula for three-dimensional rotations from the general formula for four-dimensional rotations" by Johan Ernest Mebius](https://arxiv.org/abs/math/0701759)
+    * The conversion `RotMatrix{3}` to `QuatRotaiton` is based on this paper.
+* ["Computing Exponentials of Skew Symmetric Matrices And Logarithms of Orthogonal Matrices" by Jean Gallier and Dianna Xu](https://cs.brynmawr.edu/~dxu/206-2550-2.pdf)
+    * See this article for log and exp of rotation matrices in higher dimensions.
+
+## Wikipedia articles
+* [Rotation matrix](https://en.wikipedia.org/wiki/Rotation_matrix)
+* [Quaternion](https://en.wikipedia.org/wiki/Quaternion)
+* [Wahba's problem](https://en.wikipedia.org/wiki/Wahba%27s_problem)
+    * Wahba's problem is related to the `nearest_rotation` function.
+* [Polar decomposition](https://en.wikipedia.org/wiki/Polar_decomposition)
+    * Polar decomposition is also related to the `nearest_rotation` function.
+
+## Others
+* ["Quaternions and 3d rotation, explained interactively" by 3Blue1Brown](https://www.youtube.com/watch?v=zjMuIxRvygQ)
+* ["Visualizing quaternions (4d numbers) with stereographic projection" by 3Blue1Brown](https://www.youtube.com/watch?v=d4EgbgTm0Bg)
diff --git a/docs/src/rotation_generator_types.md b/docs/src/rotation_generator_types.md
@@ -30,12 +30,12 @@ For more information, see the sidebar page.
 
 ### 2D rotations
 * `RotMatrixGenerator2{T}`
-    * Rotation matrix in 2 dimensional Euclidean space.
+    * Skew symmetric matrix in 2 dimensional Euclidean space.
 * `Angle2dGenerator`
-    * Parametrized with rotational angle.
+    * Parametrized with one real number like `Angle2d`.
 
 ### 3D rotations
 * `RotMatrixGenerator3{T}`
-    * Rotation matrix in 3 dimensional Euclidean space.
+    * Skew symmetric matrix in 3 dimensional Euclidean space.
 * `RotationVecGenerator`
-    * Rotation around given axis. The length of axis vector represents its angle.
+    * Rotation generator around given axis. The length of axis vector represents its angle.
diff --git a/src/Rotations.jl b/src/Rotations.jl
@@ -27,6 +27,7 @@ include("rotation_error.jl")
 include("rotation_generator.jl")
 include("logexp.jl")
 include("eigen.jl")
+include("rand.jl")
 include("deprecated.jl")
 
 export
@@ -57,8 +58,10 @@ export
     # Quaternion maps
     ExponentialMap, QuatVecMap, CayleyMap, MRPMap, IdentityMap,
 
-    # check validity of the rotation (is it close to unitary?)
+    # check validity of the rotation (is it close to orthonormal?)
     isrotation,
+    # check validity of the rotation (is it skew-symmetric?)
+    isrotationgenerator,
 
     # Get nearest rotation matrix
     nearest_rotation,

diff --git a/src/core_types.jl b/src/core_types.jl
@@ -40,28 +40,6 @@ Base.convert(::Type{R}, rot::Rotation{N}) where {N,R<:Rotation{N}} = R(rot)
 Base.@pure StaticArrays.similar_type(::Union{R,Type{R}}) where {R <: Rotation} = SMatrix{size(R)..., eltype(R), prod(size(R))}
 Base.@pure StaticArrays.similar_type(::Union{R,Type{R}}, ::Type{T}) where {R <: Rotation, T} = SMatrix{size(R)..., T, prod(size(R))}
 
-function Random.rand(rng::AbstractRNG, ::Random.SamplerType{R}) where R <: Rotation{2}
-    T = eltype(R)
-    if T == Any
-        T = Float64
-    end
-
-    R(2π * rand(rng, T))
-end
-
-# A random rotation can be obtained easily with unit quaternions
-# The unit sphere in R⁴ parameterizes quaternion rotations according to the
-# Haar measure of SO(3) - see e.g. http://math.stackexchange.com/questions/184086/uniform-distributions-on-the-space-of-rotations-in-3d
-function Random.rand(rng::AbstractRNG, ::Random.SamplerType{R}) where R <: Rotation{3}
-    T = eltype(R)
-    if T == Any
-        T = Float64
-    end
-
-    q = QuatRotation(randn(rng, T), randn(rng, T), randn(rng, T), randn(rng, T))
-    return R(q)
-end
-
 @inline function Base.:/(r1::Rotation, r2::Rotation)
     r1 * inv(r2)
 end
@@ -98,18 +76,27 @@ RotMatrix(x::SMatrix{N,N,T,L}) where {N,T,L} = RotMatrix{N,T,L}(x)
 
 Base.zero(::Type{RotMatrix}) = error("The dimension of rotation is not specified.")
 
-# These functions (plus size) are enough to satisfy the entire StaticArrays interface:
 for N = 2:3
     L = N*N
     RotMatrixN = Symbol(:RotMatrix, N)
     @eval begin
-        @inline RotMatrix(t::NTuple{$L})  = RotMatrix(SMatrix{$N,$N}(t))
-        @inline (::Type{RotMatrix{$N}})(t::NTuple{$L}) = RotMatrix(SMatrix{$N,$N}(t))
-        @inline RotMatrix{$N,T}(t::NTuple{$L}) where {T} = RotMatrix(SMatrix{$N,$N,T}(t))
-        @inline RotMatrix{$N,T,$L}(t::NTuple{$L}) where {T} = RotMatrix(SMatrix{$N,$N,T}(t))
         const $RotMatrixN{T} = RotMatrix{$N, T, $L}
     end
 end
+
+# These functions (plus size) are enough to satisfy the entire StaticArrays interface:
+@inline function RotMatrix(t::NTuple{L}) where L
+    n = sqrt(L)
+    if !isinteger(n)
+        throw(DimensionMismatch("The length of input tuple $(t) must be square number."))
+    end
+    N = Int(n)
+    RotMatrix(SMatrix{N,N}(t))
+end
+@inline (::Type{RotMatrix{N}})(t::NTuple) where N = RotMatrix(SMatrix{N,N}(t))
+@inline RotMatrix{N,T}(t::NTuple) where {N,T} = RotMatrix(SMatrix{N,N,T}(t))
+@inline RotMatrix{N,T,L}(t::NTuple{L}) where {N,T,L} = RotMatrix(SMatrix{N,N,T}(t))
+
 Base.@propagate_inbounds Base.getindex(r::RotMatrix, i::Int) = r.mat[i]
 @inline Base.Tuple(r::RotMatrix) = Tuple(r.mat)
 
@@ -212,26 +199,31 @@ _isrotation_eps(T) = eps(T)
     isrotation(r)
     isrotation(r, tol)
 
-Check whether `r` is a 2×2 or 3×3 rotation matrix, where `r * r'` is within
+Check whether `r` is a rotation matrix, where `r * r'` is within
 `tol` of the identity matrix (using the Frobenius norm). `tol` defaults to
 `1000 * eps(float(eltype(r)))` or `zero(T)` for integer `T`.
 """
-function isrotation(r::AbstractMatrix{T}, tol::Real = 1000 * _isrotation_eps(eltype(T))) where T
-    if size(r) == (2,2)
-        # Transpose is overloaded for many of our types, so we do it explicitly:
-        r_trans = @SMatrix [conj(r[1,1])  conj(r[2,1]);
-                            conj(r[1,2])  conj(r[2,2])]
-        d = norm((r * r_trans) - one(SMatrix{2,2}))
-    elseif size(r) == (3,3)
-        r_trans = @SMatrix [conj(r[1,1])  conj(r[2,1])  conj(r[3,1]);
-                            conj(r[1,2])  conj(r[2,2])  conj(r[3,2]);
-                            conj(r[1,3])  conj(r[2,3])  conj(r[3,3])]
-        d = norm((r * r_trans) - one(SMatrix{3,3}))
-    else
+isrotation
+
+function isrotation(r::StaticMatrix{N,N,T}, tol::Real = 1000 * _isrotation_eps(eltype(T))) where {N,T<:Real}
+    m = SMatrix(r)
+    d = norm(m*m'-one(SMatrix{N,N}))
+    return d ≤ tol && det(m) > 0
+end
+
+function isrotation(r::AbstractMatrix{T}, tol::Real = 1000 * _isrotation_eps(eltype(T))) where T<:Real
+    if !=(size(r)...)
         return false
     end
+    d = norm(r*r'-one(r))
+    return d ≤ tol && det(r) > 0
+end
 
-    return d <= tol && det(r) > 0
+function isrotation(r::AbstractMatrix{T}, tol::Real = 1000 * _isrotation_eps(eltype(real(T)))) where T<:Number
+    if !isreal(r)
+        return false
+    end
+    return isrotation(real(r), tol)
 end
 
 """

diff --git a/src/euler_types.jl b/src/euler_types.jl
@@ -38,16 +38,6 @@ for axis in [:X, :Y, :Z]
     end
 end
 
-function Random.rand(rng::AbstractRNG, ::Random.SamplerType{R}) where R <: Union{RotX,RotY,RotZ}
-    T = eltype(R)
-    if T == Any
-        T = Float64
-    end
-
-    return R(2π*rand(rng, T))
-end
-
-
 """
     struct RotX{T} <: Rotation{3,T}
     RotX(theta)
@@ -261,19 +251,6 @@ for axis1 in [:X, :Y, :Z]
     end
 end
 
-function Random.rand(rng::AbstractRNG, ::Random.SamplerType{R}) where R <: Union{RotXY,RotYZ,RotZX, RotXZ, RotYX, RotZY}
-    T = eltype(R)
-    if T == Any
-        T = Float64
-    end
-
-    # Not really sure what this distribution is, but it's also not clear what
-    # it should be! rand(RotXY) *is* invariant to pre-rotations by a RotX and
-    # post-rotations by a RotY...
-    return R(2π*rand(rng, T), 2π*rand(rng, T))
-end
-
-
 """
     struct RotXY{T} <: Rotation{3,T}
     RotXY(theta_x, theta_y)

diff --git a/src/logexp.jl b/src/logexp.jl
@@ -1,49 +1,42 @@
 ## log
-# 3d
-function Base.log(R::RotationVec)
-    x, y, z = params(R)
-    return RotationVecGenerator(x,y,z)
-end
-
-function Base.log(R::Rotation{3})
-    log(RotationVec(R))
-end
-
-
 # 2d
-function Base.log(R::Angle2d)
-    θ, = params(R)
-    return Angle2dGenerator(θ)
-end
-
-function Base.log(R::Rotation{2})
-    log(Angle2d(R))
-end
-
+Base.log(R::Angle2d) = Angle2dGenerator(R.theta)
+Base.log(R::RotMatrix{2}) = RotMatrixGenerator(log(Angle2d(R)))
+#= We can define log for Rotation{2} like this,
+but the subtypes of Rotation{2} are only Angle2d and RotMatrix{2},
+so we don't need this defnition. =#
+# Base.log(R::Rotation{2}) = log(Angle2d(R))
 
-## exp
 # 3d
-function Base.exp(R::RotationVecGenerator)
-    return RotationVec(R.x,R.y,R.z)
-end
-
-function Base.exp(R::RotationGenerator{3})
-    exp(RotationVecGenerator(R))
-end
-
-function Base.exp(R::RotMatrixGenerator{3})
-    RotMatrix(exp(RotationVecGenerator(R)))
+Base.log(R::RotationVec) = RotationVecGenerator(R.sx,R.sy,R.sz)
+Base.log(R::RotMatrix{3}) = RotMatrixGenerator(log(RotationVec(R)))
+Base.log(R::Rotation{3}) = log(RotationVec(R))
+
+# General dimensions
+function Base.log(R::RotMatrix{N}) where N
+    # This will be faster when log(::SMatrix) is implemented in StaticArrays.jl
+    @static if VERSION < v"1.7"
+        # This if block is related to this PR.
+        # https://github.com/JuliaLang/julia/pull/40573
+        S = SMatrix{N,N}(real(log(Matrix(R))))
+    else
+        S = SMatrix{N,N}(log(Matrix(R)))
+    end
+    RotMatrixGenerator((S-S')/2)
 end
 
+## exp
 # 2d
-function Base.exp(R::Angle2dGenerator)
-    return Angle2d(R.v)
-end
+Base.exp(R::Angle2dGenerator) = Angle2d(R.v)
+Base.exp(R::RotMatrixGenerator{2}) = RotMatrix(exp(Angle2dGenerator(R)))
+# Same as log(R::Rotation{2}), this definition is not necessary until someone add a new subtype of RotationGenerator{2}.
+# Base.exp(R::RotationGenerator{2}) = exp(Angle2dGenerator(R))
 
-function Base.exp(R::RotationGenerator{2})
-    exp(Angle2dGenerator(R))
-end
+# 3d
+Base.exp(R::RotationVecGenerator) = RotationVec(R.x,R.y,R.z)
+Base.exp(R::RotMatrixGenerator{3}) = RotMatrix(exp(RotationVecGenerator(R)))
+# Same as log(R::Rotation{2}), this definition is not necessary until someone add a new subtype of RotationGenerator{3}.
+# Base.exp(R::RotationGenerator{3}) = exp(RotationVecGenerator(R))
 
-function Base.exp(R::RotMatrixGenerator{2})
-    RotMatrix(exp(Angle2dGenerator(R)))
-end
+# General dimensions
+Base.exp(R::RotMatrixGenerator{N}) where N = RotMatrix(exp(SMatrix(R)))
diff --git a/src/mrps.jl b/src/mrps.jl
@@ -25,9 +25,6 @@ MRP(x::X, y::Y, z::Z) where {X,Y,Z} = MRP{promote_type(X,Y,Z)}(x, y, z)
 params(g::MRP) = SVector{3}(g.x, g.y, g.z)
 
 # ~~~~~~~~~~~~~~~ Initializers ~~~~~~~~~~~~~~~ #
-function Random.rand(rng::AbstractRNG, ::Random.SamplerType{RP}) where RP <: MRP
-    RP(rand(rng, QuatRotation))
-end
 Base.one(::Type{RP}) where RP <: MRP = RP(0.0, 0.0, 0.0)
 
 # ~~~~~~~~~~~~~~~~ Quaternion <=> MRP ~~~~~~~~~~~~~~~~~~ #