From 10ebb178a6a814dd7846337e7e6191bd9bf2db7f Mon Sep 17 00:00:00 2001 From: hyrodium Date: Tue, 16 Nov 2021 22:46:45 +0900 Subject: [PATCH 1/7] rename UnitQuaternion to QuatRotation --- README.md | 20 +++--- docs/src/3d_quaternion.md | 12 ++-- docs/src/functions.md | 6 +- docs/src/index.md | 2 +- docs/src/rotation_types.md | 2 +- perf/runbenchmarks.jl | 2 +- src/Rotations.jl | 2 +- src/angleaxis_types.jl | 52 +++++++-------- src/core_types.jl | 2 +- src/deprecated.jl | 4 +- src/derivatives.jl | 16 ++--- src/error_maps.jl | 52 +++++++-------- src/mean.jl | 6 +- src/mrps.jl | 16 ++--- src/principal_value.jl | 12 ++-- src/rodrigues_params.jl | 16 ++--- src/rotation_error.jl | 6 +- src/unitquaternion.jl | 118 +++++++++++++++++----------------- test/derivative_tests.jl | 32 ++++----- test/distribution_tests.jl | 2 +- test/eigen.jl | 2 +- test/log.jl | 2 +- test/principal_value_tests.jl | 4 +- test/quatmaps.jl | 4 +- test/rodrigues_params.jl | 4 +- test/rotation_error.jl | 8 +-- test/rotation_tests.jl | 36 +++++------ test/unitquat.jl | 62 +++++++++--------- 28 files changed, 251 insertions(+), 251 deletions(-) diff --git a/README.md b/README.md index 24a4a960..8ef3ea13 100644 --- a/README.md +++ b/README.md @@ -56,15 +56,15 @@ rotation_angle(r::Rotation) #### Initialization All rotation types support `one(R)` to construct the identity rotation for the desired parameterization. A random rotation, uniformly sampled over the space of rotations, can be sampled using `rand(R)`. For example: ```julia -r = one(UnitQuaternion) # equivalent to Quat(1.0, 0.0, 0.0, 0.0) -q = rand(UnitQuaternion) +r = one(QuatRotation) # equivalent to Quat(1.0, 0.0, 0.0, 0.0) +q = rand(QuatRotation) p = rand(MRP{Float32}) ``` #### Conversion All rotatations can be converted to another parameterization by simply calling the constructor for the desired parameterization. For example: ```julia -q = rand(UnitQuaternion) +q = rand(QuatRotation) aa = AngleAxis(q) r = RotMatrix(aa) ``` @@ -84,11 +84,11 @@ r = rand(RotMatrix{3}) # uses Float64 by default p = SVector(1.0, 2.0, 3.0) # from StaticArrays.jl, but could use any AbstractVector... # convert to a quaternion (Quat) and rotate the point -q = UnitQuaternion(r) +q = QuatRotation(r) p_rotated = q * p # Compose rotations -q2 = rand(UnitQuaternion) +q2 = rand(QuatRotation) q3 = q * q2 # Take the inverse (equivalent to transpose) @@ -145,12 +145,12 @@ j2 = Rotations.jacobian(q, p) # How does the rotated point q*p change w.r.t. the renormalized by the constructor to be a unit vector, so that `theta` always represents the rotation angle in radians. -3. **Quaternions** `UnitQuaternion{T}` +3. **Quaternions** `QuatRotation{T}` A 3D rotation parameterized by a unit quaternion. Note that the constructor will renormalize the quaternion to be a unit quaternion, and that although they follow the same multiplicative *algebra* as quaternions, it is better - to think of `UnitQuaternion` as a 3×3 matrix rather than as a quaternion *number*. + to think of `QuatRotation` as a 3×3 matrix rather than as a quaternion *number*. Previously `Quat`. @@ -219,7 +219,7 @@ differential unit quaternion. This mapping goes singular at 360°. differential unit quaternion. This mapping also goes singular at 180° but is the computationally cheapest map and often performs well. -Rotations.jl provides the `RotationError` type for representing rotation errors, that act just like a `SVector{3}` but carry the nonlinear map used to compute the error, which can also be used to convert the error back to a `UnitQuaternion` (and, by extention, any other 3D rotation parameterization). The following methods are useful for computing `RotationError`s and adding them back to the nominal rotation: +Rotations.jl provides the `RotationError` type for representing rotation errors, that act just like a `SVector{3}` but carry the nonlinear map used to compute the error, which can also be used to convert the error back to a `QuatRotation` (and, by extention, any other 3D rotation parameterization). The following methods are useful for computing `RotationError`s and adding them back to the nominal rotation: ```julia rotation_error(R1::Rotation, R2::Rotation, error_map::ErrorMap) # compute the error between `R1` and `R2` using `error_map` add_error(R::Rotation, err::RotationError) # "adds" the error to `R` by converting back a `UnitQuaterion` and composing with `R` @@ -232,7 +232,7 @@ R1 ⊕ err # alias for `add_error(R1, err)` For a practical application of these ideas, see the quatenrion-multiplicative Extended Kalman Filter (MEKF). [This article](https://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/20040037784.pdf) provides a good description. -When taking derivatives with respect to quaternions we need to account both for these mappings and the fact that local perturbations to a rotation act through composition instead of addition, as they do in vector space (e.g. `q * dq` vs `x + dx`). The following methods are useful for computing these Jacobians for `UnitQuaternion`, `RodriguesParam` or `MRP` +When taking derivatives with respect to quaternions we need to account both for these mappings and the fact that local perturbations to a rotation act through composition instead of addition, as they do in vector space (e.g. `q * dq` vs `x + dx`). The following methods are useful for computing these Jacobians for `QuatRotation`, `RodriguesParam` or `MRP` * `∇rotate(q,r)`: Jacobian of the `q*r` with respect to the rotation * `∇composition1(q2,q1)`: Jacobian of `q2*q1` with respect to q1 * `∇composition2(q2,q1,b)`: Jacobian of `q2*q1` with respect to q2 @@ -251,7 +251,7 @@ All parameterizations can be converted to and from (mutable or immutable) using StaticArrays, Rotations # export -q = UnitQuaternion(1.0,0,0,0) +q = QuatRotation(1.0,0,0,0) matrix_mutable = Array(q) matrix_immutable = SMatrix{3,3}(q) diff --git a/docs/src/3d_quaternion.md b/docs/src/3d_quaternion.md index 0841e2c7..11959e3d 100644 --- a/docs/src/3d_quaternion.md +++ b/docs/src/3d_quaternion.md @@ -4,9 +4,9 @@ using Rotations ``` -## `UnitQuaternion` +## `QuatRotation` A 3D rotation parameterized by a unit [quaternion](https://en.wikipedia.org/wiki/Quaternion) ([versor](https://en.wikipedia.org/wiki/Versor)). -Note that the constructor will renormalize the quaternion to be a unit quaternion, and that although they follow the same multiplicative *algebra* as quaternions, it is better to think of `UnitQuaternion` as a ``3 \times 3`` matrix rather than as a quaternion *number*. +Note that the constructor will renormalize the quaternion to be a unit quaternion, and that although they follow the same multiplicative *algebra* as quaternions, it is better to think of `QuatRotation` as a ``3 \times 3`` matrix rather than as a quaternion *number*. ```math \begin{aligned} @@ -17,11 +17,11 @@ Note that the constructor will renormalize the quaternion to be a unit quaternio **example** ```@repl quaternions -one(UnitQuaternion) # null rotation +one(QuatRotation) # null rotation α, β, γ = 1.2, -0.8, 0.1; -RotX(α) ≈ UnitQuaternion(cos(α/2),sin(α/2),0,0) # These matrices are equal -RotY(β) ≈ UnitQuaternion(cos(β/2),0,sin(β/2),0) # These matrices are equal -RotZ(γ) ≈ UnitQuaternion(cos(γ/2),0,0,sin(γ/2)) # These matrices are equal +RotX(α) ≈ QuatRotation(cos(α/2),sin(α/2),0,0) # These matrices are equal +RotY(β) ≈ QuatRotation(cos(β/2),0,sin(β/2),0) # These matrices are equal +RotZ(γ) ≈ QuatRotation(cos(γ/2),0,0,sin(γ/2)) # These matrices are equal ``` ## `RodriguesParam` diff --git a/docs/src/functions.md b/docs/src/functions.md index edd17b28..c625d47c 100644 --- a/docs/src/functions.md +++ b/docs/src/functions.md @@ -35,7 +35,7 @@ Rotations.params(RotYZY(R)) # Proper Euler angles, (y,z,y) Rotations.params(RotXYZ(R)) # Tait–Bryan angles, (y,z,y) Rotations.params(AngleAxis(R)) # Rotation around an axis (theta, axis_x, axis_y, axis_z) Rotations.params(RotationVec(R)) # Rotation vector (v_x, v_y, v_z) -Rotations.params(UnitQuaternion(R)) # Quaternion (w, x, y, z) +Rotations.params(QuatRotation(R)) # Quaternion (w, x, y, z) Rotations.params(RodriguesParam(R)) # Rodrigues Parameters (x, y, z) Rotations.params(MRP(R)) # Modified Rodrigues Parameters (x, y, z) ``` @@ -93,7 +93,7 @@ The following types have an algebraic structure that is homomorphic to ``SO(3)`` * `RotXYZ` (and other Euler angles) * `AngleAxis` * `RotationVec` -* `UnitQuaternion` +* `QuatRotation` * `RodriguesParam` * `MRP` @@ -118,4 +118,4 @@ Note that: * `rand(RotX)` is same as `RotX(2π*rand())`. * `rand(RotXY)` is same as `RotXY(2π*rand(), 2π*rand())`. * `rand(RotXYZ)` is **not** same as `RotXYZ(2π*rand(), 2π*rand(), 2π*rand())`. -* But `rand(RotXYZ)` is same as `RotXYZ(rand(UnitQuaternion))`. +* But `rand(RotXYZ)` is same as `RotXYZ(rand(QuatRotation))`. diff --git a/docs/src/index.md b/docs/src/index.md index 9c781349..f37f9df8 100644 --- a/docs/src/index.md +++ b/docs/src/index.md @@ -27,7 +27,7 @@ R_euler = RotXYZ(1,2,3) rotation_angle(R_euler), rotation_axis(R_euler) # Convert the rotation to unit quaternion -R_quat = UnitQuaternion(R_euler) +R_quat = QuatRotation(R_euler) # Get quaternion parameters of the rotation Rotations.params(R_quat) diff --git a/docs/src/rotation_types.md b/docs/src/rotation_types.md index 5c18d5f3..5b4003fa 100644 --- a/docs/src/rotation_types.md +++ b/docs/src/rotation_types.md @@ -43,7 +43,7 @@ For more information, see the sidebar page. * Rotation around given axis and angle. * `RotationVec` * Rotation around given axis. The length of axis vector represents its angle. -* `UnitQuaternion` +* `QuatRotation` * A 3D rotation parameterized by a unit quaternion. * `MRP` * A 3D rotation encoded by the stereographic projection of a unit quaternion. diff --git a/perf/runbenchmarks.jl b/perf/runbenchmarks.jl index 5ad2c233..5b1bb7e8 100644 --- a/perf/runbenchmarks.jl +++ b/perf/runbenchmarks.jl @@ -9,7 +9,7 @@ const suite = BenchmarkGroup() Random.seed!(1) noneuler = suite["Non-Euler conversions"] = BenchmarkGroup() -rotationtypes = [RotMatrix3{T}, UnitQuaternion{T}, MRP{T}, AngleAxis{T}, RotationVec{T}] +rotationtypes = [RotMatrix3{T}, QuatRotation{T}, MRP{T}, AngleAxis{T}, RotationVec{T}] for (from, to) in product(rotationtypes, rotationtypes) if from != to name = "$(string(from)) -> $(string(to))" diff --git a/src/Rotations.jl b/src/Rotations.jl index e6a549de..eb389749 100644 --- a/src/Rotations.jl +++ b/src/Rotations.jl @@ -32,7 +32,7 @@ export # Rotation types Rotation, RotMatrix, RotMatrix2, RotMatrix3, Angle2d, - UnitQuaternion, + QuatRotation, AngleAxis, RotationVec, RodriguesParam, MRP, RotX, RotY, RotZ, RotXY, RotYX, RotZX, RotXZ, RotYZ, RotZY, diff --git a/src/angleaxis_types.jl b/src/angleaxis_types.jl index 86702dac..479ad355 100644 --- a/src/angleaxis_types.jl +++ b/src/angleaxis_types.jl @@ -46,8 +46,8 @@ params(aa::AngleAxis) = SVector{4}(aa.theta, aa.axis_x, aa.axis_y, aa.axis_z) end # These functions are enough to satisfy the entire StaticArrays interface: -@inline (::Type{AA})(t::NTuple{9}) where {AA <: AngleAxis} = AA(UnitQuaternion(t)) # TODO: consider going directly from tuple (RotMatrix) to AngleAxis -@inline Base.getindex(aa::AngleAxis, i::Int) = UnitQuaternion(aa)[i] +@inline (::Type{AA})(t::NTuple{9}) where {AA <: AngleAxis} = AA(QuatRotation(t)) # TODO: consider going directly from tuple (RotMatrix) to AngleAxis +@inline Base.getindex(aa::AngleAxis, i::Int) = QuatRotation(aa)[i] @inline function Base.Tuple(aa::AngleAxis{T}) where T # Rodrigues' rotation formula. @@ -72,12 +72,12 @@ end c1xz + sy, c1yz - sx, one(T) - c1x2 - c1y2) end -@inline function (::Type{Q})(aa::AngleAxis) where Q <: UnitQuaternion +@inline function (::Type{Q})(aa::AngleAxis) where Q <: QuatRotation s, c = sincos(aa.theta / 2) return Q(c, s * aa.axis_x, s * aa.axis_y, s * aa.axis_z, false) end -@inline function (::Type{AA})(q::UnitQuaternion) where AA <: AngleAxis +@inline function (::Type{AA})(q::QuatRotation) where AA <: AngleAxis w = q.q.s x = q.q.v1 y = q.q.v2 @@ -118,13 +118,13 @@ function Base.:*(aa::AngleAxis, v::StaticVector) v[3] * ct + w_cross_pt[3] * st + w[3] * m) end -@inline Base.:*(aa::AngleAxis, r::Rotation) = UnitQuaternion(aa) * r -@inline Base.:*(aa::AngleAxis, r::RotMatrix) = UnitQuaternion(aa) * r -@inline Base.:*(aa::AngleAxis, r::MRP) = UnitQuaternion(aa) * r -@inline Base.:*(r::Rotation, aa::AngleAxis) = r * UnitQuaternion(aa) -@inline Base.:*(r::RotMatrix, aa::AngleAxis) = r * UnitQuaternion(aa) -@inline Base.:*(r::MRP, aa::AngleAxis) = r * UnitQuaternion(aa) -@inline Base.:*(aa1::AngleAxis, aa2::AngleAxis) = UnitQuaternion(aa1) * UnitQuaternion(aa2) +@inline Base.:*(aa::AngleAxis, r::Rotation) = QuatRotation(aa) * r +@inline Base.:*(aa::AngleAxis, r::RotMatrix) = QuatRotation(aa) * r +@inline Base.:*(aa::AngleAxis, r::MRP) = QuatRotation(aa) * r +@inline Base.:*(r::Rotation, aa::AngleAxis) = r * QuatRotation(aa) +@inline Base.:*(r::RotMatrix, aa::AngleAxis) = r * QuatRotation(aa) +@inline Base.:*(r::MRP, aa::AngleAxis) = r * QuatRotation(aa) +@inline Base.:*(aa1::AngleAxis, aa2::AngleAxis) = QuatRotation(aa1) * QuatRotation(aa2) @inline Base.inv(aa::AngleAxis) = AngleAxis(-aa.theta, aa.axis_x, aa.axis_y, aa.axis_z) @inline Base.:^(aa::AngleAxis, t::Real) = AngleAxis(aa.theta*t, aa.axis_x, aa.axis_y, aa.axis_z) @@ -163,9 +163,9 @@ params(aa::RotationVec) = SVector{3}(aa.sx, aa.sy, aa.sz) @inline RotationVec(x::X, y::Y, z::Z) where {X,Y,Z} = RotationVec{promote_type(promote_type(X, Y), Z)}(x, y, z) # These functions are enough to satisfy the entire StaticArrays interface: -@inline (::Type{RV})(t::NTuple{9}) where {RV <: RotationVec} = RV(UnitQuaternion(t)) # TODO: go through AngleAxis once it's faster -@inline Base.getindex(aa::RotationVec, i::Int) = UnitQuaternion(aa)[i] -@inline Base.Tuple(rv::RotationVec) = Tuple(UnitQuaternion(rv)) +@inline (::Type{RV})(t::NTuple{9}) where {RV <: RotationVec} = RV(QuatRotation(t)) # TODO: go through AngleAxis once it's faster +@inline Base.getindex(aa::RotationVec, i::Int) = QuatRotation(aa)[i] +@inline Base.Tuple(rv::RotationVec) = Tuple(QuatRotation(rv)) function (::Type{AA})(rv::RotationVec) where AA <: AngleAxis # TODO: consider how to deal with derivative near theta = 0. There should be a first-order expansion here. @@ -177,11 +177,11 @@ function (::Type{RV})(aa::AngleAxis) where RV <: RotationVec return RV(aa.theta * aa.axis_x, aa.theta * aa.axis_y, aa.theta * aa.axis_z) end -function (::Type{Q})(rv::RotationVec) where Q <: UnitQuaternion - return UnitQuaternion(AngleAxis(rv)) +function (::Type{Q})(rv::RotationVec) where Q <: QuatRotation + return QuatRotation(AngleAxis(rv)) end -(::Type{RV})(q::UnitQuaternion) where {RV <: RotationVec} = RV(AngleAxis(q)) +(::Type{RV})(q::QuatRotation) where {RV <: RotationVec} = RV(AngleAxis(q)) function Base.:*(rv::RotationVec{T1}, v::StaticVector{3, T2}) where {T1,T2} theta = rotation_angle(rv) @@ -195,15 +195,15 @@ function Base.:*(rv::RotationVec{T1}, v::StaticVector{3, T2}) where {T1,T2} end end -@inline Base.:*(rv::RotationVec, r::Rotation) = UnitQuaternion(rv) * r -@inline Base.:*(rv::RotationVec, r::RotMatrix) = UnitQuaternion(rv) * r -@inline Base.:*(rv::RotationVec, r::MRP) = UnitQuaternion(rv) * r -@inline Base.:*(rv::RotationVec, r::AngleAxis) = UnitQuaternion(rv) * r -@inline Base.:*(r::Rotation, rv::RotationVec) = r * UnitQuaternion(rv) -@inline Base.:*(r::RotMatrix, rv::RotationVec) = r * UnitQuaternion(rv) -@inline Base.:*(r::MRP, rv::RotationVec) = r * UnitQuaternion(rv) -@inline Base.:*(r::AngleAxis, rv::RotationVec) = r * UnitQuaternion(rv) -@inline Base.:*(rv1::RotationVec, rv2::RotationVec) = UnitQuaternion(rv1) * UnitQuaternion(rv2) +@inline Base.:*(rv::RotationVec, r::Rotation) = QuatRotation(rv) * r +@inline Base.:*(rv::RotationVec, r::RotMatrix) = QuatRotation(rv) * r +@inline Base.:*(rv::RotationVec, r::MRP) = QuatRotation(rv) * r +@inline Base.:*(rv::RotationVec, r::AngleAxis) = QuatRotation(rv) * r +@inline Base.:*(r::Rotation, rv::RotationVec) = r * QuatRotation(rv) +@inline Base.:*(r::RotMatrix, rv::RotationVec) = r * QuatRotation(rv) +@inline Base.:*(r::MRP, rv::RotationVec) = r * QuatRotation(rv) +@inline Base.:*(r::AngleAxis, rv::RotationVec) = r * QuatRotation(rv) +@inline Base.:*(rv1::RotationVec, rv2::RotationVec) = QuatRotation(rv1) * QuatRotation(rv2) @inline Base.inv(rv::RotationVec) = RotationVec(-rv.sx, -rv.sy, -rv.sz) @inline Base.:^(rv::RotationVec, t::Real) = RotationVec(rv.sx*t, rv.sy*t, rv.sz*t) diff --git a/src/core_types.jl b/src/core_types.jl index e921f4ab..3e0e274d 100644 --- a/src/core_types.jl +++ b/src/core_types.jl @@ -58,7 +58,7 @@ function Random.rand(rng::AbstractRNG, ::Random.SamplerType{R}) where R <: Rotat T = Float64 end - q = UnitQuaternion(randn(rng, T), randn(rng, T), randn(rng, T), randn(rng, T)) + q = QuatRotation(randn(rng, T), randn(rng, T), randn(rng, T), randn(rng, T)) return R(q) end diff --git a/src/deprecated.jl b/src/deprecated.jl index 01e3a6b7..f07d7072 100644 --- a/src/deprecated.jl +++ b/src/deprecated.jl @@ -2,8 +2,8 @@ # Deprecate RodriguesVec => RotationVec Base.@deprecate_binding RodriguesVec RotationVec true -# Deprecate Quat => UnitQuaternion -Base.@deprecate_binding Quat UnitQuaternion true +# Deprecate Quat => QuatRotation +Base.@deprecate_binding Quat QuatRotation true # Deprecate SPQuat => MRP Base.@deprecate_binding SPQuat MRP true diff --git a/src/derivatives.jl b/src/derivatives.jl index da33d638..288c2f76 100644 --- a/src/derivatives.jl +++ b/src/derivatives.jl @@ -30,7 +30,7 @@ to the output parameterization, centered at the value of R. jacobian(R::rotation_type, X::AbstractVector) Returns the jacobian for rotating the vector X by R. """ -function jacobian(::Type{RotMatrix}, q::UnitQuaternion) +function jacobian(::Type{RotMatrix}, q::QuatRotation) w = q.q.s x = q.q.v1 y = q.q.v2 @@ -82,10 +82,10 @@ end function jacobian(::Type{RotMatrix}, X::MRP) # get the derivatives of the quaternion w.r.t to the mrp - dQdX = jacobian(UnitQuaternion, X) + dQdX = jacobian(QuatRotation, X) # get the derivatives of the rotation matrix w.r.t to the mrp - dRdQ = jacobian(RotMatrix, UnitQuaternion(X)) + dRdQ = jacobian(RotMatrix, QuatRotation(X)) # and return return dRdQ * dQdX @@ -98,7 +98,7 @@ end # ####################################################### -function jacobian(::Type{UnitQuaternion}, X::MRP) +function jacobian(::Type{QuatRotation}, X::MRP) # differentiating # q = Quaternion((1-alpha2) / (alpha2 + 1), 2*X.x / (alpha2 + 1), 2*X.y / (alpha2 + 1), 2*X.z / (alpha2 + 1), true) @@ -137,7 +137,7 @@ end # # Jacobian converting from a Quaternion to an MRP # -function jacobian(::Type{MRP}, q::UnitQuaternion{T}) where T +function jacobian(::Type{MRP}, q::QuatRotation{T}) where T w = q.q.s x = q.q.v1 y = q.q.v2 @@ -183,7 +183,7 @@ end end # TODO: should this be jacobian(:rotate, q, X) # or something? -function jacobian(q::UnitQuaternion, X::AbstractVector) +function jacobian(q::QuatRotation, X::AbstractVector) w = q.q.s x = q.q.v1 y = q.q.v2 @@ -209,8 +209,8 @@ function jacobian(q::UnitQuaternion, X::AbstractVector) end function jacobian(spq::MRP, X::AbstractVector) - dQ = jacobian(UnitQuaternion, spq) - q = UnitQuaternion(spq) + dQ = jacobian(QuatRotation, spq) + q = QuatRotation(spq) return jacobian(q, X) * dQ end diff --git a/src/error_maps.jl b/src/error_maps.jl index fb1d38c4..e3b80289 100644 --- a/src/error_maps.jl +++ b/src/error_maps.jl @@ -6,13 +6,13 @@ A nonlinear mapping between the space of unit quaternions and three-dimensional rotation errors. These mappings are extremely useful for converting from globally nonsingular 3D rotation -representations such as `UnitQuaternion` or `RotMatrix3` to a three-parameter error that can +representations such as `QuatRotation` or `RotMatrix3` to a three-parameter error that can be efficiently used in gradient-based optimization methods that optimize deviations about a current iterate using first or second-order information. # Usage - errmap(v::AbstractVector) # "forward" map from a 3D error to a `UnitQuaternion` - inv(errmap)(R::Rotation) # "inverse" map from a rotation (via `UnitQuaternion`) to a 3D error + errmap(v::AbstractVector) # "forward" map from a 3D error to a `QuatRotation` + inv(errmap)(R::Rotation) # "inverse" map from a rotation (via `QuatRotation`) to a 3D error where `errmap <: ErrorMap` @@ -45,7 +45,7 @@ inverting an `ErrorMap`, i.e. # Usage imap(R::Rotation) # "inverse" map from a rotation to a 3D error - inv(imap)(v::AbstractVector) # "forward" map from a 3D error to a `UnitQuaternion` + inv(imap)(v::AbstractVector) # "forward" map from a 3D error to a `QuatRotation` where `imap <: InvErrorMap`. @@ -82,14 +82,14 @@ scaling(m::M) where M <: ErrorMap = scaling(M) function (::QuatVecMap)(v::AbstractVector) check_length(v, 3) μ = 1/scaling(QuatVecMap) - UnitQuaternion(sqrt(1-μ^2*v'v), μ*v[1], μ*v[2], μ*v[3]) + QuatRotation(sqrt(1-μ^2*v'v), μ*v[1], μ*v[2], μ*v[3]) end function (::CayleyMap)(g::AbstractVector) check_length(g, 3) g /= scaling(CayleyMap) M = 1/sqrt(1+g'g) - UnitQuaternion(M, M*g[1], M*g[2], M*g[3]) + QuatRotation(M, M*g[1], M*g[2], M*g[3]) end function (::MRPMap)(p::AbstractVector) @@ -97,12 +97,12 @@ function (::MRPMap)(p::AbstractVector) p /= scaling(MRPMap) n2 = p'p M = 2/(1+n2) - UnitQuaternion((1-n2)/(1+n2), M*p[1], M*p[2], M*p[3]) + QuatRotation((1-n2)/(1+n2), M*p[1], M*p[2], M*p[3]) end function (::IdentityMap)(q::AbstractVector) check_length(q, 3) - UnitQuaternion(q[1], q[2], q[3], q[4]) + QuatRotation(q[1], q[2], q[3], q[4]) end @@ -163,29 +163,29 @@ jacobian(::IdentityMap, q) = I ############################################################################################ # (emap::ErrorMap)(R::Rotation) = # throw(ArgumentError("Must use the inverse map to convert a rotation to an error state, e.g. inv(emap)(q)")) -# (emap::InvErrorMap)(R::Rotation) = emap(UnitQuaternion(R)) # automatically call the inverse map for rotations +# (emap::InvErrorMap)(R::Rotation) = emap(QuatRotation(R)) # automatically call the inverse map for rotations # NOTE: Julia v1.0 doesn't allow these methods on abstract types. Leave out for now. for imap in (InvExponentialMap,InvCayleyMap,InvMRPMap,InvQuatVecMap) @eval begin - @inline (imap::$imap)(R::Rotation) = imap(UnitQuaternion(R)) + @inline (imap::$imap)(R::Rotation) = imap(QuatRotation(R)) end end -(::InvExponentialMap)(q::UnitQuaternion) = scaling(ExponentialMap)*logm(q) -(::InvCayleyMap)(q::UnitQuaternion) = scaling(CayleyMap) * vector(q)/real(q.q) -(::InvMRPMap)(q::UnitQuaternion) = scaling(MRPMap)*vector(q)/(1+real(q.q)) -(::InvQuatVecMap)(q::UnitQuaternion) = scaling(QuatVecMap)*vector(q) * sign(real(q.q)) +(::InvExponentialMap)(q::QuatRotation) = scaling(ExponentialMap)*logm(q) +(::InvCayleyMap)(q::QuatRotation) = scaling(CayleyMap) * vector(q)/real(q.q) +(::InvMRPMap)(q::QuatRotation) = scaling(MRPMap)*vector(q)/(1+real(q.q)) +(::InvQuatVecMap)(q::QuatRotation) = scaling(QuatVecMap)*vector(q) * sign(real(q.q)) # ~~~~~~~~~~~~~~~ Inverse map Jacobians ~~~~~~~~~~~~~~~ # """ - jacobian(::InvErrorMap, q::UnitQuaternion) + jacobian(::InvErrorMap, q::QuatRotation) Jacobian of the inverse quaternion map, returning a 3×4 matrix. -For all maps: `jacobian(::InvErrorMap, UnitQuaternion(I)) = [0 I] = Hmat()'` +For all maps: `jacobian(::InvErrorMap, QuatRotation(I)) = [0 I] = Hmat()'` """ -function jacobian(::InvExponentialMap, q::UnitQuaternion, eps=1e-5) +function jacobian(::InvExponentialMap, q::QuatRotation, eps=1e-5) μ = scaling(ExponentialMap) s = scalar(q) v = vector(q) @@ -208,7 +208,7 @@ function jacobian(::InvExponentialMap, q::UnitQuaternion, eps=1e-5) end -function jacobian(::InvQuatVecMap, q::UnitQuaternion) +function jacobian(::InvQuatVecMap, q::QuatRotation) w = q.q.s μ = scaling(QuatVecMap) @@ -219,7 +219,7 @@ function jacobian(::InvQuatVecMap, q::UnitQuaternion) end -function jacobian(::InvCayleyMap, q::UnitQuaternion) +function jacobian(::InvCayleyMap, q::QuatRotation) w = q.q.s x = q.q.v1 y = q.q.v2 @@ -233,7 +233,7 @@ function jacobian(::InvCayleyMap, q::UnitQuaternion) end -function jacobian(::InvMRPMap, q::UnitQuaternion) +function jacobian(::InvMRPMap, q::QuatRotation) w = q.q.s x = q.q.v1 y = q.q.v2 @@ -247,17 +247,17 @@ function jacobian(::InvMRPMap, q::UnitQuaternion) end -jacobian(::IdentityMap, q::UnitQuaternion) = I +jacobian(::IdentityMap, q::QuatRotation) = I # ~~~~~~~~~~~~~~~ Inverse map Jacobian derivative ~~~~~~~~~~~~~~~ # """ - ∇jacobian(::InvErrorMap, q::UnitQuaternion, b::SVector{3}) + ∇jacobian(::InvErrorMap, q::QuatRotation, b::SVector{3}) Jacobian of G(q)'b, where G(q) = jacobian(::InvErrorMap, q), b is a 3-element vector """ -function ∇jacobian(::InvExponentialMap, q::UnitQuaternion, b::SVector{3}, eps=1e-5) +function ∇jacobian(::InvExponentialMap, q::QuatRotation, b::SVector{3}, eps=1e-5) μ = scaling(ExponentialMap) s = scalar(q) v = vector(q) @@ -296,7 +296,7 @@ function ∇jacobian(::InvExponentialMap, q::UnitQuaternion, b::SVector{3}, eps= end end -function ∇jacobian(::InvCayleyMap, q::UnitQuaternion, b::SVector{3}) +function ∇jacobian(::InvCayleyMap, q::QuatRotation, b::SVector{3}) w = q.q.s μ = scaling(CayleyMap) @@ -310,7 +310,7 @@ function ∇jacobian(::InvCayleyMap, q::UnitQuaternion, b::SVector{3}) ] end -function ∇jacobian(::InvMRPMap, q::UnitQuaternion, b::SVector{3}) +function ∇jacobian(::InvMRPMap, q::QuatRotation, b::SVector{3}) w = q.q.s μ = scaling(MRPMap) @@ -324,7 +324,7 @@ function ∇jacobian(::InvMRPMap, q::UnitQuaternion, b::SVector{3}) ] end -function ∇jacobian(::InvQuatVecMap, q::UnitQuaternion, b::SVector{3}) +function ∇jacobian(::InvQuatVecMap, q::QuatRotation, b::SVector{3}) μ = scaling(QuatVecMap) @SMatrix zeros(4,4) end diff --git a/src/mean.jl b/src/mean.jl index 92e36db3..1724c97f 100644 --- a/src/mean.jl +++ b/src/mean.jl @@ -29,7 +29,7 @@ Args: Find the mean `r̄` of a set of rotations `r`. The returned rotation minimizes `sum_i=1:n (||r[i] - r̄||)` with `||` the Frobenius norm, or equivalently minimizes `sum_i=1:n (sin(ϕ[i] / 2))^2`, where `ϕ[i] = rotation_angle(r[i] / r̄)`. """ -function Statistics.mean(qvec::AbstractVector{UnitQuaternion{T}}, method::Integer = 0) where T +function Statistics.mean(qvec::AbstractVector{QuatRotation{T}}, method::Integer = 0) where T #if (method == 0) M = zeros(4, 4) for i = 1:length(qvec) @@ -38,7 +38,7 @@ function Statistics.mean(qvec::AbstractVector{UnitQuaternion{T}}, method::Intege M .+= Qi * (Qi') end evec = eigfact(Symmetric(M), 4:4) - Qbar = UnitQuaternion(evec.vectors[1], evec.vectors[2], evec.vectors[3], evec.vectors[4]) # This will renormalize the quaternion... + Qbar = QuatRotation(evec.vectors[1], evec.vectors[2], evec.vectors[3], evec.vectors[4]) # This will renormalize the quaternion... #else # error("I haven't coded this") #end @@ -47,5 +47,5 @@ function Statistics.mean(qvec::AbstractVector{UnitQuaternion{T}}, method::Intege end function Statistics.mean(vec::AbstractVector{R}) where R<:Rotation - R(mean(convert(Vector{UnitQuaternion{eltype(R)}}, vec))) + R(mean(convert(Vector{QuatRotation{eltype(R)}}, vec))) end diff --git a/src/mrps.jl b/src/mrps.jl index c731f528..8f9215f7 100644 --- a/src/mrps.jl +++ b/src/mrps.jl @@ -26,18 +26,18 @@ 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, UnitQuaternion)) + RP(rand(rng, QuatRotation)) end Base.one(::Type{RP}) where RP <: MRP = RP(0.0, 0.0, 0.0) # ~~~~~~~~~~~~~~~~ Quaternion <=> MRP ~~~~~~~~~~~~~~~~~~ # -@inline function (::Type{Q})(p::MRP) where Q <: UnitQuaternion +@inline function (::Type{Q})(p::MRP) where Q <: QuatRotation n2 = p.x * p.x + p.y * p.y + p.z * p.z M = 2/(1+n2) q = Q((1-n2)/(1+n2), M*p.x, M*p.y, M*p.z, false) end -@inline function (::Type{RP})(q::UnitQuaternion) where RP<:MRP +@inline function (::Type{RP})(q::QuatRotation) where RP<:MRP w = q.q.s x = q.q.v1 y = q.q.v2 @@ -48,9 +48,9 @@ end end # ~~~~~~~~~~~~~~~ StaticArrays Interface ~~~~~~~~~~~~~~~ # -@inline (::Type{RP})(t::NTuple{9}) where RP<:MRP = convert(RP, UnitQuaternion(t)) -@inline Base.getindex(p::MRP, i::Int) = convert(UnitQuaternion, p)[i] -@inline Base.Tuple(p::MRP) = Tuple(convert(UnitQuaternion, p)) +@inline (::Type{RP})(t::NTuple{9}) where RP<:MRP = convert(RP, QuatRotation(t)) +@inline Base.getindex(p::MRP, i::Int) = convert(QuatRotation, p)[i] +@inline Base.Tuple(p::MRP) = Tuple(convert(QuatRotation, p)) # ~~~~~~~~~~~~~~~ Math Operations ~~~~~~~~~~~~~~~ # Base.inv(p::MRP) = MRP(-p.x, -p.y, -p.z) @@ -91,8 +91,8 @@ end # ~~~~~~~~~~~~~~~ Rotation ~~~~~~~~~~~~~~~ # -@inline (*)(p::MRP, r::StaticVector) = UnitQuaternion(p)*r -@inline (\)(p::MRP, r::StaticVector) = UnitQuaternion(p)\r +@inline (*)(p::MRP, r::StaticVector) = QuatRotation(p)*r +@inline (\)(p::MRP, r::StaticVector) = QuatRotation(p)\r # ~~~~~~~~~~~~~~~ Kinematics ~~~~~~~~~~~~~~~ # diff --git a/src/principal_value.jl b/src/principal_value.jl index ba820632..8d713b0b 100644 --- a/src/principal_value.jl +++ b/src/principal_value.jl @@ -6,25 +6,25 @@ particular set of numbers is better conditioned (e.g. `MRP`) or obeys a particul non-negative rotation). In order to preserve differentiability it is necessary to allow rotation representations to travel slightly away from the nominal domain; this is critical for applications such as optimization or dynamics. -This function takes a rotation type (e.g. `UnitQuaternion`, `RotXY`) and outputs a new rotation of the same type that corresponds +This function takes a rotation type (e.g. `QuatRotation`, `RotXY`) and outputs a new rotation of the same type that corresponds to the same `RotMatrix`, but that obeys certain conventions or is better conditioned. The outputs of the function have the following properties: - all angles are between between `-pi` to `pi` (except for `AngleAxis` which is between `0` and `pi`). -- all `UnitQuaternion have non-negative real part +- all `QuatRotation have non-negative real part - the components of all `MRP` have a norm that is at most 1. - the `RotationVec` rotation is at most `pi` """ principal_value(r::Rotation) = r -principal_value(q::Q) where Q <: UnitQuaternion = real(q.q) < zero(eltype(q)) ? Q(-q.q) : q +principal_value(q::Q) where Q <: QuatRotation = real(q.q) < zero(eltype(q)) ? Q(-q.q) : q function principal_value(spq::MRP{T}) where {T} - # A quat with positive real part: UnitQuaternion( qw, qx, qy, qz) + # A quat with positive real part: QuatRotation( qw, qx, qy, qz) # - # A mrp corresponding to the UnitQuaternion with a positive real part: + # A mrp corresponding to the QuatRotation with a positive real part: # MRP( qx / (1 + qw), qy / (1 + qw), qz / (1 + qw)) ≡ MRP(spx, spy, spz) # - # A mrp corresponding to the UnitQuaternion with a negative real part: + # A mrp corresponding to the QuatRotation with a negative real part: # MRP(-qx / (1 - qw), -qy / (1 - qw), -qz / (1 - qw)) ≡ MRP(snx, sny, snz) # # Claim: spx / snx = -1 / (spx^2 + spy^2 + spz^2) diff --git a/src/rodrigues_params.jl b/src/rodrigues_params.jl index 656aec87..06d2349a 100644 --- a/src/rodrigues_params.jl +++ b/src/rodrigues_params.jl @@ -24,17 +24,17 @@ params(g::RodriguesParam) = SVector{3}(g.x, g.y, g.z) # ~~~~~~~~~~~~~~~ Initializers ~~~~~~~~~~~~~~~ # @inline function Random.rand(rng::AbstractRNG, ::Random.SamplerType{RP}) where RP <: RodriguesParam - RP(rand(rng, UnitQuaternion)) + RP(rand(rng, QuatRotation)) end @inline Base.one(::Type{RP}) where RP <: RodriguesParam = RP(0.0, 0.0, 0.0) # ~~~~~~~~~~~~~~~ Quaternion <=> RP ~~~~~~~~~~~~~~~~~~ # -@inline function (::Type{Q})(g::RodriguesParam) where Q<:UnitQuaternion +@inline function (::Type{Q})(g::RodriguesParam) where Q<:QuatRotation M = 1/sqrt(1 + g.x*g.x + g.y*g.y + g.z*g.z) return Q(M, M*g.x, M*g.y, M*g.z, false) end -@inline function (::Type{G})(q::UnitQuaternion) where G<:RodriguesParam +@inline function (::Type{G})(q::QuatRotation) where G<:RodriguesParam w = q.q.s x = q.q.v1 y = q.q.v2 @@ -44,9 +44,9 @@ end end # ~~~~~~~~~~~~~~~ StaticArrays Interface ~~~~~~~~~~~~~~~ # -@inline (::Type{RP})(t::NTuple{9}) where RP<:RodriguesParam = convert(RP, UnitQuaternion(t)) -@inline Base.getindex(rp::RodriguesParam, i::Int) = convert(UnitQuaternion, rp)[i] -@inline Base.Tuple(rp::RodriguesParam) = Tuple(convert(UnitQuaternion, rp)) +@inline (::Type{RP})(t::NTuple{9}) where RP<:RodriguesParam = convert(RP, QuatRotation(t)) +@inline Base.getindex(rp::RodriguesParam, i::Int) = convert(QuatRotation, rp)[i] +@inline Base.Tuple(rp::RodriguesParam) = Tuple(convert(QuatRotation, rp)) # ~~~~~~~~~~~~~~~ Math Operations ~~~~~~~~~~~~~~~ # Base.inv(p::RodriguesParam) = RodriguesParam(-p.x, -p.y, -p.z) @@ -71,8 +71,8 @@ function (/)(g1::RodriguesParam, g2::RodriguesParam) end # ~~~~~~~~~~~~~~~ Rotation ~~~~~~~~~~~~~~~ # -(*)(g::RodriguesParam, r::StaticVector) = UnitQuaternion(g)*r -(\)(g::RodriguesParam, r::StaticVector) = inv(UnitQuaternion(g))*r +(*)(g::RodriguesParam, r::StaticVector) = QuatRotation(g)*r +(\)(g::RodriguesParam, r::StaticVector) = inv(QuatRotation(g))*r # ~~~~~~~~~~~~~~~ Kinematics ~~~~~~~~~~~~~~~ # diff --git a/src/rotation_error.jl b/src/rotation_error.jl index 7cd3a44a..6a83968a 100644 --- a/src/rotation_error.jl +++ b/src/rotation_error.jl @@ -2,7 +2,7 @@ """ RotationError{T<:Real, D<:ErrorMap} <: StaticVector{3,T} -A three-parameter rotation error, converted to/from a `UnitQuaternion` using the +A three-parameter rotation error, converted to/from a `QuatRotation` using the `ErrorMap` `D`. # Usage @@ -27,7 +27,7 @@ struct RotationError{T,D} <: StaticVector{3,T} end end -@inline UnitQuaternion(e::RotationError)::Rotation = e.map(e.err) +@inline QuatRotation(e::RotationError)::Rotation = e.map(e.err) """ rotation_error(R1::Rotation, R2::Rotation, error_map::ErrorMap) @@ -79,7 +79,7 @@ Equivalent to R1 ⊕ e """ function add_error(R1::Rotation, e::RotationError) - R1 * UnitQuaternion(e) + R1 * QuatRotation(e) end function add_error(R1::R, e::RotationError{<:Any, IdentityMap}) where R <: Rotation diff --git a/src/unitquaternion.jl b/src/unitquaternion.jl index f7d90ebe..1bc8e190 100644 --- a/src/unitquaternion.jl +++ b/src/unitquaternion.jl @@ -1,25 +1,25 @@ import Base: *, /, \, exp, ≈, == """ - UnitQuaternion{T} <: Rotation + QuatRotation{T} <: Rotation 4-parameter attitute representation that is singularity-free. Quaternions with unit norm -represent a double-cover of SO(3). The `UnitQuaternion` does NOT strictly enforce the unit +represent a double-cover of SO(3). The `QuatRotation` does NOT strictly enforce the unit norm constraint, but certain methods will assume you have a unit quaternion. Follows the Hamilton convention for quaternions. # Constructors ```julia -UnitQuaternion(w,x,y,z) -UnitQuaternion(q::AbstractVector) +QuatRotation(w,x,y,z) +QuatRotation(q::AbstractVector) ``` where `w` is the scalar (real) part, `x`,`y`, and `z` are the vector (imaginary) part, and `q = [w,x,y,z]`. """ -struct UnitQuaternion{T} <: Rotation{3,T} +struct QuatRotation{T} <: Rotation{3,T} q::Quaternion{T} - @inline function UnitQuaternion{T}(w, x, y, z, normalize::Bool = true) where T + @inline function QuatRotation{T}(w, x, y, z, normalize::Bool = true) where T if normalize inorm = inv(sqrt(w*w + x*x + y*y + z*z)) new{T}(Quaternion(w*inorm, x*inorm, y*inorm, z*inorm, true)) @@ -28,54 +28,54 @@ struct UnitQuaternion{T} <: Rotation{3,T} end end - @inline function UnitQuaternion{T}(q::Quaternion) where T + @inline function QuatRotation{T}(q::Quaternion) where T if q.norm new{T}(q) else throw(InexactError(nameof(T), T, q)) end end - UnitQuaternion{T}(q::UnitQuaternion) where T = new{T}(q.q) + QuatRotation{T}(q::QuatRotation) where T = new{T}(q.q) end # ~~~~~~~~~~~~~~~ Constructors ~~~~~~~~~~~~~~~ # # Use default map -function UnitQuaternion(w,x,y,z, normalize::Bool = true) +function QuatRotation(w,x,y,z, normalize::Bool = true) types = promote(w,x,y,z) - UnitQuaternion{eltype(types)}(w,x,y,z, normalize) + QuatRotation{eltype(types)}(w,x,y,z, normalize) end -function UnitQuaternion(q::T) where T<:Quaternion +function QuatRotation(q::T) where T<:Quaternion if q.norm - return UnitQuaternion(q.s, q.v1, q.v2, q.v3, false) + return QuatRotation(q.s, q.v1, q.v2, q.v3, false) else throw(InexactError(nameof(T), T, q)) end end -function Quaternions.Quaternion(q::UnitQuaternion) +function Quaternions.Quaternion(q::QuatRotation) return q.q end # Pass in Vectors -@inline function (::Type{Q})(q::AbstractVector, normalize::Bool = true) where Q <: UnitQuaternion +@inline function (::Type{Q})(q::AbstractVector, normalize::Bool = true) where Q <: QuatRotation check_length(q, 4) Q(q[1], q[2], q[3], q[4], normalize) end -@inline (::Type{Q})(q::StaticVector{4}, normalize::Bool = true) where Q <: UnitQuaternion = +@inline (::Type{Q})(q::StaticVector{4}, normalize::Bool = true) where Q <: QuatRotation = Q(q[1], q[2], q[3], q[4], normalize) # Copy constructors -UnitQuaternion(q::UnitQuaternion) = q +QuatRotation(q::QuatRotation) = q -# UnitQuaternion <=> Quat -# (::Type{Q})(q::Quat) where Q <: UnitQuaternion = Q(q.w, q.x, q.y, q.z, false) -# (::Type{Q})(q::UnitQuaternion) where Q <: Quat = Q(q.w, q.x, q.y, q.z, false) -# const AllQuats{T} = Union{<:Quat{T}, <:UnitQuaternion{T}} +# QuatRotation <=> Quat +# (::Type{Q})(q::Quat) where Q <: QuatRotation = Q(q.w, q.x, q.y, q.z, false) +# (::Type{Q})(q::QuatRotation) where Q <: Quat = Q(q.w, q.x, q.y, q.z, false) +# const AllQuats{T} = Union{<:Quat{T}, <:QuatRotation{T}} # ~~~~~~~~~~~~~~~ StaticArrays Interface ~~~~~~~~~~~~~~~ # -function (::Type{Q})(t::NTuple{9}) where Q<:UnitQuaternion +function (::Type{Q})(t::NTuple{9}) where Q<:QuatRotation #= This function solves the system of equations in Section 3.1 of https://arxiv.org/pdf/math/0701759.pdf. This cheap method @@ -110,7 +110,7 @@ function (::Type{Q})(t::NTuple{9}) where Q<:UnitQuaternion end -function Base.getindex(q::UnitQuaternion, i::Int) +function Base.getindex(q::QuatRotation, i::Int) w = q.q.s x = q.q.v1 y = q.q.v2 @@ -172,7 +172,7 @@ function Base.getindex(q::UnitQuaternion, i::Int) end end -function Base.Tuple(q::UnitQuaternion) +function Base.Tuple(q::QuatRotation) w = q.q.s x = q.q.v1 y = q.q.v2 @@ -202,8 +202,8 @@ function Base.Tuple(q::UnitQuaternion) end # ~~~~~~~~~~~~~~~ Getters ~~~~~~~~~~~~~~~ # -@inline scalar(q::UnitQuaternion) = real(q.q) -@inline vector(q::UnitQuaternion) = vector(q.q) +@inline scalar(q::QuatRotation) = real(q.q) +@inline vector(q::QuatRotation) = vector(q.q) @inline vector(q::Quaternion) = SVector{3}(q.v1, q.v2, q.v3) """ @@ -217,21 +217,21 @@ p = MRP(1.0, 2.0, 3.0) Rotations.params(p) == @SVector [1.0, 2.0, 3.0] # true ``` """ -@inline params(q::UnitQuaternion) = SVector{4}(q.q.s, q.q.v1, q.q.v2, q.q.v3) +@inline params(q::QuatRotation) = SVector{4}(q.q.s, q.q.v1, q.q.v2, q.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))) +function Random.rand(rng::AbstractRNG, ::Random.SamplerType{<:QuatRotation{T}}) where T + _normalize(QuatRotation{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) +@inline Base.one(::Type{Q}) where Q <: QuatRotation = Q(1.0, 0.0, 0.0, 0.0) # ~~~~~~~~~~~~~~~ Math Operations ~~~~~~~~~~~~~~~ # # Inverses -Base.inv(q::Q) where Q <: UnitQuaternion = Q(conj(q.q)) +Base.inv(q::Q) where Q <: QuatRotation = Q(conj(q.q)) -function _normalize(q::Q) where Q <: UnitQuaternion +function _normalize(q::Q) where Q <: QuatRotation w = q.q.s x = q.q.v1 y = q.q.v2 @@ -242,7 +242,7 @@ function _normalize(q::Q) where Q <: UnitQuaternion end # Identity -(::Type{Q})(I::UniformScaling) where Q <: UnitQuaternion = one(Q) +(::Type{Q})(I::UniformScaling) where Q <: QuatRotation = one(Q) # Exponentials and Logarithms """ @@ -264,10 +264,10 @@ function expm(ϕ::AbstractVector) θ = norm(ϕ) sθ,cθ = sincos(θ/2) M = sinc(θ/π/2)/2 - UnitQuaternion(cθ, ϕ[1]*M, ϕ[2]*M, ϕ[3]*M, false) + QuatRotation(cθ, ϕ[1]*M, ϕ[2]*M, ϕ[3]*M, false) end -function _log_as_quat(q::Q, eps=1e-6) where Q <: UnitQuaternion +function _log_as_quat(q::Q, eps=1e-6) where Q <: QuatRotation w = q.q.s x = q.q.v1 y = q.q.v2 @@ -283,14 +283,14 @@ function _log_as_quat(q::Q, eps=1e-6) where Q <: UnitQuaternion _pure_quaternion(M*vector(q)) end -function logm(q::UnitQuaternion) +function logm(q::QuatRotation) # Assumes unit quaternion return 2*vector(_log_as_quat(q)) end # Composition """ - (*)(q::UnitQuaternion, w::UnitQuaternion) + (*)(q::QuatRotation, w::QuatRotation) Quternion Composition @@ -302,26 +302,26 @@ rmult(w) * SVector(q) Sets the output mapping equal to the mapping of `w` """ -function (*)(q1::UnitQuaternion, q2::UnitQuaternion) - return UnitQuaternion(q1.q*q2.q) +function (*)(q1::QuatRotation, q2::QuatRotation) + return QuatRotation(q1.q*q2.q) end """ - (*)(q::UnitQuaternion, r::StaticVector) + (*)(q::QuatRotation, r::StaticVector) Rotate a vector Equivalent to `hmat()' lmult(q) * rmult(q)' hmat() * r` """ -function Base.:*(q::UnitQuaternion, r::StaticVector) # must be StaticVector to avoid ambiguity +function Base.:*(q::QuatRotation, r::StaticVector) # must be StaticVector to avoid ambiguity check_length(r, 3) w = real(q.q) v = vector(q.q) (w^2 - v'v)*r + 2*v*(v'r) + 2*w*cross(v,r) end -(\)(q1::UnitQuaternion, q2::UnitQuaternion) = inv(q1)*q2 -(/)(q1::UnitQuaternion, q2::UnitQuaternion) = q1*inv(q2) +(\)(q1::QuatRotation, q2::QuatRotation) = inv(q1)*q2 +(/)(q1::QuatRotation, q2::QuatRotation) = q1*inv(q2) """ rotation_between(from, to) @@ -337,7 +337,7 @@ function rotation_between(from::SVector{3}, to::SVector{3}) 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 UnitQuaternion(w, v[1], v[2], v[3]) # relies on normalization in constructor + @inbounds return QuatRotation(w, v[1], v[2], v[3]) # relies on normalization in constructor end # ~~~~~~~~~~~~~~~ Kinematics ~~~~~~~~~~~~~~~ $ @@ -361,18 +361,18 @@ The kinematics are extremely useful when computing the dynamics of rigid bodies, See "Fundamentals of Spacecraft Attitude Determination and Control" by Markley and Crassidis Sections 3.1-3.2 for more details. """ -function kinematics(q::Q, ω::AbstractVector) where Q <: UnitQuaternion +function kinematics(q::Q, ω::AbstractVector) where Q <: QuatRotation params(q*Q(0.0, ω[1], ω[2], ω[3], false))/2 end # ~~~~~~~~~~~~~~~ Linear Algebraic Conversions ~~~~~~~~~~~~~~~ # """ - lmult(q::UnitQuaternion) + lmult(q::QuatRotation) lmult(q::StaticVector{4}) `lmult(q2)*params(q1)` returns a vector equivalent to `q2*q1` (quaternion composition) """ -function lmult(q::UnitQuaternion) +function lmult(q::QuatRotation) w = q.q.s x = q.q.v1 y = q.q.v2 @@ -393,15 +393,15 @@ function lmult(q::Quaternion) q.v3 -q.v2 q.v1 q.s; ] end -lmult(q::StaticVector{4}) = lmult(UnitQuaternion(q, false)) +lmult(q::StaticVector{4}) = lmult(QuatRotation(q, false)) """ - rmult(q::UnitQuaternion) + rmult(q::QuatRotation) rmult(q::StaticVector{4}) `rmult(q1)*params(q2)` return a vector equivalent to `q2*q1` (quaternion composition) """ -function rmult(q::UnitQuaternion) +function rmult(q::QuatRotation) w = q.q.s x = q.q.v1 y = q.q.v2 @@ -422,12 +422,12 @@ function rmult(q::Quaternion) q.v3 q.v2 -q.v1 q.s; ] end -rmult(q::SVector{4}) = rmult(UnitQuaternion(q, false)) +rmult(q::SVector{4}) = rmult(QuatRotation(q, false)) """ tmat() -`tmat()*params(q)`return a vector equivalent to `inv(q)`, where `q` is a `UnitQuaternion` +`tmat()*params(q)`return a vector equivalent to `inv(q)`, where `q` is a `QuatRotation` """ function tmat(::Type{T}=Float64) where T SA{T}[ @@ -477,7 +477,7 @@ end # ~~~~~~~~~~~~~~~ Useful Jacobians ~~~~~~~~~~~~~~~ # """ - ∇differential(q::UnitQuaternion) + ∇differential(q::QuatRotation) Jacobian of `lmult(q) QuatMap(ϕ)`, when ϕ is near zero. @@ -485,7 +485,7 @@ Useful for converting Jacobians from R⁴ to R³ and correctly account for unit norm constraint. Jacobians for different differential quaternion parameterization are the same up to a constant. """ -function ∇differential(q::UnitQuaternion) +function ∇differential(q::QuatRotation) w = q.q.s x = q.q.v1 y = q.q.v2 @@ -500,11 +500,11 @@ function ∇differential(q::UnitQuaternion) end """ - ∇²differential(q::UnitQuaternion, b::AbstractVector) + ∇²differential(q::QuatRotation, b::AbstractVector) Jacobian of `(∂/∂ϕ lmult(q) QuatMap(ϕ))`b, evaluated at ϕ=0, and `b` has length 4. """ -function ∇²differential(q::UnitQuaternion, b::AbstractVector) +function ∇²differential(q::QuatRotation, b::AbstractVector) check_length(b, 4) b1 = -params(q)'b Diagonal(@SVector fill(b1,3)) @@ -515,9 +515,9 @@ end Jacobian of `R*r` with respect to the rotation """ -function ∇rotate(q::UnitQuaternion, r::AbstractVector) +function ∇rotate(q::QuatRotation, r::AbstractVector) check_length(r, 3) - rhat = UnitQuaternion(zero(eltype(r)), r[1], r[2], r[3], false) + rhat = QuatRotation(zero(eltype(r)), r[1], r[2], r[3], false) 2vmat()*rmult(q)'rmult(rhat) end @@ -526,7 +526,7 @@ end Jacobian of `R2*R1` with respect to `R1` """ -function ∇composition1(q2::UnitQuaternion, q1::UnitQuaternion) +function ∇composition1(q2::QuatRotation, q1::QuatRotation) lmult(q2) end @@ -535,6 +535,6 @@ end Jacobian of `R2*R1` with respect to `R2` """ -function ∇composition2(q2::UnitQuaternion, q1::UnitQuaternion) +function ∇composition2(q2::QuatRotation, q1::QuatRotation) rmult(q1) end diff --git a/test/derivative_tests.jl b/test/derivative_tests.jl index b8d81609..04775097 100644 --- a/test/derivative_tests.jl +++ b/test/derivative_tests.jl @@ -11,13 +11,13 @@ using ForwardDiff @testset "Jacobian checks" begin # Quaternion to rotation matrix - @testset "Jacobian (UnitQuaternion -> RotMatrix)" begin + @testset "Jacobian (QuatRotation -> RotMatrix)" begin for i = 1:10 # do some repeats - q = rand(UnitQuaternion) # a random quaternion + q = rand(QuatRotation) # a random quaternion # test jacobian to a Rotation matrix R_jac = Rotations.jacobian(RotMatrix, q) - FD_jac = ForwardDiff.jacobian(x -> SVector{9}(UnitQuaternion(x[1], x[2], x[3], x[4])), + FD_jac = ForwardDiff.jacobian(x -> SVector{9}(QuatRotation(x[1], x[2], x[3], x[4])), Rotations.params(q)) # compare @@ -26,13 +26,13 @@ using ForwardDiff end # MRP to UnitQuternion - @testset "Jacobian (MRP -> UnitQuaternion)" begin + @testset "Jacobian (MRP -> QuatRotation)" begin for i = 1:10 # do some repeats p = rand(MRP) # a random MRP # test jacobian to a Rotation matrix - R_jac = Rotations.jacobian(UnitQuaternion, p) - FD_jac = ForwardDiff.jacobian(x -> (q = UnitQuaternion(MRP(x[1],x[2],x[3])); + R_jac = Rotations.jacobian(QuatRotation, p) + FD_jac = ForwardDiff.jacobian(x -> (q = QuatRotation(MRP(x[1],x[2],x[3])); Rotations.params(q)), SVector(p.x, p.y, p.z)) @@ -41,11 +41,11 @@ using ForwardDiff end end - @testset "Jacobian (MRP -> UnitQuaternion) [Corner Cases]" begin + @testset "Jacobian (MRP -> QuatRotation) [Corner Cases]" begin for p = [MRP(1.0, 0.0, 0.0), MRP(0.0, 1.0, 0.0), MRP(0.0, 0.0, 1.0)] # test jacobian to a Rotation matrix - R_jac = Rotations.jacobian(UnitQuaternion, p) - FD_jac = ForwardDiff.jacobian(x -> (q = UnitQuaternion(MRP(x[1],x[2],x[3])); + R_jac = Rotations.jacobian(QuatRotation, p) + FD_jac = ForwardDiff.jacobian(x -> (q = QuatRotation(MRP(x[1],x[2],x[3])); Rotations.params(q)), SVector(p.x, p.y, p.z)) @@ -54,14 +54,14 @@ using ForwardDiff end end - # MRP to UnitQuaternion - @testset "Jacobian (UnitQuaternion -> MRP)" begin + # MRP to QuatRotation + @testset "Jacobian (QuatRotation -> MRP)" begin for i = 1:10 # do some repeats - q = rand(UnitQuaternion) # a random UnitQuaternion + q = rand(QuatRotation) # a random QuatRotation # test jacobian to a Rotation matrix R_jac = Rotations.jacobian(MRP, q) - FD_jac = ForwardDiff.jacobian(x -> (p = MRP(UnitQuaternion(x[1], x[2], x[3], x[4])); + FD_jac = ForwardDiff.jacobian(x -> (p = MRP(QuatRotation(x[1], x[2], x[3], x[4])); SVector(p.x, p.y, p.z)), Rotations.params(q)) @@ -150,14 +150,14 @@ using ForwardDiff end # rotate a point by a quaternion - @testset "Jacobian (UnitQuaternion rotation)" begin + @testset "Jacobian (QuatRotation rotation)" begin for i = 1:10 # do some repeats - q = rand(UnitQuaternion) # a random quaternion + q = rand(QuatRotation) # a random quaternion v = randn(SVector{3,Float64}) # test jacobian to a Rotation matrix R_jac = Rotations.jacobian(q, v) - FD_jac = ForwardDiff.jacobian(x -> UnitQuaternion(x[1], x[2], x[3], x[4])*v, + FD_jac = ForwardDiff.jacobian(x -> QuatRotation(x[1], x[2], x[3], x[4])*v, Rotations.params(q)) # compare diff --git a/test/distribution_tests.jl b/test/distribution_tests.jl index 6930471d..804de8c9 100644 --- a/test/distribution_tests.jl +++ b/test/distribution_tests.jl @@ -28,7 +28,7 @@ To visualize the distribution, try the following script. # TODO: consider to remove Euler rotations (#155) Type_SO3 = (RotMatrix{3}, AngleAxis, RotationVec, - UnitQuaternion, MRP, RodriguesParam, + QuatRotation, MRP, RodriguesParam, RotXYZ, RotYZX, RotZXY, RotXZY, RotYXZ, RotZYX, RotXYX, RotYZY, RotZXZ, RotXZX, RotYXY, RotZYZ) diff --git a/test/eigen.jl b/test/eigen.jl index 2caa7fe2..030e6855 100644 --- a/test/eigen.jl +++ b/test/eigen.jl @@ -1,6 +1,6 @@ @testset "Eigen_3D" begin all_types = (RotMatrix{3}, AngleAxis, RotationVec, - UnitQuaternion, RodriguesParam, MRP, + QuatRotation, RodriguesParam, MRP, RotXYZ, RotYZX, RotZXY, RotXZY, RotYXZ, RotZYX, RotXYX, RotYZY, RotZXZ, RotXZX, RotYXY, RotZYZ, RotX, RotY, RotZ, diff --git a/test/log.jl b/test/log.jl index d5d6b00e..85853030 100644 --- a/test/log.jl +++ b/test/log.jl @@ -1,6 +1,6 @@ @testset "log" begin all_types = (RotMatrix{3}, AngleAxis, RotationVec, - UnitQuaternion, RodriguesParam, MRP, + QuatRotation, RodriguesParam, MRP, RotXYZ, RotYZX, RotZXY, RotXZY, RotYXZ, RotZYX, RotXYX, RotYZY, RotZXZ, RotXZX, RotYXY, RotZYZ, RotX, RotY, RotZ, diff --git a/test/principal_value_tests.jl b/test/principal_value_tests.jl index 761fcc8e..c01e0bf9 100644 --- a/test/principal_value_tests.jl +++ b/test/principal_value_tests.jl @@ -8,9 +8,9 @@ end end -@testset "Principal Value (UnitQuaternion)" begin +@testset "Principal Value (QuatRotation)" begin for i = 1:1000 - qq = rand(UnitQuaternion) + qq = rand(QuatRotation) qq_prin = principal_value(qq) @test 0.0 ≤ real(qq_prin.q) @test qq_prin ≈ qq diff --git a/test/quatmaps.jl b/test/quatmaps.jl index d822ebd1..32cbc78e 100644 --- a/test/quatmaps.jl +++ b/test/quatmaps.jl @@ -52,12 +52,12 @@ import Rotations: CayleyMap, ExponentialMap, MRPMap, IdentityMap, QuatVecMap @test inv(emap)(emap(ϕ)) ≈ ϕ b = @SVector rand(3) @test ForwardDiff.jacobian( - q -> jacobian(inv(emap), UnitQuaternion(q, false))'b, + q -> jacobian(inv(emap), QuatRotation(q, false))'b, params(q), ) ≈ Rotations.∇jacobian(inv(emap), q, b) end - q = rand(UnitQuaternion) + q = rand(QuatRotation) function invmap(q) v = @SVector [q[2], q[3], q[4]] diff --git a/test/rodrigues_params.jl b/test/rodrigues_params.jl index 1e771efb..690a5694 100644 --- a/test/rodrigues_params.jl +++ b/test/rodrigues_params.jl @@ -58,8 +58,8 @@ import Rotations: ∇rotate, ∇composition1, ∇composition2, skew, params end @testset "kinematics" begin - # UnitQuaternion - q = rand(UnitQuaternion) + # QuatRotation + q = rand(QuatRotation) ω = @SVector rand(3) q_ = Rotations.params(q) qdot = Rotations.kinematics(q,ω) diff --git a/test/rotation_error.jl b/test/rotation_error.jl index 9b7e4d33..6e437cb3 100644 --- a/test/rotation_error.jl +++ b/test/rotation_error.jl @@ -6,7 +6,7 @@ using Test import Rotations: RotationError, rotation_error, add_error, params @testset "Rotation Error" begin -q1 = rand(UnitQuaternion) +q1 = rand(QuatRotation) dq = expm(0.1 * normalize(@SVector rand(3))) q2 = q1*dq @test q1\q2 ≈ dq @@ -17,8 +17,8 @@ e1 = rotation_error(q2, q1, emap) @test e1 ≈ q2 ⊖ q1 @test e1 ≈ params(dg) @test add_error(q1, e1) ≈ q2 -@test q1 ⊕ e1 isa UnitQuaternion -@test rotation_angle(UnitQuaternion(e1)) ≈ 0.1 +@test q1 ⊕ e1 isa QuatRotation +@test rotation_angle(QuatRotation(e1)) ≈ 0.1 aa1 = AngleAxis(q1) @test add_error(aa1, e1) ≈ q2 @@ -59,6 +59,6 @@ g = inv(emap)(q1) @test inv(inv(imap))(q1) ≈ g g = RodriguesParam(g) -@test UnitQuaternion(g) ≈ q1 +@test QuatRotation(g) ≈ q1 @test imap(g) ≈ params(g) end diff --git a/test/rotation_tests.jl b/test/rotation_tests.jl index 63fa1c31..a79e3b31 100644 --- a/test/rotation_tests.jl +++ b/test/rotation_tests.jl @@ -66,7 +66,7 @@ end ##################################################################################### rot_types = (RotMatrix{3}, AngleAxis, RotationVec, - UnitQuaternion, RodriguesParam, MRP, + QuatRotation, RodriguesParam, MRP, RotXYZ, RotYZX, RotZXY, RotXZY, RotYXZ, RotZYX, RotXYX, RotYZY, RotZXZ, RotXZX, RotYXY, RotZYZ) @@ -74,7 +74,7 @@ one_types = (RotX, RotY, RotZ) two_types = (RotXY, RotYZ, RotZX, RotXZ, RotYX, RotZY) taitbyran_types = (RotXYZ, RotYZX, RotZXY, RotXZY, RotYXZ, RotZYX) all_types = (RotMatrix{3}, AngleAxis, RotationVec, - UnitQuaternion, RodriguesParam, MRP, + QuatRotation, RodriguesParam, MRP, RotXYZ, RotYZX, RotZXY, RotXZY, RotYXZ, RotZYX, RotXYX, RotYZY, RotZXZ, RotXZX, RotYXY, RotZYZ, RotX, RotY, RotZ, @@ -198,20 +198,20 @@ all_types = (RotMatrix{3}, AngleAxis, RotationVec, @testset "Quaternion double cover" begin repeats = 100 - Q = UnitQuaternion + Q = QuatRotation for i = 1 : repeats - q = rand(UnitQuaternion) + q = rand(QuatRotation) - q2 = UnitQuaternion(-q.q) # normalize: need a tolerance + q2 = QuatRotation(-q.q) # normalize: need a tolerance @test Rotations.params(q2) ≈ -Rotations.params(q) atol = 100 * eps() @test q ≈ q2 atol = 100 * eps() - q3 = UnitQuaternion(-q.q.s, -q.q.v1, -q.q.v2, -q.q.v3, false) # don't normalize: everything is exact + q3 = QuatRotation(-q.q.s, -q.q.v1, -q.q.v2, -q.q.v3, false) # don't normalize: everything is exact @test Rotations.params(q3) == -Rotations.params(q) @test q == q3 Δq = q \ q3 - @test Δq ≈ one(UnitQuaternion) atol = 100 * eps() + @test Δq ≈ one(QuatRotation) atol = 100 * eps() end end @@ -270,14 +270,14 @@ all_types = (RotMatrix{3}, AngleAxis, RotationVec, end ######################################################################### - # Test robustness of DCM to UnitQuaternion function + # Test robustness of DCM to QuatRotation function ######################################################################### - @testset "DCM to UnitQuaternion" begin + @testset "DCM to QuatRotation" begin pert = 1e-3 for type_rot in all_types for _ = 1:100 not_orthonormal = rand(type_rot) + rand(3, 3) * pert - quat_ill_cond = UnitQuaternion(not_orthonormal) + quat_ill_cond = QuatRotation(not_orthonormal) @test 0 <= real(quat_ill_cond.q) @test norm(quat_ill_cond - nearest_rotation(not_orthonormal)) < 10 * pert end @@ -304,16 +304,16 @@ all_types = (RotMatrix{3}, AngleAxis, RotationVec, @test rotation_axis(r2) ≈ axis end end - @test norm(rotation_axis(UnitQuaternion(1.0, 0.0, 0.0, 0.0))) ≈ 1.0 + @test norm(rotation_axis(QuatRotation(1.0, 0.0, 0.0, 0.0))) ≈ 1.0 # TODO RotX, RotXY? end ######################################################################### - # Check construction of UnitQuaternion given two vectors + # Check construction of QuatRotation given two vectors ######################################################################### - @testset "Testing construction of UnitQuaternion given two vectors" begin + @testset "Testing construction of QuatRotation given two vectors" begin angle_axis_test(from, to, rot, atol) = isapprox(rot * from * norm(to) / norm(from), to; atol = atol) for i = 1 : 10000 @@ -442,15 +442,15 @@ all_types = (RotMatrix{3}, AngleAxis, RotationVec, @test convert(typeof(aa), aa) === aa w, x, y, z = 1., 2., 3., 4. - quat = UnitQuaternion(w, x, y, z) + quat = QuatRotation(w, x, y, z) @test norm(Rotations.params(quat)) ≈ 1. - quat = UnitQuaternion(w, x, y, z, false) + quat = QuatRotation(w, x, y, z, false) @test norm(Rotations.params(quat)) ≈ norm([w, x, y, z]) w, x, y, z = 1., 2., 3., 4. - quat = UnitQuaternion(w, x, y, z) + quat = QuatRotation(w, x, y, z) @test norm(Rotations.params(quat)) ≈ 1. - quat = UnitQuaternion(w, x, y, z, false) + quat = QuatRotation(w, x, y, z, false) @test norm(Rotations.params(quat)) ≈ norm([w, x, y, z]) end @@ -490,7 +490,7 @@ all_types = (RotMatrix{3}, AngleAxis, RotationVec, @test Rotations.params(RotXYZ(p1,p2,p3)) == [p1,p2,p3] @test Rotations.params(AngleAxis(p1,p2,p3,p4)) ≈ pushfirst!(normalize([p2,p3,p4]),p1) @test Rotations.params(RotationVec(p1,p2,p3)) == [p1,p2,p3] - @test Rotations.params(UnitQuaternion(p1,p2,p3,p4)) ≈ normalize([p1,p2,p3,p4]) + @test Rotations.params(QuatRotation(p1,p2,p3,p4)) ≈ normalize([p1,p2,p3,p4]) @test Rotations.params(MRP(p1,p2,p3)) == [p1,p2,p3] @test Rotations.params(RodriguesParam(p1,p2,p3)) == [p1,p2,p3] end diff --git a/test/unitquat.jl b/test/unitquat.jl index 90c007db..972255d7 100644 --- a/test/unitquat.jl +++ b/test/unitquat.jl @@ -5,22 +5,22 @@ import Rotations: kinematics, _pure_quaternion, params import Rotations: vmat, rmult, lmult, hmat, tmat @testset "Unit Quaternions" begin - q1 = rand(UnitQuaternion) - q2 = rand(UnitQuaternion) + q1 = rand(QuatRotation) + q2 = rand(QuatRotation) r = @SVector rand(3) ω = @SVector rand(3) # Constructors - @test UnitQuaternion(1.0, 0.0, 0.0, 0.0) isa UnitQuaternion{Float64} - @test UnitQuaternion(1.0, 0, 0, 0) isa UnitQuaternion{Float64} - @test UnitQuaternion(1.0, 0, 0, 0) isa UnitQuaternion{Float64} - @test UnitQuaternion(1, 0, 0, 0) isa UnitQuaternion{Int} - @test UnitQuaternion(1.0f0, 0, 0, 0) isa UnitQuaternion{Float32} + @test QuatRotation(1.0, 0.0, 0.0, 0.0) isa QuatRotation{Float64} + @test QuatRotation(1.0, 0, 0, 0) isa QuatRotation{Float64} + @test QuatRotation(1.0, 0, 0, 0) isa QuatRotation{Float64} + @test QuatRotation(1, 0, 0, 0) isa QuatRotation{Int} + @test QuatRotation(1.0f0, 0, 0, 0) isa QuatRotation{Float32} q = normalize(@SVector rand(4)) q32 = SVector{4,Float32}(q) - @test UnitQuaternion(q) isa UnitQuaternion{Float64} - @test UnitQuaternion(q32) isa UnitQuaternion{Float32} + @test QuatRotation(q) isa QuatRotation{Float64} + @test QuatRotation(q32) isa QuatRotation{Float32} r = @SVector rand(3) r32 = SVector{3,Float32}(r) @@ -28,42 +28,42 @@ import Rotations: vmat, rmult, lmult, hmat, tmat @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} - @test UnitQuaternion{Float32}(1.0, 0, 0, 0) isa UnitQuaternion{Float32} - @test UnitQuaternion{Float64}(1f0, 0, 0, 0) isa UnitQuaternion{Float64} + @test QuatRotation{Float64}(1, 0, 0, 0) isa QuatRotation{Float64} + @test QuatRotation{Float32}(1, 0, 0, 0) isa QuatRotation{Float32} + @test QuatRotation{Float32}(1.0, 0, 0, 0) isa QuatRotation{Float32} + @test QuatRotation{Float64}(1f0, 0, 0, 0) isa QuatRotation{Float64} # normalization - @test UnitQuaternion(2.0, 0, 0, 0, true) == one(UnitQuaternion) - @test UnitQuaternion(2q, true) ≈ UnitQuaternion(q) + @test QuatRotation(2.0, 0, 0, 0, true) == one(QuatRotation) + @test QuatRotation(2q, true) ≈ QuatRotation(q) # Copy constructors - q = rand(UnitQuaternion) - @test UnitQuaternion(q) === q - @test UnitQuaternion{Float32}(q) isa UnitQuaternion{Float32} - UnitQuaternion{Float32}(q) + q = rand(QuatRotation) + @test QuatRotation(q) === q + @test QuatRotation{Float32}(q) isa QuatRotation{Float32} + QuatRotation{Float32}(q) # rand - @test rand(UnitQuaternion) isa UnitQuaternion{Float64} - @test rand(UnitQuaternion{Float32}) isa UnitQuaternion{Float32} + @test rand(QuatRotation) isa QuatRotation{Float64} + @test rand(QuatRotation{Float32}) isa QuatRotation{Float32} # Test math - @test UnitQuaternion(I) isa UnitQuaternion{Float64} + @test QuatRotation(I) isa QuatRotation{Float64} ϕ = inv(ExponentialMap())(q1) @test expm(ϕ * 2) ≈ q1 q = Rotations._pure_quaternion(ϕ) - @test UnitQuaternion(exp(q)) ≈ q1 + @test QuatRotation(exp(q)) ≈ q1 @test exp(q) isa Quaternion - q = UnitQuaternion((@SVector [1, 2, 3, 4.0]), false) + q = QuatRotation((@SVector [1, 2, 3, 4.0]), false) @test 2 * q == 2 * Matrix(q) @test q * 2 == 2 * Matrix(q) # Axis-angle ϕ = 0.1 * @SVector [1, 0, 0] q = expm(ϕ) - @test q isa UnitQuaternion + @test q isa QuatRotation @test logm(expm(ϕ)) ≈ ϕ @test expm(logm(q1)) ≈ q1 @test rotation_angle(q) ≈ 0.1 @@ -96,17 +96,17 @@ import Rotations: vmat, rmult, lmult, hmat, tmat @test hmat(r) == SVector(r_pure.s, r_pure.v1, r_pure.v2, r_pure.v3) # Test Jacobians - @test ForwardDiff.jacobian(q -> UnitQuaternion(q, false) * r, params(q)) ≈ + @test ForwardDiff.jacobian(q -> QuatRotation(q, false) * r, params(q)) ≈ ∇rotate(q, r) - @test ForwardDiff.jacobian(q -> params(q2 * UnitQuaternion(q, false)), params(q1)) ≈ + @test ForwardDiff.jacobian(q -> params(q2 * QuatRotation(q, false)), params(q1)) ≈ ∇composition1(q2, q1) - @test ForwardDiff.jacobian(q -> params(UnitQuaternion(q, false) * q1), params(q2)) ≈ + @test ForwardDiff.jacobian(q -> params(QuatRotation(q, false) * q1), params(q2)) ≈ ∇composition2(q2, q1) b = @SVector rand(4) qval = params(q1) - ForwardDiff.jacobian(q -> ∇composition1(q2, UnitQuaternion(q))'b, + ForwardDiff.jacobian(q -> ∇composition1(q2, QuatRotation(q))'b, @SVector [1, 0, 0, 0.0,]) diffcomp = ϕ -> params(q2 * CayleyMap()(ϕ)) ∇diffcomp(ϕ) = ForwardDiff.jacobian(diffcomp, ϕ) @@ -125,9 +125,9 @@ import Rotations: vmat, rmult, lmult, hmat, tmat # Check ops with Float32 ϕ = SA_F32[1, 2, 3] - @test expm(SA_F32[1, 2, 3]) isa UnitQuaternion{Float32} + @test expm(SA_F32[1, 2, 3]) isa QuatRotation{Float32} - q32 = rand(UnitQuaternion{Float32}) + q32 = rand(QuatRotation{Float32}) @test Rotations._log_as_quat(q32) isa Quaternion{Float32} @test log(q32) isa SMatrix @test eltype(logm(q32)) == Float32 From e5d214ee6d699208f760135f31536b82c2a6616d Mon Sep 17 00:00:00 2001 From: hyrodium Date: Tue, 16 Nov 2021 22:50:39 +0900 Subject: [PATCH 2/7] deprecate UnitQuaternion --- src/deprecated.jl | 3 +++ test/deprecated.jl | 1 + 2 files changed, 4 insertions(+) diff --git a/src/deprecated.jl b/src/deprecated.jl index f07d7072..719aa1c3 100644 --- a/src/deprecated.jl +++ b/src/deprecated.jl @@ -5,5 +5,8 @@ Base.@deprecate_binding RodriguesVec RotationVec true # Deprecate Quat => QuatRotation Base.@deprecate_binding Quat QuatRotation true +# Deprecate UnitQuaternion => QuatRotation +Base.@deprecate_binding UnitQuaternion QuatRotation true + # Deprecate SPQuat => MRP Base.@deprecate_binding SPQuat MRP true diff --git a/test/deprecated.jl b/test/deprecated.jl index e03bd8d8..2404d99c 100644 --- a/test/deprecated.jl +++ b/test/deprecated.jl @@ -3,5 +3,6 @@ # RodriguesVec @test Base.isdeprecated(Rotations, :RodriguesVec) @test Base.isdeprecated(Rotations, :Quat) + @test Base.isdeprecated(Rotations, :UnitQuaternion) @test Base.isdeprecated(Rotations, :SPQuat) end From c41d9dfd0c88810857f2a11034b0adaf1c27ad0e Mon Sep 17 00:00:00 2001 From: hyrodium Date: Tue, 16 Nov 2021 22:52:54 +0900 Subject: [PATCH 3/7] update export --- src/Rotations.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Rotations.jl b/src/Rotations.jl index eb389749..79cb215b 100644 --- a/src/Rotations.jl +++ b/src/Rotations.jl @@ -40,7 +40,7 @@ export RotXYZ, RotYXZ, RotZXY, RotXZY, RotYZX, RotZYX, # Deprecated, but export for compatibility - RodriguesVec, Quat, SPQuat, + RodriguesVec, Quat, SPQuat, UnitQuaternion # Quaternion math ops logm, expm, ⊖, ⊕, From 0b3bda13e5d492f27067e97240788601ba5dafb0 Mon Sep 17 00:00:00 2001 From: hyrodium Date: Tue, 16 Nov 2021 22:56:17 +0900 Subject: [PATCH 4/7] update README --- README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 8ef3ea13..153adf34 100644 --- a/README.md +++ b/README.md @@ -56,7 +56,7 @@ rotation_angle(r::Rotation) #### Initialization All rotation types support `one(R)` to construct the identity rotation for the desired parameterization. A random rotation, uniformly sampled over the space of rotations, can be sampled using `rand(R)`. For example: ```julia -r = one(QuatRotation) # equivalent to Quat(1.0, 0.0, 0.0, 0.0) +r = one(QuatRotation) # equivalent to QuatRotation(1.0, 0.0, 0.0, 0.0) q = rand(QuatRotation) p = rand(MRP{Float32}) ``` @@ -83,7 +83,7 @@ r = rand(RotMatrix{3}) # uses Float64 by default # create a point p = SVector(1.0, 2.0, 3.0) # from StaticArrays.jl, but could use any AbstractVector... -# convert to a quaternion (Quat) and rotate the point +# convert to a quaternion (QuatRotation) and rotate the point q = QuatRotation(r) p_rotated = q * p @@ -152,7 +152,7 @@ j2 = Rotations.jacobian(q, p) # How does the rotated point q*p change w.r.t. the they follow the same multiplicative *algebra* as quaternions, it is better to think of `QuatRotation` as a 3×3 matrix rather than as a quaternion *number*. - Previously `Quat`. + Previously `Quat`, `UnitQuaternion`. 4. **Rotation Vector** `RotationVec{T}` From 6edc9858d818a64700ad69543c53b0b0719151c7 Mon Sep 17 00:00:00 2001 From: hyrodium Date: Tue, 16 Nov 2021 23:03:19 +0900 Subject: [PATCH 5/7] fix export --- src/Rotations.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Rotations.jl b/src/Rotations.jl index 79cb215b..aea58d3e 100644 --- a/src/Rotations.jl +++ b/src/Rotations.jl @@ -40,7 +40,7 @@ export RotXYZ, RotYXZ, RotZXY, RotXZY, RotYZX, RotZYX, # Deprecated, but export for compatibility - RodriguesVec, Quat, SPQuat, UnitQuaternion + RodriguesVec, Quat, SPQuat, UnitQuaternion, # Quaternion math ops logm, expm, ⊖, ⊕, From 37b3b95a1266899120c6c7227afa37802f75c444 Mon Sep 17 00:00:00 2001 From: hyrodium Date: Wed, 17 Nov 2021 17:12:26 +0900 Subject: [PATCH 6/7] remove old deprecation --- src/deprecated.jl | 10 ---------- test/deprecated.jl | 3 --- 2 files changed, 13 deletions(-) diff --git a/src/deprecated.jl b/src/deprecated.jl index 719aa1c3..81d7639f 100644 --- a/src/deprecated.jl +++ b/src/deprecated.jl @@ -1,12 +1,2 @@ - -# Deprecate RodriguesVec => RotationVec -Base.@deprecate_binding RodriguesVec RotationVec true - -# Deprecate Quat => QuatRotation -Base.@deprecate_binding Quat QuatRotation true - # Deprecate UnitQuaternion => QuatRotation Base.@deprecate_binding UnitQuaternion QuatRotation true - -# Deprecate SPQuat => MRP -Base.@deprecate_binding SPQuat MRP true diff --git a/test/deprecated.jl b/test/deprecated.jl index 2404d99c..ba9be5b0 100644 --- a/test/deprecated.jl +++ b/test/deprecated.jl @@ -1,8 +1,5 @@ @testset "Deprecations" begin # RodriguesVec - @test Base.isdeprecated(Rotations, :RodriguesVec) - @test Base.isdeprecated(Rotations, :Quat) @test Base.isdeprecated(Rotations, :UnitQuaternion) - @test Base.isdeprecated(Rotations, :SPQuat) end From 671ad49bd43c1ddc694a56a8658896c0c5e6af9b Mon Sep 17 00:00:00 2001 From: hyrodium Date: Wed, 17 Nov 2021 17:15:51 +0900 Subject: [PATCH 7/7] update export --- src/Rotations.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Rotations.jl b/src/Rotations.jl index aea58d3e..e2583790 100644 --- a/src/Rotations.jl +++ b/src/Rotations.jl @@ -40,7 +40,7 @@ export RotXYZ, RotYXZ, RotZXY, RotXZY, RotYZX, RotZYX, # Deprecated, but export for compatibility - RodriguesVec, Quat, SPQuat, UnitQuaternion, + UnitQuaternion, # Quaternion math ops logm, expm, ⊖, ⊕,