Correct UnitQuaternion with Quaternions.jl

Project.toml

@@ -4,6 +4,7 @@ version = "1.0.3"
 
 [deps]
 LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
+Quaternions = "94ee1d12-ae83-5a48-8b1c-48b8ff168ae0"
 Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c"
 StaticArrays = "90137ffa-7385-5640-81b9-e52037218182"
 Statistics = "10745b16-79ce-11e8-11f9-7d13ad32a3b2"
@@ -18,7 +19,8 @@ ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210"
 InteractiveUtils = "b77e0a4c-d291-57a0-90e8-8db25a27a240"
 Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c"
 Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"
+Quaternions = "94ee1d12-ae83-5a48-8b1c-48b8ff168ae0"
 Unitful = "1986cc42-f94f-5a68-af5c-568840ba703d"
 
 [targets]
-test = ["BenchmarkTools", "ForwardDiff", "Random", "Test", "InteractiveUtils", "Unitful"]
+test = ["BenchmarkTools", "ForwardDiff", "Random", "Test", "InteractiveUtils", "Unitful", "Quaternions"]

src/Rotations.jl

@@ -6,6 +6,7 @@ module Rotations
 using LinearAlgebra
 using StaticArrays
 using Random
+using Quaternions
 
 import Statistics
 
@@ -22,19 +23,23 @@ include("principal_value.jl")
 include("rodrigues_params.jl")
 include("error_maps.jl")
 include("rotation_error.jl")
+include("log.jl")
 include("eigen.jl")
 include("deprecated.jl")
 
 export
     Rotation, RotMatrix, RotMatrix2, RotMatrix3,
     Angle2d,
-    Quat, UnitQuaternion,
-    AngleAxis, RodriguesVec, RotationVec, RodriguesParam, MRP,
+    UnitQuaternion,
+    AngleAxis, RotationVec, RodriguesParam, MRP,
     RotX, RotY, RotZ,
     RotXY, RotYX, RotZX, RotXZ, RotYZ, RotZY,
     RotXYX, RotYXY, RotZXZ, RotXZX, RotYZY, RotZYZ,
     RotXYZ, RotYXZ, RotZXY, RotXZY, RotYZX, RotZYX,
 
+    # Deprecated, but export for compatibility
+    RodriguesVec, Quat, SPQuat,
+
     # Quaternion math ops
     logm, expm, ⊖, ⊕,
 

src/log.jl

@@ -0,0 +1,23 @@
+# 3d
+function Base.log(R::RotationVec)
+    x, y, z = params(R)
+    return @SMatrix [0 -z  y
+                     z  0 -x
+                    -y  x  0]
+end
+
+function Base.log(R::Rotation{3})
+    log(RotationVec(R))
+end
+
+
+# 2d
+function Base.log(R::Angle2d)
+    θ, = params(R)
+    return @SMatrix [0 -θ
+                     θ  0]
+end
+
+function Base.log(R::Rotation{2})
+    log(Angle2d(R))
+end

src/mrps.jl

@@ -48,7 +48,6 @@ end
 @inline Base.Tuple(p::MRP) = Tuple(convert(UnitQuaternion, p))
 
 # ~~~~~~~~~~~~~~~ Math Operations ~~~~~~~~~~~~~~~ #
-LinearAlgebra.norm(p::MRP) = sqrt(p.x^2 + p.y^2 + p.z^2)
 Base.inv(p::MRP) = MRP(-p.x, -p.y, -p.z)
 
 # ~~~~~~~~~~~~~~~ Composition ~~~~~~~~~~~~~~~ #

src/rodrigues_params.jl

@@ -45,7 +45,6 @@ end
 @inline Base.Tuple(rp::RodriguesParam) = Tuple(convert(UnitQuaternion, rp))
 
 # ~~~~~~~~~~~~~~~ Math Operations ~~~~~~~~~~~~~~~ #
-LinearAlgebra.norm(g::RodriguesParam) = sqrt(g.x^2 + g.y^2 + g.z^2)
 Base.inv(p::RodriguesParam) = RodriguesParam(-p.x, -p.y, -p.z)
 
 # ~~~~~~~~~~~~~~~ Composition ~~~~~~~~~~~~~~~ #

src/unitquaternion.jl

@@ -1,4 +1,4 @@
-import Base: +, -, *, /, \, exp, log, ≈, ==, inv, conj
+import Base: *, /, \, exp, ≈, ==, inv
 
 """
     UnitQuaternion{T} <: Rotation
@@ -32,6 +32,7 @@ struct UnitQuaternion{T} <: Rotation{3,T}
     end
 
     UnitQuaternion{T}(q::UnitQuaternion) where T = new{T}(q.w, q.x, q.y, q.z)
+    UnitQuaternion{T}(q::Quaternion) where T = new{T}(q.s, q.v1, q.v2, q.v3)
 end
 
 # ~~~~~~~~~~~~~~~ Constructors ~~~~~~~~~~~~~~~ #
@@ -41,6 +42,18 @@ function UnitQuaternion(w,x,y,z, normalize::Bool = true)
     UnitQuaternion{eltype(types)}(w,x,y,z, normalize)
 end
 
+function UnitQuaternion(q::T) where T<:Quaternion
+    if q.norm
+        return UnitQuaternion(q.s, q.v1, q.v2, q.v3)
+    else
+        throw(InexactError(nameof(T), T, q))
+    end
+end
+
+function Quaternions.Quaternion(q::UnitQuaternion)
+    Quaternion(q.w, q.x, q.y, q.z, true)
+end
+
 # Pass in Vectors
 @inline function (::Type{Q})(q::AbstractVector, normalize::Bool = true) where Q <: UnitQuaternion
     check_length(q, 4)
@@ -183,6 +196,7 @@ end
 # ~~~~~~~~~~~~~~~ Getters ~~~~~~~~~~~~~~~ #
 @inline scalar(q::UnitQuaternion) = q.w
 @inline vector(q::UnitQuaternion) = SVector{3}(q.x, q.y, q.z)
+@inline vector(q::Quaternion) = SVector{3}(q.v1, q.v2, q.v3)
 
 """
     params(R::Rotation)
@@ -197,77 +211,70 @@ Rotations.params(p) == @SVector [1.0, 2.0, 3.0]  # true
 """
 @inline params(q::UnitQuaternion) = SVector{4}(q.w, q.x, q.y, q.z)
 
+# TODO: this will be removed, because Quaternion is not a subtype of Rotation
+@inline params(q::Quaternion) = SVector{4}(q.s, q.v1, q.v2, q.v3)
+
 # ~~~~~~~~~~~~~~~ Initializers ~~~~~~~~~~~~~~~ #
 function Random.rand(rng::AbstractRNG, ::Random.SamplerType{<:UnitQuaternion{T}}) where T
-    normalize(UnitQuaternion{T}(randn(rng,T), randn(rng,T), randn(rng,T), randn(rng,T)))
+    _normalize(UnitQuaternion{T}(randn(rng,T), randn(rng,T), randn(rng,T), randn(rng,T)))
 end
 @inline Base.one(::Type{Q}) where Q <: UnitQuaternion = Q(1.0, 0.0, 0.0, 0.0)
 
 
 # ~~~~~~~~~~~~~~~ Math Operations ~~~~~~~~~~~~~~~ #
 
 # Inverses
-conj(q::Q) where Q <: UnitQuaternion = Q(q.w, -q.x, -q.y, -q.z, false)
-inv(q::UnitQuaternion) = conj(q)
-(-)(q::Q) where Q <: UnitQuaternion = Q(-q.w, -q.x, -q.y, -q.z, false)
+inv(q::Q) where Q <: UnitQuaternion = Q(q.w, -q.x, -q.y, -q.z, false)
 
 # Norms
-LinearAlgebra.norm(q::UnitQuaternion) = sqrt(q.w^2 + q.x^2 + q.y^2 + q.z^2)
 vecnorm(q::UnitQuaternion) = sqrt(q.x^2 + q.y^2 + q.z^2)
+vecnorm(q::Quaternion) = sqrt(q.v1^2 + q.v2^2 + q.v3^2)
 
-function LinearAlgebra.normalize(q::Q) where Q <: UnitQuaternion
-    n = inv(norm(q))
-    Q(q.w*n, q.x*n, q.y*n, q.z*n)
+function _normalize(q::Q) where Q <: UnitQuaternion
+    n = norm(params(q))
+    Q(q.w/n, q.x/n, q.y/n, q.z/n)
 end
 
 # Identity
 (::Type{Q})(I::UniformScaling) where Q <: UnitQuaternion = one(Q)
 
 # Exponentials and Logarithms
 """
-    pure_quaternion(v::AbstractVector)
-    pure_quaternion(x, y, z)
+    _pure_quaternion(v::AbstractVector)
+    _pure_quaternion(x, y, z)
 
-Create a `UnitQuaternion` with zero scalar part (i.e. `q.w == 0`).
+Create a `Quaternion` with zero scalar part (i.e. `q.w == 0`).
 """
-function pure_quaternion(v::AbstractVector)
+function _pure_quaternion(v::AbstractVector)
     check_length(v, 3)
-    UnitQuaternion(zero(eltype(v)), v[1], v[2], v[3], false)
+    Quaternion(zero(eltype(v)), v[1], v[2], v[3], false)
 end
 
-@inline pure_quaternion(x::Real, y::Real, z::Real) =
-    UnitQuaternion(zero(x), x, y, z, false)
-
-function exp(q::Q) where Q <: UnitQuaternion
-    θ = vecnorm(q)
-    sθ,cθ = sincos(θ)
-    es = exp(q.w)
-    M = es*sθ/θ
-    Q(es*cθ, q.x*M, q.y*M, q.z*M, false)
-end
+@inline _pure_quaternion(x::Real, y::Real, z::Real) =
+    Quaternion(zero(x), x, y, z, false)
 
 function expm(ϕ::AbstractVector)
     check_length(ϕ, 3)
     θ = norm(ϕ)
     sθ,cθ = sincos(θ/2)
-    M = 1//2 *sinc(θ/π/2)
+    M = sinc(θ/π/2)/2
     UnitQuaternion(cθ, ϕ[1]*M, ϕ[2]*M, ϕ[3]*M, false)
 end
 
-function log(q::Q, eps=1e-6) where Q <: UnitQuaternion
+function _log_as_quat(q::Q, eps=1e-6) where Q <: UnitQuaternion
     # Assumes unit quaternion
     θ = vecnorm(q)
     if θ > eps
         M = atan(θ, q.w)/θ
     else
         M = (1-(θ^2/(3q.w^2)))/q.w
     end
-    pure_quaternion(M*vector(q))
+    _pure_quaternion(M*vector(q))
 end
 
 function logm(q::UnitQuaternion)
     # Assumes unit quaternion
-    2*vector(log(q))
+    return 2*vector(_log_as_quat(q))
 end
 
 # Composition
@@ -305,22 +312,10 @@ function Base.:*(q::UnitQuaternion, r::StaticVector)  # must be StaticVector to
     (w^2 - v'v)*r + 2*v*(v'r) + 2*w*cross(v,r)
 end
 
-"""
-    (*)(q::UnitQuaternion, w::Real)
-
-Scalar multiplication of a quaternion. Breaks unit norm.
-"""
-function (*)(q::Q, w::Real) where Q<:UnitQuaternion
-    return Q(q.w*w, q.x*w, q.y*w, q.z*w, false)
-end
-(*)(w::Real, q::UnitQuaternion) = q*w
-
+(\)(q1::UnitQuaternion, q2::UnitQuaternion) = inv(q1)*q2  # Equivalent to inv(q1)*q2
+(/)(q1::UnitQuaternion, q2::UnitQuaternion) = q1*inv(q2)  # Equivalent to q1*inv(q2)
 
-
-(\)(q1::UnitQuaternion, q2::UnitQuaternion) = conj(q1)*q2  # Equivalent to inv(q1)*q2
-(/)(q1::UnitQuaternion, q2::UnitQuaternion) = q1*conj(q2)  # Equivalent to q1*inv(q2)
-
-(\)(q::UnitQuaternion, r::SVector{3}) = conj(q)*r          # Equivalent to inv(q)*r
+(\)(q::UnitQuaternion, r::SVector{3}) = inv(q)*r          # Equivalent to inv(q)*r
 
 """
     rotation_between(from, to)
@@ -361,7 +356,7 @@ See "Fundamentals of Spacecraft Attitude Determination and Control" by Markley a
 Sections 3.1-3.2 for more details.
 """
 function kinematics(q::Q, ω::AbstractVector) where Q <: UnitQuaternion
-    1//2 * params(q*Q(0.0, ω[1], ω[2], ω[3], false))
+    params(q*Q(0.0, ω[1], ω[2], ω[3], false))/2
 end
 
 # ~~~~~~~~~~~~~~~ Linear Algebraic Conversions ~~~~~~~~~~~~~~~ #
@@ -379,6 +374,14 @@ function lmult(q::UnitQuaternion)
         q.z -q.y  q.x  q.w;
     ]
 end
+function lmult(q::Quaternion)
+    SA[
+        q.s  -q.v1 -q.v2 -q.v3;
+        q.v1  q.s  -q.v3  q.v2;
+        q.v2  q.v3  q.s  -q.v1;
+        q.v3 -q.v2  q.v1  q.s;
+    ]
+end
 lmult(q::StaticVector{4}) = lmult(UnitQuaternion(q, false))
 
 """
@@ -395,6 +398,14 @@ function rmult(q::UnitQuaternion)
         q.z  q.y -q.x  q.w;
     ]
 end
+function rmult(q::Quaternion)
+    SA[
+        q.s  -q.v1 -q.v2 -q.v3;
+        q.v1  q.s   q.v3 -q.v2;
+        q.v2 -q.v3  q.s   q.v1;
+        q.v3  q.v2 -q.v1  q.s;
+    ]
+end
 rmult(q::SVector{4}) = rmult(UnitQuaternion(q, false))
 
 """

test/2d.jl

@@ -51,7 +51,7 @@ using Rotations, StaticArrays, Test
     # check on the inverse function
     ################################
 
-    @testset "Testing inverse()" begin
+    @testset "Testing inv()" begin
         repeats = 100
         for R in [RotMatrix{2,Float64}, Angle2d{Float64}]
             I = one(R)
@@ -67,6 +67,26 @@ using Rotations, StaticArrays, Test
         end
     end
 
+    ################################
+    # check on the norm functions
+    ################################
+
+    @testset "Testing norm() and normalize()" begin
+        repeats = 100
+        for R in [RotMatrix{2,Float64}, Angle2d{Float64}]
+            I = one(R)
+            Random.seed!(0)
+            for i = 1:repeats
+                r = rand(R)
+                @test norm(r) ≈ norm(Matrix(r))
+                if VERSION ≥ v"1.5"
+                    @test normalize(r) ≈ normalize(Matrix(r))
+                    @test normalize(r) isa SMatrix
+                end
+            end
+        end
+    end
+
     #########################################################################
     # Rotate some stuff
     #########################################################################

test/log.jl

@@ -0,0 +1,15 @@
+@testset "log" begin
+    all_types = (RotMatrix{3}, AngleAxis, RotationVec,
+                 UnitQuaternion, RodriguesParam, MRP,
+                 RotXYZ, RotYZX, RotZXY, RotXZY, RotYXZ, RotZYX,
+                 RotXYX, RotYZY, RotZXZ, RotXZX, RotYXY, RotZYZ,
+                 RotX, RotY, RotZ,
+                 RotXY, RotYZ, RotZX, RotXZ, RotYX, RotZY,
+                 RotMatrix{2}, Angle2d)
+
+    @testset "$(T)" for T in all_types, F in (one, rand)
+        R = F(T)
+        @test R ≈ exp(log(R))
+        @test log(R) isa SMatrix
+    end
+end

test/rodrigues_params.jl

@@ -30,7 +30,7 @@ import Rotations: ∇rotate, ∇composition1, ∇composition2, skew, params
 
     # Math operations
     g = rand(R)
-    @test norm(g) == sqrt(g.x^2 + g.y^2 + g.z^2)
+    @test norm(g) == norm(Matrix(g))
 
     # Test Jacobians
     R = RodriguesParam
@@ -67,7 +67,7 @@ end
     @test qdot ≈ 0.5*lmult(q)*hmat(ω)
     @test ω ≈ 2*vmat()*lmult(q)'qdot
     @test ω ≈ 2*vmat()*lmult(inv(q))*qdot
-    @test qdot ≈ 0.5 * params(q*pure_quaternion(ω))
+    @test qdot ≈ 0.5 * params(Quaternion(q)*_pure_quaternion(ω))
 
     # MRPs
     ω = @SVector rand(3)