From 3ec59bc35e11664ed2ca76a7e5ae68872c8c2e39 Mon Sep 17 00:00:00 2001 From: hyrodium Date: Thu, 28 Oct 2021 12:45:15 +0900 Subject: [PATCH 01/22] remove incorrect log(::Quaternion) --- src/unitquaternion.jl | 4 ++-- test/unitquat.jl | 3 ++- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/unitquaternion.jl b/src/unitquaternion.jl index c52a3ca4..41de6478 100644 --- a/src/unitquaternion.jl +++ b/src/unitquaternion.jl @@ -254,7 +254,7 @@ function expm(ϕ::AbstractVector) 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 @@ -267,7 +267,7 @@ end function logm(q::UnitQuaternion) # Assumes unit quaternion - 2*vector(log(q)) + 2*vector(_log_as_quat(q)) end # Composition diff --git a/test/unitquat.jl b/test/unitquat.jl index 2ae1dc0a..8c402506 100644 --- a/test/unitquat.jl +++ b/test/unitquat.jl @@ -125,7 +125,8 @@ import Rotations: vmat, rmult, lmult, hmat, tmat @test expm(SA_F32[1, 2, 3]) isa UnitQuaternion{Float32} q32 = rand(UnitQuaternion{Float32}) - @test log(q32) isa UnitQuaternion{Float32} + @test Rotations._log_as_quat(q32) isa UnitQuaternion{Float32} +# @test log(q32) isa Rotation @test eltype(logm(q32)) == Float32 @test expm(logm(q32)) ≈ q32 From 630509b58cf69f58d2a3d01a460fe7dc07b7b5aa Mon Sep 17 00:00:00 2001 From: hyrodium Date: Thu, 28 Oct 2021 13:00:22 +0900 Subject: [PATCH 02/22] add method for log(::Rotation) and return SMatrix --- src/Rotations.jl | 1 + src/log.jl | 23 +++++++++++++++++++++++ src/unitquaternion.jl | 2 +- test/log.jl | 23 +++++++++++++++++++++++ test/unitquat.jl | 2 +- 5 files changed, 49 insertions(+), 2 deletions(-) create mode 100644 src/log.jl create mode 100644 test/log.jl diff --git a/src/Rotations.jl b/src/Rotations.jl index 3d19b373..15b70c1c 100644 --- a/src/Rotations.jl +++ b/src/Rotations.jl @@ -22,6 +22,7 @@ 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") diff --git a/src/log.jl b/src/log.jl new file mode 100644 index 00000000..56465f89 --- /dev/null +++ b/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 diff --git a/src/unitquaternion.jl b/src/unitquaternion.jl index 41de6478..a2f5c19b 100644 --- a/src/unitquaternion.jl +++ b/src/unitquaternion.jl @@ -1,4 +1,4 @@ -import Base: +, -, *, /, \, exp, log, ≈, ==, inv, conj +import Base: +, -, *, /, \, exp, ≈, ==, inv, conj """ UnitQuaternion{T} <: Rotation diff --git a/test/log.jl b/test/log.jl new file mode 100644 index 00000000..7e549d01 --- /dev/null +++ b/test/log.jl @@ -0,0 +1,23 @@ +@testset "log_3D" 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) + oneaxis_types = (RotX, RotY, RotZ) + + @testset "$(T)" for T in all_types, F in (one, rand) + R = F(T) + @test R ≈ exp(log(R)) + end +end + +@testset "log_2D" begin + all_types = (RotMatrix{2}, Angle2d) + + @testset "$(T)" for T in all_types, θ in 0.0:0.1:π + R = F(T) + @test R ≈ exp(log(R)) + end +end diff --git a/test/unitquat.jl b/test/unitquat.jl index 8c402506..c3ceba3a 100644 --- a/test/unitquat.jl +++ b/test/unitquat.jl @@ -126,7 +126,7 @@ import Rotations: vmat, rmult, lmult, hmat, tmat q32 = rand(UnitQuaternion{Float32}) @test Rotations._log_as_quat(q32) isa UnitQuaternion{Float32} -# @test log(q32) isa Rotation + @test log(q32) isa SMatrix @test eltype(logm(q32)) == Float32 @test expm(logm(q32)) ≈ q32 From e0dc7876695aeacd6871b72e95fb348784e9bb61 Mon Sep 17 00:00:00 2001 From: hyrodium Date: Thu, 28 Oct 2021 16:32:02 +0900 Subject: [PATCH 03/22] fix log method for Rotation{2} --- src/log.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/log.jl b/src/log.jl index 56465f89..9a87a0e9 100644 --- a/src/log.jl +++ b/src/log.jl @@ -13,7 +13,7 @@ end # 2d function Base.log(R::Angle2d) - θ = params(R) + θ, = params(R) return @SMatrix [0 -θ θ 0] end From 8324da6a370f4caec78ca58b0344a420338025ac Mon Sep 17 00:00:00 2001 From: hyrodium Date: Thu, 28 Oct 2021 16:32:42 +0900 Subject: [PATCH 04/22] update tests for logarithm function --- test/log.jl | 16 ++++------------ test/runtests.jl | 1 + 2 files changed, 5 insertions(+), 12 deletions(-) diff --git a/test/log.jl b/test/log.jl index 7e549d01..d5d6b00e 100644 --- a/test/log.jl +++ b/test/log.jl @@ -1,23 +1,15 @@ -@testset "log_3D" begin +@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) - oneaxis_types = (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)) - end -end - -@testset "log_2D" begin - all_types = (RotMatrix{2}, Angle2d) - - @testset "$(T)" for T in all_types, θ in 0.0:0.1:π - R = F(T) - @test R ≈ exp(log(R)) + @test log(R) isa SMatrix end end diff --git a/test/runtests.jl b/test/runtests.jl index 33904e2f..1441d421 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -29,6 +29,7 @@ include("quatmaps.jl") include("rotation_error.jl") include("distribution_tests.jl") include("eigen.jl") +include("log.jl") include("deprecated.jl") include(joinpath(@__DIR__, "..", "perf", "runbenchmarks.jl")) From 12fa6dfd8e19a89ddf0291a631c3140095d581e3 Mon Sep 17 00:00:00 2001 From: hyrodium Date: Fri, 29 Oct 2021 12:04:08 +0900 Subject: [PATCH 05/22] add dependency on Quaternions.jl --- Project.toml | 1 + src/Rotations.jl | 1 + 2 files changed, 2 insertions(+) diff --git a/Project.toml b/Project.toml index 3e7c84d1..61e921bf 100644 --- a/Project.toml +++ b/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" diff --git a/src/Rotations.jl b/src/Rotations.jl index 15b70c1c..d8424bf0 100644 --- a/src/Rotations.jl +++ b/src/Rotations.jl @@ -6,6 +6,7 @@ module Rotations using LinearAlgebra using StaticArrays using Random +using Quaternions import Statistics From 9f49e467535bc42492a502a96831e68b76528375 Mon Sep 17 00:00:00 2001 From: hyrodium Date: Fri, 29 Oct 2021 12:12:44 +0900 Subject: [PATCH 06/22] update exports for deprecated types --- src/Rotations.jl | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/Rotations.jl b/src/Rotations.jl index d8424bf0..f8f8b5c1 100644 --- a/src/Rotations.jl +++ b/src/Rotations.jl @@ -30,13 +30,16 @@ 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, ⊖, ⊕, From 4300706f02ccc0dcec66b1b2de28a9cbbcb0b4df Mon Sep 17 00:00:00 2001 From: hyrodium Date: Fri, 29 Oct 2021 12:15:14 +0900 Subject: [PATCH 07/22] update around pure_quaternion --- Project.toml | 3 +- src/mrps.jl | 2 +- src/rodrigues_params.jl | 2 +- src/unitquaternion.jl | 59 +++++++++++++++++++++++++++++----------- test/rodrigues_params.jl | 4 +-- test/runtests.jl | 1 + test/unitquat.jl | 19 +++++++------ 7 files changed, 60 insertions(+), 30 deletions(-) diff --git a/Project.toml b/Project.toml index 61e921bf..ae28cfe6 100644 --- a/Project.toml +++ b/Project.toml @@ -19,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"] diff --git a/src/mrps.jl b/src/mrps.jl index c390c422..0f73e6be 100644 --- a/src/mrps.jl +++ b/src/mrps.jl @@ -48,7 +48,7 @@ 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) +# 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 ~~~~~~~~~~~~~~~ # diff --git a/src/rodrigues_params.jl b/src/rodrigues_params.jl index 1b445705..30b42b4c 100644 --- a/src/rodrigues_params.jl +++ b/src/rodrigues_params.jl @@ -45,7 +45,7 @@ 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) +# 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 ~~~~~~~~~~~~~~~ # diff --git a/src/unitquaternion.jl b/src/unitquaternion.jl index a2f5c19b..01562dd0 100644 --- a/src/unitquaternion.jl +++ b/src/unitquaternion.jl @@ -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,10 @@ function UnitQuaternion(w,x,y,z, normalize::Bool = true) UnitQuaternion{eltype(types)}(w,x,y,z, normalize) end +function Quaternions.Quaternion(q::UnitQuaternion, normalize::Bool = true) + Quaternion(q.w,q.x,q.y,q.z, normalize) +end + # Pass in Vectors @inline function (::Type{Q})(q::AbstractVector, normalize::Bool = true) where Q <: UnitQuaternion check_length(q, 4) @@ -183,6 +188,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,6 +203,9 @@ 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))) @@ -207,16 +216,18 @@ end # ~~~~~~~~~~~~~~~ Math Operations ~~~~~~~~~~~~~~~ # # Inverses +# TODO: remove conj 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) # Norms -LinearAlgebra.norm(q::UnitQuaternion) = sqrt(q.w^2 + q.x^2 + q.y^2 + q.z^2) +# 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)) + n = inv(norm(params(q))) Q(q.w*n, q.x*n, q.y*n, q.z*n) end @@ -225,32 +236,32 @@ end # 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) +@inline _pure_quaternion(x::Real, y::Real, z::Real) = + Quaternion(zero(x), x, y, z, false) -function exp(q::Q) where Q <: UnitQuaternion +function exp(q::Q) where Q <: Quaternion θ = vecnorm(q) sθ,cθ = sincos(θ) - es = exp(q.w) + es = exp(q.s) M = es*sθ/θ - Q(es*cθ, q.x*M, q.y*M, q.z*M, false) + UnitQuaternion(es*cθ, q.v1*M, q.v2*M, q.v3*M, false) end 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 @@ -262,12 +273,12 @@ function _log_as_quat(q::Q, eps=1e-6) where Q <: UnitQuaternion 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_as_quat(q)) + return 2*vector(_log_as_quat(q)) end # Composition @@ -361,7 +372,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 +390,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 +414,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)) """ diff --git a/test/rodrigues_params.jl b/test/rodrigues_params.jl index f1f10758..33de7ee5 100644 --- a/test/rodrigues_params.jl +++ b/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) diff --git a/test/runtests.jl b/test/runtests.jl index 1441d421..0d3aad11 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -3,6 +3,7 @@ using LinearAlgebra using Rotations using StaticArrays using InteractiveUtils: subtypes +using Quaternions import Unitful import Random diff --git a/test/unitquat.jl b/test/unitquat.jl index c3ceba3a..97e26954 100644 --- a/test/unitquat.jl +++ b/test/unitquat.jl @@ -1,7 +1,7 @@ using ForwardDiff import Rotations: jacobian, ∇rotate, ∇composition1, ∇composition2 -import Rotations: kinematics, pure_quaternion, params +import Rotations: kinematics, _pure_quaternion, params import Rotations: vmat, rmult, lmult, hmat, tmat @testset "Unit Quaternions" begin @@ -24,9 +24,9 @@ import Rotations: vmat, rmult, lmult, hmat, tmat r = @SVector rand(3) r32 = SVector{3,Float32}(r) - @test pure_quaternion(r) isa UnitQuaternion{Float64} - @test pure_quaternion(r32) isa UnitQuaternion{Float32} - @test pure_quaternion(r).w == 0 + @test _pure_quaternion(r) isa Quaternion{Float64} + @test _pure_quaternion(r32) isa Quaternion{Float32} + @test real(_pure_quaternion(r)) == 0 @test UnitQuaternion{Float64}(1, 0, 0, 0) isa UnitQuaternion{Float64} @test UnitQuaternion{Float32}(1, 0, 0, 0) isa UnitQuaternion{Float32} @@ -52,7 +52,7 @@ import Rotations: vmat, rmult, lmult, hmat, tmat ϕ = inv(ExponentialMap())(q1) @test expm(ϕ * 2) ≈ q1 - q = Rotations.pure_quaternion(ϕ) + q = Rotations._pure_quaternion(ϕ) @test exp(q) ≈ q1 q = UnitQuaternion((@SVector [1, 2, 3, 4.0]), false) @@ -62,12 +62,13 @@ import Rotations: vmat, rmult, lmult, hmat, tmat # Axis-angle ϕ = 0.1 * @SVector [1, 0, 0] q = expm(ϕ) + @test q isa UnitQuaternion @test logm(expm(ϕ)) ≈ ϕ @test expm(logm(q1)) ≈ q1 @test rotation_angle(q) ≈ 0.1 @test rotation_axis(q) == [1, 0, 0] - @test norm(q1 * ExponentialMap()(ϕ)) ≈ 1 + @test norm(q1 * ExponentialMap()(ϕ)) ≈ √3 @test q1 ⊖ q2 isa StaticVector{3} @test (q1 * CayleyMap()(ϕ)) ⊖ q1 ≈ ϕ @@ -81,7 +82,7 @@ import Rotations: vmat, rmult, lmult, hmat, tmat @test q3 ⊖ q2 ≈ inv(CayleyMap())(q1) q = q1 - rhat = Rotations.pure_quaternion(r) + rhat = Rotations._pure_quaternion(r) @test q * r ≈ vmat() * lmult(q) * rmult(q)' * vmat()'r @test q * r ≈ vmat() * lmult(q) * rmult(q)' * hmat(r) @test q * r ≈ vmat() * lmult(q) * lmult(rhat) * tmat() * params(q) @@ -90,7 +91,7 @@ import Rotations: vmat, rmult, lmult, hmat, tmat @test rmult(params(q)) == rmult(q) @test lmult(params(q)) == lmult(q) - @test hmat(r) == params(pure_quaternion(r)) + @test hmat(r) == params(_pure_quaternion(r)) # Test Jacobians @test ForwardDiff.jacobian(q -> UnitQuaternion(q, false) * r, params(q)) ≈ @@ -125,7 +126,7 @@ import Rotations: vmat, rmult, lmult, hmat, tmat @test expm(SA_F32[1, 2, 3]) isa UnitQuaternion{Float32} q32 = rand(UnitQuaternion{Float32}) - @test Rotations._log_as_quat(q32) isa UnitQuaternion{Float32} + @test Rotations._log_as_quat(q32) isa Quaternion{Float32} @test log(q32) isa SMatrix @test eltype(logm(q32)) == Float32 @test expm(logm(q32)) ≈ q32 From 49f4bd45022b26e14f89ce9ae7be5a3b0b17f446 Mon Sep 17 00:00:00 2001 From: hyrodium Date: Fri, 29 Oct 2021 12:21:31 +0900 Subject: [PATCH 08/22] remove incorrect LinearAlgebra.norm --- src/mrps.jl | 1 - src/rodrigues_params.jl | 1 - src/unitquaternion.jl | 1 - 3 files changed, 3 deletions(-) diff --git a/src/mrps.jl b/src/mrps.jl index 0f73e6be..784cfdaa 100644 --- a/src/mrps.jl +++ b/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 ~~~~~~~~~~~~~~~ # diff --git a/src/rodrigues_params.jl b/src/rodrigues_params.jl index 30b42b4c..bb54f22d 100644 --- a/src/rodrigues_params.jl +++ b/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 ~~~~~~~~~~~~~~~ # diff --git a/src/unitquaternion.jl b/src/unitquaternion.jl index 01562dd0..4a093740 100644 --- a/src/unitquaternion.jl +++ b/src/unitquaternion.jl @@ -222,7 +222,6 @@ inv(q::UnitQuaternion) = conj(q) (-)(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) From 67f4f1affd97fa54dcd7c6edea232deb46a2d38e Mon Sep 17 00:00:00 2001 From: hyrodium Date: Fri, 29 Oct 2021 12:21:54 +0900 Subject: [PATCH 09/22] remove incorrect *(::Real, UnitQuaternion) --- src/unitquaternion.jl | 12 ------------ test/unitquat.jl | 4 ++-- 2 files changed, 2 insertions(+), 14 deletions(-) diff --git a/src/unitquaternion.jl b/src/unitquaternion.jl index 4a093740..8f6ce6b8 100644 --- a/src/unitquaternion.jl +++ b/src/unitquaternion.jl @@ -315,18 +315,6 @@ 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) = conj(q1)*q2 # Equivalent to inv(q1)*q2 (/)(q1::UnitQuaternion, q2::UnitQuaternion) = q1*conj(q2) # Equivalent to q1*inv(q2) diff --git a/test/unitquat.jl b/test/unitquat.jl index 97e26954..19791e94 100644 --- a/test/unitquat.jl +++ b/test/unitquat.jl @@ -56,8 +56,8 @@ import Rotations: vmat, rmult, lmult, hmat, tmat @test exp(q) ≈ q1 q = UnitQuaternion((@SVector [1, 2, 3, 4.0]), false) - @test 2 * q == UnitQuaternion((@SVector [2, 4, 6, 8.0]), false) - @test q * 2 == UnitQuaternion((@SVector [2, 4, 6, 8.0]), false) + @test 2 * q == 2 * Matrix(q) + @test q * 2 == 2 * Matrix(q) # Axis-angle ϕ = 0.1 * @SVector [1, 0, 0] From c78ca5ac7dfda0084e1d7ff10f7951a0170fe284 Mon Sep 17 00:00:00 2001 From: hyrodium Date: Fri, 29 Oct 2021 12:25:48 +0900 Subject: [PATCH 10/22] remove incorrect conj(Quaternion) --- src/unitquaternion.jl | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/src/unitquaternion.jl b/src/unitquaternion.jl index 8f6ce6b8..9b005f0e 100644 --- a/src/unitquaternion.jl +++ b/src/unitquaternion.jl @@ -1,4 +1,4 @@ -import Base: +, -, *, /, \, exp, ≈, ==, inv, conj +import Base: +, -, *, /, \, exp, ≈, ==, inv """ UnitQuaternion{T} <: Rotation @@ -216,9 +216,7 @@ end # ~~~~~~~~~~~~~~~ Math Operations ~~~~~~~~~~~~~~~ # # Inverses -# TODO: remove conj -conj(q::Q) where Q <: UnitQuaternion = Q(q.w, -q.x, -q.y, -q.z, false) -inv(q::UnitQuaternion) = conj(q) +inv(q::Q) where Q <: UnitQuaternion = Q(q.w, -q.x, -q.y, -q.z, false) (-)(q::Q) where Q <: UnitQuaternion = Q(-q.w, -q.x, -q.y, -q.z, false) # Norms @@ -315,10 +313,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 -(\)(q1::UnitQuaternion, q2::UnitQuaternion) = conj(q1)*q2 # Equivalent to inv(q1)*q2 -(/)(q1::UnitQuaternion, q2::UnitQuaternion) = q1*conj(q2) # Equivalent to q1*inv(q2) +(\)(q1::UnitQuaternion, q2::UnitQuaternion) = inv(q1)*q2 # Equivalent to inv(q1)*q2 +(/)(q1::UnitQuaternion, q2::UnitQuaternion) = q1*inv(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) From 2a5b7a6c3343c93daef44c638eb0669cf2008285 Mon Sep 17 00:00:00 2001 From: hyrodium Date: Fri, 29 Oct 2021 12:32:55 +0900 Subject: [PATCH 11/22] remove incorrect -(::UnitQuaternion) --- src/unitquaternion.jl | 1 - 1 file changed, 1 deletion(-) diff --git a/src/unitquaternion.jl b/src/unitquaternion.jl index 9b005f0e..6962e986 100644 --- a/src/unitquaternion.jl +++ b/src/unitquaternion.jl @@ -217,7 +217,6 @@ end # Inverses inv(q::Q) where Q <: UnitQuaternion = Q(q.w, -q.x, -q.y, -q.z, false) -(-)(q::Q) where Q <: UnitQuaternion = Q(-q.w, -q.x, -q.y, -q.z, false) # Norms vecnorm(q::UnitQuaternion) = sqrt(q.x^2 + q.y^2 + q.z^2) From ff130683ac38475d692abc6dbbfbbc3dbe2c2ffc Mon Sep 17 00:00:00 2001 From: hyrodium Date: Fri, 29 Oct 2021 12:48:02 +0900 Subject: [PATCH 12/22] update tests around norm related functions --- test/2d.jl | 20 +++++++++++++++++++- test/rotation_tests.jl | 20 +++++++++++++++++++- 2 files changed, 38 insertions(+), 2 deletions(-) diff --git a/test/2d.jl b/test/2d.jl index 6e8a6bf3..afd4067d 100644 --- a/test/2d.jl +++ b/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,24 @@ 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)) + @test normalize(r) ≈ normalize(Matrix(r)) + @test normalize(r) isa SMatrix + end + end + end + ######################################################################### # Rotate some stuff ######################################################################### diff --git a/test/rotation_tests.jl b/test/rotation_tests.jl index de614b07..227ea035 100644 --- a/test/rotation_tests.jl +++ b/test/rotation_tests.jl @@ -129,7 +129,7 @@ all_types = (RotMatrix{3}, AngleAxis, RotationVec, # check on the inverse function ################################ - @testset "Testing inverse()" begin + @testset "Testing inv()" begin repeats = 100 I = one(RotMatrix{3,Float64}) @testset "$(R)" for R in all_types @@ -149,6 +149,24 @@ all_types = (RotMatrix{3}, AngleAxis, RotationVec, end end + ################################ + # check on the norm functions + ################################ + + @testset "Testing norm() and normalize()" begin + repeats = 100 + for R in all_types + I = one(R) + Random.seed!(0) + for i = 1:repeats + r = rand(R) + @test norm(r) ≈ norm(Matrix(r)) + @test normalize(r) ≈ normalize(Matrix(r)) + @test normalize(r) isa SMatrix + end + end + end + ######################################################################### # Rotate some stuff ######################################################################### From 7613de0bc223a40a388bf1e876639bf67cd8dac2 Mon Sep 17 00:00:00 2001 From: hyrodium Date: Fri, 29 Oct 2021 12:55:02 +0900 Subject: [PATCH 13/22] update src and test around norm related functions --- src/unitquaternion.jl | 8 ++++---- test/unitquat.jl | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/unitquaternion.jl b/src/unitquaternion.jl index 6962e986..17b09670 100644 --- a/src/unitquaternion.jl +++ b/src/unitquaternion.jl @@ -208,7 +208,7 @@ Rotations.params(p) == @SVector [1.0, 2.0, 3.0] # true # ~~~~~~~~~~~~~~~ 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) @@ -222,9 +222,9 @@ inv(q::Q) where Q <: UnitQuaternion = Q(q.w, -q.x, -q.y, -q.z, false) 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(params(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 diff --git a/test/unitquat.jl b/test/unitquat.jl index 19791e94..21101946 100644 --- a/test/unitquat.jl +++ b/test/unitquat.jl @@ -131,7 +131,7 @@ import Rotations: vmat, rmult, lmult, hmat, tmat @test eltype(logm(q32)) == Float32 @test expm(logm(q32)) ≈ q32 - @test normalize(q32) isa UnitQuaternion{Float32} + @test normalize(q32) isa SMatrix{3,3,Float32} ω = @SVector rand(3) ω32 = Float32.(ω) From 983286ba9116aa5da13a79497099a19604320cff Mon Sep 17 00:00:00 2001 From: hyrodium Date: Fri, 29 Oct 2021 17:52:05 +0900 Subject: [PATCH 14/22] remove unnecessary import from Base --- src/unitquaternion.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/unitquaternion.jl b/src/unitquaternion.jl index 17b09670..5a91dfdf 100644 --- a/src/unitquaternion.jl +++ b/src/unitquaternion.jl @@ -1,4 +1,4 @@ -import Base: +, -, *, /, \, exp, ≈, ==, inv +import Base: *, /, \, exp, ≈, ==, inv """ UnitQuaternion{T} <: Rotation From 4b630d0429acef91923ee293f6952278a58e46cd Mon Sep 17 00:00:00 2001 From: hyrodium Date: Fri, 29 Oct 2021 18:01:22 +0900 Subject: [PATCH 15/22] remove exp method for Quaternion --- src/unitquaternion.jl | 8 -------- test/unitquat.jl | 3 ++- 2 files changed, 2 insertions(+), 9 deletions(-) diff --git a/src/unitquaternion.jl b/src/unitquaternion.jl index 5a91dfdf..ea74612e 100644 --- a/src/unitquaternion.jl +++ b/src/unitquaternion.jl @@ -245,14 +245,6 @@ end @inline _pure_quaternion(x::Real, y::Real, z::Real) = Quaternion(zero(x), x, y, z, false) -function exp(q::Q) where Q <: Quaternion - θ = vecnorm(q) - sθ,cθ = sincos(θ) - es = exp(q.s) - M = es*sθ/θ - UnitQuaternion(es*cθ, q.v1*M, q.v2*M, q.v3*M, false) -end - function expm(ϕ::AbstractVector) check_length(ϕ, 3) θ = norm(ϕ) diff --git a/test/unitquat.jl b/test/unitquat.jl index 21101946..5438c918 100644 --- a/test/unitquat.jl +++ b/test/unitquat.jl @@ -53,7 +53,8 @@ import Rotations: vmat, rmult, lmult, hmat, tmat ϕ = inv(ExponentialMap())(q1) @test expm(ϕ * 2) ≈ q1 q = Rotations._pure_quaternion(ϕ) - @test exp(q) ≈ q1 + @test UnitQuaternion(exp(q)) ≈ q1 + @test exp(q) isa Quaternion q = UnitQuaternion((@SVector [1, 2, 3, 4.0]), false) @test 2 * q == 2 * Matrix(q) From 666a4cbd996f84deb12f0a96d86f2d9cdbae46f6 Mon Sep 17 00:00:00 2001 From: hyrodium Date: Fri, 29 Oct 2021 18:11:18 +0900 Subject: [PATCH 16/22] update type conversions UnitQuaternion <-> Quaternion --- src/unitquaternion.jl | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/unitquaternion.jl b/src/unitquaternion.jl index ea74612e..2cda1266 100644 --- a/src/unitquaternion.jl +++ b/src/unitquaternion.jl @@ -42,8 +42,16 @@ function UnitQuaternion(w,x,y,z, normalize::Bool = true) UnitQuaternion{eltype(types)}(w,x,y,z, normalize) end -function Quaternions.Quaternion(q::UnitQuaternion, normalize::Bool = true) - Quaternion(q.w,q.x,q.y,q.z, normalize) +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 From ca48fc5ae8cf0c0cbc390246e9b2200bdf368dd4 Mon Sep 17 00:00:00 2001 From: hyrodium Date: Fri, 29 Oct 2021 18:42:42 +0900 Subject: [PATCH 17/22] fix tests for Julia under v1.5 --- test/2d.jl | 6 ++++-- test/rotation_tests.jl | 6 ++++-- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/test/2d.jl b/test/2d.jl index afd4067d..658d61f8 100644 --- a/test/2d.jl +++ b/test/2d.jl @@ -79,8 +79,10 @@ using Rotations, StaticArrays, Test for i = 1:repeats r = rand(R) @test norm(r) ≈ norm(Matrix(r)) - @test normalize(r) ≈ normalize(Matrix(r)) - @test normalize(r) isa SMatrix + if VERSION ≥ v"1.5" + @test normalize(r) ≈ normalize(Matrix(r)) + @test normalize(r) isa SMatrix + end end end end diff --git a/test/rotation_tests.jl b/test/rotation_tests.jl index 227ea035..7a57283c 100644 --- a/test/rotation_tests.jl +++ b/test/rotation_tests.jl @@ -161,8 +161,10 @@ all_types = (RotMatrix{3}, AngleAxis, RotationVec, for i = 1:repeats r = rand(R) @test norm(r) ≈ norm(Matrix(r)) - @test normalize(r) ≈ normalize(Matrix(r)) - @test normalize(r) isa SMatrix + if VERSION ≥ v"1.5" + @test normalize(r) ≈ normalize(Matrix(r)) + @test normalize(r) isa SMatrix + end end end end From ea6f60043c5a4e629360690639917972d8756729 Mon Sep 17 00:00:00 2001 From: hyrodium Date: Sun, 31 Oct 2021 22:40:04 +0900 Subject: [PATCH 18/22] remove unnecessary lines --- src/unitquaternion.jl | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/unitquaternion.jl b/src/unitquaternion.jl index 2cda1266..8791768a 100644 --- a/src/unitquaternion.jl +++ b/src/unitquaternion.jl @@ -228,7 +228,6 @@ inv(q::Q) where Q <: UnitQuaternion = Q(q.w, -q.x, -q.y, -q.z, false) # Norms 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 _normalize(q::Q) where Q <: UnitQuaternion n = norm(params(q)) @@ -315,8 +314,6 @@ end (\)(q1::UnitQuaternion, q2::UnitQuaternion) = inv(q1)*q2 # Equivalent to inv(q1)*q2 (/)(q1::UnitQuaternion, q2::UnitQuaternion) = q1*inv(q2) # Equivalent to q1*inv(q2) -(\)(q::UnitQuaternion, r::SVector{3}) = inv(q)*r # Equivalent to inv(q)*r - """ rotation_between(from, to) From 09e03135986c7e3dcacf7425f2d0e19b94867ba1 Mon Sep 17 00:00:00 2001 From: hyrodium Date: Sun, 31 Oct 2021 22:59:22 +0900 Subject: [PATCH 19/22] remove vecnorm(q::UnitQuaternion) function --- src/unitquaternion.jl | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/unitquaternion.jl b/src/unitquaternion.jl index 8791768a..0af7b8ac 100644 --- a/src/unitquaternion.jl +++ b/src/unitquaternion.jl @@ -226,9 +226,6 @@ end # Inverses inv(q::Q) where Q <: UnitQuaternion = Q(q.w, -q.x, -q.y, -q.z, false) -# Norms -vecnorm(q::UnitQuaternion) = sqrt(q.x^2 + q.y^2 + q.z^2) - function _normalize(q::Q) where Q <: UnitQuaternion n = norm(params(q)) Q(q.w/n, q.x/n, q.y/n, q.z/n) @@ -262,7 +259,7 @@ end function _log_as_quat(q::Q, eps=1e-6) where Q <: UnitQuaternion # Assumes unit quaternion - θ = vecnorm(q) + θ = sqrt(q.x^2 + q.y^2 + q.z^2) if θ > eps M = atan(θ, q.w)/θ else From cfb4b5a275ef1810cde1396a85739101aeb5de3a Mon Sep 17 00:00:00 2001 From: hyrodium Date: Sun, 31 Oct 2021 23:14:11 +0900 Subject: [PATCH 20/22] update comments --- src/unitquaternion.jl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/unitquaternion.jl b/src/unitquaternion.jl index 0af7b8ac..8444c497 100644 --- a/src/unitquaternion.jl +++ b/src/unitquaternion.jl @@ -308,8 +308,8 @@ 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 -(\)(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) = inv(q1)*q2 +(/)(q1::UnitQuaternion, q2::UnitQuaternion) = q1*inv(q2) """ rotation_between(from, to) From 9647c8476a848199b124dc20807dc956bc9f1102 Mon Sep 17 00:00:00 2001 From: hyrodium Date: Sun, 31 Oct 2021 23:30:58 +0900 Subject: [PATCH 21/22] add methods for Base.:\(r::Rotation, v) --- src/core_types.jl | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/core_types.jl b/src/core_types.jl index 0a02acbf..b24b9591 100644 --- a/src/core_types.jl +++ b/src/core_types.jl @@ -78,6 +78,15 @@ end inv(r1) * r2 end +@inline function Base.:\(r::Rotation, v::AbstractVector) + inv(r) * v +end + +# This definition is for avoiding anbiguity +@inline function Base.:\(r::Rotation, v::StaticVector) + inv(r) * v +end + ################################################################################ ################################################################################ """ From de9cb6cb19deb8cf3cda76a71ce02dce6538424b Mon Sep 17 00:00:00 2001 From: hyrodium Date: Sun, 7 Nov 2021 20:41:20 +0900 Subject: [PATCH 22/22] update UnitQuaternion constructor --- src/unitquaternion.jl | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/unitquaternion.jl b/src/unitquaternion.jl index 8444c497..43e51cc0 100644 --- a/src/unitquaternion.jl +++ b/src/unitquaternion.jl @@ -31,8 +31,14 @@ struct UnitQuaternion{T} <: Rotation{3,T} end end + @inline function UnitQuaternion{T}(q::Quaternion) where T + if q.norm + new{T}(q.s, q.v1, q.v2, q.v3) + else + throw(InexactError(nameof(T), T, q)) + end + 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 ~~~~~~~~~~~~~~~ # @@ -44,7 +50,7 @@ end function UnitQuaternion(q::T) where T<:Quaternion if q.norm - return UnitQuaternion(q.s, q.v1, q.v2, q.v3) + return UnitQuaternion(q.s, q.v1, q.v2, q.v3, false) else throw(InexactError(nameof(T), T, q)) end