Add RotationGenerator and its subtypes

src/Rotations.jl

@@ -24,7 +24,8 @@ include("principal_value.jl")
 include("rodrigues_params.jl")
 include("error_maps.jl")
 include("rotation_error.jl")
-include("log.jl")
+include("rotation_generator.jl")
+include("logexp.jl")
 include("eigen.jl")
 include("deprecated.jl")
 
@@ -42,6 +43,14 @@ export
     # Deprecated, but export for compatibility
     UnitQuaternion,
 
+    # rotation generators
+    RotationGenerator,
+    RotMatrixGenerator,
+    RotMatrixGenerator2,
+    RotMatrixGenerator3,
+    Angle2dGenerator,
+    RotationVecGenerator,
+
     # Quaternion math ops
     logm, expm, ⊖, ⊕,
 

src/logexp.jl

@@ -0,0 +1,49 @@
+## 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
+
+
+## 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)))
+end
+
+# 2d
+function Base.exp(R::Angle2dGenerator)
+    return Angle2d(R.v)
+end
+
+function Base.exp(R::RotationGenerator{2})
+    exp(Angle2dGenerator(R))
+end
+
+function Base.exp(R::RotMatrixGenerator{2})
+    RotMatrix(exp(Angle2dGenerator(R)))
+end

src/rotation_generator.jl

@@ -0,0 +1,258 @@
+"""
+    abstract type RotationGenerator{N,T} <: StaticMatrix{N,N,T}
+
+An abstract type representing `N`-dimensional rotation generator. More abstractly, they represent
+skew-symmetric real `N`×`N` matrices.
+"""
+abstract type RotationGenerator{N,T} <: StaticMatrix{N,N,T} end
+
+Base.@pure StaticArrays.Size(::Type{RotationGenerator{N}}) where {N} = Size(N,N)
+Base.@pure StaticArrays.Size(::Type{RotationGenerator{N,T}}) where {N,T} = Size(N,N)
+Base.@pure StaticArrays.Size(::Type{R}) where {R<:RotationGenerator} = Size(supertype(R))
+Base.adjoint(r::RotationGenerator) = -r
+Base.transpose(r::RotationGenerator{N,T}) where {N,T<:Real} = -r
+
+# Generate identity-matrix with SMatrix
+# Note that ones(RotationGenerator3,dims...) is not Array{<:RotationGenerator} but Array{<:StaticMatrix{3,3}}
+Base.one(::RotationGenerator{N,T}) where {N,T} = one(SMatrix{N,N,T})
+Base.one(::Type{RotationGenerator}) = error("The dimension of rotation is not specified.")
+Base.one(::Type{<:RotationGenerator{N}}) where N = one(SMatrix{N,N})
+Base.one(::Type{<:RotationGenerator{N,T}}) where {N,T} = one(SMatrix{N,N,T})
+Base.ones(::Type{R}) where {R<:RotationGenerator} = ones(R, ()) # avoid StaticArray constructor
+Base.ones(::Type{R}, dims::Base.DimOrInd...) where {R<:RotationGenerator} = ones(typeof(one(R)),dims...)
+Base.ones(::Type{R}, dims::NTuple{N, Integer}) where {R<:RotationGenerator, N} = ones(typeof(one(R)),dims)
+
+# Generate zero rotation matrix
+Base.zero(r::RotationGenerator) = zero(typeof(r))
+
+# difference
+Base.:-(r1::RotationGenerator, r2::RotationGenerator) = r1 + (-r2)
+
+# `convert` goes through the constructors, similar to e.g. `Number`
+Base.convert(::Type{R}, rot::RotationGenerator{N}) where {N,R<:RotationGenerator{N}} = R(rot)
+
+# RotationGenerator matrices should be skew-symmetric. Only the operations we define,
+# like addition, will stay as RotationGenerators, otherwise users will get an
+# SMatrix{3,3} (e.g. rot1 * rot2 -> SMatrix)
+Base.@pure StaticArrays.similar_type(::Union{R,Type{R}}) where {R <: RotationGenerator} = SMatrix{size(R)..., eltype(R), prod(size(R))}
+Base.@pure StaticArrays.similar_type(::Union{R,Type{R}}, ::Type{T}) where {R <: RotationGenerator, T} = SMatrix{size(R)..., T, prod(size(R))}
+
+# Useful for converting arrays of rotations to another rotation eltype, for instance.
+# Only works because parameters of all the rotations are of a similar form
+# Would need to be more sophisticated if we have arbitrary dimensions, etc
+@inline function Base.promote_op(::Type{R1}, ::Type{R2}) where {R1 <: RotationGenerator, R2 <: RotationGenerator}
+    size(R1) == size(R2) || throw(DimensionMismatch("cannot promote rotations of $(size(R1)[1]) and $(size(R2)[1]) dimensions"))
+    if isleaftype(R1)
+        return R1
+    else
+        return R1{eltype(R2)}
+    end
+end
+
+################################################################################
+################################################################################
+"""
+    struct RotMatrixGenerator{N,T} <: RotationGenerator{N,T}
+
+A statically-sized, N×N skew-symmetric matrix.
+
+Note: the skew-symmetricity of the input matrix is *not* checked by the constructor.
+"""
+struct RotMatrixGenerator{N,T,L} <: RotationGenerator{N,T} # which is <: AbstractMatrix{T}
+    mat::SMatrix{N, N, T, L} # The final parameter to SMatrix is the "length" of the matrix, 3 × 3 = 9
+    RotMatrixGenerator{N,T,L}(x::AbstractArray) where {N,T,L} = new{N,T,L}(convert(SMatrix{N,N,T,L}, x))
+    # fixes #49 ambiguity introduced in StaticArrays 0.6.5
+    RotMatrixGenerator{N,T,L}(x::StaticArray) where {N,T,L} = new{N,T,L}(convert(SMatrix{N,N,T,L}, x))
+end
+RotMatrixGenerator(x::SMatrix{N,N,T,L}) where {N,T,L} = RotMatrixGenerator{N,T,L}(x)
+
+Base.zero(::Type{RotMatrixGenerator}) = 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
+    RotMatrixGeneratorN = Symbol(:RotMatrixGenerator, N)
+    @eval begin
+        @inline RotMatrixGenerator(t::NTuple{$L})  = RotMatrixGenerator(SMatrix{$N,$N}(t))
+        @inline (::Type{RotMatrixGenerator{$N}})(t::NTuple{$L}) = RotMatrixGenerator(SMatrix{$N,$N}(t))
+        @inline RotMatrixGenerator{$N,T}(t::NTuple{$L}) where {T} = RotMatrixGenerator(SMatrix{$N,$N,T}(t))
+        @inline RotMatrixGenerator{$N,T,$L}(t::NTuple{$L}) where {T} = RotMatrixGenerator(SMatrix{$N,$N,T}(t))
+        const $RotMatrixGeneratorN{T} = RotMatrixGenerator{$N, T, $L}
+    end
+end
+Base.@propagate_inbounds Base.getindex(r::RotMatrixGenerator, i::Int) = r.mat[i]
+@inline Base.Tuple(r::RotMatrixGenerator) = Tuple(r.mat)
+
+@inline RotMatrixGenerator(v::Number) = RotMatrixGenerator{2}(v)
+@inline function (::Type{RotMatrixGenerator{2}})(v::Number)
+    RotMatrixGenerator(@SMatrix [zero(v) -v; v zero(v)])
+end
+@inline function RotMatrixGenerator{2,T}(v::Number) where T
+    RotMatrixGenerator(@SMatrix T[zero(v) -v; v zero(v)])
+end
+
+# A rotation is more-or-less defined as being an orthogonal (or unitary) matrix
+Base.:-(r::RotMatrixGenerator) = RotMatrixGenerator(-r.mat)
+
+# By default, composition of rotations will go through RotMatrixGenerator, unless overridden
+@inline Base.:+(r1::RotationGenerator, r2::RotationGenerator) = RotMatrixGenerator(r1) + RotMatrixGenerator(r2)
+@inline Base.:+(r1::RotMatrixGenerator, r2::RotationGenerator) = r1 + RotMatrixGenerator(r2)
+@inline Base.:+(r1::RotationGenerator, r2::RotMatrixGenerator) = RotMatrixGenerator(r1) + r2
+@inline Base.:+(r1::RotMatrixGenerator, r2::RotMatrixGenerator) = RotMatrixGenerator(r1.mat + r2.mat)
+
+@inline Base.:*(t::Number, r::RotMatrixGenerator) = RotMatrixGenerator(t*r.mat)
+@inline Base.:*(r::RotMatrixGenerator, t::Number) = t*r
+@inline Base.:/(r::RotMatrixGenerator, t::Number) = RotMatrixGenerator(r.mat/t)
+
+# Special case multiplication of 2×2 rotation matrices: speedup using skew-symmetricity
+@inline function Base.:+(r1::RotMatrixGenerator{2}, r2::RotMatrixGenerator{2})
+    v = r1[2,1]+r2[2,1]
+    s = @SMatrix [0  -v
+                  v   0]
+    return RotMatrixGenerator(s)
+end
+
+# Special case multiplication of 3×3 rotation matrices: speedup using skew-symmetricity
+@inline function Base.:+(r1::RotMatrixGenerator{3}, r2::RotMatrixGenerator{3})
+    x = r1[6]+r2[6]
+    y = r1[7]+r2[7]
+    z = r1[2]+r2[2]
+    s = @SMatrix [ 0 -z  y
+                   z  0 -x
+                  -y  x  0]
+
+    return RotMatrixGenerator(s)
+end
+
+"""
+    struct Angle2dGenerator{T} <: RotationGenerator{2,T}
+        v::T
+    end
+
+A 2×2 rotation generator matrix (i.e. skew-symmetric matrix).
+[ 0 -v
+  v  0 ]
+"""
+struct Angle2dGenerator{T} <: RotationGenerator{2,T}
+    v::T
+    Angle2dGenerator{T}(r) where T = new{T}(r)
+end
+
+@inline function Angle2dGenerator(r::T) where T <: Number
+    Angle2dGenerator{T}(r)
+end
+
+@inline Angle2dGenerator(r::RotationGenerator{2}) = Angle2dGenerator(r[2,1])
+@inline Angle2dGenerator{T}(r::RotationGenerator{2}) where {T} = Angle2dGenerator{T}(r[2,1])
+
+@inline Base.zero(::Type{A}) where {A<: Angle2dGenerator} = A(0.0)
+
+@inline Base.:+(r1::Angle2dGenerator, r2::Angle2dGenerator) = Angle2dGenerator(r1.v + r2.v)
+@inline Base.:-(r1::Angle2dGenerator, r2::Angle2dGenerator) = Angle2dGenerator(r1.v - r2.v)
+@inline Base.:*(t::Number, r::Angle2dGenerator) = Angle2dGenerator(t*r.v)
+@inline Base.:*(r::Angle2dGenerator, t::Number) = t*r
+@inline Base.:/(r::Angle2dGenerator, t::Number) = Angle2dGenerator(r.v/t)
+@inline Base.:-(r::Angle2dGenerator) = Angle2dGenerator(-r.v)
+
+@inline function Base.getindex(r::Angle2dGenerator{T}, i::Int) where T
+    if i == 1
+        zero(T)
+    elseif i == 2
+        r.v
+    elseif i == 3
+        -r.v
+    elseif i == 4
+        zero(T)
+    else
+        throw(BoundsError(r,i))
+    end
+end
+
+
+"""
+    struct RotationVecGenerator{T} <: RotationGenerator{2,T}
+        x::T
+        y::T
+        z::T
+    end
+
+A 3×3 rotation generator matrix (i.e. skew-symmetric matrix).
+[ 0 -z  y
+  z  0 -x
+ -y  x  0]
+"""
+struct RotationVecGenerator{T} <: RotationGenerator{3,T}
+    x::T
+    y::T
+    z::T
+    RotationVecGenerator{T}(x,y,z) where T = new{T}(x,y,z)
+end
+
+@inline function RotationVecGenerator(x::X,y::Y,z::Z) where {X<:Number, Y<:Number, Z<:Number}
+    RotationVecGenerator{promote_type(X,Y,Z)}(x, y, z)
+end
+
+@inline RotationVecGenerator(r::RotationGenerator{3}) = RotationVecGenerator(r[6], r[7], r[2])
+@inline RotationVecGenerator{T}(r::RotationGenerator{3}) where {T} = RotationVecGenerator{T}(r[6], r[7], r[2])
+
+@inline Base.zero(::Type{R}) where {R<: RotationVecGenerator} = R(0.0,0.0,0.0)
+
+@inline Base.:+(r1::RotationVecGenerator, r2::RotationVecGenerator) = RotationVecGenerator(r1.x + r2.x, r1.y + r2.y, r1.z + r2.z)
+@inline Base.:-(r1::RotationVecGenerator, r2::RotationVecGenerator) = RotationVecGenerator(r1.x - r2.x, r1.y - r2.y, r1.z - r2.z)
+@inline Base.:*(t::Number, r::RotationVecGenerator) = RotationVecGenerator(t*r.x, t*r.y, t*r.z)
+@inline Base.:*(r::RotationVecGenerator, t::Number) = t*r
+@inline Base.:/(r::RotationVecGenerator, t::Number) = RotationVecGenerator(r.x/t, r.y/t, r.z/t)
+@inline Base.:-(r::RotationVecGenerator) = RotationVecGenerator(-r.x, -r.y, -r.z)
+
+@inline function Base.getindex(r::RotationVecGenerator{T}, i::Int) where T
+    if i == 1
+        zero(T)
+    elseif i == 2
+        r.z
+    elseif i == 3
+        -r.y
+    elseif i == 4
+        -r.z
+    elseif i == 5
+        zero(T)
+    elseif i == 6
+        r.x
+    elseif i == 7
+        r.y
+    elseif i == 8
+        -r.x
+    elseif i == 9
+        zero(T)
+    else
+        throw(BoundsError(r,i))
+    end
+end
+
+
+################################################################################
+################################################################################
+
+
+# A simplification and specialization of the Base.show function for AbstractArray makes
+# everything sensible at the REPL.
+function Base.show(io::IO, ::MIME"text/plain", X::RotationGenerator)
+    if !haskey(io, :compact)
+        io = IOContext(io, :compact => true)
+    end
+    summary(io, X)
+    if !isa(X, RotMatrixGenerator)
+        n_fields = length(fieldnames(typeof(X)))
+        print(io, "(")
+        for i = 1:n_fields
+            print(io, getfield(X, i))
+            if i < n_fields
+                print(io, ", ")
+            end
+        end
+        print(io, ")")
+    end
+    print(io, ":")
+    println(io)
+    io = IOContext(io, :typeinfo => eltype(X))
+    Base.print_array(io, X)
+end